diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 73976608226406fb3172844aba6097810a89d584..0cb6e600c738cb498050b31f5b54d5ac5827190d 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,6 +1,7 @@
-
stages:
- build-and-test
+ - deploy
+ - post-deploy-test
.build-and-test:
@@ -28,18 +29,12 @@ stages:
- ccache --max-size=20G
- ccache --show-stats
- # Ice configuration to run in Docker.
- - export ICE_DEFAULT_HOST="127.0.0.1"
- - export ICE_RUN_AS_ROOT="true"
-
# Activate Axii.
- source /axii/scripts/install_axii.sh
+ - _axii_auto_env_refresh
script:
- # Create workspace.
- - axii workspace create ~/workspace workspace
- - axii workspace activate workspace
- - _axii_auto_env_refresh
+ - echo "ArmarX Workspace = '$ARMARX_WORKSPACE'"
# Use workspace configuration from project.
- cp "$CI_PROJECT_DIR/.gitlab/ci/armarx-workspace.json" "$ARMARX_WORKSPACE/armarx-workspace.json"
@@ -71,14 +66,6 @@ stages:
- ccache --show-stats
- # Try starting Ice.
- - armarx switch docker_test --ice-host 127.0.0.1 --ice-port 10000 --ice-default-host 127.0.0.1 --mongo-host 127.0.0.1 --mongo-port 10001
- - armarx profile
- - armarx status || true
- #- armarx start
- # armarx status
- #- armarx stop
-
# Test.
# ToDo: Add and use `axii ws test -m "$PROJECT_MODULE"`
- cd "$PROJECT_PATH_IN_WORKSPACE/build"
@@ -93,11 +80,91 @@ build-and-test-bionic:
stage: build-and-test
extends: .build-and-test
- image: git.h2t.iar.kit.edu:5050/sw/armarx/meta/axii:latest-bionic
+ image: git.h2t.iar.kit.edu:5050/sw/armarx/armarx-gui:latest-bionic
build-and-test-jammy:
stage: build-and-test
extends: .build-and-test
- image: git.h2t.iar.kit.edu:5050/sw/armarx/meta/axii:latest-jammy
+ image: git.h2t.iar.kit.edu:5050/sw/armarx/armarx-gui:latest-jammy
+
+
+docker-bionic:
+ stage: deploy
+ needs: ["build-and-test-bionic"]
+ image:
+ name: gcr.io/kaniko-project/executor:v1.9.0-debug
+ entrypoint: [""]
+ script:
+ - /kaniko/executor
+ --context "${CI_PROJECT_DIR}"
+ --dockerfile "${CI_PROJECT_DIR}/docker/bionic"
+ --destination "${CI_REGISTRY_IMAGE}:latest-bionic"
+
+ rules:
+ - if: $CI_COMMIT_BRANCH == "master"
+
+
+docker-jammy:
+ stage: deploy
+ needs: ["build-and-test-jammy"]
+ image:
+ name: gcr.io/kaniko-project/executor:v1.9.0-debug
+ entrypoint: [""]
+ script:
+ - /kaniko/executor
+ --context "${CI_PROJECT_DIR}"
+ --dockerfile "${CI_PROJECT_DIR}/docker/jammy"
+ --destination "${CI_REGISTRY_IMAGE}:latest-jammy"
+
+ rules:
+ - if: $CI_COMMIT_BRANCH == "master"
+
+
+.test-docker-image-common:
+
+ before_script:
+ - source /axii/scripts/install_axii.sh
+ - _axii_auto_env_refresh
+
+ script:
+ - echo "ArmarX Workspace = '$ARMARX_WORKSPACE'"
+ - printenv
+
+ - axii workspace list-modules
+ - axii workspace list-modules --deps
+ - axii workspace info
+ - echo "RobotAPI directory = '$RobotAPI_DIR'"
+
+ - which armarx
+ - which armarx-package
+
+ - armarx switch docker_test --ice-host 127.0.0.1 --ice-port 10000 --ice-default-host 127.0.0.1 --mongo-host 127.0.0.1 --mongo-port 10001
+ - armarx profile
+ - armarx status || true
+
+ - cd $ArmarXGui_DIR
+ - ctest --output-on-failure .
+
+
+test-docker-image-bionic:
+ stage: post-deploy-test
+ needs: ["docker-bionic"]
+ extends: .test-docker-image-common
+
+ image: git.h2t.iar.kit.edu:5050/sw/armarx/robot-api:latest-bionic
+
+ rules:
+ - if: $CI_COMMIT_BRANCH == "master"
+
+
+test-docker-image-jammy:
+ stage: post-deploy-test
+ needs: ["docker-jammy"]
+ extends: .test-docker-image-common
+
+ image: git.h2t.iar.kit.edu:5050/sw/armarx/robot-api:latest-jammy
+
+ rules:
+ - if: $CI_COMMIT_BRANCH == "master"
diff --git a/docker/armarx-workspace.json b/docker/armarx-workspace.json
new file mode 100644
index 0000000000000000000000000000000000000000..7052317cebc5fc6d5def5789b2da698583a252f4
--- /dev/null
+++ b/docker/armarx-workspace.json
@@ -0,0 +1,14 @@
+{
+ "modules": {
+ "armarx/RobotAPI": {}
+ },
+ "global": {
+ "prepare": {
+ "cmake": {
+ "definitions": {
+ "CMAKE_BUILD_TYPE": "RelWithDebInfo"
+ }
+ }
+ }
+ }
+}
diff --git a/docker/bionic b/docker/bionic
new file mode 100644
index 0000000000000000000000000000000000000000..66e01cea105fa3c1f84f0592df96f7fecd93d40f
--- /dev/null
+++ b/docker/bionic
@@ -0,0 +1,24 @@
+FROM git.h2t.iar.kit.edu:5050/sw/armarx/armarx-gui:latest-bionic
+
+# Setup environment.
+SHELL ["/bin/bash", "-c"]
+
+# Setup apt environment.
+RUN apt-get -qq update
+
+# Setup repository in Docker.
+WORKDIR $ARMARX_WORKSPACE/armarx/RobotAPI
+COPY --chmod=755 . .
+
+# Use workspace config.
+COPY docker/armarx-workspace.json $ARMARX_WORKSPACE/armarx-workspace.json
+
+# Fix "CMake Error in CMakeLists.txt: Imported target "VirtualRobot" includes non-existent path "/usr/lib/include"
+# (caused by at least dmp)
+RUN mkdir -p /usr/lib/include
+
+# Run the upgrade.
+RUN axii workspace system --accept-apt-install
+# ToDo: Prevent this from updating the target itself.
+RUN axii workspace update --prefer-https
+RUN axii workspace upgrade
diff --git a/docker/jammy b/docker/jammy
new file mode 100644
index 0000000000000000000000000000000000000000..1eef819fef3a422d72cc063ff21054897d313ab0
--- /dev/null
+++ b/docker/jammy
@@ -0,0 +1,20 @@
+FROM git.h2t.iar.kit.edu:5050/sw/armarx/armarx-gui:latest-jammy
+
+# Setup environment.
+SHELL ["/bin/bash", "-c"]
+
+# Setup apt environment.
+RUN apt-get -qq update
+
+# Setup repository in Docker.
+WORKDIR $ARMARX_WORKSPACE/armarx/RobotAPI
+COPY --chmod=755 . .
+
+# Use workspace config.
+COPY docker/armarx-workspace.json $ARMARX_WORKSPACE/armarx-workspace.json
+
+# Run the upgrade.
+RUN axii workspace system --accept-apt-install
+# ToDo: Prevent this from updating the target itself.
+RUN axii workspace update --prefer-https
+RUN axii workspace upgrade
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 4722e9c5ce8deaabfc33a917c9b0c744d1a781fd..12dc68399fc92b3d32816dd212d75fc4b0cdab8c 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -1,23 +1,24 @@
-#include <regex>
-#include <fstream>
-
#include "VisualizationRobot.h"
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <fstream>
+#include <regex>
+
#include <VirtualRobot/SceneObject.h>
-#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
namespace armarx::viz::coin
{
namespace
{
- VirtualRobot::RobotPtr loadRobot(std::string const& project, std::string const& filename)
+ VirtualRobot::RobotPtr
+ loadRobot(std::string const& project, std::string const& filename)
{
- VirtualRobot::RobotPtr result;
+ VirtualRobot::RobotPtr result;
if (filename.empty())
{
@@ -32,7 +33,8 @@ namespace armarx::viz::coin
ARMARX_INFO << deactivateSpam()
<< "You specified the absolute path to the robot file:"
<< "\n\t'" << filename << "'"
- << "\nConsider specifying the containing ArmarX package and relative data path instead to "
+ << "\nConsider specifying the containing ArmarX package and relative "
+ "data path instead to "
<< "improve portability to other systems.";
}
// We need to always check that the file is readable otherwise, VirtualRobot::RobotIO::loadRobot crashes
@@ -59,9 +61,7 @@ namespace armarx::viz::coin
if (result)
{
result->setThreadsafe(false);
- // Do we want to propagate joint values? Probably not...
- // Closing the hand on the real robot could be implemented on another level
- result->setPropagatingJointValuesEnabled(false);
+ result->setPropagatingJointValuesEnabled(true);
}
else
{
@@ -88,7 +88,8 @@ namespace armarx::viz::coin
static std::vector<RobotInstancePool> robotCache;
- LoadedRobot getRobotFromCache(std::string const& project, std::string const& filename)
+ LoadedRobot
+ getRobotFromCache(std::string const& project, std::string const& filename)
{
// We can use a global variable, since this code is only executed in the GUI thread
@@ -104,7 +105,8 @@ namespace armarx::viz::coin
if (instancePool.usedInstances < instancePool.robots.size())
{
// 1) We have still unused instances in the pool ==> Just return one
- ARMARX_DEBUG << "Reusing robot instance from cache " << VAROUT(project) << ", " << VAROUT(filename);
+ ARMARX_DEBUG << "Reusing robot instance from cache " << VAROUT(project)
+ << ", " << VAROUT(filename);
result.robot = instancePool.robots[instancePool.usedInstances];
instancePool.usedInstances += 1;
}
@@ -112,14 +114,17 @@ namespace armarx::viz::coin
else
{
// 2) We do not have unused instances in the pool ==> Clone one
- ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", " << VAROUT(filename);
+ ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", "
+ << VAROUT(filename);
if (instancePool.robots.size() > 0)
{
- VirtualRobot::RobotPtr const& robotToClone = instancePool.robots.front();
+ VirtualRobot::RobotPtr const& robotToClone =
+ instancePool.robots.front();
float scaling = 1.0f;
bool preventCloningMeshesIfScalingIs1 = true;
- result.robot = robotToClone->clone(nullptr, scaling, preventCloningMeshesIfScalingIs1);
+ result.robot = robotToClone->clone(
+ nullptr, scaling, preventCloningMeshesIfScalingIs1);
// Insert the cloned robot into the instance pool
instancePool.robots.push_back(result.robot);
@@ -127,8 +132,10 @@ namespace armarx::viz::coin
}
else
{
- ARMARX_WARNING << "Encountered empty robot instance pool while trying to clone new instance"
- << "\nRobot: " << VAROUT(project) << ", " << VAROUT(filename)
+ ARMARX_WARNING << "Encountered empty robot instance pool while trying "
+ "to clone new instance"
+ << "\nRobot: " << VAROUT(project) << ", "
+ << VAROUT(filename)
<< "\nUsed instances: " << instancePool.usedInstances
<< "\nRobots: " << instancePool.robots.size();
}
@@ -137,7 +144,8 @@ namespace armarx::viz::coin
}
}
- ARMARX_DEBUG << "Loading robot from file " << VAROUT(project) << ", " << VAROUT(filename);
+ ARMARX_DEBUG << "Loading robot from file " << VAROUT(project) << ", "
+ << VAROUT(filename);
result.robot = loadRobot(project, filename);
if (result.robot)
{
@@ -146,13 +154,15 @@ namespace armarx::viz::coin
instancePool.filename = filename;
instancePool.robots.push_back(result.robot);
instancePool.usedInstances = 1;
- } else
+ }
+ else
{
- ARMARX_WARNING << deactivateSpam(5) << "Robot " << VAROUT(project) << ", " << VAROUT(filename) << "could not be loaded!";
+ ARMARX_WARNING << deactivateSpam(5) << "Robot " << VAROUT(project) << ", "
+ << VAROUT(filename) << "could not be loaded!";
}
return result;
}
- }
+ } // namespace
VisualizationRobot::~VisualizationRobot()
{
@@ -160,7 +170,8 @@ namespace armarx::viz::coin
{
if (instancePool.project == loaded.project && instancePool.filename == loaded.filename)
{
- ARMARX_DEBUG << "Removing robot from chace " << VAROUT(loaded.project) << ", " << VAROUT(loaded.filename);
+ ARMARX_DEBUG << "Removing robot from chace " << VAROUT(loaded.project) << ", "
+ << VAROUT(loaded.filename);
std::vector<VirtualRobot::RobotPtr>& robots = instancePool.robots;
auto robotIter = std::find(robots.begin(), robots.end(), loaded.robot);
if (robotIter != robots.end())
@@ -176,7 +187,8 @@ namespace armarx::viz::coin
{
ARMARX_WARNING << "Expected there to be at least one used instance "
<< "while trying to put robot instance back into the pool"
- << "\nRobot: " << VAROUT(loaded.project) << ", " << VAROUT(loaded.filename)
+ << "\nRobot: " << VAROUT(loaded.project) << ", "
+ << VAROUT(loaded.filename)
<< "\nUsed instances: " << instancePool.usedInstances;
}
}
@@ -184,12 +196,14 @@ namespace armarx::viz::coin
}
}
- bool VisualizationRobot::update(ElementType const& element)
+ bool
+ VisualizationRobot::update(ElementType const& element)
{
IceUtil::Time time_start = IceUtil::Time::now();
- (void) time_start;
+ (void)time_start;
- bool robotChanged = loaded.project != element.project || loaded.filename != element.filename;
+ bool robotChanged =
+ loaded.project != element.project || loaded.filename != element.filename;
if (robotChanged)
{
// The robot file changed, so reload the robot
@@ -199,8 +213,7 @@ namespace armarx::viz::coin
{
ARMARX_WARNING << deactivateSpam(10)
<< "Robot will not visualized since it could not be loaded."
- << "\nID: " << element.id
- << "\nProject: " << element.project
+ << "\nID: " << element.id << "\nProject: " << element.project
<< "\nFilename: " << element.filename;
return true;
}
@@ -253,10 +266,8 @@ namespace armarx::viz::coin
if (loadedDrawStyle & data::ModelDrawStyle::OVERRIDE_COLOR)
{
- if (loadedColor.r != element.color.r
- || loadedColor.g != element.color.g
- || loadedColor.b != element.color.b
- || loadedColor.a != element.color.a)
+ if (loadedColor.r != element.color.r || loadedColor.g != element.color.g ||
+ loadedColor.b != element.color.b || loadedColor.a != element.color.a)
{
int numChildren = node->getNumChildren();
for (int i = 0; i < numChildren; i++)
@@ -294,7 +305,8 @@ namespace armarx::viz::coin
return true;
}
- void VisualizationRobot::recreateVisualizationNodes(int drawStyle)
+ void
+ VisualizationRobot::recreateVisualizationNodes(int drawStyle)
{
VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
if (drawStyle & data::ModelDrawStyle::COLLISION)
@@ -331,8 +343,9 @@ namespace armarx::viz::coin
}
}
- void clearRobotCache()
+ void
+ clearRobotCache()
{
robotCache.clear();
}
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
index e4e5b22aa996ecb88f8b63b3db8bba4831d699cd..605911657b2bb74dc53cdf114c4304a82a41be4a 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.cpp
@@ -5,74 +5,73 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Metronome.h>
-#include <RobotAPI/libraries/core/Pose.h>
-
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
-
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx
{
- GraspProviderExamplePropertyDefinitions::GraspProviderExamplePropertyDefinitions(std::string prefix) :
+ GraspProviderExamplePropertyDefinitions::GraspProviderExamplePropertyDefinitions(
+ std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
}
- armarx::PropertyDefinitionsPtr GraspProviderExample::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ GraspProviderExample::createPropertyDefinitions()
{
ARMARX_IMPORTANT << "Prperty defs";
- armarx::PropertyDefinitionsPtr defs = new GraspProviderExamplePropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new GraspProviderExamplePropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
return defs;
-
}
- std::string GraspProviderExample::getDefaultName() const
+ std::string
+ GraspProviderExample::getDefaultName() const
{
return "GraspProviderExample";
}
- GraspProviderExample::GraspProviderExample() : writer(memoryNameSystem()), reader(memoryNameSystem())
+ GraspProviderExample::GraspProviderExample()
{
-
}
-
- void GraspProviderExample::onInitComponent()
+ void
+ GraspProviderExample::onInitComponent()
{
ARMARX_IMPORTANT << "Init";
}
-
- void GraspProviderExample::onConnectComponent()
+ void
+ GraspProviderExample::onConnectComponent()
{
- writer.connect();
- reader.connect();
+ writer.connect(memoryNameSystem());
+ reader.connect(memoryNameSystem());
task = new RunningTask<GraspProviderExample>(this, &GraspProviderExample::run);
task->start();
}
-
- void GraspProviderExample::onDisconnectComponent()
+ void
+ GraspProviderExample::onDisconnectComponent()
{
task->stop();
}
-
- void GraspProviderExample::onExitComponent()
+ void
+ GraspProviderExample::onExitComponent()
{
}
-
- void GraspProviderExample::run()
+ void
+ GraspProviderExample::run()
{
ARMARX_IMPORTANT << "Running example.";
@@ -90,9 +89,11 @@ namespace armarx
// initialize all necessary fields of a bimanual grasp candidate and use writer to commit it to memory
grasping::BimanualGraspCandidate bimanualCandidate = makeDummyBimanualGraspCandidate();
- bimanualCandidate.groupNr = i; //non-necessary field, but used to commit different candidates
+ bimanualCandidate.groupNr =
+ i; //non-necessary field, but used to commit different candidates
- writer.commitBimanualGraspCandidate(bimanualCandidate, armem::Time::Now(), "bimanualProvider");
+ writer.commitBimanualGraspCandidate(
+ bimanualCandidate, armem::Time::Now(), "bimanualProvider");
//test for writing Seqs, candidates from the same object appear as instances of the same snapshot
@@ -102,7 +103,8 @@ namespace armarx
candidatesToWrite.push_back(new grasping::GraspCandidate(candidate));
- writer.commitGraspCandidateSeq(candidatesToWrite, armem::Time::Now(), "candidateProvider");
+ writer.commitGraspCandidateSeq(
+ candidatesToWrite, armem::Time::Now(), "candidateProvider");
// test reader and debug by logging the group number of the candidate
@@ -112,15 +114,15 @@ namespace armarx
{
candidates = reader.queryLatestGraspCandidates();
}
- catch (armem::error::QueryFailed &e)
+ catch (armem::error::QueryFailed& e)
{
ARMARX_ERROR << e.makeMsg(memoryName);
}
- for (auto &[id, ca] : candidates)
+ for (auto& [id, ca] : candidates)
{
- ARMARX_INFO << "candidate with ID " << id << " has group number " << ca->groupNr ;
+ ARMARX_INFO << "candidate with ID " << id << " has group number " << ca->groupNr;
}
std::map<std::string, grasping::BimanualGraspCandidatePtr> bimanualCandidates;
@@ -129,21 +131,23 @@ namespace armarx
{
bimanualCandidates = reader.queryLatestBimanualGraspCandidates();
}
- catch (armem::error::QueryFailed &e)
+ catch (armem::error::QueryFailed& e)
{
ARMARX_ERROR << e.makeMsg(memoryName);
}
- for (auto &[id, ca] : bimanualCandidates)
+ for (auto& [id, ca] : bimanualCandidates)
{
- ARMARX_INFO << "bimanual candidate with ID " << id << " has group number " << ca->groupNr ;
+ ARMARX_INFO << "bimanual candidate with ID " << id << " has group number "
+ << ca->groupNr;
}
m.waitForNextTick();
}
}
- grasping::GraspCandidate GraspProviderExample::makeDummyGraspCandidate()
+ grasping::GraspCandidate
+ GraspProviderExample::makeDummyGraspCandidate()
{
armarx::grasping::GraspCandidate candidate = armarx::grasping::GraspCandidate();
@@ -168,9 +172,11 @@ namespace armarx
return candidate;
}
- grasping::BimanualGraspCandidate GraspProviderExample::makeDummyBimanualGraspCandidate()
+ grasping::BimanualGraspCandidate
+ GraspProviderExample::makeDummyBimanualGraspCandidate()
{
- armarx::grasping::BimanualGraspCandidate bimanualCandidate = armarx::grasping::BimanualGraspCandidate();
+ armarx::grasping::BimanualGraspCandidate bimanualCandidate =
+ armarx::grasping::BimanualGraspCandidate();
bimanualCandidate.approachVectorLeft = Vector3BasePtr(toIce(zeroVector));
bimanualCandidate.approachVectorRight = Vector3BasePtr(toIce(zeroVector));
@@ -185,4 +191,4 @@ namespace armarx
return bimanualCandidate;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
index 9f13e111eec8e51f3fa67e5d04f03d3df1fd0b64..c1a2f394afe6b50a6d5f5da71ff7296ec5e42d56 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
@@ -21,33 +21,31 @@
*/
-#include "RobotStatePredictionClientExample.h"
-#include "Impl.h"
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "Impl.h"
+#include "RobotStatePredictionClientExample.h"
namespace armarx::robot_state_prediction_client_example
{
- Component::Component() :
- pimpl(std::make_unique<Impl>(memoryNameSystem()))
+ Component::Component() : pimpl(std::make_unique<Impl>())
{
}
-
RobotStatePredictionClientExample::~RobotStatePredictionClientExample() = default;
-
- std::string Component::getDefaultName() const
+ std::string
+ Component::getDefaultName() const
{
return "RobotStatePredictionClientExample";
}
-
- armarx::PropertyDefinitionsPtr Component::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
ARMARX_CHECK_NOT_NULL(pimpl);
pimpl->defineProperties(defs, "p.");
@@ -55,13 +53,13 @@ namespace armarx::robot_state_prediction_client_example
return defs;
}
-
- void Component::onInitComponent()
+ void
+ Component::onInitComponent()
{
}
-
- void Component::onConnectComponent()
+ void
+ Component::onConnectComponent()
{
pimpl->connect(memoryNameSystem(), arviz);
pimpl->start();
@@ -70,19 +68,19 @@ namespace armarx::robot_state_prediction_client_example
RemoteGui_startRunningTask();
}
-
- void Component::onDisconnectComponent()
+ void
+ Component::onDisconnectComponent()
{
pimpl->stop();
}
-
- void Component::onExitComponent()
+ void
+ Component::onExitComponent()
{
}
-
- void Component::createRemoteGuiTab()
+ void
+ Component::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
@@ -90,8 +88,9 @@ namespace armarx::robot_state_prediction_client_example
RemoteGui_createTab(getName(), root, &tab);
}
- void Component::RemoteGui_update()
+ void
+ Component::RemoteGui_update()
{
}
-}
+} // namespace armarx::robot_state_prediction_client_example
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
index c433669630c9631e471666fa3b3cd02acd25c2f9..a303f1c393b549a907bcfa49c1434a26bd873468 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
@@ -37,7 +37,6 @@
#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
-
namespace simox::alg
{
template <class... Args>
@@ -49,7 +48,6 @@ namespace simox::alg
return conc;
}
-
template <class KeyT, class ValueT>
std::map<KeyT, ValueT>
map_from_key_value_pairs(const std::vector<KeyT>& lhs, const std::vector<ValueT>& rhs)
@@ -64,7 +62,6 @@ namespace simox::alg
return map;
}
-
template <class KeyT, class ValueT>
std::vector<ValueT>
multi_at(const std::map<KeyT, ValueT>& map,
@@ -111,15 +108,13 @@ namespace simox::alg
namespace armarx::robot_state_prediction_client_example
{
- Impl::Impl(armem::client::MemoryNameSystem& memoryNameSystem)
+ Impl::Impl()
{
- client.remote.robotReader.emplace(memoryNameSystem);
+ client.remote.robotReader.emplace();
}
-
Impl::~Impl() = default;
-
void
Impl::defineProperties(IceUtil::Handle<PropertyDefinitionContainer>& defs,
const std::string& prefix)
@@ -133,7 +128,6 @@ namespace armarx::robot_state_prediction_client_example
client.remote.robotReader->registerPropertyDefinitions(defs);
}
-
void
Impl::connect(armem::client::MemoryNameSystem& mns, viz::Client arviz)
{
@@ -148,12 +142,11 @@ namespace armarx::robot_state_prediction_client_example
ARMARX_WARNING << e.what();
}
- client.remote.robotReader->connect();
+ client.remote.robotReader->connect(mns);
this->remote.arviz = arviz;
}
-
void
Impl::start()
{
@@ -161,14 +154,12 @@ namespace armarx::robot_state_prediction_client_example
task->start();
}
-
void
Impl::stop()
{
task->stop();
}
-
void
Impl::run()
{
@@ -182,7 +173,6 @@ namespace armarx::robot_state_prediction_client_example
}
}
-
void
Impl::runOnce()
{
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
index 272f9e2e62d6ac16b49a2674a7952d96e8618975..d716f5d94d7502c4c0533485c52ccd2f0a0dc732 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
@@ -27,27 +27,25 @@
#include <ArmarXCore/util/tasks.h>
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-#include <RobotAPI/libraries/armem/client/forward_declarations.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-
#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include "RobotStatePredictionClient.h"
-
namespace armarx::robot_state_prediction_client_example
{
class Impl
{
public:
-
- Impl(armem::client::MemoryNameSystem& memoryNameSystem);
+ Impl();
~Impl();
- void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs, const std::string& prefix);
+ void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs,
+ const std::string& prefix);
void connect(armem::client::MemoryNameSystem& mns, viz::Client arviz);
void start();
@@ -58,7 +56,6 @@ namespace armarx::robot_state_prediction_client_example
public:
-
struct Properties
{
float updateFrequencyHz = 10;
@@ -66,12 +63,14 @@ namespace armarx::robot_state_prediction_client_example
std::string robotName = "Armar6";
float predictAheadSeconds = 1.0;
};
+
Properties properties;
struct Remote
{
viz::Client arviz;
};
+
Remote remote;
armarx::SimpleRunningTask<>::pointer_type task;
@@ -82,6 +81,5 @@ namespace armarx::robot_state_prediction_client_example
std::optional<std::vector<armem::MemoryID>> localizationEntityIDs;
std::optional<std::vector<armem::MemoryID>> propioceptionEntityIDs;
std::optional<viz::Robot> robotViz;
-
};
-}
+} // namespace armarx::robot_state_prediction_client_example
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index 02cbe58cef253fc0d741c182caa32b7c3d2a529c..d77d737156239241ed8a62952e788da11c99bb55 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -156,6 +156,16 @@ namespace armarx
return SkillManagerComponentPluginUser::executeSkill(info, current);
}
+ skills::manager::dto::SkillExecutionID
+ SkillsMemory::executeSkillAsync(const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current)
+ {
+ auto e = skills::SkillExecutionRequest::FromIce(info);
+ skillExecutionRequestCoreSegment.addSkillExecutionRequest(e);
+
+ return SkillManagerComponentPluginUser::executeSkillAsync(info, current);
+ }
+
void
SkillsMemory::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update,
const skills::callback::dto::ProviderID& providerId,
@@ -166,14 +176,27 @@ namespace armarx
skillEventCoreSegment.addSkillUpdateEvent(u);
}
+ IceUtil::Optional<skills::manager::dto::SkillStatusUpdate>
+ SkillsMemory::getSkillExecutionStatus(const skills::manager::dto::SkillExecutionID& executionId,
+ const Ice::Current& current)
+ {
+ auto eid = skills::SkillExecutionID::FromIce(executionId);
+ auto op = this->skillEventCoreSegment.getSkillStatusUpdate(eid);
+ if (op.has_value())
+ {
+ return op->toManagerIce();
+ }
+ return {};
+ }
+
skills::manager::dto::SkillStatusUpdateMap
- SkillsMemory::getLatestSkillExecutionStatuses(int n, const Ice::Current& current)
+ SkillsMemory::getSkillExecutionStatuses(const Ice::Current& current)
{
- auto m = skillEventCoreSegment.getLatestSkillEvents(n);
skills::manager::dto::SkillStatusUpdateMap ret;
- for (const auto& [k, v] : m)
+ auto updates = this->skillEventCoreSegment.getSkillStatusUpdates();
+ for (const auto& [k, v] : updates)
{
- ret[k.toManagerIce()] = v.toManagerIce();
+ ret.insert({k.toManagerIce(), v.toManagerIce()});
}
return ret;
}
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
index 81109b0bdf8787d6446e02fc11dcfe56ba484c4a..ead9f19122bc80266196aff40f138b4bd87c77f1 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
@@ -85,12 +85,21 @@ namespace armarx
executeSkill(const skills::manager::dto::SkillExecutionRequest& info,
const Ice::Current& current) override;
+
+ skills::manager::dto::SkillExecutionID
+ executeSkillAsync(const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current) override;
+
void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update,
const skills::callback::dto::ProviderID& id,
const Ice::Current& current) override;
+ IceUtil::Optional<skills::manager::dto::SkillStatusUpdate>
+ getSkillExecutionStatus(const skills::manager::dto::SkillExecutionID& executionId,
+ const Ice::Current& current) override;
+
skills::manager::dto::SkillStatusUpdateMap
- getLatestSkillExecutionStatuses(int n, const Ice::Current& current) override;
+ getSkillExecutionStatuses(const Ice::Current& current) override;
// WritingInterface interface
armem::data::CommitResult commit(const armem::data::Commit& commit,
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
index afbb7ef2abd07d6e718c7480ba7d84455574041d..b13f663e25b6c31fbb4ab3b919ce81431d617b70 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
+++ b/source/RobotAPI/components/skills/SkillProviderExample/CMakeLists.txt
@@ -13,10 +13,20 @@ set(COMPONENT_LIBS
set(SOURCES
SkillProviderExample.cpp
+ HelloWorld.cpp
+ Incomplete.cpp
+ Chaining.cpp
+ Callback.cpp
+ Timeout.cpp
)
set(HEADERS
SkillProviderExample.h
+ HelloWorld.h
+ Incomplete.h
+ Chaining.h
+ Callback.h
+ Timeout.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6dcea52a55191f03716aea507d13cd6c831d8d87
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp
@@ -0,0 +1,37 @@
+
+#include "Callback.h"
+
+namespace armarx::skills::provider
+{
+ CallbackSkill::CallbackSkill() : SimpleSkill(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ CallbackSkill::GetSkillDescription()
+ {
+ return SkillDescription{.skillId = skills::SkillID{.skillName = "ShowMeCallbacks"},
+ .description = "This skill does shows callbacks",
+ .timeout = armarx::core::time::Duration::MilliSeconds(1000)};
+ }
+
+ Skill::MainResult
+ CallbackSkill::main(const MainInput& in)
+ {
+ ARMARX_IMPORTANT << "Logging three updates via the callback";
+ auto up1 = std::make_shared<aron::data::Dict>();
+ up1->addElement("updateInfo", std::make_shared<aron::data::String>("Update 1"));
+
+ in.callback(skills::SkillStatus::Running, up1);
+
+ auto up2 = std::make_shared<aron::data::Dict>();
+ up2->addElement("updateInfo", std::make_shared<aron::data::String>("Update 2"));
+ in.callback(skills::SkillStatus::Running, up2);
+
+ auto up3 = std::make_shared<aron::data::Dict>();
+ up3->addElement("updateInfo", std::make_shared<aron::data::String>("Update 3"));
+ in.callback(skills::SkillStatus::Running, up3);
+
+ return {TerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Callback.h b/source/RobotAPI/components/skills/SkillProviderExample/Callback.h
new file mode 100644
index 0000000000000000000000000000000000000000..c29c2b6f3d6007b1fda30b0c781b91be03a15232
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Callback.h
@@ -0,0 +1,41 @@
+
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+
+namespace armarx::skills::provider
+{
+ class CallbackSkill : public SimpleSkill
+ {
+ public:
+ CallbackSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const MainInput&) final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..983902512c03bc28c8d4429fd22c4690aeb14833
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp
@@ -0,0 +1,40 @@
+
+
+#include "Chaining.h"
+
+namespace armarx::skills::provider
+{
+
+ ChainingSkill::ChainingSkill() : SimpleSkill(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ ChainingSkill::GetSkillDescription()
+ {
+ return SkillDescription{.skillId = armarx::skills::SkillID{.skillName = "ChainingSkill"},
+ .description =
+ "This skill calls the Timeout skill three times. The last "
+ "execution is aborted due to a timeout of this skill.",
+ .timeout = armarx::core::time::Duration::MilliSeconds(5000)};
+ }
+
+ Skill::MainResult
+ ChainingSkill::main(const MainInput& in)
+ {
+ SkillProxy prx(
+ manager,
+ skills::SkillID{.providerId = *getSkillId().providerId, .skillName = "Timeout"});
+
+ ARMARX_INFO << "CALL PROXY FIRST TIME";
+ callSubskill(prx);
+ ARMARX_INFO << "CALL PROXY SECOND TIME";
+ callSubskill(prx);
+ ARMARX_INFO << "CALL PROXY THIRD TIME";
+ callSubskill(prx);
+
+ this->throwIfSkillShouldTerminate();
+
+ return {TerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Chaining.h b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.h
new file mode 100644
index 0000000000000000000000000000000000000000..991c4a5aaab5d1be83f9684ca5bdbca1638b946d
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.h
@@ -0,0 +1,40 @@
+
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+
+namespace armarx::skills::provider
+{
+ class ChainingSkill : public SimpleSkill
+ {
+ public:
+ ChainingSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const MainInput&) final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..edf918402a4c8ed1bbe510ab553d6dc568540dd5
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
@@ -0,0 +1,47 @@
+
+
+#include "HelloWorld.h"
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include <RobotAPI/libraries/aron/core/type/variant/primitive/String.h>
+
+namespace armarx::skills::provider
+{
+ HelloWorldSkill::HelloWorldSkill() :
+ SimpleSpecializedSkill<skills::Example::HelloWorldAcceptedType>(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ HelloWorldSkill::GetSkillDescription()
+ {
+ armarx::skills::Example::HelloWorldAcceptedType root_profile_params;
+ root_profile_params.some_float = 5;
+ root_profile_params.some_int = 42;
+ root_profile_params.some_text = "YOLO";
+ root_profile_params.some_optional_text = "OPTIONAL";
+ root_profile_params.some_list_of_matrices.push_back(Eigen::Matrix3f::Zero());
+ root_profile_params.some_matrix = Eigen::Matrix3f::Zero();
+
+ return SkillDescription{.skillId = skills::SkillID{.skillName = "HelloWorld"},
+ .description = "This skill logs a message on ARMARX_IMPORTANT",
+ .rootProfileDefaults = root_profile_params.toAron(),
+ .timeout = armarx::core::time::Duration::MilliSeconds(1000),
+ .parametersType =
+ armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
+ }
+
+ Skill::MainResult
+ HelloWorldSkill::main(const SpecializedMainInput& in)
+ {
+ ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n"
+ << "I received the following data: \n"
+ << aron::data::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(
+ in.parameters.toAron())
+ .dump(2)
+ << "\n"
+ << "(executed at: " << IceUtil::Time::now() << ")";
+ return {TerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.h b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.h
new file mode 100644
index 0000000000000000000000000000000000000000..840b058fb171685db777126157fc21028db89c99
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.h
@@ -0,0 +1,42 @@
+
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// RobotAPI
+#include <RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.aron.generated.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
+
+namespace armarx::skills::provider
+{
+ // Skills:
+ class HelloWorldSkill : public SimpleSpecializedSkill<skills::Example::HelloWorldAcceptedType>
+ {
+ public:
+ HelloWorldSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::MainResult main(const SpecializedMainInput& in) final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8d6b4f277dcb36adc2b73bfe94b45b9adb500b22
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.cpp
@@ -0,0 +1,53 @@
+
+
+#include "Incomplete.h"
+
+#include <RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.aron.generated.h>
+
+#include "HelloWorld.h"
+
+namespace armarx::skills::provider
+{
+
+ IncompleteSkill::IncompleteSkill() : SimpleSkill(GetSkillDescription())
+ {
+ }
+
+ SkillDescription
+ IncompleteSkill::GetSkillDescription()
+ {
+ auto d = HelloWorldSkill::GetSkillDescription();
+ return SkillDescription{.skillId = {.skillName = "IncompleteSkill"},
+ .description = d.description,
+ .timeout = d.timeout + armarx::core::time::Duration::Seconds(2),
+ .parametersType = d.parametersType};
+ }
+
+ Skill::PrepareResult
+ IncompleteSkill::prepare()
+ {
+ if (!first_prepared)
+ {
+ first_prepared = true;
+
+ // set parameters after two seconds...
+ std::thread foo(
+ [&]()
+ {
+ auto d = HelloWorldSkill::GetSkillDescription();
+ std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+ this->setParameters(d.rootProfileDefaults);
+ });
+ foo.detach();
+ }
+
+ return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::MainResult
+ IncompleteSkill::main(const MainInput& in)
+ {
+ auto s = HelloWorldSkill();
+ return s.mainOfSkill();
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.h b/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.h
new file mode 100644
index 0000000000000000000000000000000000000000..6cf7b6e73b597b201d9d5213db160151d3a971c8
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Incomplete.h
@@ -0,0 +1,44 @@
+
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/SimpleSkill.h>
+
+namespace armarx::skills::provider
+{
+
+ class IncompleteSkill : public SimpleSkill
+ {
+ public:
+ IncompleteSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ Skill::PrepareResult prepare() final;
+ Skill::MainResult main(const MainInput&) final;
+
+ std::atomic_bool first_prepared = false;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index 2fbc92142ed7d9da9a381f450e0772cc87460b59..5ab480cd5c05556e2c7ea486ac0ec3976b1eb6df 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -1,191 +1,8 @@
-
#include "SkillProviderExample.h"
-#include <RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.aron.generated.h>
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
-#include <RobotAPI/libraries/aron/core/type/variant/primitive/String.h>
-
namespace armarx::skills::provider
{
- HelloWorldSkill::HelloWorldSkill() : Skill(GetSkillDescription())
- {
- }
-
- SkillDescription
- HelloWorldSkill::GetSkillDescription()
- {
- armarx::skills::Example::HelloWorldAcceptedType root_profile_params;
- root_profile_params.some_float = 5;
- root_profile_params.some_int = 42;
- root_profile_params.some_text = "YOLO";
- root_profile_params.some_list_of_matrices.push_back(Eigen::Matrix3f::Zero());
- //default_params.some_matrix = Eigen::Matrix3f::Zero();
-
- return SkillDescription{{"HelloWorld"},
- "This skill logs a message on ARMARX_IMPORTANT",
- root_profile_params.toAron(),
- armarx::core::time::Duration::MilliSeconds(1000),
- armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
- }
-
- Skill::MainResult
- HelloWorldSkill::main()
- {
- ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n"
- << "I received the following data: \n"
- << aron::data::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(
- parameters)
- .dump(2)
- << "\n"
- << "Type fulfilled? "
- << parameters->fullfillsType(
- armarx::skills::Example::HelloWorldAcceptedType::ToAronType())
- << "\n"
- << "(executed at: " << IceUtil::Time::now() << ")";
- return {TerminatedSkillStatus::Succeeded, nullptr};
- }
-
- IncompleteSkill::IncompleteSkill() : Skill(GetSkillDescription())
- {
- }
-
- SkillDescription
- IncompleteSkill::GetSkillDescription()
- {
- auto d = HelloWorldSkill::GetSkillDescription();
- return SkillDescription{{"IncompleteSkill"},
- d.description,
- {},
- d.timeout + armarx::core::time::Duration::Seconds(2),
- d.acceptedType};
- }
-
- Skill::PrepareResult
- IncompleteSkill::prepare()
- {
- if (!first_prepared)
- {
- first_prepared = true;
- std::thread foo(
- [&]()
- {
- auto d = HelloWorldSkill::GetSkillDescription();
- std::this_thread::sleep_for(std::chrono::milliseconds(2000));
- this->setParameters(d.rootProfileParameterization);
- });
- foo.detach();
- }
-
- auto s = HelloWorldSkill();
- s.setParameters(this->getParameters());
- s.mainOfSkill();
-
- return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
- }
-
- Skill::MainResult
- IncompleteSkill::main()
- {
- auto s = HelloWorldSkill();
- s.setParameters(this->getParameters());
- return s.mainOfSkill();
- }
-
- ChainingSkill::ChainingSkill() : Skill(GetSkillDescription())
- {
- }
-
- SkillDescription
- ChainingSkill::GetSkillDescription()
- {
- return SkillDescription{{"ChainingSkill"},
- "This skill calls the HelloWorld skill three times.",
- {},
- armarx::core::time::Duration::MilliSeconds(3000),
- nullptr};
- }
-
- Skill::MainResult
- ChainingSkill::main()
- {
- armarx::skills::Example::HelloWorldAcceptedType exec1;
- armarx::skills::Example::HelloWorldAcceptedType exec2;
- armarx::skills::Example::HelloWorldAcceptedType exec3;
-
- exec1.some_text = "Hello from the ChainingSkill 1";
- exec2.some_text = "Hello from the ChainingSkill 2";
- exec3.some_text = "Hello from the ChainingSkill 3";
-
- SkillProxy skillExecPrx(
- manager, skills::SkillID(skills::ProviderID("SkillProviderExample"), "HelloWorld"));
-
- skillExecPrx.executeSkill(getSkillId().toString(), exec1.toAron());
- skillExecPrx.executeSkill(getSkillId().toString(), exec2.toAron());
- skillExecPrx.executeSkill(getSkillId().toString(), exec3.toAron());
-
- return {TerminatedSkillStatus::Succeeded, nullptr};
- }
-
- TimeoutSkill::TimeoutSkill() :
- PeriodicSkill(GetSkillDescription(), armarx::core::time::Frequency::Hertz(5))
- {
- }
-
- SkillDescription
- TimeoutSkill::GetSkillDescription()
- {
- return SkillDescription{{"Timeout"},
- "This fails with timeout reached",
- {},
- armarx::core::time::Duration::MilliSeconds(5000),
- nullptr};
- }
-
- PeriodicSkill::StepResult
- TimeoutSkill::step()
- {
- // do heavy work
- std::this_thread::sleep_for(std::chrono::milliseconds(500));
-
- return {ActiveOrTerminatedSkillStatus::Running, nullptr};
- }
-
- CallbackSkill::CallbackSkill() : Skill(GetSkillDescription())
- {
- }
-
- SkillDescription
- CallbackSkill::GetSkillDescription()
- {
- return SkillDescription{{"ShowMeCallbacks"},
- "This skill does shows callbacks",
- {},
- armarx::core::time::Duration::MilliSeconds(1000),
- nullptr};
- }
-
- Skill::MainResult
- CallbackSkill::main()
- {
- ARMARX_IMPORTANT << "Logging three updates via the callback";
- auto up1 = std::make_shared<aron::data::Dict>();
- up1->addElement("updateInfo", std::make_shared<aron::data::String>("Update 1"));
-
- this->callback(skills::SkillStatus::Running, up1);
-
- auto up2 = std::make_shared<aron::data::Dict>();
- up2->addElement("updateInfo", std::make_shared<aron::data::String>("Update 2"));
- this->callback(skills::SkillStatus::Running, up2);
-
- auto up3 = std::make_shared<aron::data::Dict>();
- up3->addElement("updateInfo", std::make_shared<aron::data::String>("Update 3"));
- this->callback(skills::SkillStatus::Running, up3);
-
- return {TerminatedSkillStatus::Succeeded, nullptr};
- }
-
SkillProviderExample::SkillProviderExample() : SkillProviderComponentPluginUser()
{
}
@@ -213,11 +30,10 @@ namespace armarx::skills::provider
// Add another lambda example skill
{
- skills::SkillDescription fooDesc({"Foo"},
- "This skill does exactly nothing.",
- nullptr,
- armarx::core::time::Duration::MilliSeconds(1000),
- nullptr);
+ skills::SkillDescription fooDesc{.skillId = SkillID{.skillName = "Foo"},
+ .description = "This skill does exactly nothing.",
+ .timeout =
+ armarx::core::time::Duration::MilliSeconds(1000)};
addSkillFactory(fooDesc,
[]()
{
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index 190dc56a203e42aadac9f0fc483aed264fdc0685..b8391b0881f625af03b00b601983251c4d90558b 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -28,69 +28,15 @@
// RobotAPI
#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
-#include <RobotAPI/libraries/skills/provider/SkillProxy.h>
+
+#include "Callback.h"
+#include "Chaining.h"
+#include "HelloWorld.h"
+#include "Incomplete.h"
+#include "Timeout.h"
namespace armarx::skills::provider
{
- // Skills:
- class HelloWorldSkill : public Skill
- {
- public:
- HelloWorldSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- Skill::MainResult main() final;
- };
-
- class IncompleteSkill : public Skill
- {
- public:
- IncompleteSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- Skill::PrepareResult prepare() final;
- Skill::MainResult main() final;
-
- std::atomic_bool first_prepared = false;
- };
-
- class ChainingSkill : public Skill
- {
- public:
- ChainingSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- Skill::MainResult main() final;
- };
-
- class TimeoutSkill : public PeriodicSkill
- {
- public:
- TimeoutSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- PeriodicSkill::StepResult step() final;
- };
-
- class CallbackSkill : public Skill
- {
- public:
- CallbackSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- Skill::MainResult main() final;
- };
-
/**
* @defgroup Component-ExampleClient ExampleClient
* @ingroup RobotAPI-Components
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Timeout.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..31dc03ae05caf0e2be256487e3a6a4294243e9d0
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.cpp
@@ -0,0 +1,28 @@
+
+#include "Timeout.h"
+
+namespace armarx::skills::provider
+{
+
+ TimeoutSkill::TimeoutSkill() :
+ PeriodicSkill(GetSkillDescription(), armarx::core::time::Frequency::Hertz(5))
+ {
+ }
+
+ SkillDescription
+ TimeoutSkill::GetSkillDescription()
+ {
+ return SkillDescription{.skillId = SkillID{.skillName = "Timeout"},
+ .description = "This fails with timeout reached",
+ .timeout = armarx::core::time::Duration::MilliSeconds(2000)};
+ }
+
+ PeriodicSkill::StepResult
+ TimeoutSkill::step()
+ {
+ // do heavy work
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+ return {ActiveOrTerminatedSkillStatus::Running, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Timeout.h b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.h
new file mode 100644
index 0000000000000000000000000000000000000000..347e216a956c22d8d38b3b0b434ec0b5f4cf0225
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.h
@@ -0,0 +1,42 @@
+
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/PeriodicSkill.h>
+
+namespace armarx::skills::provider
+{
+
+ class TimeoutSkill : public PeriodicSkill
+ {
+ public:
+ TimeoutSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ PeriodicSkill::StepResult step() final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
index e90781ca62d7ef67751ed79b7556ab2c61f160a5..10c81ab542736b5a06bcd605f95353a619c3ad25 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
+++ b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
@@ -22,6 +22,9 @@
<ObjectChild key='some_text'>
<String />
</ObjectChild>
+ <ObjectChild key='some_optional_text'>
+ <String optional="True" />
+ </ObjectChild>
<ObjectChild key='some_list_of_matrices'>
<List>
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index 41ffefe98882f69004610a233dee16614f5b4f60..b434d1e3983096de91e73ddd2319fc843d3504a0 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -24,45 +24,51 @@
#include "GraspCandidateObserver.h"
//#include <ArmarXCore/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
-#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+
#include <RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#define TCP_POSE_CHANNEL "TCPPose"
#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
using namespace armarx;
using namespace armarx::grasping;
-GraspCandidateObserver::GraspCandidateObserver() : graspCandidateWriter(memoryNameSystem())
+GraspCandidateObserver::GraspCandidateObserver()
{
}
-void GraspCandidateObserver::onInitObserver()
+void
+GraspCandidateObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("GraspCandidatesTopicName").getValue());
offeringTopic(getProperty<std::string>("ConfigTopicName").getValue());
-
-
}
-void GraspCandidateObserver::onConnectObserver()
+void
+GraspCandidateObserver::onConnectObserver()
{
- configTopic = getTopic<GraspCandidateProviderInterfacePrx>(getProperty<std::string>("ConfigTopicName").getValue());
- graspCandidateWriter.connect();
+ configTopic = getTopic<GraspCandidateProviderInterfacePrx>(
+ getProperty<std::string>("ConfigTopicName").getValue());
+ graspCandidateWriter.connect(memoryNameSystem());
}
-PropertyDefinitionsPtr GraspCandidateObserver::createPropertyDefinitions()
+PropertyDefinitionsPtr
+GraspCandidateObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new GraspCandidateObserverPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new GraspCandidateObserverPropertyDefinitions(getConfigIdentifier()));
}
-bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter, const std::string& providerName, const GraspCandidatePtr& candidate)
+bool
+GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& filter,
+ const std::string& providerName,
+ const GraspCandidatePtr& candidate)
{
if (filter->providerName != "*" && filter->providerName != providerName)
{
@@ -72,11 +78,13 @@ bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& fi
{
return false;
}
- if (filter->approach != AnyApproach && (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
+ if (filter->approach != AnyApproach &&
+ (candidate->executionHints == 0 || filter->approach != candidate->executionHints->approach))
{
return false;
}
- if (filter->preshape != AnyAperture && (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
+ if (filter->preshape != AnyAperture &&
+ (candidate->executionHints == 0 || filter->preshape != candidate->executionHints->preshape))
{
return false;
}
@@ -87,7 +95,8 @@ bool GraspCandidateObserver::FilterMatches(const CandidateFilterConditionPtr& fi
return true;
}
-std::string GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
+std::string
+GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
{
switch (type)
{
@@ -102,7 +111,8 @@ std::string GraspCandidateObserver::ObjectTypeToString(objpose::ObjectType type)
}
}
-void GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
+void
+GraspCandidateObserver::handleProviderUpdate(const std::string& providerName, int candidateCount)
{
if (updateCounters.count(providerName) == 0)
{
@@ -118,26 +128,40 @@ void GraspCandidateObserver::handleProviderUpdate(const std::string& providerNam
{
offerChannel(providerName, "Channel of " + providerName);
}
- offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters[providerName]), "Counter that increases for each update");
- offerOrUpdateDataField(providerName, "candidateCount", Variant(candidateCount), "Number of provided candiates");
+ offerOrUpdateDataField(providerName,
+ "updateCounter",
+ Variant(updateCounters[providerName]),
+ "Counter that increases for each update");
+ offerOrUpdateDataField(
+ providerName, "candidateCount", Variant(candidateCount), "Number of provided candiates");
}
-void GraspCandidateObserver::reportGraspCandidates(const std::string& providerName, const GraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::reportGraspCandidates(const std::string& providerName,
+ const GraspCandidateSeq& candidates,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
this->candidates[providerName] = candidates;
- graspCandidateWriter.commitGraspCandidateSeq(candidates, armarx::armem::Time::Now(), providerName);
+ graspCandidateWriter.commitGraspCandidateSeq(
+ candidates, armarx::armem::Time::Now(), providerName);
handleProviderUpdate(providerName, candidates.size());
}
-void GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName, const BimanualGraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::reportBimanualGraspCandidates(const std::string& providerName,
+ const BimanualGraspCandidateSeq& candidates,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
this->bimanualCandidates[providerName] = candidates;
handleProviderUpdate(providerName, candidates.size());
}
-void GraspCandidateObserver::reportProviderInfo(const std::string& providerName, const ProviderInfoPtr& info, const Ice::Current&)
+void
+GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
+ const ProviderInfoPtr& info,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
providers[providerName] = info;
@@ -154,36 +178,37 @@ void GraspCandidateObserver::reportProviderInfo(const std::string& providerName,
offerOrUpdateDataField(providerName, "objectType", ObjectTypeToString(info->objectType), "");
}
-InfoMap GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
+InfoMap
+GraspCandidateObserver::getAvailableProvidersWithInfo(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
return providers;
}
-StringSeq GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
+StringSeq
+GraspCandidateObserver::getAvailableProviderNames(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
return getAvailableProviderNames();
}
-
-
-ProviderInfoPtr GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
+ProviderInfoPtr
+GraspCandidateObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
std::unique_lock lock(dataMutex);
checkHasProvider(providerName);
return providers[providerName];
}
-bool GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
+bool
+GraspCandidateObserver::hasProvider(const std::string& providerName, const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
return hasProvider(providerName);
}
-
-
-GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getAllCandidates(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
GraspCandidateSeq all;
@@ -194,11 +219,16 @@ GraspCandidateSeq GraspCandidateObserver::getAllCandidates(const Ice::Current&)
return all;
}
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName, const Ice::Current& c)
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByProvider(const std::string& providerName,
+ const Ice::Current& c)
{
return getCandidatesByProviders(Ice::StringSeq{providerName});
}
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByProviders(const Ice::StringSeq& providerNames, const Ice::Current& c)
+
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByProviders(const Ice::StringSeq& providerNames,
+ const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
GraspCandidateSeq all;
@@ -213,7 +243,9 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByProviders(const Ice::St
return all;
}
-GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter, const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getCandidatesByFilter(const CandidateFilterConditionPtr& filter,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
GraspCandidateSeq matching;
@@ -230,20 +262,26 @@ GraspCandidateSeq GraspCandidateObserver::getCandidatesByFilter(const CandidateF
return matching;
}
-Ice::Int GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName, const Ice::Current&)
+Ice::Int
+GraspCandidateObserver::getUpdateCounterByProvider(const std::string& providerName,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
checkHasProvider(providerName);
return updateCounters[providerName];
}
-IntMap GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
+IntMap
+GraspCandidateObserver::getAllUpdateCounters(const Ice::Current& providerName)
{
std::unique_lock lock(dataMutex);
return updateCounters;
}
-bool GraspCandidateObserver::setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const Ice::Current&)
+bool
+GraspCandidateObserver::setProviderConfig(const std::string& providerName,
+ const StringVariantBaseMap& config,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
if (providers.count(providerName) == 0)
@@ -254,19 +292,23 @@ bool GraspCandidateObserver::setProviderConfig(const std::string& providerName,
return true;
}
-void GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::setSelectedCandidates(const GraspCandidateSeq& candidates,
+ const Ice::Current&)
{
std::unique_lock lock(selectedCandidatesMutex);
selectedCandidates = candidates;
}
-GraspCandidateSeq GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
+GraspCandidateSeq
+GraspCandidateObserver::getSelectedCandidates(const Ice::Current&)
{
std::unique_lock lock(selectedCandidatesMutex);
return selectedCandidates;
}
-BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
+BimanualGraspCandidateSeq
+GraspCandidateObserver::getAllBimanualCandidates(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
BimanualGraspCandidateSeq all;
@@ -277,19 +319,25 @@ BimanualGraspCandidateSeq GraspCandidateObserver::getAllBimanualCandidates(const
return all;
}
-void GraspCandidateObserver::setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates, const Ice::Current&)
+void
+GraspCandidateObserver::setSelectedBimanualCandidates(
+ const grasping::BimanualGraspCandidateSeq& candidates,
+ const Ice::Current&)
{
std::unique_lock lock(selectedCandidatesMutex);
selectedBimanualCandidates = candidates;
}
-BimanualGraspCandidateSeq GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
+BimanualGraspCandidateSeq
+GraspCandidateObserver::getSelectedBimanualCandidates(const Ice::Current&)
{
std::unique_lock lock(selectedCandidatesMutex);
return selectedBimanualCandidates;
}
-void GraspCandidateObserver::clearCandidatesByProvider(const std::string& providerName, const Ice::Current&)
+void
+GraspCandidateObserver::clearCandidatesByProvider(const std::string& providerName,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
@@ -299,18 +347,24 @@ void GraspCandidateObserver::clearCandidatesByProvider(const std::string& provid
}
}
-bool GraspCandidateObserver::hasProvider(const std::string& providerName)
+bool
+GraspCandidateObserver::hasProvider(const std::string& providerName)
{
return providers.count(providerName) > 0;
}
-void GraspCandidateObserver::checkHasProvider(const std::string& providerName)
+
+void
+GraspCandidateObserver::checkHasProvider(const std::string& providerName)
{
if (!hasProvider(providerName))
{
- throw LocalException("Unknown provider name '") << providerName << "'. Available providers: " << getAvailableProviderNames();
+ throw LocalException("Unknown provider name '")
+ << providerName << "'. Available providers: " << getAvailableProviderNames();
}
}
-StringSeq GraspCandidateObserver::getAvailableProviderNames()
+
+StringSeq
+GraspCandidateObserver::getAvailableProviderNames()
{
StringSeq names;
for (const auto& pair : providers)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 697b087cc7f0b53edc3196b499c0f76dd3dec312..08870cfcc27981ee9e0e97f096563a35a1f51507 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -129,8 +129,8 @@ namespace armarx
}
//visu
{
- _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
- _tripFakeRobotGP.commitWrite();
+ _tripRt2NonRtRobotGP.getWriteBuffer().setIdentity();
+ _tripRt2NonRtRobotGP.commitWrite();
}
}
@@ -281,6 +281,9 @@ namespace armarx
}
}
_tripRt2NonRt.commitWrite();
+
+ _tripRt2NonRtRobotGP.getWriteBuffer() = _rtRobot->getGlobalPose();
+ _tripRt2NonRtRobotGP.commitWrite();
}
void
@@ -412,17 +415,13 @@ namespace armarx
NJointCartesianWaypointController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
const Ice::Current&)
{
- std::lock_guard g{_tripFakeRobotGPWriteMutex};
- _tripFakeRobotGP.getWriteBuffer() = p;
- _tripFakeRobotGP.commitWrite();
+ ; // No longer used ...
}
void
NJointCartesianWaypointController::resetVisualizationRobotGlobalPose(const Ice::Current&)
{
- std::lock_guard g{_tripFakeRobotGPWriteMutex};
- _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
- _tripFakeRobotGP.commitWrite();
+ ; // No longer used ...
}
void
@@ -477,7 +476,7 @@ namespace armarx
std::lock_guard g{_tripRt2NonRtMutex};
const auto& buf = _tripRt2NonRt.getUpToDateReadBuffer();
- const Eigen::Matrix4f fakeGP = _tripFakeRobotGP.getUpToDateReadBuffer();
+ const Eigen::Matrix4f fakeGP = _tripRt2NonRtRobotGP.getUpToDateReadBuffer();
const Eigen::Matrix4f gp = std::isfinite(fakeGP(0, 0)) ? fakeGP : buf.rootPose;
if (buf.tcp != buf.tcpTarg)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index dce6aed44f8bb931baf2a598c06f127cb9b20d66..59734617b4bb75cd3599f0d2c198beb7e7694bc0 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -146,8 +146,7 @@ namespace armarx
mutable std::recursive_mutex _tripRt2NonRtMutex;
TripleBuffer<RtToNonRtData> _tripRt2NonRt;
- mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
- TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
+ TripleBuffer<Eigen::Matrix4f> _tripRt2NonRtRobotGP;
//publish data
std::atomic_size_t _publishWpsNum{0};
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index e9aea0a9858d85e24b9e49ee088c18405af0668a..61d0d6c3a63f6a82f981eed63a61d6cabd57105e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -722,7 +722,7 @@ namespace armarx::RobotUnitModule
for (std::size_t idxRobot = 0; idxRobot < nodes.size(); ++idxRobot)
{
const VirtualRobot::RobotNodePtr& node = nodes.at(idxRobot);
- if (node->isRotationalJoint() || node->isTranslationalJoint())
+ if (node->isJoint())
{
const auto& name = node->getName();
if (sensorDevices.has(name))
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 8cb11c331040c5dfd12fea456c0d2391a36c9f2b..ee8522d21984cbf6936bd1b2c259e37850398d7f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -494,10 +494,13 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard{rtLoggingMutex};
- if (!rtDataStreamingEntry.count(receiver))
+
+ if (rtDataStreamingEntry.count(receiver) == 0u)
{
- throw InvalidArgumentException{"stopDataStreaming called for a nonexistent log"};
+ ARMARX_INFO << "stopDataStreaming called for a nonexistent log";
+ return;
}
+
ARMARX_INFO_S << "RobotUnit: request to stop DataStreaming for " << receiver->ice_id();
rtDataStreamingEntry.at(receiver).stopStreaming = true;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index a4d9867488a2a2f8e9cb176158ba18fc01a8c538..698df2657899773a1a111bd02ce561abd66308ef 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -21,12 +21,17 @@
*/
#include "RobotUnitModuleSelfCollisionChecker.h"
+#include <algorithm>
+#include <cstddef>
+#include <string>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/Obstacle.h>
#include <VirtualRobot/RobotNodeSet.h>
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
@@ -219,6 +224,58 @@ namespace armarx::RobotUnitModule
node->getCollisionModel()->inflateModel(minSelfDistance / 2.f);
}
}
+
+ // Remove / filter collision pairs according to robot model (XML: Physics/IgnoreCollision)
+ {
+ ARMARX_VERBOSE << "Removing ignored collision pairs";
+ // introduce vector to remove elements "in-place" via remove-erase-if idiom (not possible for sets)
+ std::vector<std::pair<std::string, std::string>> validNamePairsToCheck(namePairsToCheck.begin(), namePairsToCheck.end());
+
+ const auto isCollisionIgnored = [this](const std::string& a, const std::string& b) -> bool {
+
+ if(a == FLOOR_OBJ_STR or b == FLOOR_OBJ_STR)
+ {
+ return false;
+ }
+
+ const auto nodeA = selfCollisionAvoidanceRobot->getRobotNode(a);
+ const auto nodeB = selfCollisionAvoidanceRobot->getRobotNode(b);
+
+ if(nodeA == nullptr or nodeB == nullptr)
+ {
+ return false;
+ }
+
+ const std::vector<std::string> nodesIgnoredByA = nodeA->getIgnoredCollisionModels();
+ const std::vector<std::string> nodesIgnoredByB = nodeB->getIgnoredCollisionModels();
+
+ if(std::find(nodesIgnoredByA.begin(), nodesIgnoredByA.end(), b) != nodesIgnoredByA.end())
+ {
+ ARMARX_VERBOSE << "Ignoring collision between nodes: " << a << " -- " << b;
+ return true;
+ }
+
+ if(std::find(nodesIgnoredByB.begin(), nodesIgnoredByB.end(), a) != nodesIgnoredByB.end())
+ {
+ ARMARX_VERBOSE << "Ignoring collision between nodes: " << b << " -- " << a;
+ return true;
+ }
+
+ return false;
+
+ };
+
+ validNamePairsToCheck.erase(std::remove_if(validNamePairsToCheck.begin(), validNamePairsToCheck.end(), [&isCollisionIgnored](const auto& p) -> bool {
+ const auto& [a, b] = p;
+ return isCollisionIgnored(a, b);
+ }), validNamePairsToCheck.end());
+
+ ARMARX_VERBOSE << "Removed " << (namePairsToCheck.size() - validNamePairsToCheck.size()) << " collision pairs.";
+
+ // copy over name pairs which should not be ignored
+ namePairsToCheck = std::set(validNamePairsToCheck.begin(), validNamePairsToCheck.end());
+ }
+
//collect pairs
for (const auto& pair : namePairsToCheck)
{
@@ -227,14 +284,18 @@ namespace armarx::RobotUnitModule
? floor->getSceneObject(0)
: selfCollisionAvoidanceRobot->getRobotNode(pair.first);
+ ARMARX_CHECK_NOT_NULL(first) << pair.first;
+
VirtualRobot::SceneObjectPtr second =
(pair.second == FLOOR_OBJ_STR)
? floor->getSceneObject(0)
: selfCollisionAvoidanceRobot->getRobotNode(pair.second);
+ ARMARX_CHECK_NOT_NULL(second) << pair.second;
+
nodePairsToCheck.emplace_back(first, second);
}
- ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), nodePairsToCheck.size());
+ ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), namePairsToCheck.size());
}
void
@@ -303,21 +364,23 @@ namespace armarx::RobotUnitModule
};
while (true)
{
- const auto startT = std::chrono::high_resolution_clock::now();
+ const auto freq = checkFrequency.load();
+
+ core::time::Metronome metronome(Frequency::Hertz(freq));
+
//done
if (isShuttingDown())
{
return;
}
- const auto freq = checkFrequency.load();
const bool inEmergencyStop =
_module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive;
if (inEmergencyStop || freq == 0)
{
- ARMARX_INFO << deactivateSpam() << "Self collision checker: skipping check "
+ ARMARX_VERBOSE << deactivateSpam() << "Self collision checker: skipping check "
<< VAROUT(freq) << " " << VAROUT(inEmergencyStop);
//currently wait
- std::this_thread::sleep_for(std::chrono::microseconds{1000});
+ std::this_thread::sleep_for(std::chrono::microseconds{1'000});
continue;
}
//update robot + check
@@ -332,6 +395,7 @@ namespace armarx::RobotUnitModule
bool allJoints0 = true;
for (const auto& node : selfCollisionAvoidanceRobotNodes)
{
+ ARMARX_CHECK_NOT_NULL(node);
if (0 != node->getJointValue())
{
allJoints0 = false;
@@ -348,6 +412,10 @@ namespace armarx::RobotUnitModule
for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
{
const auto& pair = nodePairsToCheck.at(idx);
+
+ ARMARX_CHECK_NOT_NULL(pair.first);
+ ARMARX_CHECK_NOT_NULL(pair.second);
+
if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(
pair.first, pair.second))
{
@@ -370,9 +438,16 @@ namespace armarx::RobotUnitModule
<< nodePairsToCheck.size() << " pairs";
}
}
+
//sleep remaining
- std::this_thread::sleep_until(
- startT + std::chrono::microseconds{static_cast<int64_t>(1000000 / freq)});
+ const auto duration = metronome.waitForNextTick();
+
+ if(not duration.isPositive())
+ {
+ ARMARX_WARNING << deactivateSpam(10) <<
+ "Self collision checking took too long. "
+ "Exceeding time budget by " << duration.toMilliSecondsDouble() << "ms.";
+ }
}
}
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
index 1431b8b5219ef4622d21024132397690a81d9526..14cc8fb49d139d177425040e4b69334f4e9e042a 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
@@ -22,6 +22,7 @@
#include "GamepadUnit.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <linux/joystick.h>
@@ -33,6 +34,7 @@ void GamepadUnit::onInitComponent()
ARMARX_TRACE;
offeringTopic(getProperty<std::string>("GamepadTopicName").getValue());
deviceName = getProperty<std::string>("GamepadDeviceName").getValue();
+ deviceEventName = getProperty<std::string>("GamepadForceFeedbackName").getValue();
readTask = new RunningTask<GamepadUnit>(this, &GamepadUnit::run, "GamepadUnit");
}
@@ -52,7 +54,7 @@ void GamepadUnit::onConnectComponent()
ARMARX_TRACE;
if (!dataTimestamp)
{
- ARMARX_INFO << "dataTimestamp is null, waiting for value";
+ ARMARX_INFO << deactivateSpam(1) << "dataTimestamp is null, waiting for value";
return;
}
ARMARX_CHECK_NOT_NULL(dataTimestamp);
@@ -69,15 +71,23 @@ void GamepadUnit::onConnectComponent()
ARMARX_INFO << deactivateSpam(100000, std::to_string(dataTimestamp->getTimestamp())) << "No new signal from gamepad for " << age.toMilliSecondsDouble() << " milliseconds. Not sending data. Timeout: " << getProperty<int>("PublishTimeout").getValue() << " ms";
}
}, 30);
+
sendTask->start();
ARMARX_TRACE;
openGamepadConnection();
}
+void GamepadUnit::vibrate(const ::Ice::Current&)
+{
+ ARMARX_INFO << "vibration!";
+ js.executeEffect();
+}
+
bool GamepadUnit::openGamepadConnection()
{
- if (js.open(deviceName))
+ if (js.open(deviceName, deviceEventName))
{
+
ARMARX_TRACE;
ARMARX_INFO << "opened a gamepad named " << js.name << " with " << js.numberOfAxis << " axis and " << js.numberOfButtons << " buttons.";
if (js.numberOfAxis == 8 && js.numberOfButtons == 11)
@@ -185,4 +195,3 @@ armarx::PropertyDefinitionsPtr GamepadUnit::createPropertyDefinitions()
return armarx::PropertyDefinitionsPtr(new GamepadUnitPropertyDefinitions(
getConfigIdentifier()));
}
-
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
index 109ce81a17a98c5154b396e09064d9943be6ff0a..a2008600dc1f50432bbbf3f00c646153c7d7e83e 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
@@ -53,6 +53,7 @@ namespace armarx
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
defineOptionalProperty<std::string>("GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
defineOptionalProperty<std::string>("GamepadDeviceName", "/dev/input/js2", "device that will be opened as a gamepad");
+ defineOptionalProperty<std::string>("GamepadForceFeedbackName", "", "device that will be used for force feedback, leave empty to disable. See RobotAPI/source/RobotAPI/drivers/GamepadUnit/README.md for more details.");
defineOptionalProperty<int>("PublishTimeout", 2000, "In Milliseconds. Timeout after which the gamepad data is not published after, if no new data was read from the gamepad");
}
};
@@ -69,7 +70,8 @@ namespace armarx
* Detailed description of class GamepadUnit.
*/
class GamepadUnit :
- virtual public armarx::Component
+ virtual public armarx::Component,
+ virtual public GamepadUnitInterface
{
public:
/**
@@ -108,6 +110,8 @@ namespace armarx
bool openGamepadConnection();
+ void vibrate(const ::Ice::Current& = ::Ice::emptyCurrent) override;
+
private:
GamepadUnitListenerPrx topicPrx;
RunningTask<GamepadUnit>::pointer_type readTask;
@@ -116,9 +120,9 @@ namespace armarx
void run();
std::mutex mutex;
std::string deviceName;
+ std::string deviceEventName;
Joystick js;
GamepadData data;
TimestampVariantPtr dataTimestamp;
};
}
-
diff --git a/source/RobotAPI/drivers/GamepadUnit/Joystick.h b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
index 1f39074b7a1449f9d58b643d42be68f2c123c445..af7cc386dc35a8b1b8ec3122545fd0bb88fef4bd 100644
--- a/source/RobotAPI/drivers/GamepadUnit/Joystick.h
+++ b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
@@ -22,12 +22,17 @@
#pragma once
-#include<linux/joystick.h>
-#include<sys/stat.h>
-#include<fcntl.h>
+#include <cstdint>
+
+#include <fcntl.h>
+#include <sys/poll.h>
+#include <sys/stat.h>
+#include <unistd.h>
#include <ArmarXCore/core/Component.h>
+#include <linux/joystick.h>
+
namespace armarx
{
@@ -36,21 +41,39 @@ namespace armarx
private:
int fd = -1;
+ int fdEvent = -1;
js_event event;
public:
-
std::vector<int16_t> axis;
std::vector<bool> buttonsPressed;
int numberOfAxis;
int numberOfButtons;
std::string name;
- bool open(std::string const& deviceName)
+ bool
+ open(std::string const& deviceName, std::string const& deviceEventName)
{
+
fd = ::open(deviceName.c_str(), O_RDONLY);
+
+ if (!deviceEventName.empty())
+ {
+ ARMARX_INFO << "Force feedback enabled";
+ fdEvent = ::open(deviceEventName.c_str(), O_RDWR | O_CLOEXEC);
+
+ ARMARX_CHECK(fdEvent != -1);
+ } else {
+ ARMARX_INFO << "Force feedback disabled";
+ }
+
if (fd != -1)
{
+ // ARMARX_INFO << "before";
+ // executeEffect();
+ // ARMARX_INFO << "after";
+
+
ioctl(fd, JSIOCGAXES, &numberOfAxis);
ioctl(fd, JSIOCGBUTTONS, &numberOfButtons);
name.resize(255);
@@ -59,15 +82,21 @@ namespace armarx
name = name.c_str();
buttonsPressed.resize(numberOfButtons, false);
}
+
+ ARMARX_INFO << "execute effect";
+ executeEffect();
+
return fd != -1;
}
- bool opened() const
+ bool
+ opened() const
{
return fd != -1;
}
- bool pollEvent()
+ bool
+ pollEvent()
{
int bytes = read(fd, &event, sizeof(event));
@@ -89,10 +118,165 @@ namespace armarx
return true;
}
- void close()
+ void
+ executeEffect(int gain = 100, const int nTimes = 1)
+ {
+ // this feature is disabled
+ if(fdEvent < 0) return;
+
+ // see https://docs.kernel.org/input/ff.html
+
+
+ // https://xnux.eu/devices/feature/vibrator.html
+
+ int ret;
+ // pollfd pfds[1];
+ int effects;
+
+ // fd = open_event_dev("vibrator", O_RDWR | O_CLOEXEC);
+ // syscall_error(fd < 0, "Can't open vibrator event device");
+
+ ret = ioctl(fdEvent, EVIOCGEFFECTS, &effects);
+ ARMARX_CHECK(ret >= 0);
+ // syscall_error(ret < 0, "EVIOCGEFFECTS failed");
+
+ // ARMARX_CHECK(effects & FF_RUMBLE);
+
+ // Set the gain of the device
+ {
+ // int gain; between 0 and 100
+ struct input_event ie; // structure used to communicate with the driver
+
+ ie.type = EV_FF;
+ ie.code = FF_GAIN;
+ ie.value = 0xFFFFUL * gain / 100;
+
+ if (write(fdEvent, &ie, sizeof(ie)) == -1)
+ {
+ perror("set gain");
+ }
+ }
+
+
+ ff_effect e;
+ // e.type = FF_RUMBLE;
+ // e.id = -1;
+ // e.replay.length = 5000;
+ // e.replay.delay = 500;
+ // e.u.rumble.strong_magnitude = 1;
+
+ e.type = FF_PERIODIC;
+ e.id = -1;
+ e.replay.length = 5000;
+ e.replay.delay = 500;
+ e.u.periodic.waveform = FF_SQUARE;
+ e.u.periodic.period = 1000;
+ e.u.periodic.magnitude = 0xFF;
+ e.u.periodic.offset = 0xFF;
+
+ ret = ioctl(fdEvent, EVIOCSFF, &e);
+ ARMARX_CHECK(ret >= 0);
+
+ // syscall_error(ret < 0, "EVIOCSFF failed");
+
+ ARMARX_INFO << VAROUT(e.id);
+
+ input_event play;
+ play.type = EV_FF;
+ play.code = static_cast<std::uint16_t>(e.id);
+ play.value = 1;
+
+ ret = write(fdEvent, &play, sizeof(play));
+ ARMARX_CHECK(ret >= 0);
+
+ ARMARX_INFO << "Executing effect";
+
+
+ for (int i = 0; i < 5; i++)
+ {
+
+ input_event statusIe; // structure used to communicate with the driver
+ statusIe.type = EV_FF_STATUS;
+ statusIe.code = e.id;
+ // statusIe.value = 0;
+
+ ret = write(fdEvent, &statusIe, sizeof(statusIe));
+
+ // ARMARX_CHECK()
+ // ret should be FF_STATUS_PLAYING
+ ARMARX_INFO << VAROUT(ret);
+
+ sleep(1);
+ }
+
+
+ // syscall_error(ret < 0, "write failed");
+
+ ARMARX_INFO << "Executing effect";
+ // sleep(6);
+
+
+ input_event stop;
+ stop.type = EV_FF;
+ stop.code = e.id;
+ stop.value = 0;
+ [[maybe_unused]] const int stopStatus = write(fdEvent, static_cast<const void*>(&stop), sizeof(stop));
+
+
+ ret = ioctl(fdEvent, EVIOCRMFF, e.id);
+ ARMARX_CHECK(ret >= 0);
+
+ // syscall_error(ret < 0, "EVIOCRMFF failed");
+
+ // close(fdEvent);
+
+
+ /**/
+ // Set the gain of the device
+ // {
+ // // int gain; between 0 and 100
+ // struct input_event ie; // structure used to communicate with the driver
+
+ // ie.type = EV_FF;
+ // ie.code = FF_GAIN;
+ // ie.value = 0xFFFFUL * gain / 100;
+
+ // if (write(fd, &ie, sizeof(ie)) == -1)
+ // {
+ // perror("set gain");
+ // }
+ // }
+
+
+ // struct input_event play;
+ // struct input_event stop;
+
+ // // upload request to device
+ // ff_effect effect;
+ // const auto uploadStatus = ioctl(fd, EVIOCSFF, &effect);
+
+ // // Play n times
+ // play.type = EV_FF;
+ // play.code = effect.id;
+ // play.value = nTimes;
+
+ // const int playStatus = write(fd, static_cast<const void*>(&play), sizeof(play));
+
+ // // Stop an effect
+ // stop.type = EV_FF;
+ // stop.code = effect.id;
+ // stop.value = 0;
+ // const int stopStatus = write(fd, static_cast<const void*>(&stop), sizeof(stop));
+
+ // // remove effect
+ // ioctl(fd, EVIOCRMFF, effect.id);
+ }
+
+ void
+ close()
{
::close(fd);
fd = -1;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
index 2e4c5851c2417c67158785cb31226b03fff7c269..55776817eee69b3f783cda883871b88ec9679439 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
@@ -36,11 +36,11 @@ armarx::HandUnitConfigDialog::HandUnitConfigDialog(QWidget* parent) :
proxyFinderLeftHand = new IceProxyFinder<HandUnitInterfacePrx>(this);
- proxyFinderLeftHand->setSearchMask("*Unit");
+ proxyFinderLeftHand->setSearchMask("*LeftHandUnit");
ui->proxyFinderContainerLeftHand->addWidget(proxyFinderLeftHand, 0, 0, 1, 1);
proxyFinderRightHand = new IceProxyFinder<HandUnitInterfacePrx>(this);
- proxyFinderRightHand->setSearchMask("*Unit");
+ proxyFinderRightHand->setSearchMask("*RightHandUnit");
ui->proxyFinderContainerRightHand->addWidget(proxyFinderRightHand, 0, 0, 1, 1);
}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index fa8011d66e9fb45de290395eab222e9299888620..bdf8a1cd639b473cf9ac4cb4af27f8d97da81fc1 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -74,6 +74,7 @@
#include <iostream>
#include <memory>
#include <optional>
+#include <stdexcept>
#include <string>
@@ -81,8 +82,10 @@
//#define KINEMATIC_UNIT_FILE_DEFAULT_PACKAGE std::string("RobotAPI")
#define KINEMATIC_UNIT_NAME_DEFAULT "Robot"
//#define TOPIC_NAME_DEFAULT "RobotState"
-#define SLIDER_POS_DEG_MULTIPLIER 5
-#define SLIDER_POS_RAD_MULTIPLIER 100
+
+constexpr float SLIDER_POS_DEG_MULTIPLIER = 5;
+constexpr float SLIDER_POS_RAD_MULTIPLIER = 100;
+constexpr float SLIDER_POS_HEMI_MULTIPLIER = 100;
namespace armarx
{
@@ -648,15 +651,26 @@ namespace armarx
{
if (currentNode)
{
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
- const auto factor =
- isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
- float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
- float pos = currentNode->getJointValue() * conversionFactor;
-
- ui.lcdNumberKinematicUnitJointValue->display((int)pos);
- ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
+ {
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const auto factor =
+ isDeg ? SLIDER_POS_DEG_MULTIPLIER : SLIDER_POS_RAD_MULTIPLIER;
+ const float conversionFactor = isDeg ? 180.0 / M_PI : 1.0f;
+ const float pos = currentNode->getJointValue() * conversionFactor;
+
+ ui.lcdNumberKinematicUnitJointValue->display((int)pos);
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
+ }
+
+ if (currentNode->isTranslationalJoint())
+ {
+ const auto factor = SLIDER_POS_DEG_MULTIPLIER;
+ const float pos = currentNode->getJointValue();
+
+ ui.lcdNumberKinematicUnitJointValue->display((int)pos);
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
+ }
}
}
}
@@ -712,15 +726,49 @@ namespace armarx
if (currentNode)
{
- QString unit = currentNode->isRotationalJoint()
- ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
- : "mm";
+ const QString unit = [&]() -> QString
+ {
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
+ {
+ if (ui.checkBoxUseDegree->isChecked())
+ {
+ return "deg";
+ }
+
+ return "rad";
+ }
+
+ if (currentNode->isTranslationalJoint())
+ {
+ return "mm";
+ }
+
+ throw std::invalid_argument("unknown/unsupported joint type");
+ }();
+
ui.labelUnit->setText(unit);
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
- const auto factor =
- isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
- float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
+
+
+ const auto [factor, conversionFactor] = [&]() -> std::pair<float, float>
+ {
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
+ {
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ if (isDeg)
+ {
+ return {SLIDER_POS_DEG_MULTIPLIER, 180.0 / M_PI};
+ }
+ return {SLIDER_POS_RAD_MULTIPLIER, 1};
+ }
+
+ if (currentNode->isTranslationalJoint())
+ {
+ return {SLIDER_POS_DEG_MULTIPLIER, 1};
+ }
+
+ throw std::invalid_argument("unknown/unsupported joint type");
+ }();
+
jointModes[currentNode->getName()] = ePositionControl;
if (kinematicUnitInterfacePrx)
@@ -728,8 +776,8 @@ namespace armarx
kinematicUnitInterfacePrx->switchControlMode(jointModes);
}
- float lo = currentNode->getJointLimitLo() * conversionFactor;
- float hi = currentNode->getJointLimitHi() * conversionFactor;
+ const float lo = currentNode->getJointLimitLo() * conversionFactor;
+ const float hi = currentNode->getJointLimitHi() * conversionFactor;
if (hi - lo <= 0.0f)
{
@@ -745,7 +793,7 @@ namespace armarx
synchronizeRobotJointAngles();
}
- float pos = currentNode->getJointValue() * conversionFactor;
+ const float pos = currentNode->getJointValue() * conversionFactor;
ARMARX_INFO << "Setting position control for current node "
<< "(name '" << currentNode->getName() << "' with current value " << pos
<< ")";
@@ -754,8 +802,18 @@ namespace armarx
// This will initially send a position target with a small delta to the joint.
ui.horizontalSliderKinematicUnitPos->blockSignals(true);
- ui.horizontalSliderKinematicUnitPos->setMaximum(hi * factor);
- ui.horizontalSliderKinematicUnitPos->setMinimum(lo * factor);
+ const float sliderMax = hi * factor;
+ const float sliderMin = lo * factor;
+
+ ui.horizontalSliderKinematicUnitPos->setMaximum(sliderMax);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(sliderMin);
+
+ const std::size_t desiredNumberOfTicks = 1'000;
+
+ const float tickInterval = (sliderMax - sliderMin) / desiredNumberOfTicks;
+ ARMARX_INFO << VAROUT(tickInterval);
+
+ ui.horizontalSliderKinematicUnitPos->setTickInterval(tickInterval);
ui.lcdNumberKinematicUnitJointValue->display(pos);
ui.horizontalSliderKinematicUnitPos->blockSignals(false);
@@ -780,14 +838,37 @@ namespace armarx
// set the velocity to zero to stop any previous controller (e.g. torque controller)
jointVelocities[currentNode->getName()] = 0;
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
- QString unit = isRot ? (isDeg ? "deg/s" : "rad/(100*s)") : "mm/s";
+
+ const QString unit = [&]() -> QString
+ {
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint())
+ {
+ if (ui.checkBoxUseDegree->isChecked())
+ {
+ return "deg/s";
+ }
+
+ return "rad/(100*s)";
+ }
+
+ if (currentNode->isTranslationalJoint())
+ {
+ return "mm/s";
+ }
+
+ throw std::invalid_argument("unknown/unsupported joint type");
+ }();
+
+
ui.labelUnit->setText(unit);
ARMARX_INFO << "setting velocity control for current Node Name: "
<< currentNode->getName() << flush;
- float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
- float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
+
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint();
+
+ const float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
+ const float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
try
{
@@ -1105,7 +1186,7 @@ namespace armarx
const ControlMode currentControlMode = getSelectedControlMode();
const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
+ const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint() or currentNode->isFourBarJoint();
if (currentControlMode == ePositionControl)
{
@@ -1361,7 +1442,10 @@ namespace armarx
QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
float conversionFactor =
- ui.checkBoxUseDegree->isChecked() && node->isRotationalJoint() ? 180.0 / M_PI : 1;
+ ui.checkBoxUseDegree->isChecked() &&
+ (node->isRotationalJoint() or node->isHemisphereJoint() or node->isFourBarJoint())
+ ? 180.0 / M_PI
+ : 1;
ui.tableJointList->model()->setData(
index,
(int)(cutJitter(currentValue * conversionFactor) * 100) / 100.0f,
@@ -1401,7 +1485,8 @@ namespace armarx
}
float currentValue = it->second;
- if (ui.checkBoxUseDegree->isChecked() && rn[i]->isRotationalJoint())
+ if (ui.checkBoxUseDegree->isChecked() &&
+ (rn[i]->isRotationalJoint() or rn[i]->isHemisphereJoint() or rn[i]->isFourBarJoint()))
{
currentValue *= 180.0 / M_PI;
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 01cb60a2d3a6cc716f4f36be94c92767cafc918e..a8dac1d2988b3f5e149338dae4f4657745554898 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -237,8 +237,8 @@ namespace armarx
widget.treeWidgetSkills->clear();
widget.treeWidgetSkillDetails->clear();
skillsArgumentsTreeWidgetItem = nullptr;
- selectedSkill.skillId.providerId->providerName = skills::SkillID::UNKNOWN;
- selectedSkill.skillId.skillName = skills::SkillID::UNKNOWN;
+ selectedSkill.skillId.providerId->providerName = "";
+ selectedSkill.skillId.skillName = "";
}
void
@@ -311,9 +311,10 @@ namespace armarx
{
auto description = skills::SkillDescription::FromIce(desc);
auto skillId = skills::SkillID::FromIce(sid);
- auto providerId = skills::ProviderID(skillId);
+ auto providerId = skillId.providerId.value_or(
+ skills::ProviderID{.providerName = "UNKNOWN PROVIDER NAME"});
- ARMARX_CHECK(skillId.fullySpecified());
+ ARMARX_CHECK(skillId.isFullySpecified());
auto& providedSkillsMap = skills[providerId]; // create new if not existent
providedSkillsMap.insert({skillId, description});
@@ -325,7 +326,7 @@ namespace armarx
{
auto* providerItem = widget.treeWidgetSkills->topLevelItem(i);
auto providerName = providerItem->text(0).toStdString();
- skills::ProviderID providerId(providerName);
+ skills::ProviderID providerId{.providerName = providerName};
if (skills.find(providerId) == skills.end())
{
@@ -344,7 +345,9 @@ namespace armarx
auto* skillItem = providerItem->child(j);
auto skillName = skillItem->text(0).toStdString();
- skills::SkillID skillId(providerName, skillName);
+ skills::SkillID skillId{.providerId =
+ skills::ProviderID{.providerName = providerName},
+ .skillName = skillName};
if (providedSkills.find(skillId) == providedSkills.end())
{
@@ -366,7 +369,7 @@ namespace armarx
{
auto el = widget.treeWidgetSkills->topLevelItem(i);
auto providerName = el->text(0).toStdString();
- skills::ProviderID elProviderId(providerName);
+ skills::ProviderID elProviderId{.providerName = providerName};
if (providerId == elProviderId)
{
@@ -388,7 +391,7 @@ namespace armarx
{
auto el = providerItem->child(i);
auto skillName = el->text(0).toStdString();
- skills::SkillID elSkillId(providerId.providerName, skillName);
+ skills::SkillID elSkillId{providerId, skillName};
if (skillId == elSkillId)
{
@@ -436,8 +439,9 @@ namespace armarx
{
std::scoped_lock l(updateMutex);
- auto currentManagerStatuses = memory->getLatestSkillExecutionStatuses(
- 100); // we assume that there are no more than 100 new skills..
+ auto currentManagerStatuses =
+ memory
+ ->getSkillExecutionStatuses(); // we assume that there are no more than 100 new skills..
for (const auto& [k, v] : currentManagerStatuses)
{
@@ -472,16 +476,16 @@ namespace armarx
// update values
//found->setText(3,
// QString::fromStdString(
- // statusUpdate.hasBeenConstructed() ? " (\xfb) " : ""));
+ // statusUpdate.hasBeenConstructed() ? " yes " : " no "));
found->setText(4,
QString::fromStdString(
- statusUpdate.hasBeenInitialized() ? " (\xfb) " : ""));
+ statusUpdate.hasBeenInitialized() ? " yes " : " no "));
found->setText(5,
QString::fromStdString(
- statusUpdate.hasBeenPrepared() ? " (\xfb) " : ""));
+ statusUpdate.hasBeenPrepared() ? " yes " : " no "));
found->setText(6,
QString::fromStdString(
- statusUpdate.hasBeenRunning() ? " (\xfb) " : ""));
+ statusUpdate.hasBeenRunning() ? " yes " : " no "));
found->setText(7,
QString::fromStdString(
statusUpdate.hasBeenTerminated() ? " (\xfb) " : ""));
@@ -549,12 +553,12 @@ namespace armarx
char hostname[HOST_NAME_MAX];
gethostname(hostname, HOST_NAME_MAX);
- skills::SkillExecutionRequest req(selectedSkill.skillId,
+ skills::SkillExecutionRequest req{selectedSkill.skillId,
"Skills.Manager GUI (hostname: " + std::string(hostname) +
")",
params);
- ARMARX_CHECK(selectedSkill.skillId.fullySpecified()); // sanity check
+ ARMARX_CHECK(selectedSkill.skillId.isFullySpecified()); // sanity check
ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.skillId << ".";
// Note that we execute the skill in a seperate thread so that the GUI thread does not freeze.
memory->begin_executeSkill(req.toManagerIce());
@@ -619,7 +623,9 @@ namespace armarx
if (!current->parent())
{
- // no parent available. Perhaps provider clicked?
+ // no parent available. Perhaps provider clicked? Reset selected skill.
+ selectedSkill.skillId.providerId->providerName = "";
+ selectedSkill.skillId.skillName = "";
return;
}
@@ -645,7 +651,7 @@ namespace armarx
skillsArgumentsTreeWidgetItem = nullptr;
// We assert that the skill exists
- ARMARX_CHECK(skills.count(selectedSkill.skillId) > 0);
+ ARMARX_CHECK(skills.count(*selectedSkill.skillId.providerId) > 0);
ARMARX_CHECK(skills.at(*selectedSkill.skillId.providerId).count(selectedSkill.skillId) > 0);
auto skillDesc = skills.at(*selectedSkill.skillId.providerId).at(selectedSkill.skillId);
@@ -686,8 +692,8 @@ namespace armarx
skillsArgumentsTreeWidgetItem = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
{QString::fromStdString("Arguments")});
- auto aron_args = skillDesc.acceptedType;
- auto default_args_of_profile = skillDesc.rootProfileParameterization;
+ auto aron_args = skillDesc.parametersType;
+ auto default_args_of_profile = skillDesc.rootProfileDefaults;
aronTreeWidgetController =
std::make_shared<AronTreeWidgetController>(widget.treeWidgetSkillDetails,
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
index 2e52ed4cf4854583641d1a83f85822096d8f5226..5c3db21fbe8604c9842cb834565274a4294e6233 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
@@ -90,6 +90,16 @@ namespace armarx
createdAron = createdAronDict;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
unsigned int x = 0;
for (const auto& [key, value] : i->getMemberTypes())
{
@@ -113,6 +123,16 @@ namespace armarx
createdAron = createdAronDict;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
for (int x = 0; x < el->childCount(); ++x)
{
auto it = el->child(x);
@@ -134,12 +154,23 @@ namespace armarx
ARMARX_TRACE;
auto createdAronList = std::make_shared<aron::data::List>(i->getPath());
createdAron = createdAronList;
- auto* elem = parentItem->child(index);
+ auto* el = parentItem->child(index);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
auto childrenTypes = i->getChildren();
ARMARX_CHECK(childrenTypes.size() == 1);
- for (int j = 0; j < elem->childCount(); ++j)
+ for (int j = 0; j < el->childCount(); ++j)
{
- AronTreeWidgetConverterVisitor convVisitor(elem, j);
+ AronTreeWidgetConverterVisitor convVisitor(el, j);
aron::type::visit(convVisitor, childrenTypes[0]);
handleErrors(convVisitor);
@@ -156,11 +187,21 @@ namespace armarx
ARMARX_TRACE;
auto createdAronPair = std::make_shared<aron::data::List>(i->getPath());
createdAron = createdAronPair;
- auto* elem = parentItem->child(index);
+ auto* el = parentItem->child(index);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
for (int j = 0; j < 2; ++j)
{
- AronTreeWidgetConverterVisitor convVisitor(elem, j);
+ AronTreeWidgetConverterVisitor convVisitor(el, j);
handleErrors(convVisitor);
if (convVisitor.createdAron && convVisitor.isConversionSuccessful())
{
@@ -177,6 +218,16 @@ namespace armarx
createdAron = createdAronList;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
for (int x = 0; x < el->childCount(); ++x)
{
auto* it = el->child(x);
@@ -242,11 +293,22 @@ namespace armarx
createdMatrix->setShape({i->getRows(), i->getCols(), dataSize});
int totalByteSize = i->getRows() * i->getCols() * dataSize;
createdAron = createdMatrix;
+ auto* el = parentItem->child(index);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
- auto* currElem = parentItem->child(index);
- auto* rootWidget = currElem->treeWidget();
+
+ auto* rootWidget = el->treeWidget();
ARMARX_CHECK(rootWidget);
- auto* widget = rootWidget->itemWidget(currElem, 1);
+ auto* widget = rootWidget->itemWidget(el, 1);
auto* matrixWidget = EditMatrixWidget::DynamicCastAndCheck(widget);
handleErrors(matrixWidget->hasParseErrors());
@@ -280,8 +342,19 @@ namespace armarx
int dataSize = i->getElementType() == aron::type::quaternion::ElementType::FLOAT32 ? 4 : 8;
createdQuat->setShape({1, 4, dataSize});
createdQuat->setType(i->getFullName());
- auto* currTreeElem = parentItem->child(index);
- auto* itemWidget = currTreeElem->treeWidget()->itemWidget(currTreeElem, 1);
+ auto* el = parentItem->child(index);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
+ auto* itemWidget = el->treeWidget()->itemWidget(el, 1);
auto* quatWidget = QuaternionWidget::DynamicCastAndCheck(itemWidget);
// error handling
@@ -320,6 +393,17 @@ namespace armarx
{
ARMARX_TRACE;
QTreeWidgetItem* el = parentItem->child(index);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
auto* genericWidget = el->treeWidget()->itemWidget(el, 1);
auto* intEnumWidget = IntEnumWidget::DynamicCastAndCheck(genericWidget);
if (!intEnumWidget)
@@ -341,6 +425,16 @@ namespace armarx
createdAron = createdAronInt;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
if (str.empty())
{
@@ -370,6 +464,16 @@ namespace armarx
createdAron = createdAronLong;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
if (str.empty())
{
@@ -398,6 +502,16 @@ namespace armarx
createdAron = createdAronFloat;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
if (str.empty())
{
@@ -425,6 +539,16 @@ namespace armarx
createdAron = createdAronDouble;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
if (str.empty())
{
@@ -452,6 +576,16 @@ namespace armarx
createdAron = createdAronBool;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
if (str.empty())
{
@@ -479,6 +613,16 @@ namespace armarx
createdAron = createdAronString;
QTreeWidgetItem* el = parentItem->child(index);
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ // its a maybetype. We have to check the state
+ if (el->checkState(1) == Qt::CheckState::Unchecked)
+ {
+ createdAron = nullptr;
+ return;
+ }
+ }
+
std::string str = el->text(1).toStdString();
createdAronString->setValue(str);
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
index 84cb58585e3b4e7f3a058db66f3b5f3a40f54aea..5bc5a40bbd407c104ce96905850e9d0beabecb48 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
@@ -101,6 +101,11 @@ namespace armarx
createdQWidgetItem =
new AronTreeWidgetItem(isDictChild, editableValue, QString::fromStdString(key), i);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ createdQWidgetItem->setCheckState(1, Qt::CheckState::Unchecked);
+ }
createdQWidgetItem->setText(1, QString::fromStdString(defaul));
createdQWidgetItem->setText(2, QString::fromStdString(i->getShortName()));
createdQWidgetItem->setText(
@@ -129,6 +134,12 @@ namespace armarx
createdQWidgetItem =
new AronTreeWidgetItem(editableValue, false, QString::fromStdString(key), i);
+
+ if (i->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ createdQWidgetItem->setCheckState(1, Qt::CheckState::Unchecked);
+ }
+
parentOfCreatedObj->addChild(createdQWidgetItem);
for (const auto& [key, value] : i->getMemberTypes())
@@ -149,6 +160,7 @@ namespace armarx
// The DictType has only one member, its key-type. This also must be present
ARMARX_CHECK_EQUAL(i->getChildren().size(), 1);
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PairPtr& pair)
{
@@ -167,6 +179,7 @@ namespace armarx
}
}
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& tuple)
{
@@ -185,6 +198,7 @@ namespace armarx
}
}
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ListPtr& i)
{
@@ -192,6 +206,7 @@ namespace armarx
auto txt = misc::generateNumElementsText(0);
createdQWidgetItem->setText(1, txt);
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
{
@@ -233,6 +248,7 @@ namespace armarx
i->getRows(), i->getCols(), i->getElementType(), createdQWidgetItem);
toplevelWidget->setItemWidget(createdQWidgetItem, 1, matWidget);
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
{
@@ -257,16 +273,19 @@ namespace armarx
new QuaternionWidget(i->getElementType(), createdQWidgetItem);
toplevelWidget->setItemWidget(createdQWidgetItem, 1, quatWidget);
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ImagePtr& i)
{
insertNewTreeViewWidget(i, "");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
{
insertNewTreeViewWidget(i, "");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
{
@@ -278,36 +297,42 @@ namespace armarx
IntEnumWidget* widget = new IntEnumWidget(i, createdQWidgetItem);
createdQWidgetItem->treeWidget()->setItemWidget(createdQWidgetItem, 1, widget);
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::IntPtr& i)
{
editableValue = true;
insertNewTreeViewWidget(i, "0");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::LongPtr& i)
{
editableValue = true;
insertNewTreeViewWidget(i, "0");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::FloatPtr& i)
{
editableValue = true;
insertNewTreeViewWidget(i, "0.0");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::DoublePtr& i)
{
editableValue = true;
insertNewTreeViewWidget(i, "0.0");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::BoolPtr& i)
{
editableValue = true;
insertNewTreeViewWidget(i, "false");
}
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::StringPtr& i)
{
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
index cf719f8a2d1360d574fb8ebcac867378c3fd6f1b..cafc9b499b45a587efcc7a52ca531e028a8391df 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
@@ -31,7 +31,6 @@
#include "AronTreeWidgetContextMenu.h"
#include "AronTreeWidgetCreator.h"
-
template <typename T>
std::string
usString(T number, size_t precision = 3)
@@ -44,7 +43,6 @@ usString(T number, size_t precision = 3)
return ss.str();
}
-
//visitors
namespace armarx
{
@@ -101,7 +99,6 @@ namespace armarx
}
}
-
void
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DictPtr& i)
{
@@ -109,7 +106,7 @@ namespace armarx
if (i->getPath().size() == 0 ||
checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
auto* aronTreeWidget = AronTreeWidgetItem::DynamicCastAndCheck(el);
// allocate enough child items
adjustNumberOfChildren(aronTreeWidget, i->childrenSize());
@@ -123,7 +120,11 @@ namespace armarx
AronTreeWidgetSetterVisitor v(el, x++);
aron::data::visit(v, value);
}
- return;
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -132,7 +133,7 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
auto* aronTreeWidget = AronTreeWidgetItem::DynamicCastAndCheck(el);
adjustNumberOfChildren(aronTreeWidget, i->childrenSize());
@@ -154,10 +155,14 @@ namespace armarx
auto currFont = aronTreeWidget->font(1);
currFont.setItalic(true);
aronTreeWidget->setFont(1, currFont);
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
-
void
visitMatrix(EditMatrixWidget* matrixWidget,
const std::shared_ptr<armarx::aron::type::Matrix>& matrixType,
@@ -215,6 +220,7 @@ namespace armarx
}
}
}
+
void
visitQuaternion(QuaternionWidget* quatWidget,
std::shared_ptr<armarx::aron::type::Quaternion>& quatType,
@@ -256,16 +262,16 @@ namespace armarx
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::NDArrayPtr& arr)
{
// Matrices are handled as NDArray. Raw ndarrays cannot be created currently
- auto* castedEl = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
- ARMARX_CHECK(castedEl);
+ auto* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
+ ARMARX_CHECK(el);
- auto matrixCast = aron::type::Matrix::DynamicCast(castedEl->aronType);
- auto quaternionCast = aron::type::Quaternion::DynamicCast(castedEl->aronType);
+ auto matrixCast = aron::type::Matrix::DynamicCast(el->aronType);
+ auto quaternionCast = aron::type::Quaternion::DynamicCast(el->aronType);
- auto* rootWidget = castedEl->treeWidget();
+ auto* rootWidget = el->treeWidget();
ARMARX_CHECK(rootWidget);
- auto* matrixWidget = EditMatrixWidget::DynamicCast(rootWidget->itemWidget(castedEl, 1));
- auto* quaternionWidget = QuaternionWidget::DynamicCast(rootWidget->itemWidget(castedEl, 1));
+ auto* matrixWidget = EditMatrixWidget::DynamicCast(rootWidget->itemWidget(el, 1));
+ auto* quaternionWidget = QuaternionWidget::DynamicCast(rootWidget->itemWidget(el, 1));
if (matrixCast && matrixWidget)
{
@@ -280,6 +286,11 @@ namespace armarx
ARMARX_ERROR
<< "we do not support raw NDArrays. Ask Fabian Peller for more information.";
}
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
void
@@ -287,7 +298,7 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
auto* enumWidget = IntEnumWidget::DynamicCast(el->treeWidget()->itemWidget(el, 1));
auto newText = QString::fromStdString(usString<int>(i->getValue()));
if (enumWidget)
@@ -300,6 +311,11 @@ namespace armarx
// Its just an int. -> do the QTreeWidgetItem call
el->setText(1, newText);
}
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -308,8 +324,13 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
el->setText(1, QString::fromStdString(usString<long>(i->getValue())));
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -318,8 +339,13 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
el->setText(1, QString::fromStdString(usString<float>(i->getValue())));
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -328,8 +354,13 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
el->setText(1, QString::fromStdString(usString<double>(i->getValue())));
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -338,8 +369,13 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
el->setText(1, QString::fromStdString(usString<bool>(i->getValue())));
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
@@ -348,8 +384,13 @@ namespace armarx
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
- QTreeWidgetItem* el = parentItem->child(index);
+ AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
el->setText(1, QString::fromStdString(i->getValue()));
+
+ if (el->aronType && el->aronType->getMaybe() != armarx::aron::type::Maybe::NONE)
+ {
+ el->setCheckState(1, Qt::CheckState::Checked);
+ }
}
}
diff --git a/source/RobotAPI/interface/skills/SkillManagerInterface.ice b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
index b2c3b5deba9aa8239023d4559b6d20405e5b0889..f22738bda8b71cac09faf71620e226135ac93532 100644
--- a/source/RobotAPI/interface/skills/SkillManagerInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
@@ -49,8 +49,8 @@ module armarx
SkillID skillId;
string description;
armarx::core::time::dto::Duration timeout;
- aron::type::dto::AronObject acceptedType;
- aron::type::dto::AronObject returnType;
+ aron::type::dto::AronObject parametersType;
+ aron::type::dto::AronObject resultType;
aron::data::dto::Dict rootProfileDefaults;
};
@@ -74,10 +74,10 @@ module armarx
struct SkillStatusUpdate
{
SkillExecutionID executionId;
- aron::data::dto::Dict usedParams;
- callback::dti::SkillProviderCallbackInterface* usedCallbackInterface;
+ aron::data::dto::Dict parameters;
+ callback::dti::SkillProviderCallbackInterface* callbackInterface;
core::dto::Execution::Status status;
- aron::data::dto::Dict data;
+ aron::data::dto::Dict result;
};
dictionary<SkillExecutionID, SkillStatusUpdate> SkillStatusUpdateMap;
@@ -86,7 +86,7 @@ module armarx
{
SkillID skillId;
string executorName;
- aron::data::dto::Dict params;
+ aron::data::dto::Dict parameters;
};
}
@@ -99,7 +99,7 @@ module armarx
void addProvider(dto::ProviderInfo providerInfo);
- void removeProvider(dto::ProviderID provider);
+ void removeProvider(dto::ProviderID providerId);
optional(1) dto::SkillDescription getSkillDescription(
dto::SkillID skillId); // get one skilldescriptions from a provider
@@ -115,16 +115,20 @@ module armarx
dto::SkillStatusUpdate
executeSkill(dto::SkillExecutionRequest
- skillExecutionInfo); // blocks until skill is finished.
+ skillExecutionRequest); // blocks until skill is finished.
dto::SkillExecutionID executeSkillAsync(
dto::SkillExecutionRequest
skillExecutionRequest); // directly returns the execution id
- void addSkillParameters(dto::SkillExecutionID executionId,
- aron::data::dto::Dict params); // add params to a skill
+ provider::dto::ParameterUpdateResult updateSkillParameters(
+ dto::SkillExecutionID executionId,
+ aron::data::dto::Dict parameters); // add params to a skill
- void abortSkill(dto::SkillExecutionID execId); // notify a skill to stop ASAP.
+ // notify a skill to stop ASAP.
+ provider::dto::AbortSkillResult abortSkill(dto::SkillExecutionID executionId);
+ provider::dto::AbortSkillResult
+ abortSkillAsync(dto::SkillExecutionID executionId);
};
}
}
diff --git a/source/RobotAPI/interface/skills/SkillMemoryInterface.ice b/source/RobotAPI/interface/skills/SkillMemoryInterface.ice
index 934d2fabaf539697a8416f4567c734e36de25858..3d0d8ee61b23a5e184514b87792323d379c81173 100644
--- a/source/RobotAPI/interface/skills/SkillMemoryInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillMemoryInterface.ice
@@ -36,11 +36,7 @@ module armarx
module dti
{
interface SkillMemoryInterface extends armem::server::MemoryInterface,
- dti::StatechartListenerInterface, manager::dti::SkillManagerInterface
- {
- manager::dto::SkillStatusUpdateMap getLatestSkillExecutionStatuses(
- int n); // returns latest n statusupdates of all providers
- };
+ dti::StatechartListenerInterface, manager::dti::SkillManagerInterface{};
}
}
}
diff --git a/source/RobotAPI/interface/skills/SkillProviderInterface.ice b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
index 80a8a97fbdb9f6eff8f19cfaf1384338748c6232..3066c962e45326fecfdf8f08c6985bb89bf6c3d7 100644
--- a/source/RobotAPI/interface/skills/SkillProviderInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
@@ -58,9 +58,6 @@ module armarx
{
module dto
{
-
- sequence<string> StringList;
-
// The status enum of a skill
module Execution
{
@@ -77,11 +74,6 @@ module armarx
Aborted
};
}
-
- // Status updates of a skill
- class SkillStatusUpdateBase
- {
- };
}
}
}
@@ -123,8 +115,8 @@ module armarx
SkillID skillId;
string description;
armarx::core::time::dto::Duration timeout;
- aron::type::dto::AronObject acceptedType;
- aron::type::dto::AronObject returnType;
+ aron::type::dto::AronObject parametersType;
+ aron::type::dto::AronObject resultType;
aron::data::dto::Dict rootProfileDefaults;
};
@@ -135,7 +127,7 @@ module armarx
{
SkillID skillId;
string executorName;
- aron::data::dto::Dict params;
+ aron::data::dto::Dict parameters;
callback::dti::SkillProviderCallbackInterface* callbackInterface;
};
@@ -152,13 +144,25 @@ module armarx
struct SkillStatusUpdate
{
SkillExecutionID executionId;
- aron::data::dto::Dict usedParams;
- callback::dti::SkillProviderCallbackInterface* usedCallbackInterface;
+ aron::data::dto::Dict parameters;
+ callback::dti::SkillProviderCallbackInterface* callbackInterface;
core::dto::Execution::Status status;
- aron::data::dto::Dict data;
+ aron::data::dto::Dict result;
+ /// @todo maybe add in future:
+ // aron::data::dto::Dict feedback;
};
dictionary<SkillExecutionID, SkillStatusUpdate> SkillStatusUpdateMap;
+
+ struct ParameterUpdateResult
+ {
+ bool success; // false if skill is not running (anymore) or already prepared
+ };
+
+ struct AbortSkillResult
+ {
+ bool success; // false if skill is not running (anymore)
+ };
}
module dti
@@ -183,11 +187,13 @@ module armarx
dto::SkillExecutionID
executeSkillAsync(dto::SkillExecutionRequest executionInfo);
- void addSkillParameters(dto::SkillExecutionID executionId,
- aron::data::dto::Dict params); // add params to a skill
+ dto::ParameterUpdateResult
+ updateSkillParameters(dto::SkillExecutionID executionId,
+ aron::data::dto::Dict params); // add params to a skill
// try to kill a skill as soon as possible. When the skill is stopped depends on the implementation.
- void abortSkill(dto::SkillExecutionID skill);
+ dto::AbortSkillResult abortSkill(dto::SkillExecutionID skill);
+ dto::AbortSkillResult abortSkillAsync(dto::SkillExecutionID skill);
};
}
}
diff --git a/source/RobotAPI/interface/units/GamepadUnit.ice b/source/RobotAPI/interface/units/GamepadUnit.ice
index 671e1de9b99591f0d4e23ab923feac8a5e1d535d..c63ddcfdad36942cfcca6c65155c8e96c6847f79 100644
--- a/source/RobotAPI/interface/units/GamepadUnit.ice
+++ b/source/RobotAPI/interface/units/GamepadUnit.ice
@@ -62,8 +62,9 @@ module armarx
bool rightStickButton;
};
- interface GamepadUnitInterface extends armarx::SensorActorUnitInterface
+ interface GamepadUnitInterface // extends armarx::SensorActorUnitInterface
{
+ void vibrate();
};
interface GamepadUnitListener
@@ -78,4 +79,3 @@ module armarx
};
};
-
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
index 4e14baacb6040f7bf97a4855d3da7e4d252bce03..0eecc8e79de97cb3e164b195dd43cedc57b789cf 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -10,13 +10,12 @@
namespace armarx::armem
{
- GraspCandidateReader::GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ GraspCandidateReader::GraspCandidateReader()
{
}
void
- GraspCandidateReader::connect(bool use)
+ GraspCandidateReader::connect(armem::client::MemoryNameSystem& memoryNameSystem, bool use)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '" << properties.memoryName
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
index 384f5cdb0c1158b714ba8b3038e09f4707dd74bb..4b7bba78baae8c78c7c37fdc77ebdaff8e6fd154 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
@@ -12,9 +12,9 @@ namespace armarx::armem
class GraspCandidateReader
{
public:
- GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ GraspCandidateReader();
- void connect(bool use = true);
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem, bool use = true);
::armarx::grasping::GraspCandidatePtr
queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
@@ -65,8 +65,6 @@ namespace armarx::armem
} properties;
const std::string propertyPrefix = "mem.grasping.";
-
- armem::client::MemoryNameSystem& memoryNameSystem;
};
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
index 7aa20ba28af1b49404118cfedb3abd006d784738..1f14e344ee988f984813e2a402903cfb0627b940 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.cpp
@@ -6,16 +6,11 @@
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error/mns.h>
-
namespace armarx::armem
{
- GraspCandidateWriter::GraspCandidateWriter(armem::client::MemoryNameSystem& memoryNameSystem)
- : memoryNameSystem(memoryNameSystem)
- {
- }
-
- void GraspCandidateWriter::connect()
+ void
+ GraspCandidateWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "GraspCandidateWriter: Waiting for memory '" << properties.memoryName
@@ -23,33 +18,37 @@ namespace armarx::armem
try
{
memoryWriter = memoryNameSystem.useWriter(properties.memoryName);
- ARMARX_IMPORTANT << "GraspCandidateWriter: Connected to memory '" << properties.memoryName << "'";
+ ARMARX_IMPORTANT << "GraspCandidateWriter: Connected to memory '"
+ << properties.memoryName << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
ARMARX_ERROR << e.what();
return;
}
-
}
- void GraspCandidateWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ GraspCandidateWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "GraspCandidateWriter: registerPropertyDefinitions";
const std::string prefix = propertyPrefix;
- def->optional(properties.graspMemoryName, prefix + "GraspMemoryName",
+ def->optional(properties.graspMemoryName,
+ prefix + "GraspMemoryName",
"Name of the grasping memory core segment to use.");
- def->optional(properties.bimanualGraspMemoryName, prefix + "BimanualGraspMemoryName",
+ def->optional(properties.bimanualGraspMemoryName,
+ prefix + "BimanualGraspMemoryName",
"Name of the bimanual grasping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
-
}
- bool GraspCandidateWriter::commitGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
- const armem::Time& timestamp, const std::string& provider)
+ bool
+ GraspCandidateWriter::commitGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
+ const armem::Time& timestamp,
+ const std::string& provider)
{
armarx::grasping::arondto::GraspCandidate aronGraspCandidate;
std::string objectName = "UnknownObject";
@@ -65,8 +64,11 @@ namespace armarx::armem
return commitToMemory({dict}, provider, objectName, timestamp, properties.graspMemoryName);
}
- bool GraspCandidateWriter::commitBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate,
- const armem::Time& timestamp, const std::string& provider)
+ bool
+ GraspCandidateWriter::commitBimanualGraspCandidate(
+ const armarx::grasping::BimanualGraspCandidate& candidate,
+ const armem::Time& timestamp,
+ const std::string& provider)
{
armarx::grasping::arondto::BimanualGraspCandidate aronGraspCandidate;
@@ -80,11 +82,15 @@ namespace armarx::armem
auto dict = aronGraspCandidate.toAron();
- return commitToMemory({dict}, provider, objectName, timestamp, properties.bimanualGraspMemoryName);
+ return commitToMemory(
+ {dict}, provider, objectName, timestamp, properties.bimanualGraspMemoryName);
}
- bool GraspCandidateWriter::commitGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq& candidates,
- const armem::Time& timestamp, const std::string& provider)
+ bool
+ GraspCandidateWriter::commitGraspCandidateSeq(
+ const armarx::grasping::GraspCandidateSeq& candidates,
+ const armem::Time& timestamp,
+ const std::string& provider)
{
bool success = true;
@@ -107,7 +113,7 @@ namespace armarx::armem
for (const auto& [key, dict] : updates)
{
- if (! commitToMemory(dict, provider, key, timestamp, properties.graspMemoryName))
+ if (!commitToMemory(dict, provider, key, timestamp, properties.graspMemoryName))
{
success = false;
}
@@ -115,8 +121,10 @@ namespace armarx::armem
return success;
}
- bool GraspCandidateWriter::commitBimanualGraspCandidateSeq(
- const armarx::grasping::BimanualGraspCandidateSeq& candidates, const armem::Time& timestamp,
+ bool
+ GraspCandidateWriter::commitBimanualGraspCandidateSeq(
+ const armarx::grasping::BimanualGraspCandidateSeq& candidates,
+ const armem::Time& timestamp,
const std::string& provider)
{
bool success = true;
@@ -138,7 +146,7 @@ namespace armarx::armem
for (const auto& [key, dict] : updates)
{
- if (! commitToMemory(dict, provider, key, timestamp, properties.bimanualGraspMemoryName))
+ if (!commitToMemory(dict, provider, key, timestamp, properties.bimanualGraspMemoryName))
{
success = false;
}
@@ -146,14 +154,16 @@ namespace armarx::armem
return success;
}
- bool GraspCandidateWriter::commitToMemory(const std::vector<armarx::aron::data::DictPtr>& instances,
- const std::string& providerName, const std::string& entityName, const armem::Time& timestamp,
- const std::string& coreMemoryName)
+ bool
+ GraspCandidateWriter::commitToMemory(const std::vector<armarx::aron::data::DictPtr>& instances,
+ const std::string& providerName,
+ const std::string& entityName,
+ const armem::Time& timestamp,
+ const std::string& coreMemoryName)
{
std::lock_guard g{memoryWriterMutex};
- const auto result =
- memoryWriter.addSegment(coreMemoryName, providerName);
+ const auto result = memoryWriter.addSegment(coreMemoryName, providerName);
if (not result.success)
{
@@ -164,8 +174,7 @@ namespace armarx::armem
}
const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID =
- providerId.withEntityName(entityName).withTimestamp(timestamp);
+ const auto entityID = providerId.withEntityName(entityName).withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
@@ -184,4 +193,4 @@ namespace armarx::armem
return updateResult.success;
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
index 88793cb534cb18af0fefddcd72be4215eaa70e2b..526f16b0ae327f6e3465c7adfc3d2c1c7fb1b26b 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h
@@ -1,58 +1,60 @@
#pragma once
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client.h>
#include <mutex>
-#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
namespace armarx::armem
{
class GraspCandidateWriter
{
public:
- GraspCandidateWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ GraspCandidateWriter() = default;
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
bool commitGraspCandidate(const armarx::grasping::GraspCandidate& candidate,
- const armem::Time& timestamp, const std::string& provider);
+ const armem::Time& timestamp,
+ const std::string& provider);
bool commitBimanualGraspCandidate(const armarx::grasping::BimanualGraspCandidate& candidate,
- const armem::Time& timestamp, const std::string& provider);
+ const armem::Time& timestamp,
+ const std::string& provider);
bool commitGraspCandidateSeq(const armarx::grasping::GraspCandidateSeq& candidates,
- const armem::Time& timestamp, const std::string& provider);
- bool commitBimanualGraspCandidateSeq(const armarx::grasping::BimanualGraspCandidateSeq& candidates,
- const armem::Time& timestamp, const std::string& provider);
-
+ const armem::Time& timestamp,
+ const std::string& provider);
+ bool commitBimanualGraspCandidateSeq(
+ const armarx::grasping::BimanualGraspCandidateSeq& candidates,
+ const armem::Time& timestamp,
+ const std::string& provider);
private:
-
bool commitToMemory(const std::vector<armarx::aron::data::DictPtr>& instances,
- const std::string& providerName, const std::string& entityName, const armem::Time& timestamp,
+ const std::string& providerName,
+ const std::string& entityName,
+ const armem::Time& timestamp,
const std::string& coreMemoryName);
- armem::client::MemoryNameSystem& memoryNameSystem;
-
armem::client::Writer memoryWriter;
struct Properties
{
- std::string memoryName = "Grasp";
- std::string graspMemoryName = "GraspCandidate";
+ std::string memoryName = "Grasp";
+ std::string graspMemoryName = "GraspCandidate";
std::string bimanualGraspMemoryName = "BimanualGraspCandidate";
} properties;
-
std::mutex memoryWriterMutex;
const std::string propertyPrefix = "mem.grasping.";
-
};
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
index e3467eb9a250af792c5c53d578b0555b49a95efa..ce15a37df98b729bd0a0dc875ccbac13aae2108f 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
@@ -8,12 +8,17 @@ armarx_add_library(
SimoxUtility
RobotAPI::Core
RobotAPI::Aron::Common
+
+ ArViz
+ RobotAPIArmarXObjects
SOURCES
datatypes/CommonPlace.cpp
CommonPlaceLoader.cpp
+ Visu.cpp
HEADERS
datatypes/CommonPlace.h
CommonPlaceLoader.h
+ Visu.h
)
add_library(${PROJECT_NAME}::PriorKnowledge::util::CommonPlaceLoader ALIAS ${PROJECT_NAME}PriorKnowledgeCommonPlaceLoaderUtil)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cfd68864ff87c6490af33be5d40645051b0e4fe7
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.cpp
@@ -0,0 +1,24 @@
+#include "Visu.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+
+namespace armarx::priorknowledge::util::common_place
+{
+ viz::Layer
+ Visu::commonPlaceToLayer(const std::string& layerName,
+ const std::map<std::string, CommonPlaceData>& commonPlaceData) const
+ {
+ auto layer = arviz.layer(layerName);
+ for (auto& [id, data] : commonPlaceData)
+ {
+ auto o = armarx::viz::Object("");
+ o = o.fileByObjectFinder(armarx::ObjectID(data.objectId));
+ o.pose(data.globalPose).alpha(0.5);
+ layer.add(o);
+ }
+ return layer;
+ }
+
+} // namespace armarx::priorknowledge::util::common_place
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.h b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..e4df667ca056b92db65053834a1fea75b75d8eb9
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
+
+#include "datatypes/CommonPlace.h"
+
+namespace armarx::priorknowledge::util::common_place
+{
+
+ class Visu
+ {
+ public:
+ struct CommonPlaceData
+ {
+ Eigen::Matrix4f globalPose;
+ std::string objectId;
+ };
+
+ Visu(viz::Client& arviz) : arviz(arviz)
+
+ {
+ }
+
+ ~Visu() = default;
+
+ viz::Layer commonPlaceToLayer(const std::string& layerName,
+ const std::map<std::string, CommonPlaceData>& data) const;
+
+
+ public:
+ struct Settings
+ {
+
+ } settings;
+
+ protected:
+ viz::Client& arviz;
+ };
+} // namespace armarx::priorknowledge::util::common_place
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 676c1fd0752baf0c2995e479dfad2cca4f3d9960..9030bd17597399abf79e3413ec71a789f3a113ca 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -57,12 +57,14 @@ set(LIB_FILES
client/MemoryNameSystem.cpp
client/Reader.cpp
client/Writer.cpp
+ client/ReadStream.cpp
client/plugins/ListeningPluginUser.cpp
client/plugins/ListeningPlugin.cpp
client/plugins/PluginUser.cpp
client/plugins/Plugin.cpp
+ client/util/SubscriptionHandle.cpp
client/util/MemoryListener.cpp
client/util/SimpleReaderBase.cpp
client/util/SimpleWriterBase.cpp
@@ -138,6 +140,7 @@ set(LIB_HEADERS
client/MemoryNameSystem.h
client/Reader.h
client/Writer.h
+ client/ReadStream.h
client/plugins.h
client/plugins/ListeningPluginUser.h
@@ -154,6 +157,7 @@ set(LIB_HEADERS
client/query/detail/NameSelectorOps.h
client/query/detail/SelectorOps.h
+ client/util/SubscriptionHandle.h
client/util/MemoryListener.h
client/util/SimpleReaderBase.h
client/util/SimpleWriterBase.h
diff --git a/source/RobotAPI/libraries/armem/client/ReadStream.cpp b/source/RobotAPI/libraries/armem/client/ReadStream.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..688357958419900abbea45f85b2c35cf4868f26f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/ReadStream.cpp
@@ -0,0 +1,172 @@
+#include "ReadStream.h"
+
+#include <ArmarXCore/core/time/Clock.h>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+
+namespace armarx::armem::client
+{
+ ReadStream::ReadStream() : metronome{armarx::core::time::Frequency::Hertz(10)}
+ {
+ }
+
+ ReadStream::ReadStream(const Reader& reader,
+ const MemoryID& queriedId,
+ const core::time::Frequency& maxPollFrequency) :
+ reader{reader}, queriedId{queriedId}, metronome{maxPollFrequency}
+ {
+ timeStart = armarx::core::time::Clock::Now();
+ }
+
+ std::optional<wm::EntitySnapshot>
+ ReadStream::pollBlocking(const SnapshotCallbackT& callback)
+ {
+ if (isPolling.exchange(true))
+ {
+ throw armarx::armem::error::ReadStreamAlreadyPolling(queriedId, __PRETTY_FUNCTION__);
+ }
+ pollingStoppedExternally = false;
+
+ auto result = _pollBlocking(callback);
+
+ isPolling = false;
+ return result;
+ }
+
+ void
+ ReadStream::pollAsync(const SnapshotCallbackT& callback)
+ {
+ if (isPolling.exchange(true))
+ {
+ throw armarx::armem::error::ReadStreamAlreadyPolling(queriedId, __PRETTY_FUNCTION__);
+ }
+ pollingStoppedExternally = false;
+
+ this->pollingThread = std::thread([&]() { this->_pollBlocking(callback); });
+ }
+
+ std::optional<wm::EntitySnapshot>
+ ReadStream::_pollBlocking(const SnapshotCallbackT& callback)
+ {
+ while (not pollingStoppedExternally)
+ {
+ auto snapshot = _pollOnce(callback);
+ if (snapshot.has_value())
+ {
+ return snapshot;
+ }
+ }
+ return std::nullopt;
+ }
+
+ void
+ ReadStream::stop()
+ {
+ pollingStoppedExternally = true;
+ if (pollingThread.joinable())
+ {
+ pollingThread.join();
+ isPolling = false;
+ }
+ }
+
+ std::optional<wm::EntitySnapshot>
+ ReadStream::pollOnce(const SnapshotCallbackT& callback)
+ {
+ if (isPolling.exchange(true))
+ {
+ throw armarx::armem::error::ReadStreamAlreadyPolling(queriedId, __PRETTY_FUNCTION__);
+ }
+
+ auto snapshot = _pollOnce(callback);
+
+ isPolling = false;
+ return snapshot;
+ }
+
+ std::optional<wm::EntitySnapshot>
+ ReadStream::_pollOnce(const SnapshotCallbackT& callback)
+ {
+ // Make sure to not busy wait. Also wait until probably data is available in first iteration.
+ metronome.waitForNextTick();
+
+ auto timeEnd = armarx::core::time::Clock::Now();
+ auto makeQuery = [this, &timeEnd](const MemoryID& id)
+ {
+ query::Builder qb;
+ query::CoreSegmentSelector& core =
+ id.hasCoreSegmentName() ? qb.coreSegments().withID(id) : qb.coreSegments().all();
+ query::ProviderSegmentSelector& prov = id.hasProviderSegmentName()
+ ? core.providerSegments().withID(id)
+ : core.providerSegments().all();
+ query::EntitySelector& entity =
+ id.hasEntityName() ? prov.entities().withID(id) : prov.entities().all();
+ entity.snapshots().timeRange(timeStart, timeEnd);
+
+ return qb.buildQueryInput();
+ };
+
+ auto query = makeQuery(queriedId);
+
+ auto result = reader.query(query);
+
+ if (result.success)
+ {
+ using EntitySnapshotReference =
+ std::reference_wrapper<armarx::armem::wm::EntitySnapshot>;
+ // Copy references of snapshots into vector to sort them.
+ std::vector<EntitySnapshotReference> snapshots;
+
+ result.memory.forEachSnapshot([&snapshots](armarx::armem::wm::EntitySnapshot& snapshot)
+ { snapshots.push_back(snapshot); });
+
+ // Sort correctly.
+ std::sort(snapshots.begin(),
+ snapshots.end(),
+ [](const EntitySnapshotReference& a, const EntitySnapshotReference& b)
+ { return a.get().id().timestamp < b.get().id().timestamp; });
+
+ // Determine the next start time.
+ DateTime nextStart;
+ if (snapshots.size() > 0)
+ {
+ // Because they are sorted, back() has the highest time stamp.
+ nextStart = snapshots.back().get().id().timestamp +
+ armarx::core::time::Duration::MicroSeconds(1);
+ }
+ else
+ {
+ nextStart = timeStart;
+ }
+
+ // Call the callback on all snapshots.
+ for (const auto& snapshot : snapshots)
+ {
+ // Assert times in correct interval.
+ ARMARX_CHECK_LESS_EQUAL(timeStart, snapshot.get().id().timestamp);
+ ARMARX_CHECK_GREATER_EQUAL(timeEnd, snapshot.get().id().timestamp);
+
+ const bool continue_ = callback(snapshot.get());
+ if (not continue_)
+ {
+ return snapshot;
+ }
+ }
+
+ timeStart = nextStart;
+ }
+ else
+ {
+ ARMARX_WARNING
+ << deactivateSpam()
+ << "Received an error in ReadStream when querying data from a "
+ "memory. The error was '"
+ << result.errorMessage
+ << "'. Continue with stream, perhaps the memory was not yet initialized.";
+ }
+
+ return std::nullopt;
+ }
+
+
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/ReadStream.h b/source/RobotAPI/libraries/armem/client/ReadStream.h
new file mode 100644
index 0000000000000000000000000000000000000000..c86fb5b0675ddfc3fa3c4cd9abe49df5ab7a3016
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/ReadStream.h
@@ -0,0 +1,138 @@
+#pragma once
+
+#include <functional>
+#include <optional>
+#include <thread>
+
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
+#include "Reader.h"
+
+namespace armarx::armem::client
+{
+ /**
+ * @brief A stream reading entity snapshots from the memory.
+ *
+ * After constructing a ReadStream, polling can be started in three ways:
+ *
+ * 1. Run a polling loop in this thread, blocking execution until terminated.
+ * See pollBlocking().
+ * 2. Run a polling loop in a new, separate thread, until it is stopped via stop().
+ * See pollAsync() and stop().
+ * 3. Perform a single query and process the result, embedded in your own loop or other control
+ * flow logic.
+ * See pollOnce().
+ */
+ class ReadStream
+ {
+ public:
+ /**
+ * @brief Callback called on each entity snapshot in the queried ID.
+ *
+ * If it returns false, the stream is stopped.
+ */
+ using SnapshotCallbackT = std::function<bool(const wm::EntitySnapshot&)>;
+
+ /**
+ * @brief Inizialize a ReadStream which does not represent a stream.
+ */
+ ReadStream();
+
+ /**
+ * @brief Initialize a read stream.
+ *
+ * @param reader
+ * The reader to perform the queries.
+ * @param queriedId
+ * The memory ID in which all snapshots should be processed by the stream.
+ * @param maxPollFrequency
+ * The maximum frequency with which queries are performed. The
+ * real frequency might be lower.
+ */
+ ReadStream(const Reader& reader,
+ const MemoryID& queriedId,
+ const armarx::core::time::Frequency& maxPollFrequency =
+ armarx::core::time::Frequency::Hertz(10));
+
+
+ /**
+ * @brief Poll in this thread as long as callback returns true.
+ *
+ * @param callback Function to call on each entity snapshot.
+ * @return The snapshot object that returns false.
+ * @throw armarx::armem::error::ReadStreamAlreadyPolling If the stream is already polling.
+ */
+ std::optional<wm::EntitySnapshot> pollBlocking(const SnapshotCallbackT& callback);
+
+ /**
+ * @brief Poll in a new thread as long as callback returns true.
+ *
+ * Note that callback will be called in a separate thread, so take care of synchronizing
+ * access to variables in the callback appropriately.
+ *
+ * Roughly equivalent to:
+ * @code
+ * std::thread thread([]() { stream.pollBlocking(); });
+ * @endcode
+ *
+ * @param callback Function to call on each entity snapshot.
+ * @throw armarx::armem::error::ReadStreamAlreadyPolling If the stream is already polling.
+ */
+ void pollAsync(const SnapshotCallbackT& callback);
+ /**
+ * @brief Stop a running polling loop.
+ *
+ * If a polling thread has been started by pollAsync() before, joins the thread.
+ */
+ void stop();
+
+ /**
+ * @brief Perform one query and call the callbacks on each snapshot.
+ *
+ * This allows you to define your own loop, for example:
+ *
+ * @code
+ * bool condition = true;
+ * while (condition)
+ * {
+ * auto snapshot = stream.pollOnce(callback);
+ *
+ * ...
+ *
+ * if (...)
+ * {
+ * condition = false;
+ * }
+ * }
+ * @endcode
+ *
+ * @param callback Function to call on each entity snapshot.
+ * @throw armarx::armem::error::ReadStreamAlreadyPolling If the stream is already polling.
+ */
+ std::optional<wm::EntitySnapshot> pollOnce(const SnapshotCallbackT& callback);
+
+
+ private:
+ std::optional<wm::EntitySnapshot> _pollBlocking(const SnapshotCallbackT& callback);
+ std::optional<wm::EntitySnapshot> _pollOnce(const SnapshotCallbackT& callback);
+
+
+ private:
+ Reader reader;
+ MemoryID queriedId;
+
+ armarx::core::time::Metronome metronome;
+ armarx::DateTime timeStart;
+
+ std::atomic_bool isPolling = false;
+ std::atomic_bool pollingStoppedExternally = false;
+
+ std::thread pollingThread;
+ };
+
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index 875250eb836740ec46dc3cc009ded155e97a2264..d476b25ef2003edeeb183fbe4e74bcd70a6176c0 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -2,9 +2,9 @@
// STD/STL
+#include <mutex>
#include <optional>
#include <vector>
-#include <mutex>
// RobotAPI
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
@@ -15,7 +15,6 @@
#include "Query.h"
-
namespace armarx::armem::client
{
@@ -26,11 +25,11 @@ namespace armarx::armem::client
{
public:
-
/**
* @brief Construct a memory reader.
* @param memory The memory proxy.
*/
+ Reader(const Reader&) = default;
Reader(server::ReadingMemoryInterfacePrx readingMemory = nullptr,
server::PredictingMemoryInterfacePrx predictingMemory = nullptr);
@@ -41,8 +40,10 @@ namespace armarx::armem::client
QueryResult query(const QueryInput& input) const;
armem::query::data::Result query(const armem::query::data::Input& input) const;
- QueryResult query(armem::query::data::MemoryQueryPtr query, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
- QueryResult query(const armem::query::data::MemoryQuerySeq& queries, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult query(armem::query::data::MemoryQueryPtr query,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult query(const armem::query::data::MemoryQuerySeq& queries,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
QueryResult query(const QueryBuilder& queryBuilder) const;
@@ -114,7 +115,8 @@ namespace armarx::armem::client
* @return The query result.
*/
QueryResult
- queryMemoryIDs(const std::vector<MemoryID>& ids, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ queryMemoryIDs(const std::vector<MemoryID>& ids,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
@@ -132,8 +134,9 @@ namespace armarx::armem::client
* @param dataMode With or without data.
* @return The query result.
*/
- QueryResult
- getLatestSnapshotsIn(const MemoryID& id, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult getLatestSnapshotsIn(
+ const MemoryID& id,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
* @brief Get the latest snapshot under the given memory ID.
@@ -141,8 +144,9 @@ namespace armarx::armem::client
* @param dataMode With or without data.
* @return The latest contained snapshot, if any.
*/
- std::optional<wm::EntitySnapshot>
- getLatestSnapshotIn(const MemoryID& id, armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ std::optional<wm::EntitySnapshot> getLatestSnapshotIn(
+ const MemoryID& id,
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
@@ -150,8 +154,8 @@ namespace armarx::armem::client
* @param dataMode With or without data.
* @return The query result.
*/
- QueryResult
- getAllLatestSnapshots(armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
+ QueryResult getAllLatestSnapshots(
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
/**
@@ -163,7 +167,8 @@ namespace armarx::armem::client
getAll(armem::query::DataMode dataMode = armem::query::DataMode::WithData) const;
- server::dto::DirectlyStoreResult directlyStore(const server::dto::DirectlyStoreInput& input) const;
+ server::dto::DirectlyStoreResult
+ directlyStore(const server::dto::DirectlyStoreInput& input) const;
void startRecording() const;
void stopRecording() const;
@@ -201,7 +206,6 @@ namespace armarx::armem::client
int recursionDepth);
public:
-
server::ReadingMemoryInterfacePrx readingPrx;
server::PredictingMemoryInterfacePrx predictionPrx;
};
diff --git a/source/RobotAPI/libraries/armem/client/Writer.h b/source/RobotAPI/libraries/armem/client/Writer.h
index 6c4b223ce12e0ee80cd5564e2d2d3ad688661cbc..61eeef93be3164d79d09f6c5aa3371cb362b58b2 100644
--- a/source/RobotAPI/libraries/armem/client/Writer.h
+++ b/source/RobotAPI/libraries/armem/client/Writer.h
@@ -1,10 +1,8 @@
#pragma once
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
-
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-
namespace armarx::armem::client
{
@@ -24,17 +22,21 @@ namespace armarx::armem::client
class Writer
{
public:
-
/**
* @brief Construct a memory writer.
* @param memory The memory proxy.
*/
+ Writer(const Writer&) = default;
Writer(server::WritingMemoryInterfacePrx memory = nullptr);
- data::AddSegmentResult addSegment(const std::string& coreSegmentName, const std::string& providerSegmentName, bool clearWhenExists = false);
- data::AddSegmentResult addSegment(const MemoryID& providerSegmentID, bool clearWhenExists = false);
- data::AddSegmentResult addSegment(const std::pair<std::string, std::string>& names, bool clearWhenExists = false);
+ data::AddSegmentResult addSegment(const std::string& coreSegmentName,
+ const std::string& providerSegmentName,
+ bool clearWhenExists = false);
+ data::AddSegmentResult addSegment(const MemoryID& providerSegmentID,
+ bool clearWhenExists = false);
+ data::AddSegmentResult addSegment(const std::pair<std::string, std::string>& names,
+ bool clearWhenExists = false);
data::AddSegmentResult addSegment(const data::AddSegmentInput& input);
data::AddSegmentsResult addSegments(const data::AddSegmentsInput& input);
@@ -46,10 +48,9 @@ namespace armarx::armem::client
/// Commit a single entity update.
EntityUpdateResult commit(const EntityUpdate& update);
/// Commit a single entity update.
- EntityUpdateResult commit(
- const MemoryID& entityID,
- const std::vector<aron::data::DictPtr>& instancesData,
- Time referencedTime);
+ EntityUpdateResult commit(const MemoryID& entityID,
+ const std::vector<aron::data::DictPtr>& instancesData,
+ Time referencedTime);
// with bare-ice types
data::CommitResult commit(const data::Commit& commit);
@@ -63,18 +64,15 @@ namespace armarx::armem::client
}
private:
-
/// Sets `timeSent` on all entity updates and performs the commit,
data::CommitResult _commit(data::Commit& commit);
public:
-
server::WritingMemoryInterfacePrx memory;
-
};
-}
+} // namespace armarx::armem::client
namespace armarx::armem::data
{
@@ -82,4 +80,4 @@ namespace armarx::armem::data
std::ostream& operator<<(std::ostream& os, const AddSegmentsInput& rhs);
std::ostream& operator<<(std::ostream& os, const AddSegmentResult& rhs);
std::ostream& operator<<(std::ostream& os, const AddSegmentsResult& rhs);
-}
+} // namespace armarx::armem::data
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h b/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
index 168686040e37f0346757b4ee259dd0107b0e4a37..868d3ef30cd593ab09c4ac55bcdcdcbdf2f7f81d 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h
@@ -23,13 +23,12 @@
#include <string>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ComponentPlugin.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/armem/client/plugins/Plugin.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
+#include <RobotAPI/libraries/armem/client/plugins/Plugin.h>
namespace armarx::armem::client::plugins
{
@@ -45,8 +44,8 @@ namespace armarx::armem::client::plugins
class ReaderWriterPlugin : public armarx::ComponentPlugin
{
public:
- ReaderWriterPlugin(ManagedIceObject& parent, const std::string pre) :
- ComponentPlugin(parent, pre), readerWriter(memoryNameSystem())
+ ReaderWriterPlugin(ManagedIceObject& parent, const std::string& pre) :
+ ComponentPlugin(parent, pre)
{
if (not armemPlugin)
{
@@ -74,7 +73,7 @@ namespace armarx::armem::client::plugins
return;
}
- readerWriter.connect();
+ readerWriter.connect(memoryNameSystem());
}
bool
@@ -114,9 +113,8 @@ namespace armarx::armem::client::plugins
}
armarx::armem::client::plugins::Plugin* armemPlugin = nullptr;
-
- T readerWriter;
+ T readerWriter;
};
} // namespace armarx::armem::client::plugins
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
index c8a7c6c683467194d6e2248edc0e5be57d3b547b..9aa23153cbfe67047579fdedab8753e35c260002 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.cpp
@@ -2,36 +2,33 @@
#include <sstream>
+#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
-#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
-
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
namespace armarx::armem::client::util
{
- std::string MemoryListener::MakeMemoryTopicName(const MemoryID& memoryID)
+ std::string
+ MemoryListener::MakeMemoryTopicName(const MemoryID& memoryID)
{
return "MemoryUpdates." + memoryID.memoryName;
}
-
- MemoryListener::MemoryListener(ManagedIceObject* component) :
- component(component)
+ MemoryListener::MemoryListener(ManagedIceObject* component) : component(component)
{
}
-
- void MemoryListener::setComponent(ManagedIceObject* component)
+ void
+ MemoryListener::setComponent(ManagedIceObject* component)
{
this->component = component;
}
-
void
MemoryListener::updated(const std::vector<data::MemoryID>& updatedSnapshotIDs) const
{
@@ -40,7 +37,6 @@ namespace armarx::armem::client::util
updated(bos);
}
-
void
MemoryListener::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
{
@@ -76,68 +72,104 @@ namespace armarx::armem::client::util
if (not matchingSnapshotIDs.empty())
{
ARMARX_DEBUG << "Calling " << subCallbacks.size() << " callbacks"
- << " subscribing " << subscription
- << " with " << matchingSnapshotIDs.size() << " snapshot IDs ...";
- for (auto& callback : subCallbacks)
+ << " subscribing " << subscription << " with "
+ << matchingSnapshotIDs.size() << " snapshot IDs ...";
+ for (auto& managedCallback : subCallbacks)
{
try
{
- callback(subscription, matchingSnapshotIDs);
+ managedCallback.callback(subscription, matchingSnapshotIDs);
}
catch (const armarx::LocalException& e)
{
error << "Calling callback subscribing " << subscription << " failed."
<< "\nCaught armarx::LocalException:"
- << "\n" << e.getReason()
- << "\n Stacktrace: \n" << e.generateBacktrace()
<< "\n"
- ;
+ << e.getReason() << "\n Stacktrace: \n"
+ << e.generateBacktrace() << "\n";
}
catch (const std::exception& e)
{
error << "Calling callback subscribing " << subscription << " failed."
<< "\nCaught armarx::Exception:"
- << "\n" << e.what()
<< "\n"
- ;
+ << e.what() << "\n";
}
catch (...)
{
error << "Calling callback subscribing " << subscription << " failed."
<< "\nCaught unknown exception."
- << "\n"
- ;
+ << "\n";
}
}
}
}
if (error.str().size() > 0)
{
- ARMARX_WARNING << "The following issues were encountered during MemoryListener::" << __FUNCTION__ << "(): \n\n"
+ ARMARX_WARNING << "The following issues were encountered during MemoryListener::"
+ << __FUNCTION__ << "(): \n\n"
<< error.str();
}
}
-
- void
- MemoryListener::subscribe(const MemoryID& id, Callback callback)
+ SubscriptionHandle
+ MemoryListener::subscribe(const MemoryID& memoryID, Callback callback)
{
- callbacks[id].push_back(callback);
- if (component)
+ ARMARX_CHECK_NOT_EMPTY(memoryID.memoryName)
+ << "The memoryName must be specified to subscribe";
+
+ if (component and memoryRefCount[memoryID.memoryName] == 0)
{
- component->usingTopic(MakeMemoryTopicName(id));
+ component->usingTopic(MakeMemoryTopicName(memoryID));
}
+
+ auto id = nextId++;
+ callbacks[memoryID].push_back({id, callback});
+
+ memoryRefCount[memoryID.memoryName]++;
+
+ return SubscriptionHandle(this, memoryID, id);
}
+ SubscriptionHandle
+ MemoryListener::subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly callback)
+ {
+ return subscribe(
+ subscriptionID,
+ [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ { callback(updatedSnapshotIDs); });
+ }
void
- MemoryListener::subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly callback)
+ MemoryListener::unsubscribe(SubscriptionHandle& handle)
{
- subscribe(subscriptionID, [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ if (not handle.valid)
+ {
+ return;
+ }
+ handle.valid = false;
+
+ // Remove ManagedCallback with ManagedCallback.id == handle.id from callbacks[handle.memoryID]
+ auto it = std::find_if(callbacks[handle.memoryID].begin(),
+ callbacks[handle.memoryID].end(),
+ [&handle](ManagedCallback& mCb) { return mCb.id == handle.id; });
+
+ std::iter_swap(it, callbacks[handle.memoryID].end() - 1);
+ callbacks[handle.memoryID].pop_back();
+
+ memoryRefCount[handle.memoryID.memoryName]--;
+
+ if (callbacks[handle.memoryID].empty())
{
- callback(updatedSnapshotIDs);
- });
+ callbacks.erase(handle.memoryID);
+
+ // unsubscribe from memory topic if no remainig callback needs it
+ if (component and memoryRefCount[handle.memoryID.memoryName] == 0)
+ {
+ component->unsubscribeFromTopic(MakeMemoryTopicName(handle.memoryID));
+ }
+ }
}
-}
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
index 521d9ce6981e9503d90515b0fc10c518eedaae5a..80d8eb3313c44a99abcc87d0048c2a24c2a137e3 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
@@ -1,6 +1,5 @@
#pragma once
-
// STD/STL
#include <functional>
#include <unordered_map>
@@ -10,7 +9,7 @@
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
+#include "SubscriptionHandle.h"
namespace armarx
{
@@ -19,34 +18,36 @@ namespace armarx
namespace armarx::armem::client::util
{
-
/**
* @brief Handles update signals from the memory system and distributes it
* to its subsribers.
*/
class MemoryListener
{
- public:
- using Callback = std::function<void(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)>;
- using CallbackUpdatedOnly = std::function<void(const std::vector<MemoryID>& updatedSnapshotIDs)>;
+ public:
+ using Callback = std::function<void(const MemoryID& subscriptionID,
+ const std::vector<MemoryID>& updatedSnapshotIDs)>;
+ using CallbackUpdatedOnly =
+ std::function<void(const std::vector<MemoryID>& updatedSnapshotIDs)>;
template <class CalleeT>
- using MemberCallback = void(CalleeT::*)(const MemoryID& subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs);
+ using MemberCallback = void (CalleeT::*)(const MemoryID& subscriptionID,
+ const std::vector<MemoryID>& updatedSnapshotIDs);
template <class CalleeT>
- using MemberCallbackUpdatedOnly = void(CalleeT::*)(const std::vector<MemoryID>& updatedSnapshotIDs);
+ using MemberCallbackUpdatedOnly =
+ void (CalleeT::*)(const std::vector<MemoryID>& updatedSnapshotIDs);
static std::string MakeMemoryTopicName(const MemoryID& memoryID);
public:
-
MemoryListener(ManagedIceObject* component = nullptr);
void setComponent(ManagedIceObject* component);
- void subscribe(const MemoryID& subscriptionID, Callback Callback);
- void subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly Callback);
+ SubscriptionHandle subscribe(const MemoryID& subscriptionID, Callback Callback);
+ SubscriptionHandle subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly Callback);
/**
* Subscribe with a class member function:
@@ -55,28 +56,33 @@ namespace armarx::armem::client::util
* @endcode
*/
template <class CalleeT>
- void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallback<CalleeT> callback)
+ SubscriptionHandle
+ subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallback<CalleeT> callback)
{
- auto cb = [callee, callback](const MemoryID & subscriptionID, const std::vector<MemoryID>& updatedSnapshotIDs)
- {
- (callee->*callback)(subscriptionID, updatedSnapshotIDs);
- };
- subscribe(subscriptionID, cb);
+ auto cb = [callee, callback](const MemoryID& subscriptionID,
+ const std::vector<MemoryID>& updatedSnapshotIDs)
+ { (callee->*callback)(subscriptionID, updatedSnapshotIDs); };
+ return subscribe(subscriptionID, cb);
}
template <class CalleeT>
- void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallbackUpdatedOnly<CalleeT> callback)
+ SubscriptionHandle
+ subscribe(const MemoryID& subscriptionID,
+ CalleeT* callee,
+ MemberCallbackUpdatedOnly<CalleeT> callback)
{
- auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
+ auto cb =
+ [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
{
- if(callee)
+ if (callee)
{
(callee->*callback)(updatedSnapshotIDs);
}
};
- subscribe(subscriptionID, cb);
+ return subscribe(subscriptionID, cb);
}
+ void unsubscribe(SubscriptionHandle& subscriptionHandle);
/// Function handling updates from the MemoryListener ice topic.
void updated(const std::vector<MemoryID>& updatedIDs) const;
@@ -84,13 +90,21 @@ namespace armarx::armem::client::util
protected:
+ long nextId = 0;
- std::unordered_map<MemoryID, std::vector<Callback>> callbacks;
+ struct ManagedCallback
+ {
+ long id = 0;
+ Callback callback;
+ };
- private:
+ std::unordered_map<MemoryID, std::vector<ManagedCallback>> callbacks;
- armarx::ManagedIceObject* component;
+ /// memoryName -> #callbacks needing memory topic
+ std::unordered_map<std::string, int> memoryRefCount;
+ private:
+ armarx::ManagedIceObject* component;
};
-}
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
index 856bc217146eb651770fc192dd9ce8a03ebcdf3c..e14ce8848fd1fe8242d681770563bf9cddefd7cd 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -6,8 +6,7 @@
namespace armarx::armem::client::util
{
- SimpleReaderBase::SimpleReaderBase(MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ SimpleReaderBase::SimpleReaderBase()
{
}
@@ -25,14 +24,13 @@ namespace armarx::armem::client::util
}
void
- SimpleReaderBase::connect()
+ SimpleReaderBase::connect(armarx::armem::client::MemoryNameSystem& mns)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << props.memoryName << "' ...";
try
{
- memoryReaderClient =
- memoryNameSystem.useReader(MemoryID().withMemoryName(props.memoryName));
+ memoryReaderClient = mns.useReader(MemoryID().withMemoryName(props.memoryName));
ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName
<< "'";
}
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
index 45b26e7c9e8c4dd124b762d297d2fb0dca29a317..2b637193765215b81519c55397bcc51a7436041b 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -27,7 +27,6 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
-
namespace armarx::armem::client
{
class MemoryNameSystem;
@@ -43,40 +42,36 @@ namespace armarx::armem::client::util
// Properties
struct Properties
{
- std::string memoryName = "";
- std::string coreSegmentName = "";
+ std::string memoryName = "";
+ std::string coreSegmentName = "";
};
- SimpleReaderBase(MemoryNameSystem& memoryNameSystem);
+ SimpleReaderBase();
virtual ~SimpleReaderBase() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- virtual void connect();
+ virtual void connect(armarx::armem::client::MemoryNameSystem& mns);
const Properties& properties() const;
- void setProperties(const Properties& p)
+
+ void
+ setProperties(const Properties& p)
{
- props = p;
+ props = p;
}
protected:
-
- virtual std::string propertyPrefix() const = 0;
+ virtual std::string propertyPrefix() const = 0;
virtual Properties defaultProperties() const = 0;
std::mutex& memoryReaderMutex();
const armem::client::Reader& memoryReader() const;
-
- MemoryNameSystem& memoryNameSystem;
-
private:
-
Properties props;
armem::client::Reader memoryReaderClient;
std::mutex memoryMutex;
-
};
} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
index 9e27df2c2ef5d7af53529502489085e4f43c231d..e5d468c552d96d70d3ef4ae92196cdb714f39163 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
@@ -1,17 +1,14 @@
#include "SimpleWriterBase.h"
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
+#include <RobotAPI/libraries/armem/core/error.h>
namespace armarx::armem::client::util
{
- SimpleWriterBase::SimpleWriterBase(MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ SimpleWriterBase::SimpleWriterBase()
{
}
-
void
SimpleWriterBase::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -26,21 +23,19 @@ namespace armarx::armem::client::util
// TODO(fabian.reister): this might also be part of the subclass
// if the provider name has to be derived from e.g. the component name
- def->optional(props.providerName,
- prefix + "Provider",
- "Name of this provider");
+ def->optional(props.providerName, prefix + "Provider", "Name of this provider");
}
-
- void SimpleWriterBase::connect()
+ void
+ SimpleWriterBase::connect(armarx::armem::client::MemoryNameSystem& mns)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '"
- << props.memoryName << "' ...";
+ ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '" << props.memoryName << "' ...";
try
{
- memoryWriterClient = memoryNameSystem.useWriter(MemoryID().withMemoryName(props.memoryName));
- ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '" << props.memoryName << "'";
+ memoryWriterClient = mns.useWriter(MemoryID().withMemoryName(props.memoryName));
+ ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '" << props.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -49,20 +44,20 @@ namespace armarx::armem::client::util
}
}
-
- std::mutex& SimpleWriterBase::memoryWriterMutex()
+ std::mutex&
+ SimpleWriterBase::memoryWriterMutex()
{
return memoryMutex;
}
-
- armem::client::Writer& SimpleWriterBase::memoryWriter()
+ armem::client::Writer&
+ SimpleWriterBase::memoryWriter()
{
return memoryWriterClient;
}
-
- const SimpleWriterBase::Properties& SimpleWriterBase::properties() const
+ const SimpleWriterBase::Properties&
+ SimpleWriterBase::properties() const
{
return props;
}
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
index 1413299a3081f36dfaa83cce425c50994e7018c2..13a54cc5b213a90e1949f73c1b11de716c0593cc 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
@@ -27,7 +27,6 @@
#include <RobotAPI/libraries/armem/client/Writer.h>
-
namespace armarx::armem::client
{
class MemoryNameSystem;
@@ -39,26 +38,24 @@ namespace armarx::armem::client::util
class SimpleWriterBase
{
public:
-
- SimpleWriterBase(MemoryNameSystem& memoryNameSystem);
+ SimpleWriterBase();
virtual ~SimpleWriterBase() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armarx::armem::client::MemoryNameSystem& mns);
protected:
-
struct Properties
{
- std::string memoryName = "";
+ std::string memoryName = "";
std::string coreSegmentName = "";
- std::string providerName = ""; // required property
+ std::string providerName = ""; // required property
};
const Properties& properties() const;
- virtual std::string propertyPrefix() const = 0;
+ virtual std::string propertyPrefix() const = 0;
virtual Properties defaultProperties() const = 0;
std::mutex& memoryWriterMutex();
@@ -66,14 +63,10 @@ namespace armarx::armem::client::util
private:
-
Properties props;
armem::client::Writer memoryWriterClient;
std::mutex memoryMutex;
-
- MemoryNameSystem& memoryNameSystem;
-
};
} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.cpp b/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..114146c91699ff6a8a7fdf88bef61fdca734b892
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.cpp
@@ -0,0 +1,73 @@
+#include "SubscriptionHandle.h"
+
+#include "MemoryListener.h"
+
+namespace armarx::armem::client::util
+{
+ SubscriptionHandle::SubscriptionHandle(MemoryListener* memoryListener,
+ const MemoryID& memoryID,
+ long id) :
+ valid{true}, memoryListener{memoryListener}, memoryID(memoryID), id{id}
+ {
+ }
+
+ SubscriptionHandle::SubscriptionHandle() : valid{false}
+ {
+ }
+
+ SubscriptionHandle::SubscriptionHandle(SubscriptionHandle&& other) :
+ valid{other.valid},
+ memoryListener{other.memoryListener},
+ memoryID(std::move(other.memoryID)),
+ id{other.id}
+ {
+ other.valid = false;
+ }
+
+ SubscriptionHandle&
+ SubscriptionHandle::operator=(SubscriptionHandle other)
+ {
+ swap(*this, other);
+ return *this;
+ }
+
+ void
+ SubscriptionHandle::release()
+ {
+ memoryListener->unsubscribe(*this);
+ }
+
+ ScopedSubscriptionHandle::ScopedSubscriptionHandle()
+ {
+ }
+
+ ScopedSubscriptionHandle::ScopedSubscriptionHandle(SubscriptionHandle&& handle) :
+ handle(std::move(handle))
+ {
+ }
+
+ ScopedSubscriptionHandle&
+ ScopedSubscriptionHandle::operator=(SubscriptionHandle handle)
+ {
+ std::swap(this->handle, handle);
+ return *this;
+ }
+
+ ScopedSubscriptionHandle::~ScopedSubscriptionHandle()
+ {
+ handle.release();
+ }
+
+} // namespace armarx::armem::client::util
+
+namespace armarx::armem::client
+{
+ void
+ util::swap(util::SubscriptionHandle& first, util::SubscriptionHandle& second)
+ {
+ std::swap(first.valid, second.valid);
+ std::swap(first.memoryListener, second.memoryListener);
+ std::swap(first.memoryID, second.memoryID);
+ std::swap(first.id, second.id);
+ }
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.h b/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.h
new file mode 100644
index 0000000000000000000000000000000000000000..6847ac9a7273cef86d91548b2e66befc8573c015
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SubscriptionHandle.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+namespace armarx::armem::client::util
+{
+
+ class MemoryListener;
+
+ class SubscriptionHandle
+ {
+ friend class MemoryListener;
+
+ public:
+ SubscriptionHandle();
+ SubscriptionHandle(SubscriptionHandle&& other);
+
+ /**
+ * @brief Assignment operator.
+ *
+ * @note Intentional call by value, since this leverages the move constructor. See
+ * https://stackoverflow.com/a/11540204 (section "Move assignment operators").
+ */
+ SubscriptionHandle& operator=(SubscriptionHandle other);
+
+ friend void swap(SubscriptionHandle& first, SubscriptionHandle& second);
+
+ void release();
+
+ private:
+ SubscriptionHandle(MemoryListener* memoryListener, const MemoryID& memoryID, long id);
+
+ private:
+ bool valid = false;
+ MemoryListener* memoryListener = nullptr;
+ MemoryID memoryID;
+ long id = 0;
+ };
+
+ class ScopedSubscriptionHandle
+ {
+ public:
+ ScopedSubscriptionHandle();
+ ScopedSubscriptionHandle(SubscriptionHandle&& handle);
+
+ /**
+ * @brief Assignment operator.
+ *
+ * @note Intentional call by value, since this leverages the move constructor. See
+ * https://stackoverflow.com/a/11540204 (section "Move assignment operators").
+ */
+ ScopedSubscriptionHandle& operator=(SubscriptionHandle handle);
+
+ ~ScopedSubscriptionHandle();
+
+ private:
+ SubscriptionHandle handle;
+ };
+
+ void swap(SubscriptionHandle& first, SubscriptionHandle& second);
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index 354b95712761f6a14d3fb6deb5baeff6b2a18aba..7abe9891d8a1d4f599ae1e135d0dbe8e0d2a29dd 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -246,4 +246,19 @@ namespace armarx::armem::error
return sstream.str();
}
+ ReadStreamAlreadyPolling::ReadStreamAlreadyPolling(const MemoryID& queriedId,
+ const std::string& calledFunction) :
+ ArMemError(makeMsg(queriedId, calledFunction))
+ {
+ }
+
+ std::string
+ ReadStreamAlreadyPolling::makeMsg(const MemoryID& queriedId, const std::string& calledFunction)
+ {
+ std::stringstream ss;
+ ss << "The ReadStream for " << queriedId << " was already running when " << calledFunction
+ << " was called.";
+ return ss.str();
+ }
+
} // namespace armarx::armem::error
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 69ab7d4ddff8e22791272c520386e2fdff3d6e6f..67435f8d3c4142d3453fc6f6626e4e48ebb33cad 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -220,4 +220,15 @@ namespace armarx::armem::error
static std::string makeMsg(const std::string& proxyName, const std::string& message = "");
};
+ /**
+ * @brief Indicates that a ReadStream is already polling when a polling method was called.
+ */
+ class ReadStreamAlreadyPolling : public ArMemError
+ {
+ public:
+ ReadStreamAlreadyPolling(const MemoryID& queriedId, const std::string& calledFunction);
+
+ static std::string makeMsg(const MemoryID& queriedId, const std::string& calledFunction);
+ };
+
} // namespace armarx::armem::error
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
index efe2d01c13731c5c32ba876a3837b2e1816ea41a..6eafef5ed875232aa83ab237f058a3c29db08a60 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
@@ -3,29 +3,25 @@
#include <mutex>
#include <optional>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
namespace armarx::armem::grasping::known_grasps
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {}
-
- void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
@@ -38,8 +34,8 @@ namespace armarx::armem::grasping::known_grasps
"Name of the memory core segment to use for object instances.");
}
-
- void Reader::connect()
+ void
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
@@ -55,7 +51,8 @@ namespace armarx::armem::grasping::known_grasps
}
}
- std::optional<armem::grasping::arondto::KnownGraspInfo> Reader::queryKnownGraspInfo(const armem::wm::Memory& memory, const armem::Time&)
+ std::optional<armem::grasping::arondto::KnownGraspInfo>
+ Reader::queryKnownGraspInfo(const armem::wm::Memory& memory, const armem::Time&)
{
// clang-format off
const armem::wm::CoreSegment& s = memory
@@ -63,8 +60,7 @@ namespace armarx::armem::grasping::known_grasps
// clang-format on
const armem::wm::EntityInstance* instance = nullptr;
- s.forEachInstance([&instance](const wm::EntityInstance& i)
- { instance = &i; });
+ s.forEachInstance([&instance](const wm::EntityInstance& i) { instance = &i; });
if (instance == nullptr)
{
ARMARX_VERBOSE << "No entity snapshots found";
@@ -73,7 +69,9 @@ namespace armarx::armem::grasping::known_grasps
return armem::grasping::arondto::KnownGraspInfo::FromAron(instance->data());
}
- std::optional<armarx::armem::grasping::arondto::KnownGraspInfo> Reader::queryKnownGraspInfoByEntityName(const std::string& entityName, const armem::Time& timestamp)
+ std::optional<armarx::armem::grasping::arondto::KnownGraspInfo>
+ Reader::queryKnownGraspInfoByEntityName(const std::string& entityName,
+ const armem::Time& timestamp)
{
// Query all entities from all provider.
armem::client::query::Builder qb;
@@ -103,7 +101,8 @@ namespace armarx::armem::grasping::known_grasps
auto split = simox::alg::split(entityName, "/");
if (split.size() > 2) // there is more than just dataset/class
{
- ARMARX_INFO << "No grasp found for object entity " << entityName << ". Search for grasp without the index";
+ ARMARX_INFO << "No grasp found for object entity " << entityName
+ << ". Search for grasp without the index";
return queryKnownGraspInfoByEntityName(split[0] + "/" + split[1], timestamp);
}
}
@@ -111,4 +110,4 @@ namespace armarx::armem::grasping::known_grasps
return ret;
}
-} // namespace armarx::armem::attachment
+} // namespace armarx::armem::grasping::known_grasps
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
index e421d4ee185b092ece8f81f3af45116e04c3d723..d0057afc8d23a30da6e5c6335143d58c638d0557 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.h
@@ -28,38 +28,36 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
-
#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
-
namespace armarx::armem::grasping::known_grasps
{
class Reader
{
public:
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader() = default;
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
- std::optional<armem::grasping::arondto::KnownGraspInfo> queryKnownGraspInfoByEntityName(const std::string&, const armem::Time&);
- std::optional<armem::grasping::arondto::KnownGraspInfo> queryKnownGraspInfo(const armem::wm::Memory& memory, const armem::Time&);
+ std::optional<armem::grasping::arondto::KnownGraspInfo>
+ queryKnownGraspInfoByEntityName(const std::string&, const armem::Time&);
+ std::optional<armem::grasping::arondto::KnownGraspInfo>
+ queryKnownGraspInfo(const armem::wm::Memory& memory, const armem::Time&);
private:
-
struct Properties
{
- std::string memoryName = "Grasp";
- std::string coreSegmentName = "KnownGraspCandidate";
+ std::string memoryName = "Grasp";
+ std::string coreSegmentName = "KnownGraspCandidate";
} properties;
const std::string propertyPrefix = "mem.grasping.knowngrasps.";
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
mutable std::mutex memoryWriterMutex;
};
-} // namespace armarx::armem::attachment
+} // namespace armarx::armem::grasping::known_grasps
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
index e7ceabba97898778af1c13d9ad9c805357f79ac2..359ad8e8d978d0bde3e480388e5ed2b1e7f39f30 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
@@ -44,16 +44,14 @@
#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
#include <RobotAPI/libraries/armem_laser_scans/types.h>
-
namespace armarx::armem::laser_scans::client
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ Reader::Reader()
{
}
- Reader::~Reader() = default;
+ Reader::~Reader() = default;
void
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
@@ -64,7 +62,7 @@ namespace armarx::armem::laser_scans::client
}
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << constants::memoryName
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
index 55beab204b12a7f72432ef2e38789f1856058934..5bcfd2c63b9b8bc5682f2b58a2dfbebf7a78a56c 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
@@ -29,10 +29,9 @@
#include "RobotAPI/libraries/armem_laser_scans/types.h"
#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
namespace armarx
{
@@ -62,11 +61,10 @@ namespace armarx::armem::laser_scans::client
class Reader
{
public:
-
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader();
virtual ~Reader();
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
struct Query
{
@@ -99,12 +97,9 @@ namespace armarx::armem::laser_scans::client
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- armarx::armem::client::query::Builder
- buildQuery(const Query& query) const;
+ armarx::armem::client::query::Builder buildQuery(const Query& query) const;
private:
-
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
// Properties
@@ -113,7 +108,6 @@ namespace armarx::armem::laser_scans::client
} properties;
const std::string propertyPrefix = "mem.vision.laser_scans.";
-
};
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
index 8b43cef166b729c2bb7a0bb3d64df74268d1975d..771f49c22a76585026dc735f7f9697e1b28abf7b 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
@@ -6,16 +6,14 @@
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
-
namespace armarx::armem::laser_scans::client
{
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ Writer::Writer()
{
}
- Writer::~Writer() = default;
+ Writer::~Writer() = default;
void
Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
@@ -23,11 +21,10 @@ namespace armarx::armem::laser_scans::client
ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
const std::string prefix = propertyPrefix;
-
}
void
- Writer::connect()
+ Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
diff --git a/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
index 6a75f24b53d13c0ba2d885c588b5c3e944388a57..8b5a90a1867da3d84a225c73e543c694e3d632f3 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
@@ -30,7 +30,6 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-
namespace armarx::armem::laser_scans::client
{
@@ -48,11 +47,11 @@ namespace armarx::armem::laser_scans::client
class Writer
{
public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ Writer();
virtual ~Writer();
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
// MappingDataWriterInterface
/// to be called in Component::onConnectComponent
@@ -67,7 +66,6 @@ namespace armarx::armem::laser_scans::client
const armem::Time& timestamp);
private:
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
// Properties
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
index 0fd0c6a1b5258d7960e9179c649ab0286b26be5a..13cc8ff5a3e515e79e80838a875acad24a6186cd 100644
--- a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
@@ -10,13 +10,6 @@
namespace armarx::armem::locations::client
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem),
- objReader(memoryNameSystem),
- robotReader(memoryNameSystem)
- {
- }
-
void
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -28,7 +21,7 @@ namespace armarx::armem::locations::client
}
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
@@ -45,8 +38,8 @@ namespace armarx::armem::locations::client
return;
}
- objReader.connect();
- robotReader.connect();
+ objReader.connect(memoryNameSystem);
+ robotReader.connect(memoryNameSystem);
}
std::map<std::string, armarx::navigation::location::arondto::Location>
@@ -63,11 +56,11 @@ namespace armarx::armem::locations::client
{
auto i = e.findLatestInstance();
if (i)
- {
- auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
- ret[i->id().providerSegmentName + "/" + i->id().entityName] = loc;
+ {
+ auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
+ ret[i->id().providerSegmentName + "/" + i->id().entityName] = loc;
}
- });
+ });
return ret;
}
@@ -83,7 +76,7 @@ namespace armarx::armem::locations::client
for (auto& [locName, location] : locations)
{
- (void) locName;
+ (void)locName;
if (location.framedPose.header.frame == armarx::GlobalFrame)
{
location.framedPose.header.agent = ""; //sanity set
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.h b/source/RobotAPI/libraries/armem_locations/client/Reader.h
index ad895c269e52fd2e4ec1b6e6bbda037fbefedb04..7a25269d181a9079ec0f770f27fd240d25d7a5af 100644
--- a/source/RobotAPI/libraries/armem_locations/client/Reader.h
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.h
@@ -23,11 +23,11 @@ namespace armarx::armem::locations::client
std::string memoryName = "Navigation";
};
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader() = default;
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
Properties
getProperties()
@@ -44,7 +44,6 @@ namespace armarx::armem::locations::client
const std::string propertyPrefix = "mem.nav.location";
- armem::client::MemoryNameSystem& memoryNameSystem;
armarx::armem::obj::instance::Reader objReader;
armarx::armem::robot_state::RobotReader robotReader;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index ac01fb3b87a1dc4bc53b40982a38002a80cc0d23..4c9ec304ab9195abfd49fb5dcd2cadc18211fc88 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -5,19 +5,20 @@
#include <Eigen/Geometry>
+#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
#include "RobotAPI/libraries/armem/core/Commit.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/constants.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
@@ -30,20 +31,17 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "Reader: Waiting for memory '" << objects::constants::MemoryName << "' ...";
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << objects::constants::MemoryName
+ << "' ...";
try
{
memoryReader = memoryNameSystem.useReader(objects::constants::MemoryName);
- ARMARX_IMPORTANT << "Reader: Connected to memory '" << objects::constants::MemoryName << "'";
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << objects::constants::MemoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -96,7 +94,9 @@ namespace armarx::armem::articulated_object
}
std::optional<ArticulatedObject>
- Reader::get(const std::string& name, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::get(const std::string& name,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
const auto splits = simox::alg::split(name, "/");
ARMARX_CHECK_EQUAL(splits.size(), 3) << "`name` must be of form `DATASET/NAME/INSTANCE`";
@@ -118,7 +118,8 @@ namespace armarx::armem::articulated_object
ArticulatedObject
Reader::get(const ArticulatedObjectDescription& description,
const armem::Time& timestamp,
- const std::string& instanceName, const std::optional<std::string>& providerName)
+ const std::string& instanceName,
+ const std::optional<std::string>& providerName)
{
ArticulatedObject obj{.description = description,
.instance = instanceName,
@@ -131,7 +132,9 @@ namespace armarx::armem::articulated_object
}
bool
- Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::synchronize(ArticulatedObject& obj,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
ARMARX_CHECK_NOT_EMPTY(obj.instance) << "An instance name must be provided!";
@@ -148,7 +151,8 @@ namespace armarx::armem::articulated_object
}
std::vector<robot::RobotDescription>
- Reader::queryDescriptions(const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::queryDescriptions(const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -174,12 +178,14 @@ namespace armarx::armem::articulated_object
}
std::optional<robot::RobotDescription>
- Reader::queryDescription(const std::string& name, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::queryDescription(const std::string& name,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
// Query all entities from provider.
armem::client::query::Builder qb;
- if(providerName.has_value()) // query single provider
+ if (providerName.has_value()) // query single provider
{
// clang-format off
qb
@@ -189,7 +195,7 @@ namespace armarx::armem::articulated_object
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
}
- else // query all providers
+ else // query all providers
{
// clang-format off
qb
@@ -199,7 +205,7 @@ namespace armarx::armem::articulated_object
.snapshots().beforeOrAtTime(timestamp);
// clang-format on
}
-
+
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -215,7 +221,9 @@ namespace armarx::armem::articulated_object
}
std::optional<robot::RobotState>
- Reader::queryState(const std::string& instanceName, const armem::Time& timestamp, const std::optional<std::string>& providerName)
+ Reader::queryState(const std::string& instanceName,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -242,7 +250,6 @@ namespace armarx::armem::articulated_object
return getArticulatedObjectState(qResult.memory);
}
-
std::optional<robot::RobotState>
convertToRobotState(const armem::wm::EntityInstance& instance)
{
@@ -262,7 +269,8 @@ namespace armarx::armem::articulated_object
robot::RobotState robotState{.timestamp = objectPose.timestamp,
.globalPose = Eigen::Affine3f(objectPose.objectPoseGlobal),
- .jointMap = objectPose.objectJointValues, .proprioception=std::nullopt};
+ .jointMap = objectPose.objectJointValues,
+ .proprioception = std::nullopt};
return robotState;
}
@@ -288,7 +296,6 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
-
std::optional<robot::RobotDescription>
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
@@ -308,7 +315,6 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
-
std::vector<robot::RobotDescription>
Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index fc8f1da389f443a90c12aa0c0fce70c24b53c3f1..0eb5bd104f4de49f3b7dfa6cc14ce232a99933fc 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -39,11 +39,15 @@ namespace armarx::armem::articulated_object
class Reader : virtual public ReaderInterface
{
public:
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader() = default;
virtual ~Reader() = default;
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def){}
- void connect();
+ void
+ registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ }
+
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
bool synchronize(ArticulatedObject& obj,
const armem::Time& timestamp,
@@ -88,8 +92,6 @@ namespace armarx::armem::articulated_object
const std::string propertyPrefix = "mem.obj.articulated.";
- armem::client::MemoryNameSystem& memoryNameSystem;
-
armem::client::Reader memoryReader;
std::mutex memoryWriterMutex;
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index 20c3458aa7941cf7caa5a4a0dab9010f2267c035..b8755486996f7092e1687bcefc55f302566d62d2 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -22,15 +22,10 @@
#include "utils.h"
-
namespace armarx::armem::articulated_object
{
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
- void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
@@ -46,10 +41,12 @@ namespace armarx::armem::articulated_object
"Name of the memory core segment to use for object classes.");
ARMARX_IMPORTANT << "Writer: add property '" << prefix << "ProviderName'";
- def->required(properties.providerName, prefix + "write.ProviderName", "Name of this provider");
+ def->required(
+ properties.providerName, prefix + "write.ProviderName", "Name of this provider");
}
- void Writer::connect()
+ void
+ Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
@@ -80,7 +77,8 @@ namespace armarx::armem::articulated_object
memoryNameSystem.subscribe(id, this, &Writer::updateKnownObjects);
}
- void Writer::updateKnownObject(const armem::MemoryID& snapshotId)
+ void
+ Writer::updateKnownObject(const armem::MemoryID& snapshotId)
{
arondto::RobotDescription aronArticulatedObjectDescription;
// aronArticulatedObjectDescription.fromAron(snapshotId.ent);
@@ -88,21 +86,24 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): implement
}
- void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID,
- const std::vector<armem::MemoryID>& snapshotIDs)
+ void
+ Writer::updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
updateKnownObjects();
}
- void Writer::updateKnownObjects()
+ void
+ Writer::updateKnownObjects()
{
knownObjects = queryDescriptions(Time::Now());
ARMARX_INFO << "Known articulated objects " << simox::alg::get_keys(knownObjects);
}
- std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
+ std::optional<armem::MemoryID>
+ Writer::storeOrGetClass(const ArticulatedObject& obj)
{
ARMARX_TRACE;
@@ -125,7 +126,8 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
- std::optional<armem::MemoryID> Writer::storeClass(const ArticulatedObject& obj)
+ std::optional<armem::MemoryID>
+ Writer::storeClass(const ArticulatedObject& obj)
{
std::lock_guard g{memoryWriterMutex};
@@ -154,7 +156,7 @@ namespace armarx::armem::articulated_object
toAron(aronArticulatedObjectDescription, obj.description);
update.instancesData = {aronArticulatedObjectDescription.toAron()};
- update.referencedTime = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -173,17 +175,20 @@ namespace armarx::armem::articulated_object
return updateResult.snapshotID;
}
- std::string Writer::getProviderName() const
+ std::string
+ Writer::getProviderName() const
{
return properties.providerName;
}
- void Writer::setProviderName(const std::string& providerName)
+ void
+ Writer::setProviderName(const std::string& providerName)
{
this->properties.providerName = providerName;
}
- bool Writer::storeInstance(const ArticulatedObject& obj)
+ bool
+ Writer::storeInstance(const ArticulatedObject& obj)
{
std::lock_guard g{memoryWriterMutex};
@@ -192,12 +197,13 @@ namespace armarx::armem::articulated_object
ARMARX_CHECK(not obj.instance.empty()) << "An object instance name must be provided!";
const std::string entityName = obj.description.name + "/" + obj.instance;
- ARMARX_DEBUG << "Storing articulated object instance '" << entityName << "' (provider '" << properties.providerName << "')";
+ ARMARX_DEBUG << "Storing articulated object instance '" << entityName << "' (provider '"
+ << properties.providerName << "')";
const auto providerId = armem::MemoryID()
- .withMemoryName(properties.memoryName)
- .withCoreSegmentName(properties.coreInstanceSegmentName)
- .withProviderSegmentName(properties.providerName);
+ .withMemoryName(properties.memoryName)
+ .withCoreSegmentName(properties.coreInstanceSegmentName)
+ .withProviderSegmentName(properties.providerName);
armem::EntityUpdate update;
update.entityID = providerId.withEntityName(entityName);
@@ -235,7 +241,7 @@ namespace armarx::armem::articulated_object
objectInstance.pose.attachmentValid = false;
update.instancesData = {objectInstance.toAron()};
- update.referencedTime = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -250,7 +256,8 @@ namespace armarx::armem::articulated_object
return updateResult.success;
}
- bool Writer::store(const ArticulatedObject& obj)
+ bool
+ Writer::store(const ArticulatedObject& obj)
{
const std::optional<armem::MemoryID> classId = storeOrGetClass(obj);
@@ -292,22 +299,23 @@ namespace armarx::armem::articulated_object
memory.getCoreSegment(properties.coreClassSegmentName);
std::unordered_map<std::string, armem::MemoryID> descriptions;
- coreSegment.forEachEntity([&descriptions](const wm::Entity & entity)
- {
- if (entity.empty())
+ coreSegment.forEachEntity(
+ [&descriptions](const wm::Entity& entity)
{
- ARMARX_WARNING << "No entity found";
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return true;
+ }
+
+ const armem::wm::EntitySnapshot& sn = entity.getFirstSnapshot();
+ if (const auto robotDescription = convertRobotDescription(sn.getInstance(0)))
+ {
+ const armem::MemoryID snapshotID(sn.id());
+ descriptions.insert({robotDescription->name, snapshotID});
+ }
return true;
- }
-
- const armem::wm::EntitySnapshot& sn = entity.getFirstSnapshot();
- if (const auto robotDescription = convertRobotDescription(sn.getInstance(0)))
- {
- const armem::MemoryID snapshotID(sn.id());
- descriptions.insert({robotDescription->name, snapshotID});
- }
- return true;
- });
+ });
return descriptions;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index 3e827d66a496ddf8df4448788f39ca1c36722290..6245ef374edc289afa452900ee97e769ed4eae8b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -32,19 +32,17 @@
#include "interfaces.h"
-
namespace armarx::armem::articulated_object
{
- class Writer:
- virtual public WriterInterface
+ class Writer : virtual public WriterInterface
{
public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystemopti);
+ Writer() = default;
virtual ~Writer() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
bool store(const ArticulatedObject& obj) override;
@@ -59,33 +57,33 @@ namespace armarx::armem::articulated_object
private:
-
std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
- void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs);
void updateKnownObjects();
void updateKnownObject(const armem::MemoryID& snapshotId);
// TODO duplicate
- std::unordered_map<std::string, armem::MemoryID> queryDescriptions(const armem::Time& timestamp);
- std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
- std::unordered_map<std::string, armem::MemoryID> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
-
+ std::unordered_map<std::string, armem::MemoryID>
+ queryDescriptions(const armem::Time& timestamp);
+ std::optional<robot::RobotDescription>
+ getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::unordered_map<std::string, armem::MemoryID>
+ getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
struct Properties
{
- std::string memoryName = "Object";
+ std::string memoryName = "Object";
std::string coreInstanceSegmentName = "Instance";
- std::string coreClassSegmentName = "Class";
- std::string providerName = "";
+ std::string coreClassSegmentName = "Class";
+ std::string providerName = "";
- bool allowClassCreation = false;
+ bool allowClassCreation = false;
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
- armem::client::MemoryNameSystem& memoryNameSystem;
-
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
@@ -97,4 +95,4 @@ namespace armarx::armem::articulated_object
};
-} // namespace armarx::armem::articulated_object
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index b2b740b735e933245a44bbfd3ce8d1c437ff30b2..399fce56d0cc22c10de24a8b1a4ab29eb6668581 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -21,11 +21,6 @@
namespace armarx::armem::obj::instance
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
void
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -35,7 +30,7 @@ namespace armarx::armem::obj::instance
}
void
- Reader::connect()
+ Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
@@ -48,8 +43,8 @@ namespace armarx::armem::obj::instance
this->objPoseStorage =
objpose::ObjectPoseStorageInterfacePrx::checkedCast(r.readingPrx);
- ARMARX_IMPORTANT << "Connected to Memory and ObjectPoseStorage '"
- << p.memoryName << "'";
+ ARMARX_IMPORTANT << "Connected to Memory and ObjectPoseStorage '" << p.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -93,8 +88,6 @@ namespace armarx::armem::obj::instance
return requestResult.results.at(requestObject).result.success;
}
return false;
-
-
}
std::optional<objpose::ObjectPose>
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 81a0fc03092bbbe67c476d3e5f2146a28d62517a..18f52a6111d96525c0b5940b2e03084b8ff29a96 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -45,11 +45,11 @@ namespace armarx::armem::obj::instance
std::string memoryName = "Object";
};
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ Reader() = default;
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
// localization stuff. Requires an instance index to be set.
std::map<std::string, bool> requestLocalization(const ObjectID& instanceId,
@@ -74,7 +74,8 @@ namespace armarx::armem::obj::instance
return this->p;
}
- objpose::ObjectPoseStorageInterfacePrx getObjectPoseStorage() const
+ objpose::ObjectPoseStorageInterfacePrx
+ getObjectPoseStorage() const
{
return objPoseStorage;
}
@@ -84,7 +85,6 @@ namespace armarx::armem::obj::instance
const std::string propertyPrefix = "mem.obj.instance.";
- armem::client::MemoryNameSystem& memoryNameSystem;
objpose::ObjectPoseStorageInterfacePrx objPoseStorage;
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
index 8ee596b27069f82c9ff19d63efcd40e7cc819699..80d1af5590886994c04a69adb025d2767b8f7ffb 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.cpp
@@ -3,29 +3,29 @@
#include <mutex>
#include <optional>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
namespace armarx::armem::obj::instance
{
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {}
+ Writer::Writer()
+ {
+ }
- void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
@@ -38,8 +38,8 @@ namespace armarx::armem::obj::instance
"Name of the memory core segment to use for object instances.");
}
-
- void Writer::connect()
+ void
+ Writer::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Writer: Waiting for memory '" << properties.memoryName << "' ...";
@@ -55,7 +55,10 @@ namespace armarx::armem::obj::instance
}
}
- bool Writer::commitObject(const armem::arondto::ObjectInstance& inst, const std::string& provider, const armem::Time& t)
+ bool
+ Writer::commitObject(const armem::arondto::ObjectInstance& inst,
+ const std::string& provider,
+ const armem::Time& t)
{
armem::Commit c;
auto& e = c.add();
@@ -63,19 +66,21 @@ namespace armarx::armem::obj::instance
e.entityID.memoryName = properties.memoryName;
e.entityID.coreSegmentName = properties.coreSegmentName;
e.entityID.providerSegmentName = provider;
- e.entityID.entityName = inst.pose.objectID.dataset + "/" + inst.pose.objectID.className + "/" + inst.pose.objectID.instanceName;
+ e.entityID.entityName = inst.pose.objectID.dataset + "/" + inst.pose.objectID.className +
+ "/" + inst.pose.objectID.instanceName;
e.referencedTime = t;
e.sentTime = armem::Time::Now();
- e.instancesData = { inst.toAron() };
+ e.instancesData = {inst.toAron()};
auto res = memoryWriter.commit(c);
- if(!res.allSuccess())
+ if (!res.allSuccess())
{
- ARMARX_ERROR << "Failed to commit an ObjectInstance to memory: " << res.allErrorMessages();
+ ARMARX_ERROR << "Failed to commit an ObjectInstance to memory: "
+ << res.allErrorMessages();
return false;
}
return true;
}
-} // namespace armarx::armem::instance
+} // namespace armarx::armem::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
index 972227993aa5c6369ff656a594fa2e2005b0f9dd..d449502849b09de61e3abc65ab0364928502a083 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectWriter.h
@@ -28,40 +28,37 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem_objects/types.h>
-
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem::obj::instance
{
class Writer
{
public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ Writer();
virtual ~Writer() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
- bool commitObject(const armem::arondto::ObjectInstance& inst, const std::string& provider, const armem::Time&);
+ bool commitObject(const armem::arondto::ObjectInstance& inst,
+ const std::string& provider,
+ const armem::Time&);
private:
-
-
struct Properties
{
- std::string memoryName = "Object";
- std::string coreSegmentName = "Instance";
+ std::string memoryName = "Object";
+ std::string coreSegmentName = "Instance";
} properties;
const std::string propertyPrefix = "mem.obj.instance.";
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
mutable std::mutex memoryWriterMutex;
};
-} // namespace armarx::armem::instance
+} // namespace armarx::armem::obj::instance
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index bfb14cb23d8dd6a169b5b9bd916723af9cbd6a2a..db6a0a9c5898f6332df65199da67c0b4b7b16c1c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -267,6 +267,31 @@
</Dict>
</ObjectChild>
+ <ObjectChild key="extraBools">
+ <Dict>
+ <bool />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="extraShorts">
+ <Dict>
+ <int32 />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="extraInts">
+ <Dict>
+ <int32 />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key="extraBytes">
+ <Dict>
+ <int32 />
+ </Dict>
+ </ObjectChild>
+
+
</Object>
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 5fb5004c926f4997a9b3e252e8a882c92d0462ea..a8280d7e93ee5513449571a8c13d8ee086c9843d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -31,10 +31,11 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
-
-
- RobotReader::RobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem), transformReader(memoryNameSystem)
+ RobotReader::RobotReader(const RobotReader& r) :
+ properties(r.properties),
+ propertyPrefix(r.propertyPrefix),
+ memoryReader(r.memoryReader),
+ transformReader(r.transformReader)
{
}
@@ -45,17 +46,16 @@ namespace armarx::armem::robot_state
}
void
- RobotReader::connect()
+ RobotReader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
- transformReader.connect();
+ transformReader.connect(memoryNameSystem);
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "RobotReader: Waiting for memory '" << constants::memoryName << "' ...";
+ ARMARX_INFO << "RobotReader: Waiting for memory '" << constants::memoryName << "' ...";
try
{
memoryReader = memoryNameSystem.useReader(constants::memoryName);
- ARMARX_IMPORTANT << "RobotReader: Connected to memory '" << constants::memoryName
- << "'";
+ ARMARX_INFO << "RobotReader: Connected to memory '" << constants::memoryName << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -86,7 +86,7 @@ namespace armarx::armem::robot_state
.config = {}, // will be populated by synchronize
.timestamp = timestamp};
- synchronize(robot, timestamp);
+ ARMARX_CHECK(synchronize(robot, timestamp));
return robot;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 6f90a9b249286efb7d1db277ef3df492e0dd8319..dbf81e69a9e4cbc8e488df482634d3542487376c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -32,7 +32,6 @@
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
-
namespace armarx::armem::robot_state
{
/**
@@ -44,14 +43,16 @@ namespace armarx::armem::robot_state
class RobotReader : virtual public robot::ReaderInterface
{
public:
- RobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ RobotReader() = default;
+ RobotReader(const RobotReader&);
virtual ~RobotReader() = default;
- virtual void connect();
+ virtual void connect(armem::client::MemoryNameSystem& memoryNameSystem);
virtual void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) const override;
+ [[nodiscard]] bool synchronize(robot::Robot& obj,
+ const armem::Time& timestamp) const override;
std::optional<robot::Robot> get(const std::string& name,
const armem::Time& timestamp) const override;
@@ -68,7 +69,7 @@ namespace armarx::armem::robot_state
std::optional<armarx::armem::arondto::Proprioception>
queryProprioception(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ const armem::Time& timestamp) const;
using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
@@ -135,7 +136,8 @@ namespace armarx::armem::robot_state
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
std::optional<armarx::armem::arondto::Proprioception>
- getRobotProprioception(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ getRobotProprioception(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
@@ -160,9 +162,8 @@ namespace armarx::armem::robot_state
const std::string propertyPrefix = "mem.robot_state.";
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
- std::mutex memoryWriterMutex;
+ mutable std::mutex memoryWriterMutex;
client::robot_state::localization::TransformReader transformReader;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
index f971c88c3e41bf962500a1ac49e0b5ad7e5a39be..e92e13d6421c71313bdd04a488e1f43397d1f222 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
@@ -34,21 +34,20 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::robot_state
{
- RobotWriter::~RobotWriter() = default;
-
-
- RobotWriter::RobotWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ RobotWriter::RobotWriter(const RobotWriter& r) :
+ properties(r.properties), propertyPrefix(r.propertyPrefix), memoryWriter(r.memoryWriter)
{
}
+ RobotWriter::~RobotWriter() = default;
+
void
RobotWriter::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
}
void
- RobotWriter::connect()
+ RobotWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "RobotWriter: Waiting for memory '" << constants::memoryName << "' ...";
@@ -65,7 +64,6 @@ namespace armarx::armem::robot_state
}
}
-
bool
RobotWriter::storeDescription(const robot::RobotDescription& description,
const armem::Time& timestamp)
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
index f1e20614ee4ee45880e75d98ef223709b85932ae..d8ca351d6cf8643960f1707a49ef26b9697d4082 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h
@@ -32,7 +32,6 @@
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h>
-
namespace armarx::armem::robot_state
{
/**
@@ -44,10 +43,11 @@ namespace armarx::armem::robot_state
class RobotWriter : virtual public robot::WriterInterface
{
public:
- RobotWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ RobotWriter() = default;
+ RobotWriter(const RobotWriter&);
~RobotWriter() override;
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
@@ -72,7 +72,6 @@ namespace armarx::armem::robot_state
const std::string& robotName,
const armem::Time& timestamp);
-
struct Properties
{
@@ -80,9 +79,7 @@ namespace armarx::armem::robot_state
const std::string propertyPrefix = "mem.robot_state.";
- armem::client::MemoryNameSystem& memoryNameSystem;
-
- std::mutex memoryWriterMutex;
+ mutable std::mutex memoryWriterMutex;
armem::client::Writer memoryWriter;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 0e972f79138fbfc973dbab543206d94965227f03..0effa5c23d6763ff984610100b1770024e452bee 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -15,34 +15,28 @@
namespace armarx::armem::robot_state
{
-
- VirtualRobotReader::VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- RobotReader(memoryNameSystem)
- {
- }
-
- void VirtualRobotReader::connect()
- {
- RobotReader::connect();
- }
-
// TODO(fabian.reister): register property defs
- void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ void
+ VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
RobotReader::registerPropertyDefinitions(def);
}
- bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp) const
+ bool
+ VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
+ const armem::Time& timestamp) const
{
// const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
// const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
- const robot::RobotDescription robotDescription{.name = robot.getName(), .xml = PackagePath{"", ""}};
+ const robot::RobotDescription robotDescription{.name = robot.getName(),
+ .xml = PackagePath{"", ""}};
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `" << robot.getName() << "` "
+ ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
+ << robot.getName() << "` "
<< "(type `" << robot.getType() << "`)!";
return false;
}
@@ -53,15 +47,19 @@ namespace armarx::armem::robot_state
return true;
}
- VirtualRobot::RobotPtr VirtualRobotReader::getRobot(const std::string& name, const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
{
- ARMARX_VERBOSE << deactivateSpam(60) << "Querying robot description for robot '" << name << "'";
+ ARMARX_VERBOSE << deactivateSpam(60) << "Querying robot description for robot '" << name
+ << "'";
const auto description = queryDescription(name, timestamp);
if (not description)
{
- ARMARX_VERBOSE << deactivateSpam(5) << "The description of robot `" << name << "` is not a available!";
+ ARMARX_VERBOSE << deactivateSpam(5) << "The description of robot `" << name
+ << "` is not a available!";
return nullptr;
}
@@ -69,8 +67,8 @@ namespace armarx::armem::robot_state
const std::string xmlFilename = description->xml.toSystemPath();
ARMARX_CHECK(std::filesystem::exists(xmlFilename)) << xmlFilename;
- ARMARX_VERBOSE << deactivateSpam(5) << "Loading (virtual) robot '" << description->name << "' from XML file '"
- << xmlFilename << "'";
+ ARMARX_VERBOSE << deactivateSpam(5) << "Loading (virtual) robot '" << description->name
+ << "' from XML file '" << xmlFilename << "'";
auto robot = VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
ARMARX_CHECK_NOT_NULL(robot) << "Could not load robot from file `" << xmlFilename << "`";
@@ -80,24 +78,30 @@ namespace armarx::armem::robot_state
return robot;
}
- VirtualRobot::RobotPtr VirtualRobotReader::getSynchronizedRobot(const std::string& name,
- const VirtualRobot::BaseIO::RobotDescription& loadMode,
- bool blocking)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getSynchronizedRobot(const std::string& name,
+ const VirtualRobot::BaseIO::RobotDescription& loadMode,
+ bool blocking)
{
return _getSynchronizedRobot(name, armem::Time::Invalid(), loadMode, blocking);
}
VirtualRobot::RobotPtr
- VirtualRobotReader::getSynchronizedRobot(const std::string& name, const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode,
- const bool blocking)
+ VirtualRobotReader::getSynchronizedRobot(
+ const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode,
+ const bool blocking)
{
return _getSynchronizedRobot(name, timestamp, loadMode, blocking);
}
- VirtualRobot::RobotPtr VirtualRobotReader::_getSynchronizedRobot(const std::string& name, const Time& timestamp,
- const VirtualRobot::BaseIO::RobotDescription& loadMode,
- bool blocking)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::_getSynchronizedRobot(
+ const std::string& name,
+ const Time& timestamp,
+ const VirtualRobot::BaseIO::RobotDescription& loadMode,
+ bool blocking)
{
while (blocking)
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index 7a2f39c17939d30b26ef75057e1c9d302b0f68a8..1d91f05829b1c49fc5a07782ecbead486e075a5c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -40,10 +40,10 @@ namespace armarx::armem::robot_state
class VirtualRobotReader : virtual public RobotReader
{
public:
- VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ using RobotReader::RobotReader;
+
~VirtualRobotReader() override = default;
- void connect() override;
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
[[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
index fc3aed1eea251e199077d92ccb615249b1777570..e7525729d3f6d59d4bb8c3feaf237f5ff34d18de 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -18,23 +18,15 @@
#include "constants.h"
-
namespace armarx::armem::robot_state
{
- VirtualRobotWriter::VirtualRobotWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
- RobotWriter(memoryNameSystem)
- {
- }
-
-
VirtualRobotWriter::~VirtualRobotWriter() = default;
-
void
- VirtualRobotWriter::connect()
+ VirtualRobotWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
- RobotWriter::connect();
+ RobotWriter::connect(memoryNameSystem);
}
// TODO(fabian.reister): register property defs
@@ -70,12 +62,14 @@ namespace armarx::armem::robot_state
const robot::RobotState robotState{.timestamp = timestamp,
.globalPose =
robot::RobotState::Pose(robot.getGlobalPose()),
- .jointMap = robot.getJointValues(), .proprioception=std::nullopt};
-
- return RobotWriter::storeState(robotState,
- robot.getType(),
- robot.getName(),
- constants::robotRootNodeName /*robot.getRootNode()->getName()*/);
+ .jointMap = robot.getJointValues(),
+ .proprioception = std::nullopt};
+
+ return RobotWriter::storeState(
+ robotState,
+ robot.getType(),
+ robot.getName(),
+ constants::robotRootNodeName /*robot.getRootNode()->getName()*/);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
index 260032c62a1ec4db1a68e78e0660125ba7f37826..29a009714506d069ebd05dcbc988acc64eafe1de 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
@@ -29,7 +29,6 @@
#include "RobotWriter.h"
-
namespace armarx::armem::robot_state
{
/**
@@ -43,10 +42,10 @@ namespace armarx::armem::robot_state
class VirtualRobotWriter : virtual public RobotWriter
{
public:
- VirtualRobotWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ VirtualRobotWriter() = default;
~VirtualRobotWriter() override;
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
[[nodiscard]] bool storeDescription(const VirtualRobot::Robot& robot,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 21b8e55a9fa9c74d63f61dc67aea86bce381c01f..51739021f56c5b87a47b930fa39cd51a8358607d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -38,39 +38,31 @@
#include <SimoxUtility/math/pose/interpolate.h>
// ArmarX
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
// this package
-#include <RobotAPI/libraries/core/FramedPose.h>
-
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-
-#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
-
-#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
-
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::client::robot_state::localization
{
-
- TransformReader::TransformReader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem) {}
-
TransformReader::~TransformReader() = default;
- void TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ TransformReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
@@ -83,15 +75,15 @@ namespace armarx::armem::client::robot_state::localization
def->optional(properties.memoryName, prefix + "Memory");
}
- void TransformReader::connect()
+ void
+ TransformReader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "TransformReader: Waiting for memory '" << properties.memoryName
- << "' ...";
+ ARMARX_INFO << "TransformReader: Waiting for memory '" << properties.memoryName << "' ...";
try
{
memoryReader = memoryNameSystem.useReader(properties.memoryName);
- ARMARX_IMPORTANT << "TransformReader: Connected to memory '" << properties.memoryName;
+ ARMARX_INFO << "TransformReader: Connected to memory '" << properties.memoryName;
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -100,15 +92,15 @@ namespace armarx::armem::client::robot_state::localization
}
}
- TransformResult TransformReader::getGlobalPose(const std::string& agentName,
- const std::string& robotRootFrame,
- const armem::Time& timestamp) const
+ TransformResult
+ TransformReader::getGlobalPose(const std::string& agentName,
+ const std::string& robotRootFrame,
+ const armem::Time& timestamp) const
{
const TransformQuery query{.header = {.parentFrame = GlobalFrame,
- .frame = robotRootFrame,
- .agent = agentName,
- .timestamp = timestamp
- }};
+ .frame = robotRootFrame,
+ .agent = agentName,
+ .timestamp = timestamp}};
return lookupTransform(query);
}
@@ -135,13 +127,14 @@ namespace armarx::armem::client::robot_state::localization
// }
- TransformResult TransformReader::lookupTransform(const TransformQuery& query) const
+ TransformResult
+ TransformReader::lookupTransform(const TransformQuery& query) const
{
const auto& timestamp = query.header.timestamp;
ARMARX_DEBUG << "Looking up transform at timestamp " << timestamp;
const IceUtil::Time durationEpsilon = IceUtil::Time::milliSeconds(-1);
- (void) durationEpsilon;
+ (void)durationEpsilon;
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -159,22 +152,20 @@ namespace armarx::armem::client::robot_state::localization
if (not qResult.success)
{
- return
- {
- .transform =
- {
- .header = query.header,
- },
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
- query.header.frame + "' : " + qResult.errorMessage
- };
+ return {.transform =
+ {
+ .header = query.header,
+ },
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in tf lookup '" + query.header.parentFrame + " -> " +
+ query.header.frame + "' : " + qResult.errorMessage};
}
- const auto& localizationCoreSegment = qResult.memory.getCoreSegment(properties.localizationSegment);
+ const auto& localizationCoreSegment =
+ qResult.memory.getCoreSegment(properties.localizationSegment);
using armarx::armem::common::robot_state::localization::TransformHelper;
return TransformHelper::lookupTransform(localizationCoreSegment, query);
}
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 433694ec819859bdcda209dd57aad21d22512116..4d9ee5983e783e41b7e24cbce982ca65e8ed7057 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -40,15 +40,15 @@ namespace armarx::armem::client::robot_state::localization
*
* Detailed description of class ExampleClient.
*/
- class TransformReader :
- virtual public TransformReaderInterface
+ class TransformReader : virtual public TransformReaderInterface
{
public:
- TransformReader(armem::client::MemoryNameSystem& memoryNameSystem);
+ TransformReader() = default;
+ TransformReader(const TransformReader&) = default;
~TransformReader() override;
- void connect() override;
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem) override;
TransformResult getGlobalPose(const std::string& agentName,
const std::string& robotRootFrame,
@@ -59,18 +59,15 @@ namespace armarx::armem::client::robot_state::localization
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
private:
-
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Reader memoryReader;
// Properties
struct Properties
{
- std::string memoryName = "RobotState";
- std::string localizationSegment = "Localization";
+ std::string memoryName = "RobotState";
+ std::string localizationSegment = "Localization";
} properties;
-
const std::string propertyPrefix = "mem.robot_state.";
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 9f93ae53156e0da1e126ed7cc922905eb2f0796b..f01f8e9241a5ebc605c6b158f3256407a5e9bd17 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -21,45 +21,38 @@
#include "TransformWriter.h"
-#include <optional>
#include <algorithm>
#include <iterator>
#include <numeric>
+#include <optional>
#include <Eigen/Geometry>
#include <IceUtil/Time.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-
-
+#include <RobotAPI/libraries/aron/core/type/variant/Factory.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::client::robot_state::localization
{
-
- TransformWriter::TransformWriter(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem) {}
-
TransformWriter::~TransformWriter() = default;
- void TransformWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ TransformWriter::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
@@ -69,15 +62,17 @@ namespace armarx::armem::client::robot_state::localization
"Name of the localization memory core segment to use.");
}
- void TransformWriter::connect()
+ void
+ TransformWriter::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.coreSegmentID.memoryName
- << "' ...";
+ ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '"
+ << properties.coreSegmentID.memoryName << "' ...";
try
{
memoryWriter = memoryNameSystem.useWriter(properties.coreSegmentID);
- ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '" << properties.coreSegmentID << "'";
+ ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '"
+ << properties.coreSegmentID << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -86,15 +81,17 @@ namespace armarx::armem::client::robot_state::localization
}
}
- bool TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
+ bool
+ TransformWriter::commitTransform(const ::armarx::armem::robot_state::Transform& transform)
{
std::lock_guard g{memoryWriterMutex};
- const MemoryID providerId = properties.coreSegmentID.withProviderSegmentName(transform.header.agent);
+ const MemoryID providerId =
+ properties.coreSegmentID.withProviderSegmentName(transform.header.agent);
// const auto& timestamp = transform.header.timestamp;
- const MemoryID entityID = providerId.withEntityName(
- transform.header.parentFrame + "," + transform.header.frame);
- const Time timestamp = Time::Now(); // FIXME remove
+ const MemoryID entityID =
+ providerId.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
+ const Time timestamp = Time::Now(); // FIXME remove
armem::EntityUpdate update;
update.entityID = entityID;
@@ -117,4 +114,4 @@ namespace armarx::armem::client::robot_state::localization
return updateResult.success;
}
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index e553b073464e5fcf1444e14a4fef48e2034e808c..182981846d14eb775256b3a29aeed9e2a4eb483b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -29,7 +29,6 @@
#include "interfaces.h"
-
namespace armarx::armem::client::robot_state::localization
{
@@ -44,17 +43,15 @@ namespace armarx::armem::client::robot_state::localization
*
* Detailed description of class ExampleClient.
*/
- class TransformWriter :
- virtual public TransformWriterInterface
+ class TransformWriter : virtual public TransformWriterInterface
{
public:
-
- TransformWriter(armem::client::MemoryNameSystem& memoryNameSystem);
+ TransformWriter() = default;
~TransformWriter() override;
// TransformWriterInterface
/// to be called in Component::onConnectComponent
- void connect() override;
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem) override;
/// to be called in Component::addPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) override;
@@ -63,18 +60,16 @@ namespace armarx::armem::client::robot_state::localization
private:
-
- armem::client::MemoryNameSystem& memoryNameSystem;
armem::client::Writer memoryWriter;
std::mutex memoryWriterMutex;
// Properties
struct Properties
{
- MemoryID coreSegmentID { "RobotState", "Localization" };
+ MemoryID coreSegmentID{"RobotState", "Localization"};
} properties;
const std::string propertyPrefix = "mem.robot_state.";
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
index ae35822c6080701195c0e6d7d8b6c8c4ad282684..5689058cc27f39dbe86b0e5e1b3171466407ac50 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
@@ -41,7 +42,7 @@ namespace armarx::armem::client::robot_state::localization
virtual ~TransformInterface() = default;
virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
- virtual void connect() = 0;
+ virtual void connect(armem::client::MemoryNameSystem& memoryNameSystem) = 0;
};
class TransformReaderInterface : virtual public TransformInterface
@@ -65,4 +66,4 @@ namespace armarx::armem::client::robot_state::localization
virtual bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) = 0;
};
-} // namespace armarx::armem::client::robot_state::localization
\ No newline at end of file
+} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
index 4dda540b2596801f1492c4ae8ac532455af002a4..dcbe1b7783190849e6a5d8d442a0124ca22c1b4c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
@@ -1,8 +1,10 @@
#include "Armar6Converter.h"
+#include <cstddef>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/advanced.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
@@ -86,6 +88,18 @@ namespace armarx::armem::server::robot_state::proprioception
case RobotUnitDataStreaming::NodeTypeLong:
dto.extraLongs[key] = getValueAs<long>(value);
break;
+ case RobotUnitDataStreaming::NodeTypeBool:
+ dto.extraBools[key] = getValueAs<bool>(value);
+ break;
+ case RobotUnitDataStreaming::NodeTypeInt:
+ dto.extraInts[key] = getValueAs<int>(value);
+ break;
+ case RobotUnitDataStreaming::NodeTypeShort:
+ dto.extraShorts[key] = getValueAs<Ice::Short>(value);
+ break;
+ case RobotUnitDataStreaming::NodeTypeByte:
+ dto.extraBytes[key] = getValueAs<Ice::Byte>(value);
+ break;
default:
ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
<< RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
diff --git a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
index 85bf523195f80698abf478bd23382f2845344354..9b247cb3973cf28befe37289a3ff42160133f21b 100644
--- a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
+++ b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
@@ -66,11 +66,11 @@ The memory should look like the following:
<Duration />
</ObjectChild>
- <ObjectChild key='acceptedType'>
+ <ObjectChild key='parametersType'>
<AnyObject shared_ptr="1" />
</ObjectChild>
- <ObjectChild key='returnType'>
+ <ObjectChild key='resultType'>
<AnyObject shared_ptr="1" />
</ObjectChild>
@@ -95,12 +95,12 @@ The memory should look like the following:
<String />
</ObjectChild>
- <ObjectChild key='params'>
+ <ObjectChild key='parameters'>
<AnyObject shared_ptr="1" />
</ObjectChild>
</Object>
- <Object name='armarx::skills::arondto::SkillExecutionEvent'>
+ <Object name='armarx::skills::arondto::SkillStatusUpdate'>
<ObjectChild key='skillId'>
<armarx::skills::arondto::SkillID />
@@ -118,11 +118,11 @@ The memory should look like the following:
<String />
</ObjectChild>
- <ObjectChild key='params'>
+ <ObjectChild key='parameters'>
<AnyObject shared_ptr="1" />
</ObjectChild>
- <ObjectChild key='data'>
+ <ObjectChild key='result'>
<AnyObject shared_ptr="1" />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp b/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
index 4bb6f1f5128de5c8afd422114e92530ddba8bce2..3b1d8925fc6694ba7117a4ce3e0db07ecab68586 100644
--- a/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
@@ -133,7 +133,7 @@ namespace armarx::armem
fromAron(const armarx::skills::arondto::SkillID& dto, skills::SkillID& bo)
{
bo.skillName = dto.skillName;
- bo.providerId = skills::ProviderID(skills::SkillID::UNKNOWN);
+ bo.providerId = skills::ProviderID{.providerName = ""};
fromAron(dto.providerId, *bo.providerId);
}
@@ -154,8 +154,8 @@ namespace armarx::armem
fromAron(dto.skillId, bo.skillId);
bo.description = dto.description;
bo.timeout = dto.timeout;
- bo.rootProfileParameterization = dto.rootProfileParameterization;
- if (dto.acceptedType)
+ bo.rootProfileDefaults = dto.rootProfileParameterization;
+ if (dto.parametersType)
{
throw armarx::LocalException("Not implemented yet");
}
@@ -167,13 +167,13 @@ namespace armarx::armem
toAron(dto.skillId, bo.skillId);
dto.description = bo.description;
dto.timeout = bo.timeout;
- dto.rootProfileParameterization = bo.rootProfileParameterization;
- if (bo.acceptedType)
+ dto.rootProfileParameterization = bo.rootProfileDefaults;
+ if (bo.parametersType)
{
aron::type::converter::AronDatatypeConverterVisitor c;
- aron::type::visit(c, bo.acceptedType);
+ aron::type::visit(c, bo.parametersType);
- dto.acceptedType = aron::data::Dict::DynamicCastAndCheck(c.latest);
+ dto.parametersType = aron::data::Dict::DynamicCastAndCheck(c.latest);
}
}
@@ -183,7 +183,7 @@ namespace armarx::armem
{
fromAron(dto.skillId, bo.skillId);
bo.executorName = dto.executorName;
- bo.params = dto.params;
+ bo.parameters = dto.parameters;
}
void
@@ -192,11 +192,11 @@ namespace armarx::armem
{
toAron(dto.skillId, bo.skillId);
dto.executorName = bo.executorName;
- dto.params = bo.params;
+ dto.parameters = bo.parameters;
}
void
- fromAron(const armarx::skills::arondto::SkillExecutionEvent& dto, skills::SkillStatusUpdate& bo)
+ fromAron(const armarx::skills::arondto::SkillStatusUpdate& dto, skills::SkillStatusUpdate& bo)
{
static std::map<std::string, skills::SkillStatus> map{
{"Constructing", skills::SkillStatus::Constructing},
@@ -210,13 +210,13 @@ namespace armarx::armem
fromAron(dto.skillId, bo.executionId.skillId);
bo.executionId.executionStartedTime = dto.executionStartedTimestamp;
bo.executionId.executorName = dto.executorName;
- bo.usedParameterization.parameterization = dto.params;
- bo.data = dto.data;
+ bo.parameters = dto.parameters;
+ bo.result = dto.result;
bo.status = map.at(dto.status);
}
void
- toAron(armarx::skills::arondto::SkillExecutionEvent& dto, const skills::SkillStatusUpdate& bo)
+ toAron(armarx::skills::arondto::SkillStatusUpdate& dto, const skills::SkillStatusUpdate& bo)
{
static std::map<skills::SkillStatus, std::string> map{
{skills::SkillStatus::Constructing, "Constructing"},
@@ -230,8 +230,8 @@ namespace armarx::armem
toAron(dto.skillId, bo.executionId.skillId);
dto.executorName = bo.executionId.executorName;
dto.executionStartedTimestamp = bo.executionId.executionStartedTime;
- dto.params = bo.usedParameterization.parameterization;
- dto.data = bo.data;
+ dto.parameters = bo.parameters;
+ dto.result = bo.result;
dto.status = map.at(bo.status);
}
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_skills/aron_conversions.h b/source/RobotAPI/libraries/armem_skills/aron_conversions.h
index 57b7dd312183e38714b77f695c8dd5318fca6e60..38ffbe0c43ecc492cf76398abdd2f3cad122db7c 100644
--- a/source/RobotAPI/libraries/armem_skills/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_skills/aron_conversions.h
@@ -40,8 +40,8 @@ namespace armarx::armem
void toAron(armarx::skills::arondto::SkillExecutionRequest& dto,
const skills::SkillExecutionRequest& bo);
- void fromAron(const armarx::skills::arondto::SkillExecutionEvent& dto,
+ void fromAron(const armarx::skills::arondto::SkillStatusUpdate& dto,
skills::SkillStatusUpdate& bo);
- void toAron(armarx::skills::arondto::SkillExecutionEvent& dto,
+ void toAron(armarx::skills::arondto::SkillStatusUpdate& dto,
const skills::SkillStatusUpdate& bo);
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
index 6328e6d734c9cb454d301875a399cbc097d20a12..11dc8d0dfb40953de6acc8f81921884808339376 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
@@ -33,7 +33,7 @@ namespace armarx::skills::segment
SkillEventCoreSegment::addSkillUpdateEvent(const skills::SkillStatusUpdate& update)
{
// create commit about new update
- armarx::skills::arondto::SkillExecutionEvent event;
+ armarx::skills::arondto::SkillStatusUpdate event;
armem::toAron(event, update);
armem::MemoryID commitId = id();
@@ -53,40 +53,58 @@ namespace armarx::skills::segment
}
std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
- SkillEventCoreSegment::getLatestSkillEvents(int n)
+ SkillEventCoreSegment::getSkillStatusUpdates()
{
std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> ret;
auto coreSegment = this->segmentPtr;
ARMARX_CHECK(coreSegment);
- // 1. get all events and sort them by timestamp.
- std::vector<skills::SkillStatusUpdate> sorted;
coreSegment->forEachInstance(
- [&sorted](const armem::wm::EntityInstance& i)
+ [&](const armem::wm::EntityInstance& i)
{
- auto event = i.dataAs<armarx::skills::arondto::SkillExecutionEvent>();
- skills::SkillStatusUpdate up({{{""}, ""}, "", armarx::core::time::DateTime::Now()},
- {nullptr, nullptr});
+ auto event = i.dataAs<armarx::skills::arondto::SkillStatusUpdate>();
+ skills::SkillStatusUpdate up;
armem::fromAron(event, up);
- sorted.emplace_back(std::move(up));
+
+ if (auto it = ret.find(up.executionId); it != ret.end() && up < it->second)
+ {
+ return;
+ }
+
+ // set or replace
+ ret[up.executionId] = up;
});
- std::sort(sorted.begin(),
- sorted.end(),
- [](const skills::SkillStatusUpdate& a, const skills::SkillStatusUpdate& b)
- { return b < a; });
+ // for (const auto& [k, v] : ret)
+ // {
+ // ARMARX_IMPORTANT << "Skill " << k.skillId << " has stati: " << int(v.status);
+ // }
- for (const auto& el : sorted)
- {
- if (ret.size() >= (size_t)n)
- {
- break;
- }
+ return ret;
+ }
- ret.emplace(
- std::piecewise_construct, std::make_tuple(el.executionId), std::make_tuple(el));
- }
+ std::optional<skills::SkillStatusUpdate>
+ SkillEventCoreSegment::getSkillStatusUpdate(const skills::SkillExecutionID& id)
+ {
+ std::optional<skills::SkillStatusUpdate> ret = std::nullopt;
+ auto coreSegment = this->segmentPtr;
+ ARMARX_CHECK(coreSegment);
+
+ coreSegment->forEachInstance(
+ [&](const armem::wm::EntityInstance& i)
+ {
+ auto event = i.dataAs<armarx::skills::arondto::SkillStatusUpdate>();
+ skills::SkillStatusUpdate up;
+ armem::fromAron(event, up);
+ if (up.executionId == id)
+ {
+ if (!ret || (ret.has_value() && *ret < up))
+ {
+ ret = up;
+ }
+ }
+ });
return ret;
}
} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h
index 492c26090479a2be9c80442aeddc814378975486..0537cbdbdf97d80649e10baa30786d2d1fb5870c 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h
@@ -27,7 +27,10 @@ namespace armarx::skills::segment
void addSkillUpdateEvent(const skills::SkillStatusUpdate& update);
- std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> getLatestSkillEvents(int n);
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> getSkillStatusUpdates();
+
+ std::optional<skills::SkillStatusUpdate>
+ getSkillStatusUpdate(const skills::SkillExecutionID& id);
private:
};
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
index 4b57e0b44f8b9f2626514c94dd757407506efb57..833deb3f68144a10a71812f9eb39dbec455d9c0c 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
@@ -39,7 +39,9 @@ namespace armarx::skills::segment
skills::arondto::SkillExecutionRequest request;
request.fromAron(commitDataAron);
- skills::SkillExecutionRequest info({{""}, ""}, "", nullptr);
+ skills::SkillExecutionRequest info{
+ .skillId = SkillID{.providerId = ProviderID{.providerName = ""}, .skillName = ""},
+ .executorName = ""};
armem::fromAron(request, info);
return info;
}
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index f8109b4a31f9aa10cfb3caae5b557e4a0c96a391..36cbeb1f837f96ea05b131a843654481b5195bb9 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -28,6 +28,7 @@ armarx_add_library(
aron_conversions/eigen.h
json_conversions/armarx.h
+ json_conversions/framed.h
rw/time.h
rw/eigen.h
@@ -47,6 +48,7 @@ armarx_add_library(
aron_conversions/eigen.cpp
json_conversions/armarx.cpp
+ json_conversions/framed.cpp
rw/time.cpp
rw/eigen.cpp
diff --git a/source/RobotAPI/libraries/aron/common/forward_declarations.h b/source/RobotAPI/libraries/aron/common/forward_declarations.h
index 626147c3fc35e827a7b905c8a8b0e76babf3f25e..d3c7df9f82d251032ed5f1e49180f74f30cf223f 100644
--- a/source/RobotAPI/libraries/aron/common/forward_declarations.h
+++ b/source/RobotAPI/libraries/aron/common/forward_declarations.h
@@ -1,13 +1,14 @@
#pragma once
-
namespace simox::arondto
{
class AxisAlignedBoundingBox;
class Color;
class OrientedBox;
-}
+} // namespace simox::arondto
+
namespace armarx::arondto
{
class Names;
-}
+ class FrameID;
+} // namespace armarx::arondto
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions.h b/source/RobotAPI/libraries/aron/common/json_conversions.h
index a56bd44287eb16161f84e5d1ef0f5496903e2669..b29300e6cf827cfd0908521a3b9fa040b3b32a3d 100644
--- a/source/RobotAPI/libraries/aron/common/json_conversions.h
+++ b/source/RobotAPI/libraries/aron/common/json_conversions.h
@@ -1,3 +1,4 @@
#pragma once
#include "json_conversions/armarx.h"
+#include "json_conversions/framed.h"
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions/framed.cpp b/source/RobotAPI/libraries/aron/common/json_conversions/framed.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa1cf6ae4237e505e00d8ac069824a0004a66e16
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/json_conversions/framed.cpp
@@ -0,0 +1,17 @@
+#include "framed.h"
+
+#include <RobotAPI/libraries/aron/common/aron/framed.aron.generated.h>
+
+void
+armarx::arondto::to_json(nlohmann::json& j, const FrameID& bo)
+{
+ j["agent"] = bo.agent;
+ j["frame"] = bo.frame;
+}
+
+void
+armarx::arondto::from_json(const nlohmann::json& j, FrameID& bo)
+{
+ j.at("agent").get_to(bo.agent);
+ j.at("frame").get_to(bo.frame);
+}
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions/framed.h b/source/RobotAPI/libraries/aron/common/json_conversions/framed.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb7314e83628ab4ddf951766139fedeee731a0e9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/json_conversions/framed.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <SimoxUtility/json/json.h>
+
+#include <RobotAPI/libraries/aron/common/forward_declarations.h>
+
+namespace armarx::arondto
+{
+ void to_json(nlohmann::json& j, const FrameID& bo);
+ void from_json(const nlohmann::json& j, FrameID& bo);
+
+} // namespace armarx::arondto
diff --git a/source/RobotAPI/libraries/skills/core/CMakeLists.txt b/source/RobotAPI/libraries/skills/core/CMakeLists.txt
index 66795b7ecdf0b0aa0897a07f5e47e5f5d489d4a1..29145e60e305fa10decf06200685dcb99ec6a3d5 100644
--- a/source/RobotAPI/libraries/skills/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/core/CMakeLists.txt
@@ -24,6 +24,7 @@ armarx_add_library(
SkillPreparationInput.cpp
SkillParameterization.cpp
Skill.cpp
+ SkillProxy.cpp
SkillDescription.cpp
HEADERS
error/Exception.h
@@ -36,6 +37,7 @@ armarx_add_library(
SkillPreparationInput.h
SkillParameterization.h
Skill.h
+ SkillProxy.h
SkillDescription.h
)
diff --git a/source/RobotAPI/libraries/skills/core/ProviderID.cpp b/source/RobotAPI/libraries/skills/core/ProviderID.cpp
index 83330c897d05a6e47def33f19f1b8bddefcd7b90..53d6aad6807a7002a80a097d94b1e1fb5ec9f864 100644
--- a/source/RobotAPI/libraries/skills/core/ProviderID.cpp
+++ b/source/RobotAPI/libraries/skills/core/ProviderID.cpp
@@ -6,20 +6,10 @@ namespace armarx
{
namespace skills
{
- ProviderID::ProviderID(const std::string& pname) : providerName(pname)
- {
- }
-
- ProviderID::ProviderID(const skills::SkillID& skillid)
- {
- ARMARX_CHECK(skillid.providerId.has_value());
- providerName = skillid.providerId->providerName;
- }
-
bool
ProviderID::operator==(const ProviderID& other) const
{
- return providerName == other.providerName;
+ return toString() == other.toString();
}
bool
@@ -34,16 +24,22 @@ namespace armarx
return toString() < other.toString();
}
+ bool
+ ProviderID::operator<=(const ProviderID& other) const
+ {
+ return toString() <= other.toString();
+ }
+
ProviderID
ProviderID::FromIce(const manager::dto::ProviderID& s)
{
- return ProviderID(s.providerName);
+ return ProviderID{.providerName = s.providerName};
}
ProviderID
ProviderID::FromIce(const callback::dto::ProviderID& s)
{
- return ProviderID(s.providerName);
+ return ProviderID{.providerName = s.providerName};
}
manager::dto::ProviderID
@@ -59,9 +55,9 @@ namespace armarx
}
std::string
- ProviderID::toString(const std::string& prefix) const
+ ProviderID::toString() const
{
- return (prefix.empty() ? std::string("") : (prefix + PREFIX_SEPARATOR)) + providerName;
+ return providerName;
}
} // namespace skills
diff --git a/source/RobotAPI/libraries/skills/core/ProviderID.h b/source/RobotAPI/libraries/skills/core/ProviderID.h
index 0e0159d9db863364bd40f2c4be9c0aca1d7f92a3..5687f649185fa472d0afe1b7a60e6eb2eba46d34 100644
--- a/source/RobotAPI/libraries/skills/core/ProviderID.h
+++ b/source/RobotAPI/libraries/skills/core/ProviderID.h
@@ -15,17 +15,10 @@ namespace armarx
class ProviderID
{
public:
- static const constexpr char* PREFIX_SEPARATOR = "->";
-
- std::string providerName;
-
- ProviderID() = delete;
- ProviderID(const skills::SkillID& skillid);
- ProviderID(const std::string& pName);
-
bool operator==(const ProviderID& other) const;
bool operator!=(const ProviderID& other) const;
bool operator<(const ProviderID& other) const;
+ bool operator<=(const ProviderID& other) const;
manager::dto::ProviderID toManagerIce() const;
callback::dto::ProviderID toCallbackIce() const;
@@ -33,7 +26,9 @@ namespace armarx
static ProviderID FromIce(const manager::dto::ProviderID&);
static ProviderID FromIce(const callback::dto::ProviderID&);
- std::string toString(const std::string& prefix = "") const;
+ std::string toString() const;
+
+ std::string providerName;
};
std::ostream& operator<<(std::ostream& os, const ProviderID& id);
diff --git a/source/RobotAPI/libraries/skills/core/ProviderInfo.cpp b/source/RobotAPI/libraries/skills/core/ProviderInfo.cpp
index 7ae36dbfa185225e13853229790465692501a78f..135b81c594aaf004d65170c81f65a1a6b9529a07 100644
--- a/source/RobotAPI/libraries/skills/core/ProviderInfo.cpp
+++ b/source/RobotAPI/libraries/skills/core/ProviderInfo.cpp
@@ -4,13 +4,6 @@ namespace armarx
{
namespace skills
{
- ProviderInfo::ProviderInfo(const ProviderID& pid,
- const provider::dti::SkillProviderInterfacePrx& i,
- const std::map<SkillID, SkillDescription>& skills) :
- providerId(pid), providerInterface(i), providedSkills(skills)
- {
- }
-
skills::manager::dto::ProviderInfo
ProviderInfo::toIce() const
{
@@ -33,7 +26,7 @@ namespace armarx
{
m.insert({skills::SkillID::FromIce(k), skills::SkillDescription::FromIce(v)});
}
- return ProviderInfo(skills::ProviderID::FromIce(i.providerId), i.providerInterface, m);
+ return ProviderInfo{skills::ProviderID::FromIce(i.providerId), i.providerInterface, m};
}
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/ProviderInfo.h b/source/RobotAPI/libraries/skills/core/ProviderInfo.h
index 92c5484c327f7f5766d406c3ce383e34ee15c246..438a2cf9d894f25f97150dc10ab6c5d9538cdc1e 100644
--- a/source/RobotAPI/libraries/skills/core/ProviderInfo.h
+++ b/source/RobotAPI/libraries/skills/core/ProviderInfo.h
@@ -23,11 +23,6 @@ namespace armarx
provider::dti::SkillProviderInterfacePrx providerInterface;
std::map<SkillID, SkillDescription> providedSkills;
- ProviderInfo() = delete;
- ProviderInfo(const ProviderID& pid,
- const provider::dti::SkillProviderInterfacePrx& i,
- const std::map<SkillID, SkillDescription>& skills);
-
skills::manager::dto::ProviderInfo toIce() const;
static ProviderInfo FromIce(const manager::dto::ProviderInfo&);
diff --git a/source/RobotAPI/libraries/skills/core/Skill.cpp b/source/RobotAPI/libraries/skills/core/Skill.cpp
index 19169d62783970774c350a94aabbb355d580e8b5..7d94de3e90815a156289948fae26463ce5f28247 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/core/Skill.cpp
@@ -4,7 +4,7 @@ namespace armarx
{
namespace skills
{
- Skill::Skill(const SkillDescription& desc) : description(desc)
+ Skill::Skill(const SkillDescription& desc) : description(desc), constructing(true)
{
// replace constructor if you want to have a specific logging tag
Logging::setTag("armarx::skills::" + description.skillId.toString());
@@ -14,17 +14,74 @@ namespace armarx
void
Skill::installConditionWithCallback(std::function<bool()>&& f, std::function<void()>&& cb)
{
- std::lock_guard l(conditionCallbacksMutex);
+ std::scoped_lock l(conditionCallbacksMutex);
conditionCallbacks.push_back({f, cb});
}
+ std::optional<TerminatedSkillStatusUpdate>
+ Skill::callSubskill(const SkillProxy& prx, const aron::data::DictPtr& params)
+ {
+ auto eid = callSubskillAsync(prx, params);
+ auto ret = prx.join(eid);
+ return ret;
+ }
+
+ skills::SkillExecutionID
+ Skill::callSubskillAsync(const skills::SkillProxy& prx, const aron::data::DictPtr& params)
+ {
+ std::unique_lock l(subskillsMutex);
+
+ std::string executorHistory = this->executorName + "->" + getSkillId().toString();
+ auto eid = prx.executeSkillAsync(executorHistory, params);
+ this->subskills.push_back(eid);
+ return eid;
+ }
+
+ void
+ Skill::updateParameters(const aron::data::DictPtr& d)
+ {
+ std::scoped_lock l(this->parametersMutex);
+ if (this->parameters == nullptr)
+ {
+ // set params as there has been no update before.
+ this->parameters = d;
+ }
+ else
+ {
+ // merge params into existing. Note that this may update already set params.
+ this->parameters->mergeAndReplaceCopy(d);
+ }
+ }
+
+ void
+ Skill::setParameters(const aron::data::DictPtr& d)
+ {
+ // we only set the params if the skill is not already running
+ if (running or exiting or finished)
+ {
+ return;
+ }
+
+ std::scoped_lock l(this->parametersMutex);
+ this->parameters = d;
+ }
+
+ aron::data::DictPtr
+ Skill::getParameters() const
+ {
+ return this->parameters;
+ }
+
Skill::InitResult
Skill::_init()
{
//ARMARX_IMPORTANT << "Initializing skill '" << description.skillName << "'";
- conditionCallbacks.clear();
- running = true;
- started = armarx::core::time::DateTime::Now();
+ this->initializing = true;
+ this->constructing = false;
+ this->preparing = false;
+ this->running = false;
+ this->exiting = false;
+ this->finished = false;
// install timeout condition
installConditionWithCallback(
@@ -37,7 +94,8 @@ namespace armarx
[&]()
{
armarx::core::time::Metronome metronome(conditionCheckingThreadFrequency);
- while (running) // when the skill ends/aborts this variable will be set to false
+ while (initializing or preparing or
+ running) // when the skill ends/aborts this variable will be set to false
{
{
std::scoped_lock l(conditionCallbacksMutex);
@@ -51,13 +109,15 @@ namespace armarx
}
}
}
+
const auto sleepDuration = metronome.waitForNextTick();
if (not sleepDuration.isPositive())
{
- ARMARX_WARNING
- << deactivateSpam() << "PeriodicSkill: execution took too long ("
- << -sleepDuration << " vs "
- << conditionCheckingThreadFrequency.toCycleDuration() << ")";
+ ARMARX_WARNING << deactivateSpam()
+ << "ConditionCheckingThread: execution took too long ("
+ << -sleepDuration << " vs "
+ << conditionCheckingThreadFrequency.toCycleDuration()
+ << ")";
}
}
});
@@ -67,18 +127,24 @@ namespace armarx
Skill::PrepareResult
Skill::_prepare()
{
- if (checkWhetherSkillShouldStopASAP())
+ this->preparing = true;
+ this->initializing = false;
+ this->constructing = false;
+ this->running = false;
+ this->exiting = false;
+ this->finished = false;
+
+ if (shouldSkillTerminate())
{
return {.status = ActiveOrTerminatedSkillStatus::Aborted};
}
// Default nothing to prepare
- std::scoped_lock l(parametersMutex);
- if (not description.acceptedType)
+ if (not description.parametersType)
{
return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
}
- if (this->parameters && this->parameters->fullfillsType(description.acceptedType))
+ if (this->parameters && this->parameters->fullfillsType(description.parametersType))
{
// wait until parameters fulfill type
return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
@@ -91,7 +157,12 @@ namespace armarx
Skill::MainResult
Skill::_main()
{
- // Nothing here yet...
+ this->running = true;
+ this->initializing = false;
+ this->constructing = false;
+ this->preparing = false;
+ this->exiting = false;
+ this->finished = false;
return {.status = TerminatedSkillStatus::Succeeded};
}
@@ -99,78 +170,174 @@ namespace armarx
Skill::_exit()
{
// ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
- running = false; // stop checking conditions
+ this->exiting = true;
+ this->running = false;
+ this->initializing = false;
+ this->constructing = false;
+ this->preparing = false;
+ this->finished = false;
if (conditionCheckingThread.joinable())
{
conditionCheckingThread.join();
}
exited = armarx::core::time::DateTime::Now();
+
+ this->finished = true;
+ this->exiting = false;
return {.status = TerminatedSkillStatus::Succeeded};
}
Skill::InitResult
Skill::initSkill()
{
+ std::scoped_lock l(parametersMutex);
auto _res = this->_init();
auto res = this->init();
- return {.status = skills::MergeSkillStatus(_res.status, res.status)};
+ return {.status = skills::mergeSkillStatuseses(_res.status, res.status)};
}
Skill::PrepareResult
Skill::prepareSkill()
{
+ std::scoped_lock l(parametersMutex);
auto _res = this->_prepare();
auto res = this->prepare();
- return {.status = skills::MergeSkillStatus(_res.status, res.status)};
+ return {.status = skills::mergeSkillStatuseses(_res.status, res.status)};
}
Skill::MainResult
Skill::mainOfSkill()
{
+ std::scoped_lock l(parametersMutex);
auto _res = this->_main();
auto res = this->main();
- return {.status = skills::MergeSkillStatus(_res.status, res.status), .data = res.data};
+ return {.status = skills::mergeSkillStatuseses(_res.status, res.status),
+ .data = res.data};
}
Skill::ExitResult
Skill::exitSkill()
{
+ std::scoped_lock l(parametersMutex);
auto res = this->exit();
auto _res = this->_exit();
- return {.status = skills::MergeSkillStatus(_res.status, res.status)};
+ return {.status = skills::mergeSkillStatuseses(_res.status, res.status)};
}
void
- Skill::notifyTimeoutReached()
+ Skill::throwIfSkillShouldTerminate(const std::function<void()>& do_before,
+ const std::string& abortedMessage)
{
- timeoutReached = true;
- onTimeoutReached();
+ if (shouldSkillTerminate())
+ {
+ do_before();
+ throwIfSkillShouldTerminate(abortedMessage);
+ }
+ }
+
+ void
+ Skill::throwIfSkillShouldTerminate(const std::string& abortedMessage)
+ {
+ if (stopped)
+ {
+ std::string message =
+ std::string("The skill '" + getSkillId().toString() + "' was asked to stop.");
+ message += abortedMessage.empty() ? "" : " Additional message: " + abortedMessage;
+
+ throw error::SkillAbortedException(message);
+ return;
+ }
+
+ if (timeoutReached)
+ {
+ std::string message =
+ std::string("The skill '" + getSkillId().toString() + "' reached timeout.");
+ message += abortedMessage.empty() ? "" : " Additional message: " + abortedMessage;
+
+ ARMARX_WARNING << message;
+ throw error::SkillFailedException(message);
+ }
}
Skill::MainResult
- Skill::MakeAbortedResult(aron::data::DictPtr data)
+ Skill::MakeSucceededResult(aron::data::DictPtr data)
{
return MainResult{
- .status = TerminatedSkillStatus::Aborted,
+ .status = TerminatedSkillStatus::Succeeded,
.data = data,
};
}
+ Skill::MainResult
+ Skill::MakeFailedResult()
+ {
+ return MainResult{
+ .status = TerminatedSkillStatus::Failed,
+ .data = nullptr,
+ };
+ }
+
+ Skill::MainResult
+ Skill::MakeAbortedResult()
+ {
+ return MainResult{
+ .status = TerminatedSkillStatus::Aborted,
+ .data = nullptr,
+ };
+ }
+
void
- Skill::notifySkillToStopASAP()
+ Skill::notifySkillToStop()
{
+ std::scoped_lock l(subskillsMutex);
stopped = true;
+ _onStopRequested();
onStopRequested();
}
+ void
+ Skill::notifyTimeoutReached()
+ {
+ std::scoped_lock l(subskillsMutex);
+ timeoutReached = true;
+ _onTimeoutReached();
+ onTimeoutReached();
+ }
+
bool
- Skill::checkWhetherSkillShouldStopASAP() const
+ Skill::shouldSkillTerminate() const
{
return stopped || timeoutReached;
}
// condition effects
+ void
+ Skill::_onTimeoutReached()
+ {
+ if (!manager)
+ {
+ return;
+ }
+ for (const auto& execId : subskills)
+ {
+ manager->abortSkillAsync(execId.toManagerIce());
+ }
+ }
+
+ void
+ Skill::_onStopRequested()
+ {
+ if (!manager)
+ {
+ return;
+ }
+ for (const auto& execId : subskills)
+ {
+ manager->abortSkillAsync(execId.toManagerIce());
+ }
+ }
+
void
Skill::onTimeoutReached()
{
diff --git a/source/RobotAPI/libraries/skills/core/Skill.h b/source/RobotAPI/libraries/skills/core/Skill.h
index b147e455f07039d430ab3113e8fec18298c09458..5ec80e77d9956e8516ae84ffec16313bab0c9206 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.h
+++ b/source/RobotAPI/libraries/skills/core/Skill.h
@@ -19,6 +19,7 @@
#include "SkillDescription.h"
#include "SkillID.h"
#include "SkillPreparationInput.h"
+#include "SkillProxy.h"
#include "SkillStatusUpdate.h"
#include "error/Exception.h"
@@ -32,108 +33,136 @@ namespace armarx
using CallbackT =
std::function<void(const SkillStatus s, const armarx::aron::data::DictPtr&)>;
+ /// A result struct for skill initialization
struct InitResult
{
TerminatedSkillStatus status;
};
+ /// A result struct for skill preparing
struct PrepareResult
{
ActiveOrTerminatedSkillStatus status;
};
+ /// A result struct for th main method of a skill
struct MainResult
{
TerminatedSkillStatus status;
aron::data::DictPtr data = nullptr;
};
+ /// A result struct for skill exit function
struct ExitResult
{
TerminatedSkillStatus status;
};
+ /// We completely remove the default constructor! A skill without a desciption cannot exist
Skill() = delete;
+
+ /// Constructor of a skill for inheritance. Every skill must have a skill description
Skill(const SkillDescription&);
- virtual ~Skill() = default;
- /// The id of the skill (combination of provider and name must be unique).
+ /// Virtual destructor of a skill
+ virtual ~Skill()
+ {
+ //ARMARX_IMPORTANT << "DESTROY SKILL " << getSkillId();
+ }
+
+ /// Get the id of the skill
SkillID
getSkillId() const
{
return description.skillId;
}
+ /// Get the description of a skill
SkillDescription
getSkillDescription() const
{
return description;
}
+ /// Set the provider id of the description of the skill.
+ /// This method is called when creating a skill in a skill provider
void
setProviderId(const skills::ProviderID& pid)
{
description.skillId.providerId = pid;
}
+ void
+ setCallback(const CallbackT& callback)
+ {
+ this->callback = callback;
+ }
+
+ void
+ setManager(const manager::dti::SkillManagerInterfacePrx& manager)
+ {
+ this->manager = manager;
+ }
+
+ void
+ setExecutorName(const std::string& executorName)
+ {
+ this->executorName = executorName;
+ }
+
+ /// Prepare a skill once. This method is called in a loop as long as it returns RUNNING
+ /// If the loop does not terminate with SUCCEDED the skill execution is failed.
PrepareResult prepareSkill();
+ /// Initialization of a skill. Called directly after construction.
+ /// If this method does not return SUCCEEDED the skill execution is failed.
InitResult initSkill();
+ /// Main method of a skill.
MainResult mainOfSkill();
+ /// Exit method of a skill. It is guaranteed that exit is always called
+ /// (unless there is a segfault or similar)
ExitResult exitSkill();
- // Condition listeners
- // used to notify the skill from extern to stop
- void notifySkillToStopASAP();
+ /// Notify the skill from extern to stop
+ void notifySkillToStop();
- // returns whether the skill should terminate as soon as possible
- bool checkWhetherSkillShouldStopASAP() const;
+ /// Returns whether the skill should terminate as soon as possible
+ bool shouldSkillTerminate() const;
- // merge parameters to the local parameters of the skill
- void
- addParameters(const aron::data::DictPtr& d)
- {
- std::scoped_lock l(this->parametersMutex);
- if (this->parameters == nullptr)
- {
- this->parameters = d;
- }
- else
- {
- this->parameters->mergeAndReplaceCopy(d);
- }
- }
+ /// Merge parameters to the local parameters of the skill
+ void updateParameters(const aron::data::DictPtr& d);
- // hard set the parameters, ignoring everything that has been set or merged before
- void
- setParameters(const aron::data::DictPtr& d)
- {
- std::scoped_lock l(this->parametersMutex);
- this->parameters = d;
- }
+ /// Hard set the parameters, ignoring everything that has been set or merged before
+ void setParameters(const aron::data::DictPtr& d);
- // get the parameters
- aron::data::DictPtr
- getParameters() const
- {
- std::scoped_lock l(this->parametersMutex);
- return this->parameters;
- }
+ /// Get the parameters of a skill that have been set so far
+ aron::data::DictPtr getParameters() const;
protected:
- static MainResult MakeAbortedResult(aron::data::DictPtr data = nullptr);
+ void throwIfSkillShouldTerminate(const std::string& abortedMessage = "");
+ void throwIfSkillShouldTerminate(const std::function<void()>& do_before,
+ const std::string& abortedMessage = "");
+
+ static MainResult MakeSucceededResult(aron::data::DictPtr data = nullptr);
+ static MainResult MakeFailedResult();
+ static MainResult MakeAbortedResult();
// fires if the skill reaches timeout
void notifyTimeoutReached();
+ private:
// helper methods to do all the static initialization stuff
InitResult _init();
PrepareResult _prepare();
MainResult _main();
ExitResult _exit();
+ void _onTimeoutReached();
+ void _onStopRequested();
+
+ protected:
/// Override this method with the actual implementation.
virtual InitResult init();
@@ -146,7 +175,7 @@ namespace armarx
/// Override this method with the actual implementation.
virtual ExitResult exit();
- private:
+ protected:
/// Override these methods if you want to do something special when notification comes
virtual void onTimeoutReached();
virtual void onStopRequested();
@@ -156,42 +185,59 @@ namespace armarx
void installConditionWithCallback(std::function<bool()>&& f,
std::function<void()>&& cb);
+ /// call a subskill and block until the subskill terminates.
+ /// If you call a subskill this way it will be stopped if the current skill stops.
+ std::optional<TerminatedSkillStatusUpdate>
+ callSubskill(const skills::SkillProxy& prx, const aron::data::DictPtr& = nullptr);
+
+ /// Similar to callSubskill but non-blocking
+ skills::SkillExecutionID callSubskillAsync(const skills::SkillProxy& prx,
+ const aron::data::DictPtr& = nullptr);
+
public:
// running params
- armarx::core::time::DateTime started = armarx::core::time::DateTime::Invalid();
+ armarx::core::time::DateTime started = armarx::core::time::DateTime::Now();
armarx::core::time::DateTime exited = armarx::core::time::DateTime::Invalid();
- // parameterization. Will be set from implementation wrapper.
- // const params after initialization!!
- CallbackT callback;
+ protected:
+ // parameterization. Will be set from implementation wrapper. No getters available
+ // const after construction!!
+ CallbackT callback = CallbackT();
manager::dti::SkillManagerInterfacePrx manager = nullptr;
- std::string executorName = "INVALID EXECUTOR NAME";
+ std::string executorName = "";
protected:
- // non-const params
+ // non-const params. Const after preparation. Getters are available
mutable std::mutex parametersMutex;
armarx::aron::data::DictPtr parameters = nullptr;
// The descripion of the skill
SkillDescription description;
- // active conditions. First is condition (bool return func)
- mutable std::mutex conditionCallbacksMutex;
- std::vector<std::pair<std::function<bool()>, std::function<void()>>>
- conditionCallbacks = {};
-
-
- /// Use conditions this way
+ // Status variables
+ std::atomic_bool constructing = true;
std::atomic_bool initializing = false;
std::atomic_bool preparing = false;
std::atomic_bool running = false;
+ std::atomic_bool exiting = false;
+ std::atomic_bool finished = false;
+
+ // Conditionals to indicate that an event has occured. Use conditions this way
std::atomic_bool stopped = false;
std::atomic_bool timeoutReached = false;
private:
- std::thread conditionCheckingThread; // A therad that checks the conditions frequently
+ // active conditions. First is condition (bool return func)
+ mutable std::mutex conditionCallbacksMutex;
+ std::vector<std::pair<std::function<bool()>, std::function<void()>>>
+ conditionCallbacks = {};
+
+ std::thread conditionCheckingThread; // A thread that checks the conditions frequently
armarx::Frequency conditionCheckingThreadFrequency = armarx::Frequency::Hertz(20);
+
+ mutable std::mutex subskillsMutex;
+ std::vector<skills::SkillExecutionID> subskills;
};
template <class T>
diff --git a/source/RobotAPI/libraries/skills/core/SkillDescription.cpp b/source/RobotAPI/libraries/skills/core/SkillDescription.cpp
index 6b51a08843a699d765124b2f59468227707c2778..2fa37b33ceab97b944e55644c99c530ba3f371c3 100644
--- a/source/RobotAPI/libraries/skills/core/SkillDescription.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillDescription.cpp
@@ -7,31 +7,15 @@ namespace armarx
{
namespace skills
{
-
- SkillDescription::SkillDescription(const SkillID& id,
- const std::string& desc,
- const aron::data::DictPtr& data,
- const armarx::core::time::Duration& timeout,
- const aron::type::ObjectPtr& acceptedType,
- const aron::type::ObjectPtr& returnType) :
- skillId(id),
- description(desc),
- rootProfileParameterization(data),
- timeout(timeout),
- acceptedType(acceptedType),
- returnType(returnType)
- {
- }
-
provider::dto::SkillDescription
SkillDescription::toProviderIce() const
{
provider::dto::SkillDescription ret;
- ret.acceptedType = aron::type::Object::ToAronObjectDTO(acceptedType);
- ret.returnType = aron::type::Object::ToAronObjectDTO(returnType);
+ ret.parametersType = aron::type::Object::ToAronObjectDTO(parametersType);
+ ret.resultType = aron::type::Object::ToAronObjectDTO(resultType);
ret.description = description;
ret.skillId = skillId.toProviderIce();
- ret.rootProfileDefaults = aron::data::Dict::ToAronDictDTO(rootProfileParameterization);
+ ret.rootProfileDefaults = aron::data::Dict::ToAronDictDTO(rootProfileDefaults);
armarx::core::time::toIce(ret.timeout, timeout);
return ret;
@@ -41,11 +25,11 @@ namespace armarx
SkillDescription::toManagerIce() const
{
manager::dto::SkillDescription ret;
- ret.acceptedType = aron::type::Object::ToAronObjectDTO(acceptedType);
- ret.returnType = aron::type::Object::ToAronObjectDTO(returnType);
+ ret.parametersType = aron::type::Object::ToAronObjectDTO(parametersType);
+ ret.resultType = aron::type::Object::ToAronObjectDTO(resultType);
ret.description = description;
ret.skillId = skillId.toManagerIce();
- ret.rootProfileDefaults = aron::data::Dict::ToAronDictDTO(rootProfileParameterization);
+ ret.rootProfileDefaults = aron::data::Dict::ToAronDictDTO(rootProfileDefaults);
armarx::core::time::toIce(ret.timeout, timeout);
return ret;
@@ -57,13 +41,14 @@ namespace armarx
{
armarx::core::time::Duration _d;
armarx::core::time::fromIce(i.timeout, _d);
- return SkillDescription(
- SkillID::FromIce(i.skillId, pid),
- i.description,
- armarx::aron::data::Dict::FromAronDictDTO(i.rootProfileDefaults),
- _d,
- armarx::aron::type::Object::FromAronObjectDTO(i.acceptedType),
- armarx::aron::type::Object::FromAronObjectDTO(i.returnType));
+ return SkillDescription{
+ .skillId = SkillID::FromIce(i.skillId, pid),
+ .description = i.description,
+ .rootProfileDefaults =
+ armarx::aron::data::Dict::FromAronDictDTO(i.rootProfileDefaults),
+ .timeout = _d,
+ .parametersType = armarx::aron::type::Object::FromAronObjectDTO(i.parametersType),
+ .resultType = armarx::aron::type::Object::FromAronObjectDTO(i.resultType)};
}
SkillDescription
@@ -71,13 +56,14 @@ namespace armarx
{
armarx::core::time::Duration _d;
armarx::core::time::fromIce(i.timeout, _d);
- return SkillDescription(
- SkillID::FromIce(i.skillId),
- i.description,
- armarx::aron::data::Dict::FromAronDictDTO(i.rootProfileDefaults),
- _d,
- armarx::aron::type::Object::FromAronObjectDTO(i.acceptedType),
- armarx::aron::type::Object::FromAronObjectDTO(i.returnType));
+ return SkillDescription{
+ .skillId = SkillID::FromIce(i.skillId),
+ .description = i.description,
+ .rootProfileDefaults =
+ armarx::aron::data::Dict::FromAronDictDTO(i.rootProfileDefaults),
+ .timeout = _d,
+ .parametersType = armarx::aron::type::Object::FromAronObjectDTO(i.parametersType),
+ .resultType = armarx::aron::type::Object::FromAronObjectDTO(i.resultType)};
}
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillDescription.h b/source/RobotAPI/libraries/skills/core/SkillDescription.h
index 429af240e75b267f9f9a0d103d1d9a9c5836be4a..4bc8a3cd8fe92f7ebdf13c32b1e812be8596fbbd 100644
--- a/source/RobotAPI/libraries/skills/core/SkillDescription.h
+++ b/source/RobotAPI/libraries/skills/core/SkillDescription.h
@@ -17,24 +17,12 @@ namespace armarx
{
struct SkillDescription
{
- SkillDescription() = delete;
- SkillDescription(const SkillDescription&) = default;
- SkillDescription(const SkillID& id,
- const std::string& desc,
- const aron::data::DictPtr& data = nullptr,
- const armarx::core::time::Duration& timeout =
- armarx::core::time::Duration::MilliSeconds(-1),
- const aron::type::ObjectPtr& acceptedType = nullptr,
- const aron::type::ObjectPtr& returnType = nullptr);
-
- SkillDescription& operator=(const SkillDescription&) = default;
-
SkillID skillId;
- std::string description;
- aron::data::DictPtr rootProfileParameterization;
- armarx::core::time::Duration timeout;
- aron::type::ObjectPtr acceptedType;
- aron::type::ObjectPtr returnType;
+ std::string description = "";
+ aron::data::DictPtr rootProfileDefaults = nullptr;
+ armarx::core::time::Duration timeout = armarx::core::time::Duration::MilliSeconds(-1);
+ aron::type::ObjectPtr parametersType = nullptr;
+ aron::type::ObjectPtr resultType = nullptr;
provider::dto::SkillDescription toProviderIce() const;
manager::dto::SkillDescription toManagerIce() const;
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp b/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
index 3c52c6be7ba8f52f8a6b023e65c23f897225910e..bb2494ac219fb656fb8383d151dba7b7cfe187cc 100644
--- a/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
@@ -4,13 +4,6 @@ namespace armarx
{
namespace skills
{
- SkillExecutionID::SkillExecutionID(const SkillID& id,
- const std::string& executorName,
- const armarx::core::time::DateTime& time) :
- skillId(id), executorName(executorName), executionStartedTime(time)
- {
- }
-
skills::manager::dto::SkillExecutionID
SkillExecutionID::toManagerIce() const
{
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionID.h b/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
index bd1bfb326f466f499174182f87540fd7276ca016..474eef2f6df424780006dbd3bb51348136de63ef 100644
--- a/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
@@ -35,26 +35,26 @@ namespace armarx
bool
operator==(const SkillExecutionID& other) const
{
- if (skillId != other.skillId)
- {
- return false;
- }
- if (executionStartedTime != other.executionStartedTime)
- {
- return false;
- }
- if (executorName != other.executorName)
- {
- return false;
- }
- return true;
+ return this->toString() == other.toString();
}
bool
operator<(const SkillExecutionID& other) const
{
- // We explicitly do not compare skillIds as we ONLY want to bring the executionids in some (temporal) order
- return executionStartedTime < other.executionStartedTime;
+ return this->toString() < other.toString();
+ }
+
+ bool
+ operator<=(const SkillExecutionID& other) const
+ {
+ return this->toString() <= other.toString();
+ }
+
+ std::string
+ toString() const
+ {
+ return skillId.toString() + " requested by " + executorName + " at " +
+ executionStartedTime.toDateTimeString();
}
skills::manager::dto::SkillExecutionID toManagerIce() const;
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.cpp b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.cpp
index 91fca69cae13c465c68a6bf2a7553f42b1b39304..2d6a0181bbbb8990f44ada113c55a69a532ee99f 100644
--- a/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.cpp
@@ -4,22 +4,13 @@ namespace armarx
{
namespace skills
{
- SkillExecutionRequest::SkillExecutionRequest(
- const skills::SkillID& skill,
- const std::string executorName,
- const armarx::aron::data::DictPtr& params,
- const callback::dti::SkillProviderCallbackInterfacePrx& callback) :
- skillId(skill), executorName(executorName), params(params), callbackInterface(callback)
- {
- }
-
manager::dto::SkillExecutionRequest
SkillExecutionRequest::toManagerIce() const
{
manager::dto::SkillExecutionRequest ret;
ret.skillId = skillId.toManagerIce();
ret.executorName = executorName;
- ret.params = armarx::aron::data::Dict::ToAronDictDTO(params);
+ ret.parameters = armarx::aron::data::Dict::ToAronDictDTO(parameters);
return ret;
}
@@ -29,7 +20,7 @@ namespace armarx
provider::dto::SkillExecutionRequest ret;
ret.skillId = skillId.toProviderIce();
ret.executorName = executorName;
- ret.params = armarx::aron::data::Dict::ToAronDictDTO(params);
+ ret.parameters = armarx::aron::data::Dict::ToAronDictDTO(parameters);
ret.callbackInterface = callbackInterface;
return ret;
}
@@ -39,7 +30,7 @@ namespace armarx
{
return {skills::SkillID::FromIce(req.skillId),
req.executorName,
- armarx::aron::data::Dict::FromAronDictDTO(req.params),
+ armarx::aron::data::Dict::FromAronDictDTO(req.parameters),
nullptr};
}
@@ -49,7 +40,7 @@ namespace armarx
{
return {skills::SkillID::FromIce(req.skillId, providerId),
req.executorName,
- armarx::aron::data::Dict::FromAronDictDTO(req.params),
+ armarx::aron::data::Dict::FromAronDictDTO(req.parameters),
req.callbackInterface};
}
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h
index 9eaa00791ddfb4cf3f100e793f713fa828d314e5..d1f5bbcdd175f9f3516d45856186d453af2f987e 100644
--- a/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h
@@ -21,18 +21,6 @@ namespace armarx
class SkillExecutionRequest
{
public:
- skills::SkillID skillId;
- std::string executorName;
- armarx::aron::data::DictPtr params;
- callback::dti::SkillProviderCallbackInterfacePrx callbackInterface;
-
- SkillExecutionRequest() = delete;
- SkillExecutionRequest(
- const skills::SkillID& skill,
- const std::string executorName,
- const armarx::aron::data::DictPtr& params,
- const callback::dti::SkillProviderCallbackInterfacePrx& callback = nullptr);
-
manager::dto::SkillExecutionRequest toManagerIce() const;
provider::dto::SkillExecutionRequest toProviderIce() const;
@@ -40,6 +28,12 @@ namespace armarx
static SkillExecutionRequest
FromIce(const provider::dto::SkillExecutionRequest&,
const std::optional<skills::ProviderID>& providerId = std::nullopt);
+
+
+ skills::SkillID skillId;
+ std::string executorName;
+ armarx::aron::data::DictPtr parameters = nullptr;
+ callback::dti::SkillProviderCallbackInterfacePrx callbackInterface = nullptr;
};
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.cpp b/source/RobotAPI/libraries/skills/core/SkillID.cpp
index 9bb0b36a881642fbbdb7c21841dc5597e050fe7b..ece268aeeec9234a8dd4b2dcaa437d616988317e 100644
--- a/source/RobotAPI/libraries/skills/core/SkillID.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillID.cpp
@@ -4,37 +4,10 @@ namespace armarx
{
namespace skills
{
- SkillID::SkillID(const std::string& skillName) :
- providerId(std::nullopt), skillName(skillName)
- {
- }
-
- SkillID::SkillID(const ProviderID& providerId, const std::string& skillName) :
- providerId(providerId), skillName(skillName)
- {
- if (simox::alg::contains(providerId.providerName, NAME_SEPARATOR) ||
- simox::alg::contains(skillName, NAME_SEPARATOR))
- {
- throw error::SkillException(
- __PRETTY_FUNCTION__,
- std::string("A skill provider or a skill contains the blacklisted token '") +
- NAME_SEPARATOR + "'.");
- }
-
- if (simox::alg::contains(providerId.providerName, PREFIX_SEPARATOR) ||
- simox::alg::contains(skillName, PREFIX_SEPARATOR))
- {
- throw error::SkillException(
- __PRETTY_FUNCTION__,
- std::string("A skill provider or a skill contains the blacklisted token '") +
- PREFIX_SEPARATOR + "'.");
- }
- }
-
bool
SkillID::operator==(const SkillID& other) const
{
- return providerId == other.providerId && skillName == other.skillName;
+ return this->toString() == other.toString();
}
bool
@@ -49,10 +22,18 @@ namespace armarx
return toString() < other.toString();
}
+ bool
+ SkillID::operator<=(const SkillID& other) const
+ {
+ return toString() <= other.toString();
+ }
+
SkillID
SkillID::FromIce(const manager::dto::SkillID& s)
{
- return SkillID(s.providerId.providerName, s.skillName);
+ return SkillID{.providerId =
+ skills::ProviderID{.providerName = s.providerId.providerName},
+ .skillName = s.skillName};
}
SkillID
@@ -61,15 +42,15 @@ namespace armarx
{
if (providerId.has_value())
{
- return SkillID(*providerId, s.skillName);
+ return SkillID{.providerId = *providerId, .skillName = s.skillName};
}
- return SkillID(s.skillName);
+ return SkillID{.skillName = s.skillName};
}
manager::dto::SkillID
SkillID::toManagerIce() const
{
- ARMARX_CHECK(fullySpecified());
+ ARMARX_CHECK(isFullySpecified());
return {providerId->toManagerIce(), skillName};
}
@@ -80,15 +61,13 @@ namespace armarx
}
std::string
- SkillID::toString(const std::string& prefix) const
+ SkillID::toString() const
{
if (providerId.has_value())
{
- return (prefix.empty() ? std::string("") : (prefix + PREFIX_SEPARATOR)) +
- providerId->providerName + NAME_SEPARATOR + skillName;
+ return providerId->providerName + NAME_SEPARATOR + skillName;
}
- return (prefix.empty() ? std::string("") : (prefix + PREFIX_SEPARATOR)) + UNKNOWN +
- NAME_SEPARATOR + skillName;
+ return NAME_SEPARATOR + skillName;
}
} // namespace skills
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.h b/source/RobotAPI/libraries/skills/core/SkillID.h
index 39561595db477c926f809bc5a51644510f8da484..e611a82828442ad378dde77d4d90edf477b5e5f1 100644
--- a/source/RobotAPI/libraries/skills/core/SkillID.h
+++ b/source/RobotAPI/libraries/skills/core/SkillID.h
@@ -17,41 +17,33 @@ namespace armarx
class SkillID
{
public:
- static const constexpr char* PREFIX_SEPARATOR = "->";
static const constexpr char* NAME_SEPARATOR = "/";
- static const constexpr char* UNKNOWN = "UNKNOWN";
-
- std::optional<ProviderID> providerId;
- std::string skillName;
-
- SkillID() = delete;
- SkillID(const std::string& skillName);
- SkillID(const ProviderID& providerId, const std::string& skillName);
bool operator==(const SkillID& other) const;
bool operator!=(const SkillID& other) const;
bool operator<(const SkillID& other) const;
+ bool operator<=(const SkillID& other) const;
bool
- fullySpecified() const
+ isFullySpecified() const
{
- return skillSpecified() && providerSpecified();
+ return isSkillSpecified() and isProviderSpecified();
}
bool
- skillSpecified() const
+ isSkillSpecified() const
{
- return skillName != UNKNOWN;
+ return not skillName.empty();
}
bool
- providerSpecified() const
+ isProviderSpecified() const
{
if (not providerId.has_value())
{
return false;
}
- return providerId->providerName != UNKNOWN;
+ return not providerId->providerName.empty();
}
manager::dto::SkillID toManagerIce() const;
@@ -61,7 +53,10 @@ namespace armarx
static SkillID FromIce(const provider::dto::SkillID&,
const std::optional<ProviderID>& providerId = std::nullopt);
- std::string toString(const std::string& prefix = "") const;
+ std::string toString() const;
+
+ std::optional<ProviderID> providerId = std::nullopt;
+ std::string skillName;
};
std::ostream& operator<<(std::ostream& os, const SkillID& id);
diff --git a/source/RobotAPI/libraries/skills/core/SkillParameterization.h b/source/RobotAPI/libraries/skills/core/SkillParameterization.h
index 5358eafa7591ad2ea7331b65427f0be062d1d194..9b54b3fff3bfe0aaedcb79d9c275165aefe5a37d 100644
--- a/source/RobotAPI/libraries/skills/core/SkillParameterization.h
+++ b/source/RobotAPI/libraries/skills/core/SkillParameterization.h
@@ -10,25 +10,5 @@ namespace armarx
{
namespace skills
{
- struct SkillParameterization
- {
- SkillParameterization() = delete;
-
- SkillParameterization(const aron::data::DictPtr& params,
- const callback::dti::SkillProviderCallbackInterfacePrx& i) :
- parameterization(params), callbackInterface(i)
- {
- }
-
- static SkillParameterization
- FromIce(const armarx::aron::data::dto::DictPtr& params,
- const callback::dti::SkillProviderCallbackInterfacePrx& i)
- {
- return SkillParameterization(armarx::aron::data::Dict::FromAronDictDTO(params), i);
- }
-
- aron::data::DictPtr parameterization;
- callback::dti::SkillProviderCallbackInterfacePrx callbackInterface;
- };
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillProxy.cpp b/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f73c43dfd152c756f80e5ac3e83e901fe59e8ee1
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillProxy.cpp
@@ -0,0 +1,118 @@
+#include "SkillProxy.h"
+
+#include <chrono>
+#include <thread>
+
+namespace armarx
+{
+ namespace skills
+ {
+ SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
+ const SkillID& skillId) :
+ manager(manager)
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ skillDescription = SkillDescription::FromIce(
+ manager->getSkillDescription(skillId.toManagerIce()).value());
+ ARMARX_CHECK(skillDescription.skillId.isFullySpecified());
+ }
+
+ SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
+ const SkillDescription& skillDesc) :
+ manager(manager), skillDescription(skillDesc)
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ ARMARX_CHECK(skillDesc.skillId.isFullySpecified());
+ }
+
+ SkillDescription
+ SkillProxy::getSkillDescription() const
+ {
+ return skillDescription;
+ }
+
+ SkillID
+ SkillProxy::getSkillId() const
+ {
+ return skillDescription.skillId;
+ }
+
+ TerminatedSkillStatusUpdate
+ SkillProxy::executeSkill(const std::string& executorName,
+ const aron::data::DictPtr& params) const
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ skills::manager::dto::SkillExecutionRequest req;
+ req.executorName = executorName;
+ req.parameters = aron::data::Dict::ToAronDictDTO(params);
+ req.skillId = skillDescription.skillId.toManagerIce();
+
+ auto terminatingUpdateIce = manager->executeSkill(req);
+ return TerminatedSkillStatusUpdate::FromIce(terminatingUpdateIce);
+ }
+
+ SkillExecutionID
+ SkillProxy::executeSkillAsync(const std::string& executorName,
+ const aron::data::DictPtr& params) const
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ skills::manager::dto::SkillExecutionRequest req;
+ req.executorName = executorName;
+ req.parameters = aron::data::Dict::ToAronDictDTO(params);
+ req.skillId = skillDescription.skillId.toManagerIce();
+
+ auto execIdIce = manager->executeSkillAsync(req);
+ return SkillExecutionID::FromIce(execIdIce);
+ }
+
+ std::optional<TerminatedSkillStatusUpdate>
+ SkillProxy::join(const SkillExecutionID& executionId) const
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ auto s = this->manager->getSkillExecutionStatus(executionId.toManagerIce());
+
+ while (s)
+ {
+ auto statusUpdate = skills::SkillStatusUpdate::FromIce(*s);
+
+ if (statusUpdate.hasBeenTerminated())
+ {
+ break;
+ }
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ s = this->manager->getSkillExecutionStatus(executionId.toManagerIce());
+ }
+
+ if (!s)
+ {
+ // Either the manager already removed the result (then it is unknown) or the execution id does not exist.
+ return std::nullopt;
+ }
+
+ return TerminatedSkillStatusUpdate::FromIce(*s);
+ }
+
+ bool
+ SkillProxy::abortSkill(const SkillExecutionID& id) const
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ auto r = manager->abortSkill(id.toManagerIce());
+ return r.success;
+ }
+
+ bool
+ SkillProxy::abortSkillAsync(const SkillExecutionID& id) const
+ {
+ ARMARX_CHECK_NOT_NULL(manager);
+ auto r = manager->abortSkillAsync(id.toManagerIce());
+ return r.success;
+ }
+
+ aron::data::DictPtr
+ SkillProxy::getRootProfileParameters() const
+ {
+ return skillDescription.rootProfileDefaults;
+ }
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillProxy.h b/source/RobotAPI/libraries/skills/core/SkillProxy.h
new file mode 100644
index 0000000000000000000000000000000000000000..e20ca01a0bc4b6de877e34a3ad6bc0edcbaa4689
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillProxy.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ /* Manages the remote execution of a skill and converts the ice types */
+ class SkillProxy : public armarx::Logging
+ {
+ public:
+ /// We remove the default constructor as every skill proxy requires a manager
+ SkillProxy() = delete;
+
+ /// set the skill proxy using a skillId. Queries the manager to get the description.
+ SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
+ const SkillID& skillId);
+
+ /// set the proxy using a skill description
+ SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
+ const SkillDescription& skillDesc);
+
+ /// copy ctor
+ SkillProxy(const SkillProxy& o) = default;
+
+ /// get the skill description
+ SkillDescription getSkillDescription() const;
+
+ /// get the skill id from the skill description
+ SkillID getSkillId() const;
+
+ // Provide a similar API as the skillprovider
+ /// execute a skill and block until skill terminates
+ TerminatedSkillStatusUpdate
+ executeSkill(const std::string& executorName,
+ const aron::data::DictPtr& params = nullptr) const;
+
+ /// execute a skill. Do not block during execution
+ SkillExecutionID executeSkillAsync(const std::string& executorName,
+ const aron::data::DictPtr& params = nullptr) const;
+
+ /// poll execution status and block until its null or terminated
+ std::optional<TerminatedSkillStatusUpdate>
+ join(const SkillExecutionID& executionId) const;
+
+ /// ask skill to abort ASAP. Blocks until skill stopped
+ bool abortSkill(const SkillExecutionID& executionId) const;
+
+ /// ask skill to abort ASAP
+ bool abortSkillAsync(const SkillExecutionID& executionId) const;
+
+ // Utiliy methods
+ /// get the default parameters of the skill. TODO: Skill profiles in memory!
+ aron::data::DictPtr getRootProfileParameters() const;
+
+ protected:
+ manager::dti::SkillManagerInterfacePrx manager;
+ SkillDescription skillDescription;
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
index 36ac32ed7ca1217e63f41329bb3a199269a95989..986aab1bc9be867aa918c14e4471ac9fe3e7cf3f 100644
--- a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
@@ -37,7 +37,7 @@ namespace armarx
}
TerminatedSkillStatus
- MergeSkillStatus(const TerminatedSkillStatus t1, const TerminatedSkillStatus t2)
+ mergeSkillStatuseses(const TerminatedSkillStatus t1, const TerminatedSkillStatus t2)
{
// if both are equal
if (t1 == t2)
@@ -56,8 +56,8 @@ namespace armarx
}
ActiveOrTerminatedSkillStatus
- MergeSkillStatus(const ActiveOrTerminatedSkillStatus t1,
- const ActiveOrTerminatedSkillStatus t2)
+ mergeSkillStatuseses(const ActiveOrTerminatedSkillStatus t1,
+ const ActiveOrTerminatedSkillStatus t2)
{
// if both are equal
if (t1 == t2)
@@ -239,12 +239,6 @@ namespace armarx
throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
}
- SkillStatusUpdateBase::SkillStatusUpdateBase(const SkillExecutionID& exec,
- const SkillParameterization& param) :
- executionId(exec), usedParameterization(param)
- {
- }
-
manager::dto::SkillStatusUpdate
SkillStatusUpdateBase::toManagerIce() const
{
@@ -253,9 +247,9 @@ namespace armarx
ret.executionId.executorName = executionId.executorName;
armarx::core::time::toIce(ret.executionId.executionStartedTime,
executionId.executionStartedTime);
- ret.usedParams = aron::data::Dict::ToAronDictDTO(usedParameterization.parameterization);
- ret.usedCallbackInterface = usedParameterization.callbackInterface;
- ret.data = aron::data::Dict::ToAronDictDTO(data);
+ ret.parameters = aron::data::Dict::ToAronDictDTO(parameters);
+ ret.callbackInterface = callbackInterface;
+ ret.result = aron::data::Dict::ToAronDictDTO(result);
return ret;
}
@@ -267,9 +261,9 @@ namespace armarx
ret.executionId.executorName = executionId.executorName;
armarx::core::time::toIce(ret.executionId.executionStartedTime,
executionId.executionStartedTime);
- ret.usedParams = aron::data::Dict::ToAronDictDTO(usedParameterization.parameterization);
- ret.usedCallbackInterface = usedParameterization.callbackInterface;
- ret.data = aron::data::Dict::ToAronDictDTO(data);
+ ret.parameters = aron::data::Dict::ToAronDictDTO(parameters);
+ ret.callbackInterface = callbackInterface;
+ ret.result = aron::data::Dict::ToAronDictDTO(result);
return ret;
}
@@ -324,9 +318,10 @@ namespace armarx
TerminatedSkillStatusUpdate
TerminatedSkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
{
- TerminatedSkillStatusUpdate ret(skills::SkillExecutionID::FromIce(update.executionId),
- skills::SkillParameterization::FromIce(
- update.usedParams, update.usedCallbackInterface));
+ TerminatedSkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
skills::fromIce(update.status, ret.status);
return ret;
}
@@ -335,10 +330,10 @@ namespace armarx
TerminatedSkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update,
const std::optional<skills::ProviderID>& providerId)
{
- TerminatedSkillStatusUpdate ret(
- skills::SkillExecutionID::FromIce(update.executionId, providerId),
- skills::SkillParameterization::FromIce(update.usedParams,
- update.usedCallbackInterface));
+ TerminatedSkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
skills::fromIce(update.status, ret.status);
return ret;
}
@@ -346,9 +341,10 @@ namespace armarx
SkillStatusUpdate
SkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
{
- SkillStatusUpdate ret(skills::SkillExecutionID::FromIce(update.executionId),
- skills::SkillParameterization::FromIce(
- update.usedParams, update.usedCallbackInterface));
+ SkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
skills::fromIce(update.status, ret.status);
return ret;
}
@@ -357,9 +353,10 @@ namespace armarx
SkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update,
const std::optional<skills::ProviderID>& providerId)
{
- SkillStatusUpdate ret(skills::SkillExecutionID::FromIce(update.executionId, providerId),
- skills::SkillParameterization::FromIce(
- update.usedParams, update.usedCallbackInterface));
+ SkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
skills::fromIce(update.status, ret.status);
return ret;
}
@@ -367,10 +364,10 @@ namespace armarx
ActiveOrTerminatedSkillStatusUpdate
ActiveOrTerminatedSkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
{
- ActiveOrTerminatedSkillStatusUpdate ret(
- skills::SkillExecutionID::FromIce(update.executionId),
- skills::SkillParameterization::FromIce(update.usedParams,
- update.usedCallbackInterface));
+ ActiveOrTerminatedSkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
skills::fromIce(update.status, ret.status);
return ret;
}
@@ -380,10 +377,10 @@ namespace armarx
const provider::dto::SkillStatusUpdate& update,
const std::optional<skills::ProviderID>& providerId)
{
- ActiveOrTerminatedSkillStatusUpdate ret(
- skills::SkillExecutionID::FromIce(update.executionId, providerId),
- skills::SkillParameterization::FromIce(update.usedParams,
- update.usedCallbackInterface));
+ ActiveOrTerminatedSkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface}};
skills::fromIce(update.status, ret.status);
return ret;
}
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
index 368dbe8b4a806b74698448099298d35802242658..ace31510a8a362ed45289c4facdd51c97ba22fbb 100644
--- a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
@@ -55,23 +55,20 @@ namespace armarx
ActiveOrTerminatedSkillStatus& ret);
void fromIce(const core::dto::Execution::Status& status, SkillStatus& ret);
- TerminatedSkillStatus MergeSkillStatus(const TerminatedSkillStatus t1,
- const TerminatedSkillStatus t2);
- ActiveOrTerminatedSkillStatus MergeSkillStatus(const ActiveOrTerminatedSkillStatus t1,
- const ActiveOrTerminatedSkillStatus t2);
+ TerminatedSkillStatus mergeSkillStatuseses(const TerminatedSkillStatus t1,
+ const TerminatedSkillStatus t2);
+ ActiveOrTerminatedSkillStatus mergeSkillStatuseses(const ActiveOrTerminatedSkillStatus t1,
+ const ActiveOrTerminatedSkillStatus t2);
struct SkillStatusUpdateBase
{
// header
SkillExecutionID executionId;
- SkillParameterization usedParameterization;
-
- SkillStatusUpdateBase() = delete;
- SkillStatusUpdateBase(const SkillExecutionID& exec, const SkillParameterization& param);
- SkillStatusUpdateBase(const SkillStatusUpdateBase&) = default;
+ aron::data::DictPtr parameters;
+ callback::dti::SkillProviderCallbackInterfacePrx callbackInterface;
// data
- aron::data::DictPtr data = nullptr;
+ aron::data::DictPtr result = nullptr;
manager::dto::SkillStatusUpdate toManagerIce() const;
@@ -83,14 +80,18 @@ namespace armarx
{
TerminatedSkillStatus status = TerminatedSkillStatus::Failed;
- using SkillStatusUpdateBase::SkillStatusUpdateBase;
-
bool
hasBeenTerminated() const
{
return true;
}
+ bool
+ hasBeenSucceeded() const
+ {
+ return status == TerminatedSkillStatus::Succeeded;
+ }
+
manager::dto::SkillStatusUpdate toManagerIce() const;
provider::dto::SkillStatusUpdate toProviderIce() const;
@@ -108,8 +109,6 @@ namespace armarx
{
ActiveOrTerminatedSkillStatus status = ActiveOrTerminatedSkillStatus::Failed;
- using SkillStatusUpdateBase::SkillStatusUpdateBase;
-
bool
hasBeenTerminated() const
{
@@ -118,6 +117,12 @@ namespace armarx
status == ActiveOrTerminatedSkillStatus::Aborted;
}
+ bool
+ hasBeenSucceeded() const
+ {
+ return status == ActiveOrTerminatedSkillStatus::Succeeded;
+ }
+
manager::dto::SkillStatusUpdate toManagerIce() const;
provider::dto::SkillStatusUpdate toProviderIce() const;
@@ -135,8 +140,6 @@ namespace armarx
{
SkillStatus status = SkillStatus::Constructing;
- using SkillStatusUpdateBase::SkillStatusUpdateBase;
-
bool
operator<(const SkillStatusUpdate& o) const
{
@@ -204,6 +207,12 @@ namespace armarx
status == SkillStatus::Aborted;
}
+ bool
+ hasBeenSucceeded() const
+ {
+ return status == SkillStatus::Succeeded;
+ }
+
manager::dto::SkillStatusUpdate toManagerIce() const;
provider::dto::SkillStatusUpdate toProviderIce() const;
diff --git a/source/RobotAPI/libraries/skills/core/error/Exception.h b/source/RobotAPI/libraries/skills/core/error/Exception.h
index b73ccbf1e0271f82cf95107bd6beaecdcb4440a8..5e10baed1e9af3131226320397c11919d7a826e0 100644
--- a/source/RobotAPI/libraries/skills/core/error/Exception.h
+++ b/source/RobotAPI/libraries/skills/core/error/Exception.h
@@ -24,42 +24,71 @@
#pragma once
// STD/STL
+#include <map>
#include <string>
#include <vector>
-#include <map>
// ArmarX
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/exceptions/Exception.h>
-
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::skills::error
{
/**
- * @brief A base class for aron exceptions. All aron exceptions inherit from this class
+ * @brief A base class for skill exceptions. All skill exceptions inherit from this class
*/
- class SkillException :
- public armarx::LocalException
+ class SkillException : public armarx::LocalException
{
public:
SkillException() = delete;
+
SkillException(const std::string& prettymethod, const std::string& reason) :
LocalException(prettymethod + ": " + reason + ".")
{
}
};
+ class SkillAbortedException : public armarx::LocalException
+ {
+ public:
+ SkillAbortedException() = delete;
+
+ SkillAbortedException(const std::string& reason) : LocalException(reason)
+ {
+ }
+
+ SkillAbortedException(const std::string& prettymethod, const std::string& reason) :
+ LocalException(prettymethod + ": " + reason + ".")
+ {
+ }
+ };
+
+ class SkillFailedException : public armarx::LocalException
+ {
+ public:
+ SkillFailedException() = delete;
+
+ SkillFailedException(const std::string& reason) : LocalException(reason)
+ {
+ }
+
+ SkillFailedException(const std::string& prettymethod, const std::string& reason) :
+ LocalException(prettymethod + ": " + reason + ".")
+ {
+ }
+ };
+
/**
* @brief The NotImplementedYetException class
*/
- class NotImplementedYetException :
- public SkillException
+ class NotImplementedYetException : public SkillException
{
public:
NotImplementedYetException() = delete;
+
NotImplementedYetException(const std::string& prettymethod) :
SkillException(prettymethod, "This method is not yet implemented!")
{
}
};
-}
+} // namespace armarx::skills::error
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 51694c8d9f2ff130f0684a0c0dda268e40c09979..3999c956b3ec3d85be444e7b27d56c27fbc2d98e 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -49,12 +49,11 @@ namespace armarx::plugins
SkillManagerComponentPlugin::addProvider(const skills::ProviderInfo& providerInfo)
{
std::scoped_lock l(skillProviderMapMutex);
- if (skillProviderMap.find(providerInfo.providerId.providerName) == skillProviderMap.end())
+ if (skillProviderMap.find(providerInfo.providerId) == skillProviderMap.end())
{
ARMARX_INFO << "Adding a provider with name '" << providerInfo.providerId.providerName
<< "'.";
- skillProviderMap.insert(
- {providerInfo.providerId.providerName, providerInfo.providerInterface});
+ skillProviderMap.insert({providerInfo.providerId, providerInfo.providerInterface});
}
else
{
@@ -62,7 +61,7 @@ namespace armarx::plugins
<< providerInfo.providerId.providerName
<< "' but the provider already exists. "
<< "Overwriting the old provider info.";
- skillProviderMap[providerInfo.providerId.providerName] = providerInfo.providerInterface;
+ skillProviderMap[providerInfo.providerId] = providerInfo.providerInterface;
}
}
@@ -70,7 +69,7 @@ namespace armarx::plugins
SkillManagerComponentPlugin::removeProvider(const skills::ProviderID& providerId)
{
std::scoped_lock l(skillProviderMapMutex);
- if (auto it = skillProviderMap.find(providerId.providerName); it != skillProviderMap.end())
+ if (auto it = skillProviderMap.find(providerId); it != skillProviderMap.end())
{
ARMARX_INFO << "Removing a provider with name '" << providerId.providerName << "'.";
skillProviderMap.erase(it);
@@ -85,16 +84,16 @@ namespace armarx::plugins
skills::SkillStatusUpdate
SkillManagerComponentPlugin::executeSkill(const skills::SkillExecutionRequest& executionRequest)
{
- ARMARX_CHECK(executionRequest.skillId.fullySpecified());
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
std::unique_lock l(skillProviderMapMutex);
- skills::ProviderID provderId(executionRequest.skillId);
+ skills::ProviderID provderId(*executionRequest.skillId.providerId);
// TODO: Really support regexes!
if (executionRequest.skillId.providerId->providerName == "*")
{
- provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId.skillName);
+ provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId);
}
@@ -116,11 +115,11 @@ namespace armarx::plugins
try
{
- skills::SkillExecutionRequest provider_executionRequest(
- executionRequest.skillId,
- executionRequest.executorName,
- executionRequest.params,
- myPrx);
+ skills::SkillExecutionRequest provider_executionRequest{
+ .skillId = executionRequest.skillId,
+ .executorName = executionRequest.executorName,
+ .parameters = executionRequest.parameters,
+ .callbackInterface = myPrx};
auto async =
provider->begin_executeSkill(provider_executionRequest.toProviderIce());
@@ -158,16 +157,16 @@ namespace armarx::plugins
SkillManagerComponentPlugin::executeSkillAsync(
const skills::SkillExecutionRequest& executionRequest)
{
- ARMARX_CHECK(executionRequest.skillId.fullySpecified());
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
std::unique_lock l(skillProviderMapMutex);
- skills::ProviderID provderId(executionRequest.skillId);
+ skills::ProviderID provderId(*executionRequest.skillId.providerId);
// TODO: Really support regexes!
if (executionRequest.skillId.providerId->providerName == "*")
{
- provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId.skillName);
+ provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId);
}
@@ -189,11 +188,11 @@ namespace armarx::plugins
try
{
- skills::SkillExecutionRequest provider_executionRequest(
- executionRequest.skillId,
- executionRequest.executorName,
- executionRequest.params,
- myPrx);
+ skills::SkillExecutionRequest provider_executionRequest{
+ .skillId = executionRequest.skillId,
+ .executorName = executionRequest.executorName,
+ .parameters = executionRequest.parameters,
+ .callbackInterface = myPrx};
auto async =
provider->begin_executeSkillAsync(provider_executionRequest.toProviderIce());
@@ -228,11 +227,11 @@ namespace armarx::plugins
}
}
- void
- SkillManagerComponentPlugin::addSkillParameters(const skills::SkillExecutionID& executionId,
- const aron::data::DictPtr& data)
+ bool
+ SkillManagerComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
+ const aron::data::DictPtr& data)
{
- ARMARX_CHECK(executionId.skillId.fullySpecified());
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
std::unique_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
@@ -246,29 +245,34 @@ namespace armarx::plugins
ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
<< providerId << "'. Removing it from skills.";
skillProviderMap.erase(it);
- return;
+ return false;
}
try
{
- auto async = provider->begin_addSkillParameters(executionId.toProviderIce(),
- data->toAronDictDTO());
+ auto async = provider->begin_updateSkillParameters(executionId.toProviderIce(),
+ data->toAronDictDTO());
l.unlock(); // allow parallel e.g. stopping
- provider->end_addSkillParameters(async);
- return;
+ auto r = provider->end_updateSkillParameters(async);
+ return r.success;
}
catch (...)
{
ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
<< providerId << "'. Removing it from skills on next execute.";
+ return false;
}
}
+ else
+ {
+ return false;
+ }
}
- void
+ bool
SkillManagerComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
{
- ARMARX_CHECK(executionId.skillId.fullySpecified());
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
std::unique_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
@@ -282,28 +286,71 @@ namespace armarx::plugins
ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
<< providerId << "'. Removing it from skills.";
skillProviderMap.erase(it);
- return;
+ return false;
}
try
{
auto async = provider->begin_abortSkill(executionId.toProviderIce());
l.unlock(); // allow parallel e.g. stopping
- provider->end_abortSkill(async);
- return;
+ auto r = provider->end_abortSkill(async);
+ return r.success;
}
catch (...)
{
ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
<< providerId << "'. Removing it from skills on next execute.";
+ return false;
}
}
+ else
+ {
+ return false;
+ }
+ }
+
+ bool
+ SkillManagerComponentPlugin::abortSkillAsync(const skills::SkillExecutionID& executionId)
+ {
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
+ it != skillProviderMap.end())
+ {
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+ return false;
+ }
+
+ try
+ {
+ auto async = provider->begin_abortSkill(executionId.toProviderIce());
+ return true;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills on next execute.";
+ return false;
+ }
+ }
+ else
+ {
+ return false;
+ }
}
std::optional<skills::SkillDescription>
SkillManagerComponentPlugin::getSkillDescription(const skills::SkillID& skillId)
{
- ARMARX_CHECK(skillId.fullySpecified());
+ ARMARX_CHECK(skillId.isFullySpecified());
std::unique_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(*skillId.providerId); it != skillProviderMap.end())
@@ -406,7 +453,7 @@ namespace armarx::plugins
SkillManagerComponentPlugin::getSkillExecutionStatus(
const skills::SkillExecutionID& executionId)
{
- ARMARX_CHECK(executionId.skillId.fullySpecified());
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
std::unique_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
@@ -532,9 +579,6 @@ namespace armarx
this->plugin->removeProvider(i);
}
- using SkillProviderInterfacePrxMap =
- std::map<std::string, skills::provider::dti::SkillProviderInterfacePrx>;
-
skills::manager::dto::SkillStatusUpdate
SkillManagerComponentPluginUser::executeSkill(
const skills::manager::dto::SkillExecutionRequest& info,
@@ -553,23 +597,38 @@ namespace armarx
return this->plugin->executeSkillAsync(e).toManagerIce();
}
- void
- SkillManagerComponentPluginUser::addSkillParameters(
+ skills::provider::dto::ParameterUpdateResult
+ SkillManagerComponentPluginUser::updateSkillParameters(
const skills::manager::dto::SkillExecutionID& info,
const aron::data::dto::DictPtr& params,
const Ice::Current& current)
{
+ skills::provider::dto::ParameterUpdateResult ret;
auto a = armarx::aron::data::Dict::FromAronDictDTO(params);
auto e = skills::SkillExecutionID::FromIce(info);
- this->plugin->addSkillParameters(e, a);
+ ret.success = this->plugin->updateSkillParameters(e, a);
+ return ret;
}
- void
+ skills::provider::dto::AbortSkillResult
SkillManagerComponentPluginUser::abortSkill(const skills::manager::dto::SkillExecutionID& id,
const Ice::Current& current)
{
+ skills::provider::dto::AbortSkillResult ret;
auto i = skills::SkillExecutionID::FromIce(id);
- this->plugin->abortSkill(i);
+ ret.success = this->plugin->abortSkill(i);
+ return ret;
+ }
+
+ skills::provider::dto::AbortSkillResult
+ SkillManagerComponentPluginUser::abortSkillAsync(
+ const skills::manager::dto::SkillExecutionID& id,
+ const Ice::Current& /*unused*/)
+ {
+ skills::provider::dto::AbortSkillResult ret;
+ auto i = skills::SkillExecutionID::FromIce(id);
+ ret.success = this->plugin->abortSkillAsync(i);
+ return ret;
}
void
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index 57b0250a1aecf9216c34ec8d76933a59d00cb9aa..8aeea6f1ab7d387ce6c46e4212e1455c1c2a9435 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -37,10 +37,12 @@ namespace armarx::plugins
skills::SkillExecutionID executeSkillAsync(const skills::SkillExecutionRequest& req);
- void addSkillParameters(const skills::SkillExecutionID& id,
- const aron::data::DictPtr& data);
+ bool updateSkillParameters(const skills::SkillExecutionID& id,
+ const aron::data::DictPtr& data);
- void abortSkill(const skills::SkillExecutionID& id);
+ bool abortSkill(const skills::SkillExecutionID& id);
+
+ bool abortSkillAsync(const skills::SkillExecutionID& id);
std::optional<skills::SkillDescription> getSkillDescription(const skills::SkillID& id);
@@ -86,17 +88,24 @@ namespace armarx
executeSkillAsync(const skills::manager::dto::SkillExecutionRequest& skillExecutionRequest,
const Ice::Current& current) override;
- void addSkillParameters(const skills::manager::dto::SkillExecutionID& executionId,
- const aron::data::dto::DictPtr& params,
- const Ice::Current& current) override;
+ skills::provider::dto::ParameterUpdateResult
+ updateSkillParameters(const skills::manager::dto::SkillExecutionID& executionId,
+ const aron::data::dto::DictPtr& params,
+ const Ice::Current& current) override;
void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update,
const skills::callback::dto::ProviderID& id,
const Ice::Current& current) override;
- void abortSkill(const skills::manager::dto::SkillExecutionID& id,
+ skills::provider::dto::AbortSkillResult
+ abortSkill(const skills::manager::dto::SkillExecutionID& id,
+ const Ice::Current& current) override;
+
+ skills::provider::dto::AbortSkillResult
+ abortSkillAsync(const skills::manager::dto::SkillExecutionID& id,
const Ice::Current& current) override;
+
skills::manager::dto::SkillDescriptionMap
getSkillDescriptions(const Ice::Current& current) override;
diff --git a/source/RobotAPI/libraries/skills/provider/CMakeLists.txt b/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
index e5df870817e13549d985ae816081e5e989c79ee8..9cb59c319fdf937ee9eb341610071f44114de3c1 100644
--- a/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
@@ -21,7 +21,9 @@ armarx_add_library(
LambdaSkill.cpp
SimpleSkill.cpp
SimpleSpecializedSkill.cpp
- SkillProxy.cpp
+ SimplePeriodicSpecializedSkill.cpp
+ SimplePeriodicSkill.cpp
+ SpecializedSkillProxy.cpp
PeriodicSkill.cpp
SpecializedSkill.cpp
PeriodicSpecializedSkill.cpp
@@ -35,7 +37,9 @@ armarx_add_library(
LambdaSkill.h
SimpleSkill.h
SimpleSpecializedSkill.h
- SkillProxy.h
+ SimplePeriodicSpecializedSkill.h
+ SimplePeriodicSkill.h
+ SpecializedSkillProxy.h
PeriodicSkill.h
SpecializedSkill.h
PeriodicSpecializedSkill.h
diff --git a/source/RobotAPI/libraries/skills/provider/LambdaSkill.h b/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
index dda6c472c711f9cae1c3d6ac396218ef10f50ca5..16d39dd39578e25ecb901815e10311e7e57bab72 100644
--- a/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
@@ -9,16 +9,16 @@ namespace armarx
class LambdaSkill : public Skill
{
public:
- using FunT = std::function<TerminatedSkillStatus()>;
+ using FunctionType = std::function<TerminatedSkillStatus()>;
LambdaSkill() = delete;
- LambdaSkill(const SkillDescription& desc, const FunT& f) : Skill(desc), fun(f){};
+ LambdaSkill(const SkillDescription& desc, const FunctionType& f) : Skill(desc), fun(f){};
private:
MainResult main() override;
private:
- FunT fun;
+ FunctionType fun;
};
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
index 56994fb334ee2129dd2ad3cec995308ac14b6124..fa354d03d242c9e19b95a4a846309227503ae182 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
@@ -36,53 +36,39 @@ namespace armarx::skills
{
}
- PeriodicSkill::StepResult
- PeriodicSkill::stepOfSkill()
- {
- return this->step();
- }
-
Skill::MainResult
PeriodicSkill::main()
{
armarx::core::time::Metronome metronome(frequency);
- while (not Skill::checkWhetherSkillShouldStopASAP())
+ while (true)
{
- const auto res = stepOfSkill();
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step();
switch (res.status)
{
case ActiveOrTerminatedSkillStatus::Running:
// nothing to do here
break;
case ActiveOrTerminatedSkillStatus::Aborted:
- return {TerminatedSkillStatus::Aborted, res.data};
+ return MakeAbortedResult();
case ActiveOrTerminatedSkillStatus::Succeeded:
- return {TerminatedSkillStatus::Succeeded, res.data};
+ return MakeSucceededResult(res.data);
case ActiveOrTerminatedSkillStatus::Failed:
- return {TerminatedSkillStatus::Failed, res.data};
+ return MakeFailedResult();
}
const auto sleepDuration = metronome.waitForNextTick();
if (not sleepDuration.isPositive())
{
- ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
- << -sleepDuration << " too long. Expected "
- << frequency.toCycleDuration() << ")";
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
}
}
- if (stopped)
- {
- return {TerminatedSkillStatus::Aborted, nullptr};
- }
-
- if (timeoutReached)
- {
- ARMARX_WARNING << "The skill " << getSkillId().toString() << " reached timeout!";
- return {TerminatedSkillStatus::Failed, nullptr};
- }
-
+ // never happens
throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
}
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
index b11239dcf8074c8e53383137b0744bd720f802ff..aff91f19677f0ec362fbd7ca34ab2f3976661b96 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
@@ -41,8 +41,6 @@ namespace armarx::skills
PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency);
- StepResult stepOfSkill();
-
protected:
/// Do not use anymore
Skill::MainResult main() final;
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
index 1c98f112680263ac1bfb3006d8eb9be28f8e901c..914c6da0221973624309a57666064669e05de14a 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -50,12 +50,6 @@ namespace armarx::skills
{
}
- StepResult
- stepOfSkill()
- {
- return this->step();
- }
-
protected:
/// Do not use anymore
Skill::MainResult
@@ -63,42 +57,33 @@ namespace armarx::skills
{
armarx::core::time::Metronome metronome(frequency);
- while (not Skill::checkWhetherSkillShouldStopASAP())
+ while (true)
{
- const auto statusUpdate = stepOfSkill();
- switch (statusUpdate.status)
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step();
+ switch (res.status)
{
case ActiveOrTerminatedSkillStatus::Running:
- // nothing to do here
+ // nothing to do here. break switch
break;
case ActiveOrTerminatedSkillStatus::Aborted:
- return {TerminatedSkillStatus::Aborted, statusUpdate.data};
+ return Skill::MakeAbortedResult();
case ActiveOrTerminatedSkillStatus::Succeeded:
- return {TerminatedSkillStatus::Succeeded, statusUpdate.data};
+ return Skill::MakeSucceededResult(res.data);
case ActiveOrTerminatedSkillStatus::Failed:
- return {TerminatedSkillStatus::Failed, statusUpdate.data};
+ return Skill::MakeFailedResult();
}
const auto sleepDuration = metronome.waitForNextTick();
if (not sleepDuration.isPositive())
{
- ARMARX_INFO << deactivateSpam() << "PeriodicSkill: execution took too long ("
- << -sleepDuration << " too long. Expected "
- << frequency.toCycleDuration() << ")";
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
}
}
- if (stopped)
- {
- return {TerminatedSkillStatus::Aborted, nullptr};
- }
-
- if (timeoutReached)
- {
- ARMARX_WARNING << "The skill " << getSkillId().toString() << " reached timeout!";
- return {TerminatedSkillStatus::Failed, nullptr};
- }
-
throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
}
@@ -111,10 +96,6 @@ namespace armarx::skills
return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
}
- private:
- using Skill::stopped;
- using Skill::timeoutReached;
-
protected:
const armarx::Frequency frequency;
};
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..33a6475b3a9e2e01a30c40f4cee60e304a600b40
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
@@ -0,0 +1,54 @@
+#include "SimplePeriodicSkill.h"
+
+namespace armarx::skills
+{
+ SimplePeriodicSkill::SimplePeriodicSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ SimpleSkill(skillDescription), frequency(frequency)
+ {
+ }
+
+ Skill::MainResult
+ SimplePeriodicSkill::main(const MainInput& in)
+ {
+ armarx::core::time::Metronome metronome(frequency);
+
+ while (true)
+ {
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step(in);
+ switch (res.status)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ // nothing to do here. break switch
+ break;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ return MakeAbortedResult();
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ return MakeSucceededResult(res.data);
+ case ActiveOrTerminatedSkillStatus::Failed:
+ return MakeFailedResult();
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
+ }
+ }
+
+ // never happens
+ throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ SimplePeriodicSkill::StepResult
+ SimplePeriodicSkill::step(const MainInput& in)
+ {
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << description.skillId
+ << "'. Please overwrite this method!";
+ return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
+ }
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ca70d213277e50fd5350458e62b8c8428050b52
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include "PeriodicSkill.h"
+#include "SimpleSkill.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SimplePeriodicSkill : public SimpleSkill
+ {
+ public:
+ using Base = SimpleSkill;
+
+ using Base::Base;
+
+ using StepResult = PeriodicSkill::StepResult;
+
+ SimplePeriodicSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency);
+
+ protected:
+ /// Do not use anymore
+ Skill::MainResult main(const MainInput& in) final;
+
+ /// Override this method with your own step function
+ virtual StepResult step(const MainInput& in);
+
+ protected:
+ const armarx::Frequency frequency;
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a04c2b3d821e7e821dd2158a98a636d9b3338b03
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.cpp
@@ -0,0 +1,6 @@
+#include "SimplePeriodicSpecializedSkill.h"
+
+namespace armarx::skills
+{
+
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..ce3c968f587a7984fb606e8485297aa56ffff04a
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
@@ -0,0 +1,79 @@
+#pragma once
+
+#include "PeriodicSkill.h"
+#include "SimpleSpecializedSkill.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ template <class AronT>
+ class SimplePeriodicSpecializedSkill : public SimpleSpecializedSkill<AronT>
+ {
+
+ public:
+ using Base = SimpleSpecializedSkill<AronT>;
+ using ParamType = AronT;
+
+ using Base::Base;
+
+ using StepResult = PeriodicSkill::StepResult;
+
+ SimplePeriodicSpecializedSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ Base(skillDescription), frequency(frequency)
+ {
+ }
+
+ protected:
+ /// Do not use anymore
+ Skill::MainResult
+ main(const typename Base::SpecializedMainInput& in) final
+ {
+ armarx::core::time::Metronome metronome(frequency);
+
+ while (true)
+ {
+ this->throwIfSkillShouldTerminate();
+
+ const auto res = step(in);
+ switch (res.status)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ // nothing to do here. break switch
+ break;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ return Skill::MakeAbortedResult();
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ return Skill::MakeSucceededResult(res.data);
+ case ActiveOrTerminatedSkillStatus::Failed:
+ return Skill::MakeFailedResult();
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
+ << ": execution took too long (" << -sleepDuration
+ << " too long. Expected " << frequency.toCycleDuration() << ")";
+ }
+ }
+
+ // never happens
+ throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ /// Override this method with your own step function
+ virtual StepResult
+ step(const typename Base::SpecializedMainInput& in)
+ {
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << this->description.skillId
+ << "'. Please overwrite this method!";
+ return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
+ }
+
+ protected:
+ const armarx::Frequency frequency;
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp
index 96ec8391771ba52b6a5d6d813f3692a3116e12b0..b6e834b4cf3114bd0938328482d00a2d243eb60e 100644
--- a/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp
@@ -3,42 +3,42 @@
namespace armarx::skills
{
Skill::InitResult
- SimpleSKill::init()
+ SimpleSkill::init()
{
InitInput i;
i.executorName = this->executorName;
- i.params = this->parameters;
+ i.parameters = this->parameters;
return this->init(i);
}
Skill::MainResult
- SimpleSKill::main()
+ SimpleSkill::main()
{
MainInput i;
i.executorName = this->executorName;
- i.params = this->parameters;
+ i.parameters = this->parameters;
i.callback = this->callback;
return this->main(i);
}
Skill::ExitResult
- SimpleSKill::exit()
+ SimpleSkill::exit()
{
ExitInput i;
i.executorName = this->executorName;
- i.params = this->parameters;
+ i.parameters = this->parameters;
return this->exit(i);
}
Skill::InitResult
- SimpleSKill::init(const InitInput& in)
+ SimpleSkill::init(const InitInput& in)
{
// Default nothing to init
return {.status = TerminatedSkillStatus::Succeeded};
}
Skill::MainResult
- SimpleSKill::main(const MainInput& in)
+ SimpleSkill::main(const MainInput& in)
{
// This is just a dummy implementation
ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillId
@@ -47,7 +47,7 @@ namespace armarx::skills
}
Skill::ExitResult
- SimpleSKill::exit(const ExitInput& in)
+ SimpleSkill::exit(const ExitInput& in)
{
// Default nothing to exit
return {.status = TerminatedSkillStatus::Succeeded};
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSkill.h b/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
index c4b736dfb0b96f12fb42ffcf6eb6d0218252daa2..45e4715d8bfaf4cc06c98199c67679396c071674 100644
--- a/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
@@ -6,28 +6,30 @@ namespace armarx
{
namespace skills
{
- class SimpleSKill : public Skill
+ class SimpleSkill : public Skill
{
public:
- using Skill::Skill;
+ using Base = Skill;
+
+ using Base::Base;
struct InitInput
{
std::string executorName;
- aron::data::DictPtr params;
+ aron::data::DictPtr parameters;
};
struct MainInput
{
std::string executorName;
- aron::data::DictPtr params;
+ aron::data::DictPtr parameters;
CallbackT callback;
};
struct ExitInput
{
std::string executorName;
- aron::data::DictPtr params;
+ aron::data::DictPtr parameters;
};
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
index a62ca4ae5c7a2aa54c231beb6805a1b71d044f7a..c134754888f6749a834ffdd1e256b0aed9660562 100644
--- a/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
@@ -1,95 +1,88 @@
#pragma once
-#include <RobotAPI/libraries/skills/core/Skill.h>
+#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
namespace armarx
{
namespace skills
{
template <class AronT>
- class SimpleSpecializedSkill : public Skill
+ class SimpleSpecializedSkill : public SpecializedSkill<AronT>
{
public:
+ using Base = SpecializedSkill<AronT>;
using ParamType = AronT;
- using Skill::Skill;
+
+ using Base::Base;
struct SpecializedInitInput
{
std::string executorName;
- AronT params;
+ AronT parameters;
};
struct SpecializedMainInput
{
std::string executorName;
- AronT params;
- CallbackT callback;
+ AronT parameters;
+ Skill::CallbackT callback;
};
struct SpecializedExitInput
{
std::string executorName;
- AronT params;
+ AronT parameters;
};
protected:
// legacy methods
- virtual InitResult
+ virtual Skill::InitResult
init(const SpecializedInitInput& in)
{
- return InitResult{.status = TerminatedSkillStatus::Succeeded};
+ return Skill::InitResult{.status = TerminatedSkillStatus::Succeeded};
}
- virtual MainResult
+ virtual Skill::MainResult
main(const SpecializedMainInput& in)
{
- ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillId
+ ARMARX_IMPORTANT << "Dummy executing skill '" << this->description.skillId
<< "'. Please overwrite this method.";
return Skill::MainResult{.status = TerminatedSkillStatus::Succeeded,
.data = nullptr};
}
- virtual ExitResult
+ virtual Skill::ExitResult
exit(const SpecializedExitInput& in)
{
- return ExitResult{.status = TerminatedSkillStatus::Succeeded};
+ return Skill::ExitResult{.status = TerminatedSkillStatus::Succeeded};
}
- InitResult
+ Skill::InitResult
init() final
{
- AronT p;
- p.fromAron(this->parameters);
-
SpecializedInitInput i;
i.executorName = this->executorName;
- i.params = p;
+ i.parameters = this->getParameters();
return this->init(i);
}
- MainResult
+ Skill::MainResult
main() final
{
- AronT p;
- p.fromAron(this->parameters);
-
SpecializedMainInput i;
i.executorName = this->executorName;
i.callback = this->callback;
- i.params = p;
+ i.parameters = this->getParameters();
return this->main(i);
}
- ExitResult
+ Skill::ExitResult
exit() final
{
- AronT p;
- p.fromAron(this->parameters);
-
SpecializedExitInput i;
i.executorName = this->executorName;
- i.params = p;
+ i.parameters = this->getParameters();
return this->exit(i);
}
};
diff --git a/source/RobotAPI/libraries/skills/provider/SkillContext.h b/source/RobotAPI/libraries/skills/provider/SkillContext.h
index df5a90eb7abf887c0c262794f52aee9016095120..a5efb050b3e3e2fa005f20b5f6690026ddcb8573 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillContext.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillContext.h
@@ -7,7 +7,9 @@ namespace armarx
{
namespace skills
{
- /* A virtual base class for skill contexts. It is not required to use a context for skills but it eases the management of dependencies and properties for providers */
+ /* A virtual base class for skill contexts.
+ * It is not required to use a context for skills but it eases the management of
+ * dependencies and properties for providers */
class SkillContext
{
public:
diff --git a/source/RobotAPI/libraries/skills/provider/SkillFactory.h b/source/RobotAPI/libraries/skills/provider/SkillFactory.h
index ea4cd5edbff7130b560e7725fc57cda70841612d..1e3b40e3daf092e9214fc65846ec1c58b57e2b52 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillFactory.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillFactory.h
@@ -8,35 +8,43 @@ namespace armarx
{
namespace skills
{
- class SkillFactory
+ class SkillBlueprint
{
public:
- using FunTSkill = std::function<std::unique_ptr<Skill>()>;
+ using FunctionTypeToCreateSkill = std::function<std::unique_ptr<Skill>()>;
- SkillFactory(const FunTSkill& s) : createS(s)
+ SkillBlueprint(const FunctionTypeToCreateSkill& s) : _createSkill(s)
{
}
+ SkillBlueprint(FunctionTypeToCreateSkill&& s) : _createSkill(std::move(s))
+ {
+ }
+
+ SkillBlueprint(const SkillBlueprint&) = default;
+
+ virtual ~SkillBlueprint() = default;
+
template <class _Skill, class... Args>
requires isSkill<_Skill>
- static std::unique_ptr<SkillFactory>
+ static std::unique_ptr<SkillBlueprint>
ForSkill(Args&&... args)
-
{
- // capture params in new lambda
- // (https://stackoverflow.com/questions/47496358/c-lambdas-how-to-capture-variadic-parameter-pack-from-the-upper-scope)
- auto createS = [... args = std::forward<Args>(args)]()
- { return std::make_unique<_Skill>(std::forward<Args>(args)...); };
+ auto _createSkill = [=]()
+ {
+ auto ptr = std::make_unique<_Skill>(args...);
+ return ptr;
+ };
- auto ret = std::make_unique<SkillFactory>(createS);
+ auto ret = std::make_unique<SkillBlueprint>(std::move(_createSkill));
return ret;
}
virtual std::unique_ptr<Skill>
createSkill(const skills::ProviderID& pid) const
{
- auto s = createS();
+ auto s = _createSkill();
s->setProviderId(pid);
return s;
}
@@ -50,11 +58,11 @@ namespace armarx
protected:
- FunTSkill createS;
+ FunctionTypeToCreateSkill _createSkill;
};
template <class T>
- concept isSkillFactory = std::is_base_of<SkillFactory, T>::value;
+ concept isSkillBlueprint = std::is_base_of<SkillBlueprint, T>::value;
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index d24535a2241afc4493ab354968313151217e5105..a42d7808284d958d965cfba0dd70aa866a47cc17 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -26,7 +26,9 @@ namespace armarx::plugins
const std::string providerName = p.getName();
// register self to manager
- skills::ProviderInfo i(skills::ProviderID(providerName), myPrx, getSkillDescriptions());
+ skills::ProviderInfo i{.providerId = skills::ProviderID{.providerName = providerName},
+ .providerInterface = myPrx,
+ .providedSkills = getSkillDescriptions()};
ARMARX_INFO << "Adding provider to manager: " << i.providerId;
manager->addProvider(i.toIce()); // add provider info to manager
@@ -54,7 +56,7 @@ namespace armarx::plugins
}
void
- SkillProviderComponentPlugin::addSkillFactory(std::unique_ptr<skills::SkillFactory>&& fac)
+ SkillProviderComponentPlugin::addSkillFactory(std::unique_ptr<skills::SkillBlueprint>&& fac)
{
if (!fac)
{
@@ -90,18 +92,18 @@ namespace armarx::plugins
// }
}
- skills::SkillFactory*
+ skills::SkillBlueprint*
SkillProviderComponentPlugin::addSkillFactory(const skills::SkillDescription& desc,
- const skills::LambdaSkill::FunT& f)
+ const skills::LambdaSkill::FunctionType& f)
{
return addSkillFactory<skills::LambdaSkill>(desc, f);
}
- skills::SkillFactory*
+ skills::SkillBlueprint*
SkillProviderComponentPlugin::getSkillFactory(const armarx::skills::SkillID& skillId)
{
// NON BLOCKING: WE ASSERT THAT THE LOCK IS ALREADY TAKEN
- ARMARX_CHECK(skillId.fullySpecified());
+ ARMARX_CHECK(skillId.isFullySpecified());
if (skillFactories.count(skillId) == 0)
{
@@ -110,17 +112,17 @@ namespace armarx::plugins
}
auto* facPtr = skillFactories.at(skillId).get();
- return static_cast<skills::SkillFactory*>(facPtr);
+ return static_cast<skills::SkillBlueprint*>(facPtr);
}
std::optional<skills::SkillStatusUpdate>
SkillProviderComponentPlugin::getSkillExecutionStatus(
const skills::SkillExecutionID& execId) const
{
- ARMARX_CHECK(execId.skillId.skillSpecified());
+ ARMARX_CHECK(execId.skillId.isSkillSpecified());
const std::unique_lock l(skillExecutionsMutex);
- if (skillExecutions.find(execId) != skillExecutions.end())
+ if (skillExecutions.find(execId) == skillExecutions.end())
{
ARMARX_WARNING << "Skill execution for skill '" + execId.skillId.toString() +
"' not found!";
@@ -147,12 +149,22 @@ namespace armarx::plugins
std::optional<skills::SkillDescription>
SkillProviderComponentPlugin::getSkillDescription(const skills::SkillID& skillId) const
{
- ARMARX_CHECK(skillId.fullySpecified());
+ ARMARX_CHECK(skillId.isFullySpecified());
const std::unique_lock l(skillFactoriesMutex);
- if (skillFactories.find(skillId) != skillFactories.end())
+ if (skillFactories.find(skillId) == skillFactories.end())
{
- ARMARX_WARNING << "Skill description for skill '" + skillId.toString() + "' not found!";
+ std::stringstream ss;
+ ss << "Skill description for skill '" + skillId.toString() +
+ "' not found! Found instead: {"
+ << "\n";
+ for (const auto& [k, _] : skillFactories)
+ {
+ ss << "\t" << k.toString() << "\n";
+ }
+ ss << "}";
+ ARMARX_WARNING << ss.str();
+
return std::nullopt;
}
@@ -166,7 +178,7 @@ namespace armarx::plugins
const std::unique_lock l(skillFactoriesMutex);
for (const auto& [key, fac] : skillFactories)
{
- ARMARX_CHECK(key.fullySpecified());
+ ARMARX_CHECK(key.isFullySpecified());
skillDesciptions.insert({key, fac->createSkillDescription(*key.providerId)});
}
return skillDesciptions;
@@ -176,66 +188,56 @@ namespace armarx::plugins
SkillProviderComponentPlugin::executeSkill(
const skills::SkillExecutionRequest& executionRequest)
{
- ARMARX_CHECK(executionRequest.skillId.fullySpecified());
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
- // The skill will be executed in a seperate thread
- std::thread execution;
+ skills::SkillExecutionID executionId{.skillId = executionRequest.skillId,
+ .executorName = executionRequest.executorName,
+ .executionStartedTime =
+ armarx::core::time::DateTime::Now()};
- // setup input args for skill execution
- skills::SkillParameterization usedParameterization{executionRequest.params,
- executionRequest.callbackInterface};
+ skills::SkillStatusUpdate ret{
+ {executionId, executionRequest.parameters, executionRequest.callbackInterface}};
- skills::SkillExecutionID executionId(executionRequest.skillId,
- executionRequest.executorName,
- armarx::core::time::DateTime::Now());
-
- skills::SkillStatusUpdate ret{executionId, usedParameterization};
+ skills::detail::SkillRuntime* wrapper;
{
auto l1 = std::unique_lock{skillFactoriesMutex};
- const auto& fac = getSkillFactory(executionRequest.skillId);
- ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionRequest.skillId;
+ const auto& fac = getSkillFactory(executionId.skillId);
+ ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionId.skillId;
+
+ {
+ const std::unique_lock l2{skillExecutionsMutex};
+ auto it =
+ skillExecutions.emplace(std::piecewise_construct,
+ std::make_tuple(executionId),
+ std::make_tuple(*fac,
+ executionId,
+ executionRequest.parameters,
+ executionRequest.callbackInterface));
+ wrapper = &it.first->second;
+ }
// async start execution. But we wait for the execution to finish at the end of this method
- execution = std::thread(
+ wrapper->execution = std::thread(
[&]()
{
- skills::detail::SkillImplementationWrapper* wrapper;
- {
- const std::unique_lock l2{skillExecutionsMutex};
- auto it = skillExecutions.emplace(
- std::piecewise_construct,
- std::make_tuple(executionId),
- std::make_tuple(*fac, executionId, usedParameterization));
-
- ARMARX_CHECK(it.second)
- << "For some reason a skill '" << executionId.skillId.toString()
- << "' execution failed when instantiating the "
- "wrapper class and adding it to the internally used map. This "
- "should "
- "not happen!!";
-
- wrapper = &it.first->second;
- }
-
-
// execute waits until the previous execution finishes.
auto x = wrapper->executeSkill();
- ret.data = x.data;
+ ret.result = x.result;
ret.status = armarx::skills::toSkillStatus(x.status);
});
} // release lock. We don't know how long the skill needs to finish and we have to release the lock for being able to abort the execution
- if (execution.joinable())
+ if (wrapper && wrapper->execution.joinable())
{
- execution.join();
+ wrapper->execution.join();
// tidy up map of executions
- const std::unique_lock l2{skillExecutionsMutex};
- if (auto it = skillExecutions.find(executionId); it != skillExecutions.end())
- {
- skillExecutions.erase(it);
- }
+ // const std::unique_lock l2{skillExecutionsMutex};
+ // if (auto it = skillExecutions.find(executionId); it != skillExecutions.end())
+ // {
+ // skillExecutions.erase(it);
+ // }
}
return ret;
}
@@ -244,62 +246,62 @@ namespace armarx::plugins
SkillProviderComponentPlugin::executeSkillAsync(
const skills::SkillExecutionRequest& executionRequest)
{
- ARMARX_CHECK(executionRequest.skillId.fullySpecified());
-
- // The skill will be executed in a seperate thread
- std::thread execution;
-
- // setup input args for skill execution
- skills::SkillParameterization usedParameterization{executionRequest.params,
- executionRequest.callbackInterface};
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
- skills::SkillExecutionID executionId(executionRequest.skillId,
+ skills::SkillExecutionID executionId{executionRequest.skillId,
executionRequest.executorName,
- armarx::core::time::DateTime::Now());
+ armarx::core::time::DateTime::Now()};
+ skills::detail::SkillRuntime* wrapper;
{
auto l1 = std::unique_lock{skillFactoriesMutex};
- const auto& fac = getSkillFactory(executionRequest.skillId);
- ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionRequest.skillId;
+ const auto& fac = getSkillFactory(executionId.skillId);
+ ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionId.skillId;
- // async start execution. But we wait for the execution to finish at the end of this method
- execution = std::thread(
+ {
+ const std::unique_lock l2{skillExecutionsMutex};
+ auto it =
+ skillExecutions.emplace(std::piecewise_construct,
+ std::make_tuple(executionId),
+ std::make_tuple(*fac,
+ executionId,
+ executionRequest.parameters,
+ executionRequest.callbackInterface));
+ wrapper = &it.first->second;
+ }
+
+ wrapper->execution = std::thread(
[&]()
{
- skills::detail::SkillImplementationWrapper* wrapper;
- {
- const std::unique_lock l2{skillExecutionsMutex};
- auto it = skillExecutions.emplace(
- std::piecewise_construct,
- std::make_tuple(executionId),
- std::make_tuple(*fac, executionId, usedParameterization));
-
- ARMARX_CHECK(it.second)
- << "For some reason a skill '" << executionId.skillId.toString()
- << "' execution failed when instantiating the "
- "wrapper class and adding it to the internally used map. This "
- "should "
- "not happen!!";
-
- wrapper = &it.first->second;
- }
-
-
// execute waits until the previous execution finishes.
auto x = wrapper->executeSkill();
});
- } // release lock. We don't know how long the skill needs to finish and we have to release the lock for being able to abort the execution
+ }
+
+ // wait until skill is constructed. This assures, that a status update exists.
+ while (true)
+ {
+ {
+ std::scoped_lock l(wrapper->skillStatusesMutex);
+
+ if (wrapper->statusUpdate.hasBeenConstructed())
+ {
+ break;
+ }
+ }
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ }
- execution.detach();
return executionId;
}
- void
- SkillProviderComponentPlugin::addSkillParameters(const skills::SkillExecutionID& executionId,
- const armarx::aron::data::DictPtr& input)
+ bool
+ SkillProviderComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
+ const armarx::aron::data::DictPtr& input)
{
- ARMARX_CHECK(executionId.skillId.fullySpecified());
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
const std::scoped_lock l{skillExecutionsMutex};
auto it = skillExecutions.find(executionId);
@@ -307,7 +309,7 @@ namespace armarx::plugins
{
ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
"' found! Ignoring prepareSkill request.";
- return;
+ return false;
}
std::scoped_lock l2{it->second.skillStatusesMutex};
@@ -315,16 +317,17 @@ namespace armarx::plugins
{
ARMARX_INFO << "Could not prepare the skill '" + executionId.skillId.toString() +
"' because its not in preparing phase.";
- return;
+ return false;
}
- it->second.addSkillParameters(input);
+ it->second.updateSkillParameters(input);
+ return true;
}
- void
+ bool
SkillProviderComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
{
- ARMARX_CHECK(executionId.skillId.fullySpecified());
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
const std::unique_lock l(skillExecutionsMutex);
auto it = skillExecutions.find(executionId);
@@ -332,10 +335,45 @@ namespace armarx::plugins
{
ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
"' found! Ignoring abortSkill request.";
- return;
+ return false;
+ }
+
+ auto& runtime = it->second;
+ runtime.stopSkill();
+
+ while (true)
+ {
+ {
+ std::scoped_lock l(runtime.skillStatusesMutex);
+ auto status = runtime.statusUpdate;
+
+ if (status.hasBeenTerminated())
+ {
+ break;
+ }
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ }
+
+ return true;
+ }
+
+ bool
+ SkillProviderComponentPlugin::abortSkillAsync(const skills::SkillExecutionID& executionId)
+ {
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ const std::unique_lock l(skillExecutionsMutex);
+ auto it = skillExecutions.find(executionId);
+ if (it == skillExecutions.end())
+ {
+ ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
+ "' found! Ignoring abortSkill request.";
+ return false;
}
it->second.stopSkill();
+ return true;
}
} // namespace armarx::plugins
@@ -352,7 +390,7 @@ namespace armarx
const skills::provider::dto::SkillID& skillId,
const Ice::Current& /*unused*/)
{
- auto id = skills::SkillID::FromIce(skillId, getName());
+ auto id = skills::SkillID::FromIce(skillId, skills::ProviderID{.providerName = getName()});
auto o = plugin->getSkillDescription(id);
if (o.has_value())
{
@@ -377,7 +415,8 @@ namespace armarx
const skills::provider::dto::SkillExecutionID& executionId,
const Ice::Current& /*unused*/)
{
- auto execId = skills::SkillExecutionID::FromIce(executionId, getName());
+ auto execId = skills::SkillExecutionID::FromIce(
+ executionId, skills::ProviderID{.providerName = getName()});
auto o = plugin->getSkillExecutionStatus(execId);
if (o.has_value())
{
@@ -403,7 +442,8 @@ namespace armarx
const skills::provider::dto::SkillExecutionRequest& info,
const Ice::Current& /*unused*/)
{
- auto exec = skills::SkillExecutionRequest::FromIce(info, getName());
+ auto exec = skills::SkillExecutionRequest::FromIce(
+ info, skills::ProviderID{.providerName = getName()});
auto up = this->plugin->executeSkill(exec);
return up.toProviderIce();
}
@@ -413,28 +453,48 @@ namespace armarx
const skills::provider::dto::SkillExecutionRequest& info,
const Ice::Current& current /*unused*/)
{
- auto exec = skills::SkillExecutionRequest::FromIce(info, getName());
+ auto exec = skills::SkillExecutionRequest::FromIce(
+ info, skills::ProviderID{.providerName = getName()});
auto id = this->plugin->executeSkillAsync(exec);
return id.toProviderIce();
}
- void
- SkillProviderComponentPluginUser::addSkillParameters(
+ skills::provider::dto::ParameterUpdateResult
+ SkillProviderComponentPluginUser::updateSkillParameters(
const skills::provider::dto::SkillExecutionID& id,
const aron::data::dto::DictPtr& input,
const Ice::Current& current /*unused*/)
{
- auto exec = skills::SkillExecutionID::FromIce(id, getName());
+ skills::provider::dto::ParameterUpdateResult res;
+
+ auto exec =
+ skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
auto prep = armarx::aron::data::Dict::FromAronDictDTO(input);
- this->plugin->addSkillParameters(exec, prep);
+ res.success = this->plugin->updateSkillParameters(exec, prep);
+ return res;
}
- void
+ skills::provider::dto::AbortSkillResult
SkillProviderComponentPluginUser::abortSkill(const skills::provider::dto::SkillExecutionID& id,
const Ice::Current& /*unused*/)
{
- auto exec = skills::SkillExecutionID::FromIce(id, getName());
- this->plugin->abortSkill(exec);
+ skills::provider::dto::AbortSkillResult res;
+ auto exec =
+ skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
+ res.success = this->plugin->abortSkill(exec);
+ return res;
+ }
+
+ skills::provider::dto::AbortSkillResult
+ SkillProviderComponentPluginUser::abortSkillAsync(
+ const skills::provider::dto::SkillExecutionID& id,
+ const Ice::Current& /*unused*/)
+ {
+ skills::provider::dto::AbortSkillResult res;
+ auto exec =
+ skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
+ res.success = this->plugin->abortSkillAsync(exec);
+ return res;
}
const std::experimental::observer_ptr<plugins::SkillProviderComponentPlugin>&
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index e03d73e26da0ae1fbec86daeca5bd8f1021f5a9f..812cc0a8814db843793038ebdd16a3a927c912cf 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -49,31 +49,31 @@ namespace armarx::plugins
void preOnDisconnectComponent() override;
- void addSkillFactory(std::unique_ptr<skills::SkillFactory>&&);
+ void addSkillFactory(std::unique_ptr<skills::SkillBlueprint>&&);
- skills::SkillFactory* addSkillFactory(const skills::SkillDescription&,
- const skills::LambdaSkill::FunT&);
+ skills::SkillBlueprint* addSkillFactory(const skills::SkillDescription&,
+ const skills::LambdaSkill::FunctionType&);
- template <class _Skill, typename... Args>
+ template <class SkillT, typename... Args>
- requires skills::isSkill<_Skill> skills::SkillFactory*
+ requires skills::isSkill<SkillT> skills::SkillBlueprint*
addSkillFactory(Args&&... args)
{
- auto fac = skills::SkillFactory::ForSkill<_Skill>(std::forward<Args>(args)...);
+ auto fac = skills::SkillBlueprint::ForSkill<SkillT>(std::forward<Args>(args)...);
auto* facPtr = fac.get();
addSkillFactory(std::move(fac));
- return static_cast<skills::SkillFactory*>(facPtr);
+ return static_cast<skills::SkillBlueprint*>(facPtr);
}
- template <typename _FactoryT, typename... Args>
+ template <typename FactoryT, typename... Args>
- requires skills::isSkillFactory<_FactoryT> _FactoryT*
+ requires skills::isSkillBlueprint<FactoryT> FactoryT*
addCustomSkillFactory(Args&&... args)
{
- auto fac = std::make_unique<_FactoryT>(std::forward<Args>(args)...);
+ auto fac = std::make_unique<FactoryT>(std::forward<Args>(args)...);
auto* facPtr = fac.get();
addCustomSkillFactory(std::move(fac));
- return static_cast<_FactoryT*>(facPtr);
+ return static_cast<FactoryT*>(facPtr);
}
// Ice forwards
@@ -90,23 +90,25 @@ namespace armarx::plugins
skills::SkillExecutionID
executeSkillAsync(const skills::SkillExecutionRequest& executionInfo);
- void addSkillParameters(const skills::SkillExecutionID& id,
- const armarx::aron::data::DictPtr& params);
+ bool updateSkillParameters(const skills::SkillExecutionID& id,
+ const armarx::aron::data::DictPtr& params);
- void abortSkill(const skills::SkillExecutionID& execId);
+ bool abortSkill(const skills::SkillExecutionID& execId);
+
+ bool abortSkillAsync(const skills::SkillExecutionID& execId);
private:
- skills::SkillFactory* getSkillFactory(const armarx::skills::SkillID& name);
+ skills::SkillBlueprint* getSkillFactory(const armarx::skills::SkillID& name);
+
skills::manager::dti::SkillManagerInterfacePrx manager;
skills::provider::dti::SkillProviderInterfacePrx myPrx;
mutable std::mutex skillFactoriesMutex;
- std::map<skills::SkillID, std::unique_ptr<skills::SkillFactory>> skillFactories;
+ std::map<skills::SkillID, std::unique_ptr<skills::SkillBlueprint>> skillFactories;
mutable std::mutex skillExecutionsMutex;
- std::map<skills::SkillExecutionID, skills::detail::SkillImplementationWrapper>
- skillExecutions;
+ std::map<skills::SkillExecutionID, skills::detail::SkillRuntime> skillExecutions;
friend class armarx::SkillProviderComponentPluginUser;
};
@@ -145,11 +147,17 @@ namespace armarx
executeSkillAsync(const skills::provider::dto::SkillExecutionRequest& executionInfo,
const Ice::Current& current = Ice::Current()) override;
- void addSkillParameters(const skills::provider::dto::SkillExecutionID& executionId,
- const armarx::aron::data::dto::DictPtr& input,
- const Ice::Current& current = Ice::Current()) override;
+ skills::provider::dto::ParameterUpdateResult
+ updateSkillParameters(const skills::provider::dto::SkillExecutionID& executionId,
+ const armarx::aron::data::dto::DictPtr& parameters,
+ const Ice::Current& current = Ice::Current()) override;
+
+ skills::provider::dto::AbortSkillResult
+ abortSkill(const skills::provider::dto::SkillExecutionID& skill,
+ const Ice::Current& current = Ice::Current()) override;
- void abortSkill(const skills::provider::dto::SkillExecutionID& skill,
+ skills::provider::dto::AbortSkillResult
+ abortSkillAsync(const skills::provider::dto::SkillExecutionID& skill,
const Ice::Current& current = Ice::Current()) override;
// Plugin
@@ -157,49 +165,31 @@ namespace armarx
getSkillProviderPlugin() const;
protected:
- // -----------------------------------------------------------------------------------------
- // LEGACY, TODO: NEEDS TESTING
- // -----------------------------------------------------------------------------------------
- template <class _Skill>
-
- requires skills::isSkill<_Skill> skills::SkillFactory*
- addSkill(std::unique_ptr<_Skill>&& s)
- {
- return addSkillFactory<_Skill>();
- }
-
- template <class _Skill, typename... Args>
-
- requires skills::isSkill<_Skill> skills::SkillFactory*
- addSkill(Args&&... args)
- {
- return addSkillFactory<_Skill>(std::forward<Args>(args)...);
- }
-
// -----------------------------------------------------------------------------------------
// New
// -----------------------------------------------------------------------------------------
- skills::SkillFactory*
- addSkillFactory(const skills::SkillDescription& desc, const skills::LambdaSkill::FunT& f)
+ skills::SkillBlueprint*
+ addSkillFactory(const skills::SkillDescription& desc,
+ const skills::LambdaSkill::FunctionType& f)
{
auto ret = plugin->addSkillFactory(desc, f);
return ret;
}
- template <class _Skill, typename... Args>
+ template <class SkillT, typename... Args>
- requires skills::isSkill<_Skill> skills::SkillFactory*
+ requires skills::isSkill<SkillT> skills::SkillBlueprint*
addSkillFactory(Args&&... args)
{
- return plugin->addSkillFactory<_Skill>(std::forward<Args>(args)...);
+ return plugin->addSkillFactory<SkillT>(std::forward<Args>(args)...);
}
- template <typename _Fac, typename... Args>
+ template <typename FacT, typename... Args>
- requires skills::isSkillFactory<_Fac> _Fac*
+ requires skills::isSkillBlueprint<FacT> FacT*
addCustomSkillFactory(Args&&... args)
{
- return plugin->addSkillFactory<_Fac>(std::forward<Args>(args)...);
+ return plugin->addSkillFactory<FacT>(std::forward<Args>(args)...);
}
private:
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp b/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
deleted file mode 100644
index 3a969b2819eb8ec9fd2328dfe069ede85fa480f0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-#include "SkillProxy.h"
-
-namespace armarx
-{
- namespace skills
- {
- SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
- const SkillID& skillId) :
- manager(manager),
- skillDescription(SkillDescription::FromIce(
- manager->getSkillDescription(skillId.toManagerIce()).value()))
- {
- }
-
- SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
- const SkillDescription& skillDesc) :
- manager(manager), skillDescription(skillDesc)
- {
- ARMARX_CHECK(skillDesc.skillId.fullySpecified());
- }
-
- TerminatedSkillStatusUpdate
- SkillProxy::executeSkill(const std::string& executorName, const aron::data::DictPtr& params)
- {
- skills::manager::dto::SkillExecutionRequest req;
- req.executorName = executorName;
- req.params = params->toAronDictDTO();
- req.skillId = skillDescription.skillId.toManagerIce();
-
- auto terminatingUpdateIce = manager->executeSkill(req);
- return TerminatedSkillStatusUpdate::FromIce(terminatingUpdateIce);
- }
-
- SkillExecutionID
- SkillProxy::executeSkillAsync(const std::string& executorName,
- const aron::data::DictPtr& params)
- {
- skills::manager::dto::SkillExecutionRequest req;
- req.executorName = executorName;
- req.params = params->toAronDictDTO();
- req.skillId = skillDescription.skillId.toManagerIce();
-
- auto execReqIce = manager->executeSkillAsync(req);
- return SkillExecutionID::FromIce(execReqIce);
- }
-
- void
- SkillProxy::abortSkill(const SkillExecutionID& id)
- {
- // TODO: This will be used in the future, do not remove it!
- manager->abortSkill(id.toManagerIce());
- }
-
- aron::data::DictPtr
- SkillProxy::getDefaultParameters(const std::string& profileName)
- {
- if (profileName == "root")
- {
- return skillDescription.rootProfileParameterization;
- }
- // TODO @fabianPK
- return nullptr;
- }
- } // namespace skills
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.h b/source/RobotAPI/libraries/skills/provider/SkillProxy.h
deleted file mode 100644
index 4cb09be7920e8496313fa59d5eef8c1615ef21d0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-#include <RobotAPI/libraries/skills/core/Skill.h>
-
-namespace armarx
-{
- namespace skills
- {
- /* Manages the remote execution of a skill and converts the ice types */
- class SkillProxy : public armarx::Logging
- {
- public:
- SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
- const SkillID& skillId);
- SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager,
- const SkillDescription& skillDesc);
-
- // Provide a similar API as the skillprovider
- TerminatedSkillStatusUpdate executeSkill(const std::string& executorName,
- const aron::data::DictPtr& params = nullptr);
-
- SkillExecutionID executeSkillAsync(const std::string& executorName,
- const aron::data::DictPtr& params = nullptr);
-
- void abortSkill(const SkillExecutionID& executorName);
-
- // Utiliy methods
- aron::data::DictPtr getDefaultParameters(const std::string& profileName = "root");
-
- private:
- manager::dti::SkillManagerInterfacePrx manager;
- SkillDescription skillDescription;
- };
- } // namespace skills
-} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index d551bd88df7a19144dac07930233dc5293ffc089..2fded9a0a50ede4cdc3e8d76861217fbc1127c58 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -23,17 +23,16 @@ namespace armarx
void
setParameters(const AronT& d)
{
- std::scoped_lock l(this->parametersMutex);
- this->parameters = d;
-
Base::setParameters(d.toAron());
}
+ /// Overwrite getter for parameters. Shadow Skill::getParameters()
AronT
getParameters() const
{
- std::scoped_lock l(this->parametersMutex);
- return this->parameters;
+ AronT d;
+ d.fromAron(this->parameters);
+ return d;
}
/// returns the accepted type of the skill
@@ -45,8 +44,6 @@ namespace armarx
protected:
- mutable std::mutex parametersMutex;
- AronT parameters;
};
} // namespace skills
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.cpp b/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f372c5f55247795f91d956f92d84eb6ac35ee77
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.cpp
@@ -0,0 +1,8 @@
+#include "SpecializedSkillProxy.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h
new file mode 100644
index 0000000000000000000000000000000000000000..4dd53171c4cf17c7298cf9ba59c364ddcf81eb8e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkillProxy.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/core/SkillProxy.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ template <class AronT>
+ class SpecializedSkillProxy : public SkillProxy
+ {
+ public:
+ using SkillProxy::SkillProxy;
+
+ // Provide a similar API as the skillprovider
+ TerminatedSkillStatusUpdate
+ executeSkill(const std::string& executorName, const AronT& params)
+ {
+ return SkillProxy::executeSkill(executorName, params.toAron());
+ }
+
+ SkillExecutionID
+ executeSkillAsync(const std::string& executorName, const AronT& params = nullptr)
+ {
+ return SkillProxy::executeSkillAsync(executorName, params.toAron());
+ }
+
+ // Utiliy methods
+ AronT
+ getRootProfileParameters()
+ {
+ auto dict = SkillProxy::getRootProfileParameters();
+ if (dict)
+ {
+ return AronT::FromAron(dict);
+ }
+ return {};
+ }
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 41679d93a07ea883f9cf199431d83c217b33a3b6..60db0fa913274594078d59c3f7a930b86e1423be 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -4,17 +4,18 @@ namespace armarx
{
namespace skills::detail
{
- SkillImplementationWrapper::SkillImplementationWrapper(
- const skills::SkillFactory& fac,
+ SkillRuntime::SkillRuntime(
+ const skills::SkillBlueprint& fac,
const skills::SkillExecutionID& execId,
- const skills::SkillParameterization& param) :
- factory(fac), statusUpdate(execId, param)
+ const aron::data::DictPtr& initial_parameters,
+ const skills::callback::dti::SkillProviderCallbackInterfacePrx& callbackInterface) :
+ factory(fac), statusUpdate{{execId, initial_parameters, callbackInterface}}
{
}
// ask a skill to stop
void
- SkillImplementationWrapper::stopSkill()
+ SkillRuntime::stopSkill()
{
while (this->skill == nullptr)
{
@@ -24,11 +25,11 @@ namespace armarx
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
- this->skill->notifySkillToStopASAP();
+ this->skill->notifySkillToStop();
}
void
- SkillImplementationWrapper::addSkillParameters(const aron::data::DictPtr& i)
+ SkillRuntime::updateSkillParameters(const aron::data::DictPtr& i)
{
while (this->skill == nullptr)
{
@@ -38,11 +39,11 @@ namespace armarx
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
- this->skill->addParameters(i);
+ this->skill->updateParameters(i);
}
TerminatedSkillStatusUpdate
- SkillImplementationWrapper::executeSkill()
+ SkillRuntime::executeSkill()
{
std::scoped_lock l(this->executionMutex);
// -------------------------------------------------------------------------------------
@@ -56,14 +57,16 @@ namespace armarx
// setup basic vars and lambdas
// -------------------------------------------------------------------------------------
// actually we should lock... however this is only read access and the members are const throughout the execution...
- ARMARX_CHECK(statusUpdate.executionId.skillId.fullySpecified());
+ ARMARX_CHECK(statusUpdate.executionId.skillId.isFullySpecified());
- const auto& initial_aron_params = statusUpdate.usedParameterization.parameterization;
- const auto& callback_interface = statusUpdate.usedParameterization.callbackInterface;
+ const auto& initial_aron_params = statusUpdate.parameters;
+ const auto& callback_interface = statusUpdate.callbackInterface;
const auto& skillId = statusUpdate.executionId.skillId;
const auto& skillName = skillId.skillName;
const auto& providerId = *skillId.providerId;
const auto& executorName = statusUpdate.executionId.executorName;
+ const auto& manager =
+ skills::manager::dti::SkillManagerInterfacePrx::checkedCast(callback_interface);
ARMARX_INFO_S << "Executing skill: " << skillName;
@@ -72,11 +75,11 @@ namespace armarx
{
std::unique_lock l(skillStatusesMutex);
statusUpdate.status = status;
- statusUpdate.data = data;
+ statusUpdate.result = data;
if (skill) // if skill has been constructed
{
// update parameterization
- statusUpdate.usedParameterization.parameterization = skill->getParameters();
+ statusUpdate.parameters = skill->getParameters();
}
if (callback_interface) // if callback interface is used
@@ -114,27 +117,41 @@ namespace armarx
// -------------------------------------------------------------------------------------
// construct skill
// -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Construct skill: " << skillName;
+
updateStatus(SkillStatus::Constructing);
this->skill = this->factory.createSkill(providerId);
- this->skill->executorName = (executorName);
- this->skill->manager = skills::manager::dti::SkillManagerInterfacePrx::checkedCast(
- callback_interface); // ugly. Get managerPrx from manager!
- ;
- this->skill->callback = [&](const SkillStatus s, const armarx::aron::data::DictPtr& d)
- { updateStatus(s, d); };
+ this->skill->setExecutorName(executorName);
+ this->skill->setManager(manager);
+ this->skill->setCallback([&](const SkillStatus s, const armarx::aron::data::DictPtr& d)
+ { updateStatus(s, d); });
// set initial parameters that were attached to the execution request (only add as we are not sure whether some updates already arrived)
- skill->addParameters(initial_aron_params);
+ this->updateSkillParameters(initial_aron_params);
+
+ auto makeAbortedResult = [&](const std::string& message)
+ {
+ updateStatus(SkillStatus::Aborted, createErrorMessageData(message));
+
+ std::unique_lock l(skillStatusesMutex);
+ auto terminated = TerminatedSkillStatusUpdate{
+ {.executionId = statusUpdate.executionId,
+ .parameters = statusUpdate.parameters,
+ .callbackInterface = statusUpdate.callbackInterface}};
+ terminated.status = TerminatedSkillStatus::Aborted;
+ return terminated;
+ };
- auto makeTerminationResult = [&](const std::string& message)
+ auto makeFailedResult = [&](const std::string& message)
{
updateStatus(SkillStatus::Failed, createErrorMessageData(message));
std::unique_lock l(skillStatusesMutex);
- auto terminated = TerminatedSkillStatusUpdate(statusUpdate.executionId,
- statusUpdate.usedParameterization);
- terminated.data = statusUpdate.data;
+ auto terminated = TerminatedSkillStatusUpdate{
+ {.executionId = statusUpdate.executionId,
+ .parameters = statusUpdate.parameters,
+ .callbackInterface = statusUpdate.callbackInterface}};
terminated.status = TerminatedSkillStatus::Failed;
return terminated;
};
@@ -154,42 +171,60 @@ namespace armarx
// }
- auto exitAndMakeTerminationResult = [&](const std::string& message)
+ auto exitAndMakeFailedResult = [&](const std::string& message)
+ {
+ skill->exitSkill(); // try to exit skill. Ignore return value
+ return makeFailedResult(message);
+ };
+ auto exitAndMakeAbortedResult = [&](const std::string& message)
{
skill->exitSkill(); // try to exit skill. Ignore return value
- return makeTerminationResult(message);
+ return makeAbortedResult(message);
};
+ // Construction succeeded!
// -------------------------------------------------------------------------------------
// Init skill
// -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Init skill: " << skillName;
+ updateStatus(SkillStatus::Initializing);
+
try
{
- updateStatus(SkillStatus::Initializing);
Skill::InitResult initRet = skill->initSkill();
if (initRet.status != TerminatedSkillStatus::Succeeded)
{
std::string message = "SkillError 101: The initialization of skill '" +
skillName + "' did not succeed.";
- return exitAndMakeTerminationResult(message);
+ return exitAndMakeFailedResult(message);
}
}
-
+ catch (const error::SkillAbortedException& ex)
+ {
+ return exitAndMakeAbortedResult(GetHandledExceptionString());
+ }
+ catch (const error::SkillFailedException& ex)
+ {
+ return exitAndMakeFailedResult(GetHandledExceptionString());
+ }
catch (const std::exception& ex)
{
std::string message =
"SkillError 101e: An error occured during the initialization of skill '" +
skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
- return exitAndMakeTerminationResult(message);
+ return exitAndMakeFailedResult(message);
}
+ // Init succeeded!
// -------------------------------------------------------------------------------------
// Prepare skill
// -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Prepare skill: " << skillName;
+ updateStatus(SkillStatus::Preparing);
+
try
{
- updateStatus(SkillStatus::Preparing);
auto prepareRet = skill->prepareSkill();
while (prepareRet.status == ActiveOrTerminatedSkillStatus::Running)
{
@@ -197,18 +232,25 @@ namespace armarx
<< skillName << " fulfilled. Waiting...";
// wait...
- prepareRet = skill->prepareSkill();
std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ prepareRet = skill->prepareSkill();
}
if (prepareRet.status != ActiveOrTerminatedSkillStatus::Succeeded)
{
std::string message = "SkillError 201: The prepare method of skill '" +
skillName + "' did not succeed.";
- return exitAndMakeTerminationResult(message);
+ return exitAndMakeFailedResult(message);
}
}
-
+ catch (const error::SkillAbortedException& ex)
+ {
+ return exitAndMakeAbortedResult(GetHandledExceptionString());
+ }
+ catch (const error::SkillFailedException& ex)
+ {
+ return exitAndMakeFailedResult(GetHandledExceptionString());
+ }
catch (const std::exception& ex)
{
std::string message = "SkillError 201e: An error occured during waiting for skill "
@@ -216,15 +258,17 @@ namespace armarx
skillName +
"'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
- return exitAndMakeTerminationResult(message);
+ return exitAndMakeFailedResult(message);
}
+ // Prepare succeeded!
+ // -------------------------------------------------------------------------------------
+ // Main of skill
+ // -------------------------------------------------------------------------------------
// execute. If the skill fails for some reason, from this point it will always execute its exit function.
+ ARMARX_INFO_S << "Main skill: " << skillName;
updateStatus(SkillStatus::Running);
- // Init succeeded!
-
-
Skill::MainResult mainRet;
try
{
@@ -233,21 +277,31 @@ namespace armarx
{
std::string message = "SkillError 501: The main method of skill '" + skillName +
"' did not succeed.";
- return exitAndMakeTerminationResult(message);
+ return exitAndMakeFailedResult(message);
}
}
-
+ catch (const error::SkillAbortedException& ex)
+ {
+ return exitAndMakeAbortedResult(GetHandledExceptionString());
+ }
+ catch (const error::SkillFailedException& ex)
+ {
+ return exitAndMakeFailedResult(GetHandledExceptionString());
+ }
catch (const std::exception& ex)
{
std::string message =
"SkillError 501e: An error occured during the main method of skill '" +
skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
- return exitAndMakeTerminationResult(message);
+ return exitAndMakeFailedResult(message);
}
// Main succeeded!
-
+ // -------------------------------------------------------------------------------------
+ // Exit of skill
+ // -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Exit skill: " << skillName;
try
{
Skill::ExitResult exitRet = skill->exitSkill();
@@ -255,31 +309,38 @@ namespace armarx
{
std::string message = "SkillError 601: The exit method of skill '" + skillName +
"' did not succeed.";
- return makeTerminationResult(message);
+ return makeFailedResult(message);
}
}
-
+ catch (const error::SkillAbortedException& ex)
+ {
+ return makeAbortedResult(GetHandledExceptionString());
+ }
+ catch (const error::SkillFailedException& ex)
+ {
+ return makeFailedResult(GetHandledExceptionString());
+ }
catch (const std::exception& ex)
{
std::string message =
"SkillError 601e: An error occured during the exit method of skill '" +
skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
- return makeTerminationResult(message);
+ return makeFailedResult(message);
}
// Exit succeeded!
-
-
// All succeeded!
{
updateStatus(SkillStatus::Succeeded);
// return result of main method
std::unique_lock l(skillStatusesMutex);
- TerminatedSkillStatusUpdate ret(statusUpdate.executionId,
- statusUpdate.usedParameterization);
- ret.data = mainRet.data;
+ TerminatedSkillStatusUpdate ret{
+ {.executionId = statusUpdate.executionId,
+ .parameters = statusUpdate.parameters,
+ .callbackInterface = statusUpdate.callbackInterface}};
+ ret.result = mainRet.data;
ret.status = TerminatedSkillStatus::Succeeded;
return ret;
}
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index 45108724d3ce8ec2bf1e9e1559ebb9be5142fdb6..e5a5b1f82c3db4e5939fd4c880a738373967402b 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -15,10 +15,10 @@ namespace armarx
{
namespace detail
{
- class SkillImplementationWrapper
+ class SkillRuntime
{
private:
- const skills::SkillFactory& factory;
+ const skills::SkillBlueprint factory;
std::unique_ptr<Skill> skill;
mutable std::mutex executionMutex;
@@ -29,10 +29,14 @@ namespace armarx
mutable std::mutex skillStatusesMutex;
SkillStatusUpdate statusUpdate;
+ // Set exteranally to store the execution somewhere
+ std::thread execution;
+
// ctor
- SkillImplementationWrapper(const skills::SkillFactory& fac,
- const skills::SkillExecutionID&,
- const skills::SkillParameterization&);
+ SkillRuntime(const skills::SkillBlueprint& fac,
+ const skills::SkillExecutionID&,
+ const aron::data::DictPtr&,
+ const skills::callback::dti::SkillProviderCallbackInterfacePrx&);
// execute a skill. The parameterization is copied. T
// the return type additionally contains the input configuration (similar to the status updates used in callbacks)
@@ -42,7 +46,7 @@ namespace armarx
void stopSkill();
// add parameters to a skill. Must be concurrent to execute skill
- void addSkillParameters(const aron::data::DictPtr& i);
+ void updateSkillParameters(const aron::data::DictPtr& i);
};
} // namespace detail
} // namespace skills
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
index 562ddabd56b6d07428f8ce2b386fa682ac95dd1f..56dc86697810e10870f6b409b3f6f810ee9b0729 100644
--- a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
@@ -3,30 +3,34 @@
//#include <ArmarXCore/core/time/TimeUtil.h>
//#include <ArmarXCore/observers/variant/DatafieldRef.h>
-#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
#include "ObjectMemoryGroupStatechartContext.generated.h"
namespace armarx::ObjectMemoryGroup
{
// DO NOT EDIT NEXT LINE
- RequestObjects::SubClassRegistry RequestObjects::Registry(RequestObjects::GetName(), &RequestObjects::CreateInstance);
+ RequestObjects::SubClassRegistry RequestObjects::Registry(RequestObjects::GetName(),
+ &RequestObjects::CreateInstance);
- RequestObjects::RequestObjects(const XMLStateConstructorParams& stateData):
- XMLStateTemplate<RequestObjects>(stateData), RequestObjectsGeneratedBase<RequestObjects>(stateData)
+ RequestObjects::RequestObjects(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<RequestObjects>(stateData),
+ RequestObjectsGeneratedBase<RequestObjects>(stateData)
{
}
- void RequestObjects::onEnter()
+ void
+ RequestObjects::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
}
- void RequestObjects::run()
+ void
+ RequestObjects::run()
{
if (not in.getEnable())
{
@@ -39,7 +43,8 @@ namespace armarx::ObjectMemoryGroup
const std::string provider = in.isProviderSet() ? in.getProvider() : "";
const std::vector<std::string> objectIdsString = in.getObjectIds();
- const armarx::Duration relativeTimeout = armarx::Duration::MilliSeconds(in.getRelativeTimeoutMilliseconds());
+ const armarx::Duration relativeTimeout =
+ armarx::Duration::MilliSeconds(in.getRelativeTimeoutMilliseconds());
std::stringstream info;
std::stringstream warn;
@@ -49,21 +54,22 @@ namespace armarx::ObjectMemoryGroup
{
try
{
- armarx::ObjectID id = armarx::ObjectID::FromString(idString);
- objectIds.push_back(id);
- info << "Requesting object " << id << "\n";
+ armarx::ObjectID id = armarx::ObjectID::FromString(idString);
+ objectIds.push_back(id);
+ info << "Requesting object " << id << "\n";
}
catch (const armarx::LocalException& e)
{
- warn << "\nGiven object ID '" << idString << "' could not parsed as ObjectID: " << e.what();
+ warn << "\nGiven object ID '" << idString
+ << "' could not parsed as ObjectID: " << e.what();
}
}
auto context = getContext<ObjectMemoryGroupStatechartContext>();
armarx::armem::client::MemoryNameSystem mns(getMemoryNameSystem(), context);
- Reader reader{mns};
- reader.connect();
+ Reader reader;
+ reader.connect(mns);
objpose::ObjectPoseStorageInterfacePrx storage = reader.getObjectPoseStorage();
armarx::objpose::observer::RequestObjectsInput input;
@@ -91,7 +97,8 @@ namespace armarx::ObjectMemoryGroup
{
if (result.result.success)
{
- info << "Requested object " << id << " via provider '" << result.providerName << "'.\n";
+ info << "Requested object " << id << " via provider '" << result.providerName
+ << "'.\n";
}
else
{
@@ -105,8 +112,9 @@ namespace armarx::ObjectMemoryGroup
}
if (not warn.str().empty())
{
- ARMARX_WARNING << "The following issues occurred whhen requesting objects for localization:"
- << warn.str();
+ ARMARX_WARNING
+ << "The following issues occurred whhen requesting objects for localization:"
+ << warn.str();
}
emitSuccess();
@@ -118,16 +126,17 @@ namespace armarx::ObjectMemoryGroup
// // execution time should be short (<100ms)
//}
- void RequestObjects::onExit()
+ void
+ RequestObjects::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
-
// DO NOT EDIT NEXT FUNCTION
- XMLStateFactoryBasePtr RequestObjects::CreateInstance(XMLStateConstructorParams stateData)
+ XMLStateFactoryBasePtr
+ RequestObjects::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new RequestObjects(stateData));
}
-}
+} // namespace armarx::ObjectMemoryGroup