diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
new file mode 100644
index 0000000000000000000000000000000000000000..0cb6e600c738cb498050b31f5b54d5ac5827190d
--- /dev/null
+++ b/.gitlab-ci.yml
@@ -0,0 +1,170 @@
+stages:
+ - build-and-test
+ - deploy
+ - post-deploy-test
+
+
+.build-and-test:
+
+ cache:
+ # https://docs.gitlab.com/ee/ci/caching/#share-caches-across-jobs-in-different-branches
+ key: one-key-to-rule-them-all
+ paths:
+ - .apt
+ - .ccache
+
+ before_script:
+ # Apt cache configuration.
+ - rm -rf /var/cache/apt/archives || true
+ - rm -f /etc/apt/apt.conf.d/docker-clean # Remove docker-clean script to avoid cache deletion.
+ - mkdir .apt || true
+ - ln -s "$CI_PROJECT_DIR/.apt" /var/cache/apt/archives
+
+ # Update apt info.
+ - apt-get update
+
+ # Ccache configuration and introspection.
+ - apt-get install ccache --yes
+ - ccache --set-config=cache_dir="$CI_PROJECT_DIR/.ccache"
+ - ccache --max-size=20G
+ - ccache --show-stats
+
+ # Activate Axii.
+ - source /axii/scripts/install_axii.sh
+ - _axii_auto_env_refresh
+
+ script:
+ - echo "ArmarX Workspace = '$ARMARX_WORKSPACE'"
+
+ # Use workspace configuration from project.
+ - cp "$CI_PROJECT_DIR/.gitlab/ci/armarx-workspace.json" "$ARMARX_WORKSPACE/armarx-workspace.json"
+ - cat "$ARMARX_WORKSPACE/armarx-workspace.json"
+
+ - axii workspace env
+ - _axii_auto_env_refresh
+
+ - echo "Workspace information:"
+ - axii workspace list-modules
+ - axii workspace list-modules --deps
+ - axii workspace info
+
+ - export PROJECT_MODULE="armarx/RobotAPI"
+ - export PROJECT_PATH_IN_WORKSPACE="$armarx__RobotAPI__PATH"
+
+ # Symlink project directory into Axii workspace.
+ - mkdir -p "$(dirname $PROJECT_PATH_IN_WORKSPACE)"
+ - ln -s "$CI_PROJECT_DIR" "$PROJECT_PATH_IN_WORKSPACE"
+
+ # Fix "CMake Error in CMakeLists.txt: Imported target "VirtualRobot" includes non-existent path "/usr/lib/include"
+ # (caused by at least dmp)
+ - mkdir -p /usr/lib/include
+
+ # Upgrade.
+ - axii workspace system --accept-apt-install
+ - axii workspace update --prefer-https
+ - axii workspace upgrade -m "$PROJECT_MODULE"
+
+ - ccache --show-stats
+
+ # Test.
+ # ToDo: Add and use `axii ws test -m "$PROJECT_MODULE"`
+ - cd "$PROJECT_PATH_IN_WORKSPACE/build"
+ - ctest --output-on-failure --output-junit "$CI_PROJECT_DIR/report.xml" .
+
+ artifacts:
+ reports:
+ junit: report.xml
+
+
+build-and-test-bionic:
+ stage: build-and-test
+ extends: .build-and-test
+
+ 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/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/.gitlab/ci/armarx-workspace.json b/.gitlab/ci/armarx-workspace.json
new file mode 100644
index 0000000000000000000000000000000000000000..ae5ce8a95c44efa6def8c0525c11708a114a1429
--- /dev/null
+++ b/.gitlab/ci/armarx-workspace.json
@@ -0,0 +1,17 @@
+{
+ "modules": {
+ "tools/ccache/default": {},
+ "armarx/RobotAPI": {}
+ },
+ "global": {
+ "prepare": {
+ "cmake": {
+ "definitions": {
+ "CMAKE_BUILD_TYPE": "RelWithDebInfo",
+ "CMAKE_C_COMPILER_LAUNCHER": "$CCACHE",
+ "CMAKE_CXX_COMPILER_LAUNCHER": "$CCACHE"
+ }
+ }
+ }
+ }
+}
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/scenarios/RobotHealthTest/RobotHealthTest.scx b/scenarios/RobotHealthTest/RobotHealthTest.scx
index c0dde0a3b95ecb6a6155a07f818b0343badd6e6d..68dad197bc70f86a5cdef0161121f48d0d312800 100644
--- a/scenarios/RobotHealthTest/RobotHealthTest.scx
+++ b/scenarios/RobotHealthTest/RobotHealthTest.scx
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
<scenario name="RobotHealthTest" creation="2018-11-30.11:42:03" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
- <application name="RobotHealthApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotHealthApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="RobotHealthDummyApp" instance="HealthDummy1" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotHealthDummyApp" instance="HealthDummy2" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/RobotHealthTest/config/RobotHealthApp.cfg b/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
index b91501c1a405672cec4ce8b06ea9bcf83f758c4b..ebffea693fd85455a4f16d02ab490a8cad798bc3 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
@@ -109,20 +109,12 @@
# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.RobotHealth.AggregatedRobotHealthTopicName: Name of the AggregatedRobotHealthTopic
+# ArmarX.RobotHealth.DebugObserverTopicName: Name of the topic the DebugObserver listens on
# Attributes:
-# - Default: AggregatedRobotHealthTopic
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.AggregatedRobotHealthTopicName = AggregatedRobotHealthTopic
-
-
-# ArmarX.RobotHealth.EmergencyStopTopicName: The name of the topic over which changes of the emergencyStopState are sent.
-# Attributes:
-# - Default: EmergencyStop
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotHealth.EmergencyStopTopicName = EmergencyStop
+# ArmarX.RobotHealth.DebugObserverTopicName = DebugObserver
# ArmarX.RobotHealth.EnableProfiling: enable profiler which is used for logging performance events
@@ -159,37 +151,36 @@
# ArmarX.RobotHealth.MinimumLoggingLevel = Undefined
-# ArmarX.RobotHealth.ObjectName: Name of IceGrid well-known object
+# ArmarX.RobotHealth.Name of the AggregatedRobotHealthTopic: Name of the `AggregatedRobotHealth` topic to publish data to.
# Attributes:
-# - Default: ""
+# - Default: AggregatedRobotHealthTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.ObjectName = ""
+# ArmarX.RobotHealth.Name of the AggregatedRobotHealthTopic = AggregatedRobotHealthTopic
-# ArmarX.RobotHealth.ReportErrorsWithSpeech:
+# ArmarX.RobotHealth.ObjectName: Name of IceGrid well-known object
# Attributes:
-# - Default: true
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RobotHealth.ReportErrorsWithSpeech = true
+# ArmarX.RobotHealth.ObjectName = ""
-# ArmarX.RobotHealth.RequiredComponents: Comma separated list of required components
+# ArmarX.RobotHealth.RequiredTags: Tags that should be requested.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.RequiredComponents = ""
+# ArmarX.RobotHealth.RequiredTags = ""
-# ArmarX.RobotHealth.RobotHealthTopicName: Name of the RobotHealth topic
+# ArmarX.RobotHealth.RobotHealthTopic: Name of the RobotHealth topic
# Attributes:
# - Default: RobotHealthTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.RobotHealthTopicName = RobotHealthTopic
+# ArmarX.RobotHealth.RobotHealthTopic = RobotHealthTopic
# ArmarX.RobotHealth.RobotUnitName: No Description
@@ -208,20 +199,12 @@ ArmarX.RobotHealth.RobotUnitName = RobotUnitSimulation
ArmarX.RobotHealth.RobotUnitRequired = 0
-# ArmarX.RobotHealth.SpeechMinimumReportInterval: Time that has to pass between reported messages in seconds.
+# ArmarX.RobotHealth.The name of the topic over which changes of the emergencyStopState are sent.: Name of the `EmergencyStop` topic to publish data to.
# Attributes:
-# - Default: 60
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotHealth.SpeechMinimumReportInterval = 60
-
-
-# ArmarX.RobotHealth.TextToSpeechTopicName: Name of the TextToSpeech topic
-# Attributes:
-# - Default: TextToSpeech
+# - Default: EmergencyStop
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.TextToSpeechTopicName = TextToSpeech
+# ArmarX.RobotHealth.The name of the topic over which changes of the emergencyStopState are sent. = EmergencyStop
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
diff --git a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
index 5df0ff4cfd0fd82e8c0c7b9155be4e797fd14959..0f14e83d287e44accc961037ba069f3ea77dce46 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
index 60d2ee3f291cbdb09a97eb89500b1fd7426e4a72..e157c2704c9191a48e3daccf14a545f125e72c51 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
@@ -148,7 +148,7 @@ ArmarX.RobotHealthDummy.ObjectName = HealthDummy2
# - Default: nanosleep
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealthDummy.SleepMode = nanosleep
+ArmarX.RobotHealthDummy.SleepMode = std::this_thread::sleep_for
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
diff --git a/scenarios/SkillProviderTest/SkillProviderTest.scx b/scenarios/SkillProviderTest/SkillProviderTest.scx
index aa275e46094978c888fe4732db5b70eca27052ac..7e5aa06a7ce69f63fcc6fc606f55d17b704b82a1 100644
--- a/scenarios/SkillProviderTest/SkillProviderTest.scx
+++ b/scenarios/SkillProviderTest/SkillProviderTest.scx
@@ -2,5 +2,9 @@
<scenario name="SkillProviderTest" creation="2022-06-02.15:33:15" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
<application name="SkillProviderExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SkillsMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/SkillProviderTest/config/ArVizStorage.cfg b/scenarios/SkillProviderTest/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..302ac28c37dd28de3e68fb4fe4c2174faa4ec3bf
--- /dev/null
+++ b/scenarios/SkillProviderTest/config/ArVizStorage.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# ArVizStorage properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.ArVizStorage.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArVizStorage.EnableProfiling = false
+
+
+# ArmarX.ArVizStorage.HistoryPath: Destination path where the history is serialized to
+# Attributes:
+# - Default: RobotAPI/ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.HistoryPath = RobotAPI/ArVizStorage
+
+
+# ArmarX.ArVizStorage.MaxHistorySize: How many layer updates are saved in the history until they are compressed
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.MaxHistorySize = 1000
+
+
+# ArmarX.ArVizStorage.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ArVizStorage.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArVizStorage.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.ObjectName = ""
+
+
+# ArmarX.ArVizStorage.TopicName: Layer updates are sent over this topic.
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.TopicName = ArVizTopic
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/SkillProviderTest/config/DebugObserver.cfg b/scenarios/SkillProviderTest/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..8dc7ead26b3bd2f7678b3b3e7a1b00c01213225d
--- /dev/null
+++ b/scenarios/SkillProviderTest/config/DebugObserver.cfg
@@ -0,0 +1,221 @@
+# ==================================================================
+# DebugObserver properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.CreateUpdateFrequenciesChannel = false
+
+
+# ArmarX.DebugObserver.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.DebugObserver.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DebugObserver.EnableProfiling = false
+
+
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.DebugObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.MaxHistorySize = 5000
+
+
+# ArmarX.DebugObserver.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.DebugObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.DebugObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DebugObserver.ObjectName = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/SkillProviderTest/config/MemoryNameSystem.cfg b/scenarios/SkillProviderTest/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b8bc70a66ca7f32a628886ad1bf13e373f9750d3
--- /dev/null
+++ b/scenarios/SkillProviderTest/config/MemoryNameSystem.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# MemoryNameSystem properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.MemoryNameSystem.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MemoryNameSystem.EnableProfiling = false
+
+
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryNameSystem.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.ObjectName = ""
+
+
+# ArmarX.MemoryNameSystem.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/SkillProviderTest/config/RemoteGuiProviderApp.cfg b/scenarios/SkillProviderTest/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4b6abea40d72afd7d313ee47a9b191f3b26de30d
--- /dev/null
+++ b/scenarios/SkillProviderTest/config/RemoteGuiProviderApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# RemoteGuiProviderApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteGuiProvider.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RemoteGuiProvider.EnableProfiling = false
+
+
+# ArmarX.RemoteGuiProvider.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.RemoteGuiProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RemoteGuiProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.ObjectName = ""
+
+
+# ArmarX.RemoteGuiProvider.TopicName: Name of the topic on which updates to the remote state are reported.
+# Attributes:
+# - Default: RemoteGuiTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.TopicName = RemoteGuiTopic
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/SkillProviderTest/config/SkillsMemory.cfg b/scenarios/SkillProviderTest/config/SkillsMemory.cfg
index 57d1fbc563156ae096bdaad0324321323a1a0724..fc086fbe56a016013571883b41f8f8fb721f55b6 100644
--- a/scenarios/SkillProviderTest/config/SkillsMemory.cfg
+++ b/scenarios/SkillProviderTest/config/SkillsMemory.cfg
@@ -18,6 +18,15 @@
# ArmarX.ApplicationName = ""
+# ArmarX.AutodiscoverPackages: If enabled, will discover all ArmarX packages based on the environment variables. Otherwise, the `DefaultPackages` and `AdditionalPackages` properties are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AutodiscoverPackages = true
+
+
# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
@@ -175,21 +184,13 @@
# ArmarX.SkillMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
-# ArmarX.SkillMemory.mem.ltm.enable_querying:
+# ArmarX.SkillMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.enable_querying = false
-
-
-# ArmarX.SkillMemory.mem.ltm.mode:
-# Attributes:
-# - Default: DISABLED
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.mode = DISABLED
+# ArmarX.SkillMemory.mem.ltm.enabled = false
# ArmarX.SkillMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 28c1a21d9e549011313e6e0d36368cd5c7edee23..9638058676450ed6fcb7d8ff884cf7ebf8beb67e 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -142,7 +142,7 @@ set(HEADERS
ArVizStorage.h
)
-armarx_add_component_executable("${SOURCES}" "${HEADERS}")
+armarx_add_component_executable("${SOURCES};${HEADERS}" "${HEADERS}")
add_subdirectory(Example)
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index a01c4b37c08698a77fea6588d46963fee6e6c6df..12dc68399fc92b3d32816dd212d75fc4b0cdab8c 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -1,28 +1,29 @@
-#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())
{
ARMARX_INFO << deactivateSpam() << "No filename provided for robot.";
- return result;
+ return nullptr;
}
std::string fullFilename;
@@ -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
@@ -41,7 +43,7 @@ namespace armarx::viz::coin
{
ARMARX_INFO << deactivateSpam()
<< "Unable to find readable file for name: " << filename;
- return result;
+ return nullptr;
}
try
@@ -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;
}
@@ -226,15 +239,21 @@ namespace armarx::viz::coin
// robot.setGlobalPose(pose, false);
// Check joint values for changes
- bool jointValuesChanged = false;
- for (auto& pair : element.jointValues)
+
+ for (const auto& pair : element.jointValues)
{
std::string const& nodeName = pair.first;
float newJointValue = pair.second;
VirtualRobot::RobotNodePtr robotNode = robot.getRobotNode(nodeName);
- float oldJointValue = robotNode->getJointValue();
- float diff = std::abs(newJointValue - oldJointValue);
- jointValuesChanged = diff > 0.001f;
+
+ if (robotNode == nullptr)
+ {
+ continue;
+ }
+
+ const float oldJointValue = robotNode->getJointValue();
+ const float diff = std::abs(newJointValue - oldJointValue);
+ const bool jointValuesChanged = diff > 0.001f;
if (jointValuesChanged)
{
// Only set the joint values if they changed
@@ -247,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++)
@@ -288,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)
@@ -325,8 +343,9 @@ namespace armarx::viz::coin
}
}
- void clearRobotCache()
+ void
+ clearRobotCache()
{
robotCache.clear();
}
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
index e028bc55648f02446b5980cc00124950d4564219..34894b6ea9c449d13f47af00d2c012a47f764f1d 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
@@ -80,7 +80,7 @@ namespace armarx
}
void
- RobotHealthDummy::busydwait(long microseconds)
+ RobotHealthDummy::busywait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
@@ -95,51 +95,60 @@ namespace armarx
{
auto args = RobotHealthHeartbeatArgs();
args.identifier = getName();
+ armarx::core::time::toIce(args.maximumCycleTimeError, armarx::core::time::Duration::MilliSeconds(1000));
+ armarx::core::time::toIce(args.maximumCycleTimeWarning, armarx::core::time::Duration::MilliSeconds(500));
robotHealthTopicPrx->signUp(args);
ARMARX_INFO << "starting rinning task";
while (!dummyTask->isStopped())
{
- long beforeTopicCall = TimeUtil::GetTime().toMicroSeconds();
+ auto beforeTopicCall = armarx::core::time::DateTime::Now();
//ARMARX_INFO << "send heartbeat";
armarx::core::time::dto::DateTime now;
armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
robotHealthTopicPrx->heartbeat(getName(), now);
- long afterTopicCall = TimeUtil::GetTime().toMicroSeconds();
+ auto afterTopicCall = armarx::core::time::DateTime::Now();
+
if (sleepmode == "nanosleep")
{
- NanoSleep(10 * 1000 * 1000);
+ NanoSleep(10 * 1000 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "sleepwait")
{
- sleepwait(10 * 1000);
+ sleepwait(10 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "yieldwait")
{
- yieldwait(10 * 1000);
+ yieldwait(10 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "busywait")
{
- busydwait(10 * 1000);
+ busywait(10 * 1000); // wait 10 milliseconds
+ }
+ else if (sleepmode == "std::this_thread::sleep_for")
+ {
+ std::this_thread::sleep_for(std::chrono::microseconds(10 * 1000));
}
else
{
throw LocalException("Unknown sleepmode.");
}
- long afterSleep = TimeUtil::GetTime().toMicroSeconds();
- long topicCallDelay = afterTopicCall - beforeTopicCall;
- long sleepDelay = afterSleep - afterTopicCall;
- if (sleepDelay > 20000)
+ auto afterSleep = armarx::core::time::DateTime::Now();
+ auto topicCallDelay = afterTopicCall - beforeTopicCall;
+ auto sleepDelay = afterSleep - afterTopicCall;
+ if (sleepDelay.toMicroSeconds() > 20000)
{
- ARMARX_IMPORTANT << sleepmode << ": " << sleepDelay << "us";
+ ARMARX_IMPORTANT << sleepmode << " took long: " << sleepDelay << "us";
}
- if (topicCallDelay > 1000)
+ if (topicCallDelay.toMicroSeconds() > 1000)
{
- ARMARX_IMPORTANT << "topic: " << topicCallDelay << "us";
+ ARMARX_IMPORTANT << "topic took long: " << topicCallDelay << "us";
}
}
+
+ robotHealthTopicPrx->unregister(args.identifier);
}
void
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
index cfd8d369d0335768c2e85dd735f1d31d1e30e60d..a841720145b96f925c427443f3998475620509bb 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
@@ -78,7 +78,7 @@ namespace armarx
int NanoSleep(long nanosec);
void yieldwait(long microseconds);
- void busydwait(long microseconds);
+ void busywait(long microseconds);
void sleepwait(long microseconds);
protected:
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index e7fdc5eaa92b960a964d78deecce66dfd242227f..7fea318b5249b557e92cb1e207f3f04e0de70321 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -205,6 +205,8 @@ namespace armarx
return armem::MemoryID(result.segmentID);
}
+ // COMMIT
+
armem::MemoryID
ExampleMemoryClient::commitSingleSnapshot(const armem::MemoryID& entityID)
{
@@ -272,10 +274,12 @@ namespace armarx
}
}
+ // QUERY
+
void
ExampleMemoryClient::queryLatestSnapshot(const armem::MemoryID& entityID)
{
- ARMARX_IMPORTANT << "Querying latest snapshot: "
+ ARMARX_IMPORTANT << "Querying latest snapshot in entity: "
<< "\n- entityID: \t'" << entityID << "'";
armem::client::query::Builder builder;
@@ -295,6 +299,8 @@ namespace armarx
ARMARX_IMPORTANT << "Getting entity via ID";
armem::wm::Memory& memory = qResult.memory;
+ ARMARX_CHECK(memory.hasInstances());
+
ARMARX_CHECK_GREATER_EQUAL(memory.size(), 1);
const armem::wm::Entity* entity = memory.findEntity(entityID);
@@ -335,13 +341,23 @@ namespace armarx
if (qResult.success)
{
armem::wm::Memory memory = std::move(qResult.memory);
- const armem::wm::EntitySnapshot& entitySnapshot =
- memory.getEntity(snapshotID).getLatestSnapshot();
+ {
+ const armem::wm::EntitySnapshot& entitySnapshot = memory.getLatestSnapshot();
- ARMARX_INFO << "Result snapshot: "
- << "\n- time: \t" << entitySnapshot.time() << "\n- # instances: \t"
- << entitySnapshot.size();
+ ARMARX_INFO << "Result snapshot: "
+ << "\n- time: \t" << entitySnapshot.time()
+ << "\n- # instances: \t" << entitySnapshot.size();
+ }
+ {
+ const armem::wm::EntitySnapshot& entitySnapshot =
+ memory.getEntity(snapshotID).getLatestSnapshot();
+
+ ARMARX_INFO << "Result snapshot: "
+ << "\n- time: \t" << entitySnapshot.time()
+ << "\n- # instances: \t" << entitySnapshot.size();
+ }
}
+
else
{
ARMARX_ERROR << qResult.errorMessage;
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 75882cf47a238bca9e67a1131829f75c20869ea2..d77d737156239241ed8a62952e788da11c99bb55 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -27,19 +27,19 @@
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
namespace armarx
{
- SkillsMemory::SkillsMemory():
+ SkillsMemory::SkillsMemory() :
ReadWritePluginUser(),
StatechartListenerComponentPluginUser(),
SkillManagerComponentPluginUser(),
@@ -50,10 +50,11 @@ namespace armarx
{
}
-
- armarx::PropertyDefinitionsPtr SkillsMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ SkillsMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
const std::string prefix = "mem.";
statechartListenerProviderSegment.defineProperties(defs, prefix + "statechartlistener.");
@@ -64,14 +65,14 @@ namespace armarx
return defs;
}
-
- std::string SkillsMemory::getDefaultName() const
+ std::string
+ SkillsMemory::getDefaultName() const
{
return "SkillMemory";
}
-
- void SkillsMemory::onInitComponent()
+ void
+ SkillsMemory::onInitComponent()
{
statechartListenerProviderSegment.init();
executableSkillCoreSegment.init();
@@ -79,24 +80,23 @@ namespace armarx
skillEventCoreSegment.init();
}
-
- void SkillsMemory::onConnectComponent()
+ void
+ SkillsMemory::onConnectComponent()
{
}
-
- void SkillsMemory::onDisconnectComponent()
+ void
+ SkillsMemory::onDisconnectComponent()
{
-
}
-
- void SkillsMemory::onExitComponent()
+ void
+ SkillsMemory::onExitComponent()
{
}
-
- armem::data::CommitResult SkillsMemory::commit(const armem::data::Commit& commit, const Ice::Current& current)
+ armem::data::CommitResult
+ SkillsMemory::commit(const armem::data::Commit& commit, const Ice::Current& current)
{
// This function is overloaded to check for skill executions
// First pass arg to parent func to ensure that data is written into memory and that clients are notified
@@ -107,14 +107,16 @@ namespace armarx
for (const auto& up : commit.updates)
{
- if (up.entityID.coreSegmentName == skills::segment::SkillExecutionRequestCoreSegment::CoreSegmentName)
+ if (up.entityID.coreSegmentName ==
+ skills::segment::SkillExecutionRequestCoreSegment::CoreSegmentName)
{
for (const auto& instance : up.instancesData)
{
ARMARX_CHECK_NOT_NULL(instance);
- skills::manager::dto::SkillExecutionRequest exInfo = skillExecutionRequestCoreSegment.convertCommit(instance);
- SkillManagerComponentPluginUser::executeSkill(exInfo, current);
+ skills::SkillExecutionRequest exInfo =
+ skillExecutionRequestCoreSegment.convertCommit(instance);
+ SkillManagerComponentPluginUser::executeSkill(exInfo.toManagerIce(), current);
}
}
}
@@ -122,52 +124,99 @@ namespace armarx
return result;
}
- void SkillsMemory::addProvider(const skills::manager::dto::ProviderInfo& info, const Ice::Current ¤t)
+ void
+ SkillsMemory::addProvider(const skills::manager::dto::ProviderInfo& info,
+ const Ice::Current& current)
{
+ ARMARX_INFO << "Adding provider to skill memory: " << info.providerId.providerName;
SkillManagerComponentPluginUser::addProvider(info, current);
// log skills to memory
- executableSkillCoreSegment.addSkillProvider(info);
+ auto p = skills::ProviderInfo::FromIce(info);
+ executableSkillCoreSegment.addSkillProvider(p);
}
- void SkillsMemory::removeProvider(const std::string& skillProviderName, const Ice::Current ¤t)
+ void
+ SkillsMemory::removeProvider(const skills::manager::dto::ProviderID& provider,
+ const Ice::Current& current)
{
- executableSkillCoreSegment.removeSkillProvider(skillProviderName);
+ executableSkillCoreSegment.removeSkillProvider(provider.providerName);
// remove skills from memory
- SkillManagerComponentPluginUser::removeProvider(skillProviderName, current);
+ SkillManagerComponentPluginUser::removeProvider(provider, current);
}
- skills::provider::dto::SkillStatusUpdate SkillsMemory::executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current ¤t)
+ skills::manager::dto::SkillStatusUpdate
+ SkillsMemory::executeSkill(const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current)
{
- skills::manager::dto::SkillExecutionRequest requestCopy = info;
- if (requestCopy.skillId.providerName == "*")
- {
- // sanitize the provider name if set to 'any'
- requestCopy.skillId.providerName = getFirstProviderNameThatHasSkill(requestCopy.skillId.skillName);
- }
+ auto e = skills::SkillExecutionRequest::FromIce(info);
+ skillExecutionRequestCoreSegment.addSkillExecutionRequest(e);
- skillExecutionRequestCoreSegment.addSkillExecutionRequest(requestCopy);
- return SkillManagerComponentPluginUser::executeSkill(requestCopy, current);
+ return SkillManagerComponentPluginUser::executeSkill(info, current);
}
- void SkillsMemory::updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update, const Ice::Current ¤t)
+ skills::manager::dto::SkillExecutionID
+ SkillsMemory::executeSkillAsync(const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current)
{
- skillEventCoreSegment.addSkillUpdateEvent(update);
+ 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,
+ const Ice::Current& current)
+ {
+ auto p = skills::ProviderID::FromIce(providerId);
+ auto u = skills::SkillStatusUpdate::FromIce(update, p);
+ 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::getSkillExecutionStatuses(const Ice::Current& current)
+ {
+ skills::manager::dto::SkillStatusUpdateMap ret;
+ auto updates = this->skillEventCoreSegment.getSkillStatusUpdates();
+ for (const auto& [k, v] : updates)
+ {
+ ret.insert({k.toManagerIce(), v.toManagerIce()});
+ }
+ return ret;
+ }
/*
* Statechart stuff
*/
- void SkillsMemory::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& x, const Ice::Current&)
+ void
+ SkillsMemory::reportStatechartTransitionWithParameters(
+ const ProfilerStatechartTransitionWithParameters& x,
+ const Ice::Current&)
{
statechartListenerProviderSegment.reportStatechartTransitionWithParameters(x);
}
- void SkillsMemory::reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList& x, const Ice::Current&)
+ void
+ SkillsMemory::reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList& x,
+ const Ice::Current&)
{
statechartListenerProviderSegment.reportStatechartTransitionWithParametersList(x);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
index 91506c16fb87793083328d173d973f19ad268e75..ead9f19122bc80266196aff40f138b4bd87c77f1 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.h
@@ -30,16 +30,14 @@
#include <ArmarXCore/observers/ObserverObjectFactories.h>
#include <RobotAPI/interface/skills/SkillMemoryInterface.h>
-
-#include <RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h>
-#include <RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-
#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
-#include <RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.h>
+#include <RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.h>
#include <RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h>
-#include <RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h>
#include <RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h>
+#include <RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h>
+#include <RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.h>
+#include <RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h>
namespace armarx
{
@@ -63,27 +61,51 @@ namespace armarx
virtual public SkillManagerComponentPluginUser
{
public:
-
SkillsMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
// Override StatechartListener
- void reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&, const Ice::Current&) override;
- void reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList&, const Ice::Current&) override;
+ void
+ reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&,
+ const Ice::Current&) override;
+ void reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList&,
+ const Ice::Current&) override;
// Override SkillManager to add memory functions
- void addProvider(const skills::manager::dto::ProviderInfo& info, const Ice::Current ¤t) override;
- void removeProvider(const std::string&, const Ice::Current ¤t) override;
- skills::provider::dto::SkillStatusUpdate executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current ¤t) override;
- void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& statusUpdate, const Ice::Current ¤t) override;
+ void addProvider(const skills::manager::dto::ProviderInfo& info,
+ const Ice::Current& current) override;
+
+ void removeProvider(const skills::manager::dto::ProviderID& provider,
+ const Ice::Current& current) override;
+
+ skills::manager::dto::SkillStatusUpdate
+ 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
+ getSkillExecutionStatuses(const Ice::Current& current) override;
// WritingInterface interface
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+ armem::data::CommitResult commit(const armem::data::Commit& commit,
+ const Ice::Current&) override;
protected:
-
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -101,12 +123,12 @@ namespace armarx
private:
-
static constexpr const char* MemoryName = "Skill";
struct Properties
{
};
+
Properties p;
skills::segment::StatechartListenerProviderSegment statechartListenerProviderSegment;
@@ -114,4 +136,4 @@ namespace armarx
skills::segment::SkillEventCoreSegment skillEventCoreSegment;
skills::segment::SkillExecutionRequestCoreSegment skillExecutionRequestCoreSegment;
};
-}
+} // namespace armarx
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..d57f66c3a0e682d382dc91052c9a45b7acb639e7
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
@@ -0,0 +1,46 @@
+
+
+#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_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 65b94a3807343d850003a53bbdbabc438a2d4bcc..5ab480cd5c05556e2c7ea486ac0ec3976b1eb6df 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -1,140 +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 default_params;
- default_params.some_float = 5;
- default_params.some_int = 42;
- default_params.some_text = "YOLO";
- default_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",
- {},
- armarx::core::time::Duration::MilliSeconds(1000),
- armarx::skills::Example::HelloWorldAcceptedType::ToAronType(),
- default_params.toAron()};
- }
-
- Skill::MainResult
- HelloWorldSkill::main(const MainInput& in)
- {
- ARMARX_IMPORTANT << "Hi, from the Hello World Skill.\n"
- << "I received the following data: \n"
- << aron::data::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(
- in.params)
- .dump(2)
- << "\n"
- << "(executed at: " << IceUtil::Time::now() << ")";
- return {TerminatedSkillStatus::Succeeded, nullptr};
- }
-
- 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(const MainInput& in)
- {
- 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, {"SkillProviderExample", "HelloWorld"});
-
- skillExecPrx.executeFullSkill(getSkillId().toString(), exec1.toAron());
- skillExecPrx.executeFullSkill(getSkillId().toString(), exec2.toAron());
- skillExecPrx.executeFullSkill(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(1000),
- nullptr};
- }
-
- PeriodicSkill::StepResult
- TimeoutSkill::step(const MainInput& in)
- {
- // do heavy work
- std::this_thread::sleep_for(std::chrono::milliseconds(200));
-
- 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(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(up1);
-
- auto up2 = std::make_shared<aron::data::Dict>();
- up2->addElement("updateInfo", std::make_shared<aron::data::String>("Update 2"));
- in.callback(up2);
-
- auto up3 = std::make_shared<aron::data::Dict>();
- up3->addElement("updateInfo", std::make_shared<aron::data::String>("Update 3"));
- in.callback(up3);
-
- return {TerminatedSkillStatus::Succeeded, nullptr};
- }
-
SkillProviderExample::SkillProviderExample() : SkillProviderComponentPluginUser()
{
}
@@ -157,51 +25,38 @@ namespace armarx::skills::provider
SkillProviderExample::onInitComponent()
{
// Add example skill
- addSkill(std::make_unique<HelloWorldSkill>());
-
- // Add another lambda example skill
- {
- skills::SkillDescription fooDesc;
- fooDesc.acceptedType = nullptr; // accept everything
- fooDesc.description = "This skill does exactly nothing.";
- fooDesc.skillName = "Foo";
- fooDesc.timeout = armarx::core::time::Duration::MilliSeconds(1000);
- addSkill(
- [](const std::string& clientId, const aron::data::DictPtr&)
- {
- std::cout << "Hello from Foo. The skill was called from " << clientId << "."
- << std::endl;
- return TerminatedSkillStatus::Succeeded;
- },
- fooDesc);
- }
+ ARMARX_INFO << "Adding skill HelloWorldSkill";
+ addSkillFactory<HelloWorldSkill>();
// Add another lambda example skill
{
- skills::SkillDescription fooDesc;
- fooDesc.acceptedType = nullptr; // accept everything
- fooDesc.description = "This skill dies hard.";
- fooDesc.skillName = "Die";
- fooDesc.timeout = armarx::core::time::Duration::MilliSeconds(1000);
- addSkill(
- [](const std::string& clientId, const aron::data::DictPtr&)
- {
- std::cout << "bye bye... segfaulting on purpose now!" << std::endl;
- Skill* nullSkill = NULL;
- nullSkill->getSkillId(); // DEAD!
- return TerminatedSkillStatus::Succeeded;
- },
- fooDesc);
+ skills::SkillDescription fooDesc{.skillId = SkillID{.skillName = "Foo"},
+ .description = "This skill does exactly nothing.",
+ .timeout =
+ armarx::core::time::Duration::MilliSeconds(1000)};
+ addSkillFactory(fooDesc,
+ []()
+ {
+ std::cout << "Hello from Foo." << std::endl;
+ return TerminatedSkillStatus::Succeeded;
+ });
}
// Add another example skill
- addSkill(std::make_unique<CallbackSkill>());
+ ARMARX_INFO << "Adding skill CallbackSkill";
+ addSkillFactory<CallbackSkill>();
// Add timeout skill
- addSkill(std::make_unique<TimeoutSkill>());
+ ARMARX_INFO << "Adding skill TimeoutSkill";
+ addSkillFactory<TimeoutSkill>();
// chaining
- addSkill(std::make_unique<ChainingSkill>());
+ ARMARX_INFO << "Adding skill ChainingSkill";
+ addSkillFactory<ChainingSkill>();
+
+ // incomplete and prepare
+ ARMARX_INFO << "Adding skill IncompleteSkill";
+ addSkillFactory<IncompleteSkill>();
}
void
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index 2aad033f1f00a2523dd8dcf15b03979eab92b339..b8391b0881f625af03b00b601983251c4d90558b 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -28,55 +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(const MainInput& in) final;
- };
-
- class ChainingSkill : public Skill
- {
- public:
- ChainingSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- Skill::MainResult main(const MainInput& in) final;
- };
-
- class TimeoutSkill : public PeriodicSkill
- {
- public:
- TimeoutSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- PeriodicSkill::StepResult step(const MainInput& in) final;
- };
-
- class CallbackSkill : public Skill
- {
- public:
- CallbackSkill();
-
- static SkillDescription GetSkillDescription();
-
- private:
- Skill::MainResult main(const MainInput& in) 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/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/Devices/GlobalRobotPoseSensorDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
index 0a11b09754f6a47f7b52e3fe0958f66aeeb034bd..2bb041a7b577bf9a9443dcb4f405949e840a636e 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
@@ -121,10 +121,14 @@ namespace armarx
const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
-
- if (sensorGlobalPositionCorrection == nullptr or sensorRelativePosition == nullptr)
+ if (sensorGlobalPositionCorrection == nullptr)
+ {
+ ARMARX_ERROR << "The global position correction sensor is not available.";
+ return;
+ }
+ if (sensorRelativePosition == nullptr)
{
- ARMARX_ERROR << "one of the sensors is not available";
+ ARMARX_ERROR << "The relative position sensor is not available.";
return;
}
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/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index a7083b662e54b0a0c02ad847c3e0f197bce74d58..cc1e89e592642c8adbd137c071023a472284e4ec 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -33,6 +33,44 @@
namespace armarx::RobotUnitModule
{
+
+ RobotDataPropertyDefinitions::RobotDataPropertyDefinitions(std::string prefix) :
+ ModuleBasePropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("RobotFileName",
+ "Robot file name, e.g. robot_model.xml");
+ defineOptionalProperty<std::string>("RobotFileNameProject",
+ "",
+ "Project in which the robot filename is located "
+ "(if robot is loaded from an external project)");
+
+ defineOptionalProperty<std::string>(
+ "RobotName",
+ "",
+ "Override robot name if you want to load multiple robots of the same type");
+ defineOptionalProperty<std::string>(
+ "RobotNodeSetName",
+ "Robot",
+ "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
+ defineOptionalProperty<std::string>(
+ "PlatformName",
+ "Platform",
+ "Name of the platform needs to correspond to a node in the virtual robot.");
+ defineOptionalProperty<bool>("PlatformAndLocalizationUnitsEnabled",
+ true,
+ "Enable or disable the platform and localization units.");
+ defineOptionalProperty<std::string>("PlatformInstanceName",
+ "Platform",
+ "Name of the platform instance (will publish "
+ "values on PlatformInstanceName + 'State')");
+ }
+
+ bool
+ RobotData::arePlatformAndLocalizationUnitsEnabled() const
+ {
+ return _arePlatformAndLocalizationUnitsEnabled;
+ }
+
const std::string&
RobotData::getRobotPlatformName() const
{
@@ -112,6 +150,8 @@ namespace armarx::RobotUnitModule
robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
robotFileName = getProperty<std::string>("RobotFileName").getValue();
robotPlatformName = getProperty<std::string>("PlatformName").getValue();
+ _arePlatformAndLocalizationUnitsEnabled =
+ getProperty<bool>("PlatformAndLocalizationUnitsEnabled").getValue();
robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
//load robot
@@ -160,4 +200,5 @@ namespace armarx::RobotUnitModule
robotPool.reset(new RobotPool(robot, 10));
}
}
+
} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
index 7b5defcbfbec169a83e7185f688a5016851c24ab..c6e94bd515bea5a6f88597c935eb277c234a33d2 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -40,32 +40,7 @@ namespace armarx::RobotUnitModule
class RobotDataPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- RobotDataPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
- {
- defineRequiredProperty<std::string>("RobotFileName",
- "Robot file name, e.g. robot_model.xml");
- defineOptionalProperty<std::string>("RobotFileNameProject",
- "",
- "Project in which the robot filename is located "
- "(if robot is loaded from an external project)");
-
- defineOptionalProperty<std::string>(
- "RobotName",
- "",
- "Override robot name if you want to load multiple robots of the same type");
- defineOptionalProperty<std::string>(
- "RobotNodeSetName",
- "Robot",
- "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
- defineOptionalProperty<std::string>(
- "PlatformName",
- "Platform",
- "Name of the platform needs to correspond to a node in the virtual robot.");
- defineOptionalProperty<std::string>("PlatformInstanceName",
- "Platform",
- "Name of the platform instance (will publish "
- "values on PlatformInstanceName + 'State')");
- }
+ RobotDataPropertyDefinitions(std::string prefix);
};
/**
@@ -99,21 +74,26 @@ namespace armarx::RobotUnitModule
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
public:
+ bool arePlatformAndLocalizationUnitsEnabled() const;
+
/**
* @brief Returns the name of the robot's platform
* @return The name of the robot's platform
*/
const std::string& getRobotPlatformName() const;
+
/**
* @brief Returns the name of the robot's RobotNodeSet
* @return The name of the robot's RobotNodeSet
*/
const std::string& getRobotNodetSeName() const;
+
/**
* @brief Returns the name of the project containing the robot's model
* @return The name of the project containing the robot's model
*/
const std::string& getRobotProjectName() const;
+
/**
* @brief Returns the file name of the robot's model
* @return The file name of the robot's model
@@ -150,6 +130,8 @@ namespace armarx::RobotUnitModule
std::string robotFileName;
/// @brief The name of the robot's platform
std::string robotPlatformName;
+ /// @brief Indicates whether the robot platform unit is enabled.
+ bool _arePlatformAndLocalizationUnitsEnabled = false;
/// @brief The name of the robot's platform instance
std::string robotPlatformInstanceName;
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/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index ad3cc0f97680eabc2519b58598823294c770733f..bd92c93f31a9007c338b9ae4bcd52d9ebe1c4d50 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -163,29 +163,54 @@ namespace armarx::RobotUnitModule
void
Units::initializeDefaultUnits()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto beg = TimeUtil::GetTime(true);
{
+ ARMARX_TRACE;
auto guard = getGuard();
+
+ ARMARX_TRACE;
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
ARMARX_INFO << "initializing default units";
+
+ ARMARX_TRACE;
initializeKinematicUnit();
ARMARX_DEBUG << "KinematicUnit initialized";
- ARMARX_DEBUG << "initializing LocalizationUnit";
- initializeLocalizationUnit();
- ARMARX_DEBUG << "LocalizationUnit initialized";
+ ARMARX_TRACE;
+ if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
+ {
+ ARMARX_DEBUG << "initializing LocalizationUnit";
+ initializeLocalizationUnit();
+ ARMARX_DEBUG << "LocalizationUnit initialized";
+ }
+ ARMARX_TRACE;
+
+ if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
+ {
+ ARMARX_DEBUG << "initializing PlatformUnit";
+ initializePlatformUnit();
+ ARMARX_DEBUG << "PlatformUnit initialized";
+ }
- initializePlatformUnit();
- ARMARX_DEBUG << "PlatformUnit initialized";
+ ARMARX_TRACE;
initializeForceTorqueUnit();
ARMARX_DEBUG << "ForceTorqueUnit initialized";
+
+ ARMARX_TRACE;
initializeInertialMeasurementUnit();
ARMARX_DEBUG << "InertialMeasurementUnit initialized";
+
+ ARMARX_TRACE;
initializeTrajectoryControllerUnit();
ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
+
+ ARMARX_TRACE;
initializeTcpControllerUnit();
ARMARX_DEBUG << "TcpControllerUnit initialized";
+
+ ARMARX_TRACE;
}
ARMARX_INFO << "initializing default units...done! "
<< (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
@@ -371,6 +396,8 @@ namespace armarx::RobotUnitModule
using UnitT = PlatformSubUnit;
using IfaceT = PlatformUnitInterface;
+ ARMARX_TRACE;
+
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
//check if unit is already added
@@ -378,12 +405,14 @@ namespace armarx::RobotUnitModule
{
return;
}
+ ARMARX_TRACE;
//is there a platform dev?
if (_module<RobotData>().getRobotPlatformName().empty())
{
ARMARX_INFO << "no platform unit created since no platform name was given";
return;
}
+ ARMARX_TRACE;
if (!_module<Devices>().getControlDevices().has(
_module<RobotData>().getRobotPlatformName()))
{
@@ -392,16 +421,19 @@ namespace armarx::RobotUnitModule
<< _module<RobotData>().getRobotPlatformName() << "' was not found";
return;
}
+ ARMARX_TRACE;
const ControlDevicePtr& controlDevice =
_module<Devices>().getControlDevices().at(_module<RobotData>().getRobotPlatformName());
const SensorDevicePtr& sensorDevice =
_module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
JointController* jointVel =
controlDevice->getJointController(ControlModes::HolonomicPlatformVelocity);
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(jointVel);
ARMARX_CHECK_EXPRESSION(
sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
//add it
+ ARMARX_TRACE;
const std::string configName = getProperty<std::string>("PlatformUnitName");
const std::string confPre = getConfigDomain() + "." + configName + ".";
Ice::PropertiesPtr properties = getIceProperties()->clone();
@@ -411,9 +443,11 @@ namespace armarx::RobotUnitModule
_module<RobotData>().getRobotPlatformInstanceName());
ARMARX_DEBUG << "creating unit " << configName
<< " using these properties: " << properties->getPropertiesForPrefix("");
+ ARMARX_TRACE;
IceInternal::Handle<UnitT> unit =
Component::create<UnitT>(properties, configName, getConfigDomain());
//config
+ ARMARX_TRACE;
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
config->initialVelocityX = 0;
@@ -427,8 +461,10 @@ namespace armarx::RobotUnitModule
config,
false,
true));
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(ctrl);
unit->pt = ctrl;
+ ARMARX_TRACE;
NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
new NJointHolonomicPlatformRelativePositionControllerConfig;
@@ -457,9 +493,11 @@ namespace armarx::RobotUnitModule
configGlobalPositionCtrlCfg,
false,
true));
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
unit->pt = ctrl;
unit->globalPosCtrl = ctrlGlobalPosition;
+ ARMARX_TRACE;
unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(
_module<RobotData>().getRobotPlatformName());
@@ -470,12 +508,14 @@ namespace armarx::RobotUnitModule
void
Units::initializeLocalizationUnit()
{
+ ARMARX_TRACE;
ARMARX_DEBUG << "initializeLocalizationUnit";
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = LocalizationSubUnit;
using IfaceT = LocalizationUnitInterface;
+ ARMARX_TRACE;
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
//check if unit is already added
@@ -483,13 +523,18 @@ namespace armarx::RobotUnitModule
{
return;
}
+ ARMARX_TRACE;
ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition";
+ ARMARX_CHECK(
+ _module<Devices>().getSensorDevices().has(_module<RobotData>().getRobotPlatformName()))
+ << _module<RobotData>().getRobotPlatformName();
const SensorDevicePtr& sensorDeviceRelativePosition =
_module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()
->asA<SensorValueHolonomicPlatformRelativePosition>());
+ ARMARX_TRACE;
// const SensorDevicePtr& sensorDevicePoseCorrection = _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName());
// ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueGlobalPoseCorrection>());
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index e1c717c58e00be94df3035916b5d3d4f0a50a589..bedfc430f0dee9f2030ac495d2ba6dd74bfa1350 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -82,8 +82,8 @@ namespace armarx
ControlThreadOutputBuffer::forEachNewLoggingEntry(ConsumerFunctor consumer)
{
ARMARX_TRACE;
- ARMARX_CHECK_EXPRESSION(isInitialized);
- ARMARX_VERBOSE << VAROUT(entries.size());
+ ARMARX_CHECK(isInitialized);
+ ARMARX_DEBUG << VAROUT(entries.size());
const size_t writePosition_local = writePosition.load(); // copy to prevent external changes
if (writePosition_local - onePastLoggingReadPosition >= numEntries)
{
@@ -107,7 +107,7 @@ namespace armarx
}
//consume all
const std::size_t num = writePosition_local - onePastLoggingReadPosition;
- ARMARX_VERBOSE << num << " new entries to be treated";
+ ARMARX_DEBUG << num << " new entries to be treated";
for (std::size_t offset = 0; onePastLoggingReadPosition < writePosition_local;
++onePastLoggingReadPosition, ++offset)
{
@@ -276,10 +276,8 @@ namespace armarx
}
}
- const auto PotentiallyMinimizeMember = [](auto & member, bool minimize) {
- return minimize ? 0 : member;
- };
-
+ const auto PotentiallyMinimizeMember = [](auto& member, bool minimize)
+ { return minimize ? 0 : member; };
detail::RtMessageLogBuffer::RtMessageLogBuffer(const detail::RtMessageLogBuffer& other,
bool minimize) :
@@ -301,13 +299,13 @@ namespace armarx
maxAlign{1}
{
ARMARX_DEBUG << "copying RtMessageLogBuffer with minimize = " << minimize << " "
- << VAROUT(initialBufferSize) << " " << VAROUT(initialBufferEntryNumbers) << " "
- << VAROUT(bufferMaxSize) << " " << VAROUT(bufferMaxNumberEntries) << " "
- << VAROUT(buffer.size()) << " " << VAROUT(other.bufferSpace) << " "
- << VAROUT(bufferPlace) << " " << VAROUT(entries.size()) << " "
- << VAROUT(entriesWritten) << " " << VAROUT(requiredAdditionalBufferSpace) << " "
- << VAROUT(requiredAdditionalEntries) << " " << VAROUT(messagesLost) << " "
- << VAROUT(maxAlign);
+ << VAROUT(initialBufferSize) << " " << VAROUT(initialBufferEntryNumbers) << " "
+ << VAROUT(bufferMaxSize) << " " << VAROUT(bufferMaxNumberEntries) << " "
+ << VAROUT(buffer.size()) << " " << VAROUT(other.bufferSpace) << " "
+ << VAROUT(bufferPlace) << " " << VAROUT(entries.size()) << " "
+ << VAROUT(entriesWritten) << " " << VAROUT(requiredAdditionalBufferSpace)
+ << " " << VAROUT(requiredAdditionalEntries) << " " << VAROUT(messagesLost)
+ << " " << VAROUT(maxAlign);
for (std::size_t idx = 0; idx < other.entries.size() && other.entries.at(idx); ++idx)
{
const RtMessageLogEntryBase* entry = other.entries.at(idx);
@@ -361,10 +359,12 @@ namespace armarx
deleteAll();
if (requiredAdditionalEntries || entries.size() < numEntries)
{
- const auto numExcessEntries = std::max(requiredAdditionalEntries, numEntries - entries.size());
+ const auto numExcessEntries =
+ std::max(requiredAdditionalEntries, numEntries - entries.size());
const auto requiredSize = entries.size() + numExcessEntries;
ARMARX_WARNING << "Iteration " << iterationCount << " required "
- << requiredAdditionalEntries << " | " << numExcessEntries << " additional message entries. \n"
+ << requiredAdditionalEntries << " | " << numExcessEntries
+ << " additional message entries. \n"
<< "The requested total number of entries is " << requiredSize << ". \n"
<< "The current number of entries is " << entries.size() << ". \n"
<< "The maximal number of entries is "
@@ -514,10 +514,10 @@ namespace armarx
{
ARMARX_TRACE;
ARMARX_DEBUG << "Copy ControlThreadOutputBufferEntry with parameters: " << VAROUT(minimize)
- << " " << VAROUT(writeTimestamp) << " " << VAROUT(sensorValuesTimestamp) << " "
- << VAROUT(timeSinceLastIteration) << " " << VAROUT(iteration) << " "
- << VAROUT(buffer.size()) << " " << VAROUT(messages.buffer.size()) << " "
- << VAROUT(messages.entries.size());
+ << " " << VAROUT(writeTimestamp) << " " << VAROUT(sensorValuesTimestamp) << " "
+ << VAROUT(timeSinceLastIteration) << " " << VAROUT(iteration) << " "
+ << VAROUT(buffer.size()) << " " << VAROUT(messages.buffer.size()) << " "
+ << VAROUT(messages.entries.size());
void* place = buffer.data();
std::size_t space = buffer.size();
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index b247f12167424f45a2e87c690461d5aaab5516d0..129771f55fec581f806de5e09d8c7a7d928f2ac5 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -114,7 +114,7 @@ namespace armarx::introspection
const std::string& name)
{
ARMARX_TRACE;
- ARMARX_CHECK_EQUAL(0, entries.count(name));
+ ARMARX_CHECK_EQUAL(entries.count(name), 0) << name;
entries.add(name, Entry(name, ptr));
return entries.at(name);
}
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/GuiHealthClient/GuiHealthClientWidgetController.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
index d976d77e6742b63152b172b7a55107929405302d..4868ebda8519f9d571c0a62a17ccbecd66e5a18d 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
@@ -28,6 +28,7 @@
#include <qrgb.h>
#include <qtablewidget.h>
#include <string>
+#include <iomanip>
#include <QLabel>
#include <QPushButton>
@@ -110,6 +111,24 @@ namespace armarx
robotHealthTopicPrx->heartbeat(getName(), now);
}
+
+ std::string to_string_rounded(float value, int decimals = 100)
+ {
+ std::stringstream ss;
+ ss << std::fixed << std::setprecision(3) << value;
+ return ss.str();
+ }
+
+
+ QTableWidgetItem* make_item(const std::string& text,
+ Qt::AlignmentFlag horAlignment = Qt::AlignLeft)
+ {
+ auto* item = new QTableWidgetItem(QString::fromStdString(text));
+ item->setTextAlignment(horAlignment | Qt::AlignVCenter);
+ return item;
+ }
+
+
void
GuiHealthClientWidgetController::updateSummaryTimerClb()
{
@@ -147,64 +166,63 @@ namespace armarx
const auto& entry = summaryVals.at(i);
std::string stateRepr;
- QColor color;
+ QColor stateColor;
switch(entry.state)
{
case HealthOK:
stateRepr = "yes";
- color.setRgb(0, 255, 0); // green
+ stateColor.setRgb(0, 255, 0); // green
break;
case HealthWarning:
stateRepr = "yes";
- color.setRgb(255, 165, 0); // orange
+ stateColor.setRgb(255, 165, 0); // orange
break;
case HealthError:
stateRepr = "no";
- color.setRgb(255, 0, 0); // red
+ stateColor.setRgb(255, 0, 0); // red
break;
}
const std::string hostname = entry.lastReferenceTimestamp.hostname;
- const std::string timeSinceLastArrivalRepr = std::to_string(entry.timeSinceLastArrival.microSeconds / 1000);
- const std::string timeToLastReferenceRepr = std::to_string(entry.timeSinceLastUpdateReference.microSeconds / 1000);
+ const std::string timeSinceLastArrivalRepr = to_string_rounded(entry.timeSinceLastArrival.microSeconds / 1000.0);
+ const std::string timeToLastReferenceRepr = to_string_rounded(entry.timeSinceLastUpdateReference.microSeconds / 1000.0);
const std::string tagsRepr = serializeList(entry.tags);
- const long syncErrorMilliSeconds = std::abs(entry.timeSinceLastArrival.microSeconds - entry.timeSinceLastUpdateReference.microSeconds) / 1000;
-
+ const float syncErrorMilliSeconds = std::abs(entry.timeSinceLastArrival.microSeconds - entry.timeSinceLastUpdateReference.microSeconds) / 1000.0;
tableWidget->setItem(i, 0, new QTableWidgetItem(QString::fromStdString(entry.identifier)));
-
- auto* requiredItem = new QTableWidgetItem(QString::fromStdString(entry.required ? "yes" : "no"));
- requiredItem->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
- tableWidget->setItem(i, 1, requiredItem);
-
- auto* stateItem = new QTableWidgetItem(QString::fromStdString(stateRepr));
- stateItem->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
- tableWidget->setItem(i, 2, stateItem);
- tableWidget->item(i, 2)->setBackgroundColor(color);
+ tableWidget->setItem(i, 1, make_item(entry.required ? "yes" : "no", Qt::AlignHCenter));
+
+ {
+ auto* item = make_item(stateRepr, Qt::AlignHCenter);
+ item->setBackgroundColor(stateColor);
+ tableWidget->setItem(i, 2, item);
+ }
tableWidget->setItem(i, 3, new QTableWidgetItem(QString::fromStdString(tagsRepr)));
-
- tableWidget->setItem(i, 4, new QTableWidgetItem(QString::fromStdString(timeSinceLastArrivalRepr)));
- tableWidget->setItem(i, 5, new QTableWidgetItem(QString::fromStdString(timeToLastReferenceRepr)));
-
- tableWidget->setItem(i, 6, new QTableWidgetItem(QString::fromStdString(std::to_string(syncErrorMilliSeconds))));
+ tableWidget->setItem(i, 4, make_item(timeSinceLastArrivalRepr, Qt::AlignRight));
+ tableWidget->setItem(i, 5, make_item(timeToLastReferenceRepr, Qt::AlignRight));
- if(syncErrorMilliSeconds > 20)
{
+ auto* item = make_item(to_string_rounded(syncErrorMilliSeconds), Qt::AlignRight);
+
QColor timeSyncColor;
- timeSyncColor.setRgb(255, 0, 0);
- tableWidget->item(i, 6)->setBackgroundColor(timeSyncColor);
- }else {
- QColor timeSyncColor;
- timeSyncColor.setRgb(0, 255, 0);
- tableWidget->item(i, 6)->setBackgroundColor(timeSyncColor);
+ if (syncErrorMilliSeconds > 20.)
+ {
+ timeSyncColor.setRgb(255, 0, 0);
+ }
+ else
+ {
+ timeSyncColor.setRgb(0, 255, 0);
+ }
+ item->setBackgroundColor(timeSyncColor);
+
+ tableWidget->setItem(i, 6, item);
}
-
- tableWidget->setItem(i, 7, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeWarning.microSeconds / 1000))));
- tableWidget->setItem(i, 8, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeError.microSeconds / 1000))));
+ tableWidget->setItem(i, 7, make_item(to_string_rounded(entry.maximumCycleTimeWarning.microSeconds / 1000.), Qt::AlignRight));
+ tableWidget->setItem(i, 8, make_item(to_string_rounded(entry.maximumCycleTimeError.microSeconds / 1000.), Qt::AlignRight));
tableWidget->setItem(i, 9, new QTableWidgetItem(QString::fromStdString(hostname)));
}
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 5437d499db20e1c4a29a59efe98f2c1b92a341fa..7c7cfae4d2fba28158d7bbe6cb2a3cb89fd5c195 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -22,330 +22,342 @@
* GNU General Public License
*/
#include "KinematicUnitGuiPlugin.h"
-#include "KinematicUnitConfigDialog.h"
-#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
-#include <RobotAPI/interface/core/NameValueMap.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/json.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/services/tasks/RunningTask.h"
#include "ArmarXCore/core/time/Metronome.h"
#include "ArmarXCore/core/time/forward_declarations.h"
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <ArmarXCore/core/ArmarXManager.h>
-
-#include <ArmarXCore/util/json/JSONObject.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/util/json/JSONObject.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <SimoxUtility/json.h>
-#include <SimoxUtility/algorithm/string.h>
+#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
+#include <RobotAPI/interface/core/NameValueMap.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+#include "KinematicUnitConfigDialog.h"
// Qt headers
-#include <Qt>
-#include <QtGlobal>
-#include <QSpinBox>
-#include <QSlider>
+#include <QCheckBox>
+#include <QClipboard>
+#include <QInputDialog>
#include <QPushButton>
+#include <QSlider>
+#include <QSpinBox>
#include <QStringList>
#include <QTableView>
-#include <QCheckBox>
#include <QTableWidget>
-#include <QClipboard>
-#include <QInputDialog>
+#include <Qt>
+#include <QtGlobal>
#include <qtimer.h>
-#include <Inventor/SoDB.h>
-#include <Inventor/Qt/SoQt.h>
#include <ArmarXCore/observers/filters/MedianFilter.h>
+#include <Inventor/Qt/SoQt.h>
+#include <Inventor/SoDB.h>
+
// System
+#include <cmath>
#include <cstddef>
#include <cstdio>
-#include <memory>
-#include <string>
-#include <optional>
#include <cstdlib>
-#include <iostream>
-#include <cmath>
-
#include <filesystem>
+#include <iostream>
+#include <memory>
+#include <optional>
+#include <stdexcept>
+#include <string>
//#define KINEMATIC_UNIT_FILE_DEFAULT std::string("RobotAPI/robots/Armar3/ArmarIII.xml")
//#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
{
-KinematicUnitGuiPlugin::KinematicUnitGuiPlugin()
-{
-
- qRegisterMetaType<DebugInfo>("DebugInfo");
+ KinematicUnitGuiPlugin::KinematicUnitGuiPlugin()
+ {
- addWidget<KinematicUnitWidgetController>();
-}
+ qRegisterMetaType<DebugInfo>("DebugInfo");
-KinematicUnitWidgetController::KinematicUnitWidgetController() :
- kinematicUnitNode(nullptr),
- enableValueValidator(true),
- historyTime(100000), // 1/10 s
- currentValueMax(5.0f)
-{
- rootVisu = NULL;
- debugLayerVisu = NULL;
+ addWidget<KinematicUnitWidgetController>();
+ }
- // init gui
- ui.setupUi(getWidget());
- getWidget()->setEnabled(false);
+ KinematicUnitWidgetController::KinematicUnitWidgetController() :
+ kinematicUnitNode(nullptr),
+ enableValueValidator(true),
+ historyTime(100000), // 1/10 s
+ currentValueMax(5.0f)
+ {
+ rootVisu = NULL;
+ debugLayerVisu = NULL;
- ui.tableJointList->setItemDelegateForColumn(eTabelColumnAngleProgressbar, &delegate);
+ // init gui
+ ui.setupUi(getWidget());
+ getWidget()->setEnabled(false);
- ui.radioButtonUnknown->setHidden(true);
-}
+ ui.tableJointList->setItemDelegateForColumn(eTabelColumnAngleProgressbar, &delegate);
-void KinematicUnitWidgetController::onInitComponent()
-{
- ARMARX_INFO << flush;
- verbose = true;
+ ui.radioButtonUnknown->setHidden(true);
+ }
+ void
+ KinematicUnitWidgetController::onInitComponent()
+ {
+ ARMARX_INFO << flush;
+ verbose = true;
- rootVisu = new SoSeparator;
- rootVisu->ref();
- robotVisu = new SoSeparator;
- robotVisu->ref();
- rootVisu->addChild(robotVisu);
- // create the debugdrawer component
- std::string debugDrawerComponentName = "KinemticUnitGUIDebugDrawer_" + getName();
- ARMARX_INFO << "Creating component " << debugDrawerComponentName;
- debugDrawer = Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
- showVisuLayers(false);
+ rootVisu = new SoSeparator;
+ rootVisu->ref();
+ robotVisu = new SoSeparator;
+ robotVisu->ref();
+ rootVisu->addChild(robotVisu);
- if (mutex3D)
- {
- //ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
- debugDrawer->setMutex(mutex3D);
- }
- else
- {
- ARMARX_ERROR << " No 3d mutex available...";
- }
+ // create the debugdrawer component
+ std::string debugDrawerComponentName = "KinemticUnitGUIDebugDrawer_" + getName();
+ ARMARX_INFO << "Creating component " << debugDrawerComponentName;
+ debugDrawer =
+ Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
+ showVisuLayers(false);
- ArmarXManagerPtr m = getArmarXManager();
- m->addObject(debugDrawer, false);
+ if (mutex3D)
+ {
+ //ARMARX_IMPORTANT << "mutex3d:" << mutex3D.get();
+ debugDrawer->setMutex(mutex3D);
+ }
+ else
+ {
+ ARMARX_ERROR << " No 3d mutex available...";
+ }
+ ArmarXManagerPtr m = getArmarXManager();
+ m->addObject(debugDrawer, false);
- {
- std::unique_lock lock(*mutex3D);
- debugLayerVisu = new SoSeparator();
- debugLayerVisu->ref();
- debugLayerVisu->addChild(debugDrawer->getVisualization());
- rootVisu->addChild(debugLayerVisu);
- }
- connectSlots();
+ {
+ std::unique_lock lock(*mutex3D);
+ debugLayerVisu = new SoSeparator();
+ debugLayerVisu->ref();
+ debugLayerVisu->addChild(debugDrawer->getVisualization());
+ rootVisu->addChild(debugLayerVisu);
+ }
- usingProxy(kinematicUnitName);
-}
+ connectSlots();
-void KinematicUnitWidgetController::onConnectComponent()
-{
- // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
- jointCurrentHistory.clear();
- jointCurrentHistory.set_capacity(5);
+ usingProxy(kinematicUnitName);
+ }
- // jointAnglesUpdateFrequency = new filters::MedianFilter(100);
- kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+ void
+ KinematicUnitWidgetController::onConnectComponent()
+ {
+ // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
+ jointCurrentHistory.clear();
+ jointCurrentHistory.set_capacity(5);
- lastJointAngleUpdateTimestamp = Clock::Now();
- robotVisu->removeAllChildren();
+ // jointAnglesUpdateFrequency = new filters::MedianFilter(100);
+ kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
- robot.reset();
+ lastJointAngleUpdateTimestamp = Clock::Now();
+ robotVisu->removeAllChildren();
- std::string rfile;
- Ice::StringSeq includePaths;
+ robot.reset();
- // Get robot filename
- try
- {
- Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
- packages.push_back(Application::GetProjectName());
- ARMARX_VERBOSE << "ArmarX packages " << packages;
+ std::string rfile;
+ Ice::StringSeq includePaths;
- for (const std::string& projectName : packages)
+ // Get robot filename
+ try
{
- if (projectName.empty())
+ Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
+ packages.push_back(Application::GetProjectName());
+ ARMARX_VERBOSE << "ArmarX packages " << packages;
+
+ for (const std::string& projectName : packages)
{
- continue;
+ if (projectName.empty())
+ {
+ continue;
+ }
+
+ CMakePackageFinder project(projectName);
+ auto pathsString = project.getDataDir();
+ ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": "
+ << pathsString;
+ Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
+ ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": "
+ << projectIncludePaths;
+ includePaths.insert(
+ includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
- CMakePackageFinder project(projectName);
- auto pathsString = project.getDataDir();
- ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": " << pathsString;
- Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
- ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
- }
-
- rfile = kinematicUnitInterfacePrx->getRobotFilename();
- ARMARX_VERBOSE << "Relative robot file " << rfile;
- ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
- ARMARX_VERBOSE << "Absolute robot file " << rfile;
+ rfile = kinematicUnitInterfacePrx->getRobotFilename();
+ ARMARX_VERBOSE << "Relative robot file " << rfile;
+ ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
+ ARMARX_VERBOSE << "Absolute robot file " << rfile;
- robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
- }
- catch (...)
- {
- ARMARX_ERROR << "Unable to retrieve robot filename.";
- }
-
- try
- {
- ARMARX_INFO << "Loading robot from file " << rfile;
- robot = loadRobotFile(rfile);
- }
- catch (const std::exception& e)
- {
- ARMARX_ERROR << "Failed to init robot: " << e.what();
- }
- catch (...)
- {
- ARMARX_ERROR << "Failed to init robot";
- }
+ robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unable to retrieve robot filename.";
+ }
- if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
- {
- getObjectScheduler()->terminate();
- if (getWidget()->parentWidget())
+ try
{
- getWidget()->parentWidget()->close();
+ ARMARX_INFO << "Loading robot from file " << rfile;
+ robot = loadRobotFile(rfile);
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init robot: " << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Failed to init robot";
}
- return;
- }
- // Check robot name and disable setZero Button if necessary
- if (not simox::alg::starts_with(robot->getName(), "Armar3"))
- {
- ARMARX_VERBOSE << "Disable the SetZero button because the robot name is '" << robot->getName() << "'.";
- ui.pushButtonKinematicUnitPos1->setDisabled(true);
- }
+ if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
+ {
+ getObjectScheduler()->terminate();
+ if (getWidget()->parentWidget())
+ {
+ getWidget()->parentWidget()->close();
+ }
+ return;
+ }
- kinematicUnitFile = rfile;
- robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
+ // Check robot name and disable setZero Button if necessary
+ if (not simox::alg::starts_with(robot->getName(), "Armar3"))
+ {
+ ARMARX_VERBOSE << "Disable the SetZero button because the robot name is '"
+ << robot->getName() << "'.";
+ ui.pushButtonKinematicUnitPos1->setDisabled(true);
+ }
- kinematicUnitVisualization = getCoinVisualization(robot);
- kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
- robotVisu->addChild(kinematicUnitNode);
+ kinematicUnitFile = rfile;
+ robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
- // Fetch the current joint angles.
- synchronizeRobotJointAngles();
+ kinematicUnitVisualization = getCoinVisualization(robot);
+ kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
+ robotVisu->addChild(kinematicUnitNode);
- initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
- initGUIJointListTable(robotNodeSet);
+ // Fetch the current joint angles.
+ synchronizeRobotJointAngles();
- const auto initialDebugInfo = kinematicUnitInterfacePrx->getDebugInfo();
+ initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
+ initGUIJointListTable(robotNodeSet);
- initializeUi(initialDebugInfo);
+ const auto initialDebugInfo = kinematicUnitInterfacePrx->getDebugInfo();
- QMetaObject::invokeMethod(this, "resetSlider");
- enableMainWidgetAsync(true);
-
- updateTask = new RunningTask<KinematicUnitWidgetController>(this, &KinematicUnitWidgetController::runUpdate);
- updateTask->start();
-}
+ initializeUi(initialDebugInfo);
-void KinematicUnitWidgetController::runUpdate()
-{
- Metronome metronome(Frequency::Hertz(10));
+ QMetaObject::invokeMethod(this, "resetSlider");
+ enableMainWidgetAsync(true);
- while(kinematicUnitInterfacePrx)
- {
- fetchData();
- metronome.waitForNextTick();
+ updateTask = new RunningTask<KinematicUnitWidgetController>(
+ this, &KinematicUnitWidgetController::runUpdate);
+ updateTask->start();
}
- ARMARX_INFO << "Connection to kinemetic unit lost. Update task terminates.";
-}
-
-void KinematicUnitWidgetController::onDisconnectComponent()
-{
- kinematicUnitInterfacePrx = nullptr;
-
- if(updateTask)
+ void
+ KinematicUnitWidgetController::runUpdate()
{
- updateTask->stop();
- updateTask->join();
- updateTask = nullptr;
- }
+ Metronome metronome(Frequency::Hertz(10));
- // killTimer(updateTimerId);
- enableMainWidgetAsync(false);
+ while (kinematicUnitInterfacePrx)
+ {
+ fetchData();
+ metronome.waitForNextTick();
+ }
- {
- std::unique_lock lock(mutexNodeSet);
- robot.reset();
- robotNodeSet.reset();
- currentNode.reset();
+ ARMARX_INFO << "Connection to kinemetic unit lost. Update task terminates.";
}
+ void
+ KinematicUnitWidgetController::onDisconnectComponent()
{
- std::unique_lock lock(*mutex3D);
- robotVisu->removeAllChildren();
- debugLayerVisu->removeAllChildren();
- }
+ kinematicUnitInterfacePrx = nullptr;
-}
-
-void KinematicUnitWidgetController::onExitComponent()
-{
- kinematicUnitInterfacePrx = nullptr;
-
- if(updateTask)
- {
- updateTask->stop();
- updateTask->join();
- updateTask = nullptr;
- }
+ if (updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
- enableMainWidgetAsync(false);
+ // killTimer(updateTimerId);
+ enableMainWidgetAsync(false);
- {
- std::unique_lock lock(*mutex3D);
+ {
+ std::unique_lock lock(mutexNodeSet);
+ robot.reset();
+ robotNodeSet.reset();
+ currentNode.reset();
+ }
- if (robotVisu)
{
+ std::unique_lock lock(*mutex3D);
robotVisu->removeAllChildren();
- robotVisu->unref();
- robotVisu = NULL;
+ debugLayerVisu->removeAllChildren();
}
+ }
- if (debugLayerVisu)
+ void
+ KinematicUnitWidgetController::onExitComponent()
+ {
+ kinematicUnitInterfacePrx = nullptr;
+
+ if (updateTask)
{
- debugLayerVisu->removeAllChildren();
- debugLayerVisu->unref();
- debugLayerVisu = NULL;
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
}
- if (rootVisu)
+ enableMainWidgetAsync(false);
+
{
- rootVisu->removeAllChildren();
- rootVisu->unref();
- rootVisu = NULL;
+ std::unique_lock lock(*mutex3D);
+
+ if (robotVisu)
+ {
+ robotVisu->removeAllChildren();
+ robotVisu->unref();
+ robotVisu = NULL;
+ }
+
+ if (debugLayerVisu)
+ {
+ debugLayerVisu->removeAllChildren();
+ debugLayerVisu->unref();
+ debugLayerVisu = NULL;
+ }
+
+ if (rootVisu)
+ {
+ rootVisu->removeAllChildren();
+ rootVisu->unref();
+ rootVisu = NULL;
+ }
}
- }
- /*
+ /*
if (debugDrawer && debugDrawer->getObjectScheduler())
{
ARMARX_INFO << "Removing DebugDrawer component...";
@@ -353,757 +365,926 @@ void KinematicUnitWidgetController::onExitComponent()
ARMARX_INFO << "Removing DebugDrawer component...done";
}
*/
-}
-
-QPointer<QDialog> KinematicUnitWidgetController::getConfigDialog(QWidget* parent)
-{
- if (!dialog)
- {
- dialog = new KinematicUnitConfigDialog(parent);
- dialog->setName(dialog->getDefaultName());
}
- return qobject_cast<KinematicUnitConfigDialog*>(dialog);
-}
-
-void KinematicUnitWidgetController::configured()
-{
- ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
- kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
- enableValueValidator = dialog->ui->checkBox->isChecked();
- viewerEnabled = dialog->ui->checkBox3DViewerEnabled->isChecked();
- historyTime = dialog->ui->spinBoxHistory->value() * 1000;
- currentValueMax = dialog->ui->doubleSpinBoxMaxMinCurrent->value();
-}
-
-void KinematicUnitWidgetController::loadSettings(QSettings* settings)
-{
- kinematicUnitName = settings->value("kinematicUnitName", KINEMATIC_UNIT_NAME_DEFAULT).toString().toStdString();
- enableValueValidator = settings->value("enableValueValidator", true).toBool();
- viewerEnabled = settings->value("viewerEnabled", true).toBool();
- historyTime = settings->value("historyTime", 100).toInt() * 1000;
- currentValueMax = settings->value("currentValueMax", 5.0).toFloat();
-}
-
-void KinematicUnitWidgetController::saveSettings(QSettings* settings)
-{
- settings->setValue("kinematicUnitName", QString::fromStdString(kinematicUnitName));
- settings->setValue("enableValueValidator", enableValueValidator);
- settings->setValue("viewerEnabled", viewerEnabled);
- assert(historyTime % 1000 == 0);
- settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
- settings->setValue("currentValueMax", currentValueMax);
-}
-
-
-void KinematicUnitWidgetController::showVisuLayers(bool show)
-{
- if (debugDrawer)
+ QPointer<QDialog>
+ KinematicUnitWidgetController::getConfigDialog(QWidget* parent)
{
- if (show)
+ if (!dialog)
{
- debugDrawer->enableAllLayers();
- }
- else
- {
- debugDrawer->disableAllLayers();
+ dialog = new KinematicUnitConfigDialog(parent);
+ dialog->setName(dialog->getDefaultName());
}
+
+ return qobject_cast<KinematicUnitConfigDialog*>(dialog);
}
-}
-void KinematicUnitWidgetController::copyToClipboard()
-{
- NameValueMap values;
+ void
+ KinematicUnitWidgetController::configured()
{
- std::unique_lock lock(mutexNodeSet);
-
- ARMARX_CHECK_NOT_NULL(kinematicUnitInterfacePrx);
- const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
-
- const auto selectedControlMode = getSelectedControlMode();
-
- if (selectedControlMode == ePositionControl)
- {
- values = debugInfo.jointAngles;
- }
- else if (selectedControlMode == eVelocityControl)
- {
- values = debugInfo.jointVelocities;
- }
+ ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
+ kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
+ enableValueValidator = dialog->ui->checkBox->isChecked();
+ viewerEnabled = dialog->ui->checkBox3DViewerEnabled->isChecked();
+ historyTime = dialog->ui->spinBoxHistory->value() * 1000;
+ currentValueMax = dialog->ui->doubleSpinBoxMaxMinCurrent->value();
}
- JSONObjectPtr serializer = new JSONObject();
- for (auto& kv : values)
+ void
+ KinematicUnitWidgetController::loadSettings(QSettings* settings)
{
- serializer->setFloat(kv.first, kv.second);
+ kinematicUnitName = settings->value("kinematicUnitName", KINEMATIC_UNIT_NAME_DEFAULT)
+ .toString()
+ .toStdString();
+ enableValueValidator = settings->value("enableValueValidator", true).toBool();
+ viewerEnabled = settings->value("viewerEnabled", true).toBool();
+ historyTime = settings->value("historyTime", 100).toInt() * 1000;
+ currentValueMax = settings->value("currentValueMax", 5.0).toFloat();
}
- const QString json = QString::fromStdString(serializer->asString(true));
- QClipboard* clipboard = QApplication::clipboard();
- clipboard->setText(json);
- QApplication::processEvents();
-}
-
-void KinematicUnitWidgetController::updateGuiElements()
-{
- // modelUpdateCB();
-}
-
-void KinematicUnitWidgetController::updateKinematicUnitListInDialog()
-{
-}
-
-void KinematicUnitWidgetController::modelUpdateCB()
-{
-}
-
-SoNode* KinematicUnitWidgetController::getScene()
-{
- if (viewerEnabled)
+ void
+ KinematicUnitWidgetController::saveSettings(QSettings* settings)
{
- ARMARX_INFO << "Returning scene ";
- return rootVisu;
+ settings->setValue("kinematicUnitName", QString::fromStdString(kinematicUnitName));
+ settings->setValue("enableValueValidator", enableValueValidator);
+ settings->setValue("viewerEnabled", viewerEnabled);
+ assert(historyTime % 1000 == 0);
+ settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
+ settings->setValue("currentValueMax", currentValueMax);
}
- else
+
+ void
+ KinematicUnitWidgetController::showVisuLayers(bool show)
{
- ARMARX_INFO << "viewer disabled - returning null scene";
- return NULL;
+ if (debugDrawer)
+ {
+ if (show)
+ {
+ debugDrawer->enableAllLayers();
+ }
+ else
+ {
+ debugDrawer->disableAllLayers();
+ }
+ }
}
-}
-
-void KinematicUnitWidgetController::connectSlots()
-{
- connect(ui.pushButtonKinematicUnitPos1, SIGNAL(clicked()), this, SLOT(kinematicUnitZeroPosition()));
-
- connect(ui.nodeListComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectJoint(int)));
- connect(ui.horizontalSliderKinematicUnitPos, SIGNAL(valueChanged(int)), this, SLOT(sliderValueChanged(int)));
- connect(ui.horizontalSliderKinematicUnitPos, SIGNAL(sliderReleased()), this, SLOT(resetSliderToZeroPosition()));
-
- connect(ui.radioButtonPositionControl, SIGNAL(clicked(bool)), this, SLOT(setControlModePosition()));
- connect(ui.radioButtonVelocityControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeVelocity()));
- connect(ui.radioButtonTorqueControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeTorque()));
- connect(ui.pushButtonFromJson, SIGNAL(clicked()), this, SLOT(on_pushButtonFromJson_clicked()));
-
- connect(ui.copyToClipboard, SIGNAL(clicked()), this, SLOT(copyToClipboard()));
- connect(ui.showDebugLayer, SIGNAL(toggled(bool)), this, SLOT(showVisuLayers(bool)), Qt::QueuedConnection);
-
- connect(this, SIGNAL(jointAnglesReported()), this, SLOT(updateJointAnglesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointVelocitiesReported()), this, SLOT(updateJointVelocitiesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointTorquesReported()), this, SLOT(updateJointTorquesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointCurrentsReported()), this, SLOT(updateJointCurrentsTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointMotorTemperaturesReported()), this, SLOT(updateMotorTemperaturesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointControlModesReported()), this, SLOT(updateControlModesTable()), Qt::QueuedConnection);
- connect(this, SIGNAL(jointStatusesReported()), this, SLOT(updateJointStatusesTable()), Qt::QueuedConnection);
-
- connect(ui.tableJointList, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(selectJointFromTableWidget(int, int)), Qt::QueuedConnection);
-
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(resetSlider()), Qt::QueuedConnection);
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModePosition()));
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeVelocity()));
- connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeTorque()));
-
- connect(this, SIGNAL(onDebugInfoReceived(const DebugInfo&)), this, SLOT(debugInfoReceived(const DebugInfo&)));
-}
-
-void KinematicUnitWidgetController::initializeUi(const DebugInfo& debugInfo)
-{
- //signal clicked is not emitted if you call setDown(), setChecked() or toggle().
-
- // there is no default control mode
- setControlModeRadioButtonGroup(ControlMode::eUnknown);
+ void
+ KinematicUnitWidgetController::copyToClipboard()
+ {
+ NameValueMap values;
+ {
+ std::unique_lock lock(mutexNodeSet);
- ui.widgetSliderFactor->setVisible(false);
+ ARMARX_CHECK_NOT_NULL(kinematicUnitInterfacePrx);
+ const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
- fetchData();
-}
+ const auto selectedControlMode = getSelectedControlMode();
+ if (selectedControlMode == ePositionControl)
+ {
+ values = debugInfo.jointAngles;
+ }
+ else if (selectedControlMode == eVelocityControl)
+ {
+ values = debugInfo.jointVelocities;
+ }
+ }
-void KinematicUnitWidgetController::kinematicUnitZeroVelocity()
-{
- if (!robotNodeSet)
- {
- return;
+ JSONObjectPtr serializer = new JSONObject();
+ for (auto& kv : values)
+ {
+ serializer->setFloat(kv.first, kv.second);
+ }
+ const QString json = QString::fromStdString(serializer->asString(true));
+ QClipboard* clipboard = QApplication::clipboard();
+ clipboard->setText(json);
+ QApplication::processEvents();
}
- std::unique_lock lock(mutexNodeSet);
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- NameValueMap vels;
- NameControlModeMap jointModes;
-
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateGuiElements()
{
- jointModes[rn[i]->getName()] = eVelocityControl;
- vels[rn[i]->getName()] = 0.0f;
+ // modelUpdateCB();
}
- try
+ void
+ KinematicUnitWidgetController::updateKinematicUnitListInDialog()
{
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointVelocities(vels);
}
- catch (...)
+
+ void
+ KinematicUnitWidgetController::modelUpdateCB()
{
}
- const auto selectedControlMode = getSelectedControlMode();
- if (selectedControlMode == eVelocityControl)
+ SoNode*
+ KinematicUnitWidgetController::getScene()
{
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ if (viewerEnabled)
+ {
+ ARMARX_INFO << "Returning scene ";
+ return rootVisu;
+ }
+ else
+ {
+ ARMARX_INFO << "viewer disabled - returning null scene";
+ return NULL;
+ }
}
-}
+ void
+ KinematicUnitWidgetController::connectSlots()
+ {
+ connect(ui.pushButtonKinematicUnitPos1,
+ SIGNAL(clicked()),
+ this,
+ SLOT(kinematicUnitZeroPosition()));
+
+ connect(
+ ui.nodeListComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectJoint(int)));
+ connect(ui.horizontalSliderKinematicUnitPos,
+ SIGNAL(valueChanged(int)),
+ this,
+ SLOT(sliderValueChanged(int)));
+
+ connect(ui.horizontalSliderKinematicUnitPos,
+ SIGNAL(sliderReleased()),
+ this,
+ SLOT(resetSliderToZeroPosition()));
+
+ connect(ui.radioButtonPositionControl,
+ SIGNAL(clicked(bool)),
+ this,
+ SLOT(setControlModePosition()));
+ connect(ui.radioButtonVelocityControl,
+ SIGNAL(clicked(bool)),
+ this,
+ SLOT(setControlModeVelocity()));
+ connect(
+ ui.radioButtonTorqueControl, SIGNAL(clicked(bool)), this, SLOT(setControlModeTorque()));
+ connect(
+ ui.pushButtonFromJson, SIGNAL(clicked()), this, SLOT(on_pushButtonFromJson_clicked()));
+
+ connect(ui.copyToClipboard, SIGNAL(clicked()), this, SLOT(copyToClipboard()));
+ connect(ui.showDebugLayer,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(showVisuLayers(bool)),
+ Qt::QueuedConnection);
+
+ connect(this,
+ SIGNAL(jointAnglesReported()),
+ this,
+ SLOT(updateJointAnglesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointVelocitiesReported()),
+ this,
+ SLOT(updateJointVelocitiesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointTorquesReported()),
+ this,
+ SLOT(updateJointTorquesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointCurrentsReported()),
+ this,
+ SLOT(updateJointCurrentsTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointMotorTemperaturesReported()),
+ this,
+ SLOT(updateMotorTemperaturesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointControlModesReported()),
+ this,
+ SLOT(updateControlModesTable()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(jointStatusesReported()),
+ this,
+ SLOT(updateJointStatusesTable()),
+ Qt::QueuedConnection);
+
+ connect(ui.tableJointList,
+ SIGNAL(cellDoubleClicked(int, int)),
+ this,
+ SLOT(selectJointFromTableWidget(int, int)),
+ Qt::QueuedConnection);
+
+ connect(ui.checkBoxUseDegree,
+ SIGNAL(clicked()),
+ this,
+ SLOT(resetSlider()),
+ Qt::QueuedConnection);
+ connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModePosition()));
+ connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeVelocity()));
+ connect(ui.checkBoxUseDegree, SIGNAL(clicked()), this, SLOT(setControlModeTorque()));
+
+ connect(this,
+ SIGNAL(onDebugInfoReceived(const DebugInfo&)),
+ this,
+ SLOT(debugInfoReceived(const DebugInfo&)));
+ }
+
+ void
+ KinematicUnitWidgetController::initializeUi(const DebugInfo& debugInfo)
+ {
+ //signal clicked is not emitted if you call setDown(), setChecked() or toggle().
+
+ // there is no default control mode
+ setControlModeRadioButtonGroup(ControlMode::eUnknown);
+
+ ui.widgetSliderFactor->setVisible(false);
-void KinematicUnitWidgetController::resetSlider()
-{
- const auto selectedControlMode = getSelectedControlMode();
-
- if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
- {
- resetSliderToZeroPosition();
+ fetchData();
}
- else if (selectedControlMode == ePositionControl)
+
+ void
+ KinematicUnitWidgetController::kinematicUnitZeroVelocity()
{
- if (currentNode)
+ if (!robotNodeSet)
{
- 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));
+ return;
}
- }
-
-}
-void KinematicUnitWidgetController::resetSliderToZeroPosition()
-{
- const auto selectedControlMode = getSelectedControlMode();
+ std::unique_lock lock(mutexNodeSet);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ NameValueMap vels;
+ NameControlModeMap jointModes;
- if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
- {
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
- ui.lcdNumberKinematicUnitJointValue->display(SLIDER_ZERO_POSITION);
- }
-}
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ jointModes[rn[i]->getName()] = eVelocityControl;
+ vels[rn[i]->getName()] = 0.0f;
+ }
-void KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
-{
- ARMARX_VERBOSE << "Setting control mode of radio button group to " << controlMode;
+ try
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointVelocities(vels);
+ }
+ catch (...)
+ {
+ }
- switch(controlMode)
- {
- case eDisabled:
- case eUnknown:
- case ePositionVelocityControl:
- ui.radioButtonUnknown->setChecked(true);
- break;
- case ePositionControl:
- ui.radioButtonPositionControl->setChecked(true);
- break;
- case eVelocityControl:
- ui.radioButtonVelocityControl->setChecked(true);
- break;
- case eTorqueControl:
- ui.radioButtonTorqueControl->setChecked(true);
- break;
+ const auto selectedControlMode = getSelectedControlMode();
+ if (selectedControlMode == eVelocityControl)
+ {
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ }
}
-}
-
-void KinematicUnitWidgetController::setControlModePosition()
-{
- if (!ui.radioButtonPositionControl->isChecked())
+ void
+ KinematicUnitWidgetController::resetSlider()
{
- return;
- }
- NameControlModeMap jointModes;
- // selectedControlMode = ePositionControl;
- ui.widgetSliderFactor->setVisible(false);
+ const auto selectedControlMode = getSelectedControlMode();
- // FIXME currentNode should be passed to this function!
+ if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
+ {
+ resetSliderToZeroPosition();
+ }
+ else if (selectedControlMode == ePositionControl)
+ {
+ if (currentNode)
+ {
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ {
+ 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));
+ }
+ }
+ }
+ }
- if (currentNode)
+ void
+ KinematicUnitWidgetController::resetSliderToZeroPosition()
{
- QString unit = currentNode->isRotationalJoint()
- ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
- : "mm";
- 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;
- jointModes[currentNode->getName()] = ePositionControl;
+ const auto selectedControlMode = getSelectedControlMode();
- if (kinematicUnitInterfacePrx)
+ if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
{
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ ui.lcdNumberKinematicUnitJointValue->display(SLIDER_ZERO_POSITION);
}
+ }
- float lo = currentNode->getJointLimitLo() * conversionFactor;
- float hi = currentNode->getJointLimitHi() * conversionFactor;
+ void
+ KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
+ {
+ ARMARX_VERBOSE << "Setting control mode of radio button group to " << controlMode;
- if (hi - lo <= 0.0f)
+ switch (controlMode)
{
- return;
+ case eDisabled:
+ case eUnknown:
+ case ePositionVelocityControl:
+ ui.radioButtonUnknown->setChecked(true);
+ break;
+ case ePositionControl:
+ ui.radioButtonPositionControl->setChecked(true);
+ break;
+ case eVelocityControl:
+ ui.radioButtonVelocityControl->setChecked(true);
+ break;
+ case eTorqueControl:
+ ui.radioButtonTorqueControl->setChecked(true);
+ break;
}
+ }
+ void
+ KinematicUnitWidgetController::setControlModePosition()
+ {
+ if (!ui.radioButtonPositionControl->isChecked())
{
- // currentNode->getJointValue() can we wrong after we re-connected to the robot unit.
- // E.g., it can be 0 although the torso joint was at -365 before the unit disconnected.
- // Therefore, we first have to fetch the actual joint values and use that one.
- // However, this should actually not be necessary, as the robot model should be updated
- // via the topics.
- synchronizeRobotJointAngles();
+ return;
}
+ NameControlModeMap jointModes;
+ // selectedControlMode = ePositionControl;
+ ui.widgetSliderFactor->setVisible(false);
- float pos = currentNode->getJointValue() * conversionFactor;
- ARMARX_INFO << "Setting position control for current node "
- << "(name '" << currentNode->getName() << "' with current value " << pos << ")";
+ // FIXME currentNode should be passed to this function!
- // Setting the slider position to pos will set the position to the slider tick closest to pos
- // This will initially send a position target with a small delta to the joint.
- ui.horizontalSliderKinematicUnitPos->blockSignals(true);
+ if (currentNode)
+ {
+ const QString unit = [&]() -> QString
+ {
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ {
+ if (ui.checkBoxUseDegree->isChecked())
+ {
+ return "deg";
+ }
- ui.horizontalSliderKinematicUnitPos->setMaximum(hi * factor);
- ui.horizontalSliderKinematicUnitPos->setMinimum(lo * factor);
- ui.lcdNumberKinematicUnitJointValue->display(pos);
+ return "rad";
+ }
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
- }
-}
+ if (currentNode->isTranslationalJoint())
+ {
+ return "mm";
+ }
-void KinematicUnitWidgetController::setControlModeVelocity()
-{
- if (!ui.radioButtonVelocityControl->isChecked())
- {
- return;
- }
- NameControlModeMap jointModes;
- NameValueMap jointVelocities;
+ throw std::invalid_argument("unknown/unsupported joint type");
+ }();
- if (currentNode)
- {
- jointModes[currentNode->getName()] = eVelocityControl;
+ ui.labelUnit->setText(unit);
- // 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";
- 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 auto [factor, conversionFactor] = [&]() -> std::pair<float, float>
+ {
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ {
+ 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;
- try
- {
if (kinematicUnitInterfacePrx)
{
kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
}
- }
- catch (...)
- {
- }
+ const float lo = currentNode->getJointLimitLo() * conversionFactor;
+ const float hi = currentNode->getJointLimitHi() * conversionFactor;
- ui.widgetSliderFactor->setVisible(true);
+ if (hi - lo <= 0.0f)
+ {
+ return;
+ }
- ui.horizontalSliderKinematicUnitPos->blockSignals(true);
- ui.horizontalSliderKinematicUnitPos->setMaximum(hi);
- ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
- }
-}
+ {
+ // currentNode->getJointValue() can we wrong after we re-connected to the robot unit.
+ // E.g., it can be 0 although the torso joint was at -365 before the unit disconnected.
+ // Therefore, we first have to fetch the actual joint values and use that one.
+ // However, this should actually not be necessary, as the robot model should be updated
+ // via the topics.
+ synchronizeRobotJointAngles();
+ }
-ControlMode KinematicUnitWidgetController::getSelectedControlMode() const
-{
- if(ui.radioButtonPositionControl->isChecked())
- {
- return ControlMode::ePositionControl;
- }
+ const float pos = currentNode->getJointValue() * conversionFactor;
+ ARMARX_INFO << "Setting position control for current node "
+ << "(name '" << currentNode->getName() << "' with current value " << pos
+ << ")";
- if (ui.radioButtonVelocityControl->isChecked())
- {
- return ControlMode::eVelocityControl;
- }
+ // Setting the slider position to pos will set the position to the slider tick closest to pos
+ // This will initially send a position target with a small delta to the joint.
+ ui.horizontalSliderKinematicUnitPos->blockSignals(true);
- if (ui.radioButtonTorqueControl->isChecked())
- {
- return ControlMode::eTorqueControl;
- }
+ const float sliderMax = hi * factor;
+ const float sliderMin = lo * factor;
- // if no button is checked, then the joint is likely initialized but no controller has been loaded yet
- // (well, the no movement controller should be active)
- return ControlMode::eUnknown;
+ ui.horizontalSliderKinematicUnitPos->setMaximum(sliderMax);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(sliderMin);
-
-}
+ const std::size_t desiredNumberOfTicks = 1'000;
-void KinematicUnitWidgetController::setControlModeTorque()
-{
- if (!ui.radioButtonTorqueControl->isChecked())
- {
- return;
+ const float tickInterval = (sliderMax - sliderMin) / desiredNumberOfTicks;
+ ARMARX_INFO << VAROUT(tickInterval);
+
+ ui.horizontalSliderKinematicUnitPos->setTickInterval(tickInterval);
+ ui.lcdNumberKinematicUnitJointValue->display(pos);
+
+ ui.horizontalSliderKinematicUnitPos->blockSignals(false);
+ resetSlider();
+ }
}
- NameControlModeMap jointModes;
- if (currentNode)
+ void
+ KinematicUnitWidgetController::setControlModeVelocity()
{
- jointModes[currentNode->getName()] = eTorqueControl;
- ui.labelUnit->setText("Ncm");
- ARMARX_INFO << "setting torque control for current Node Name: " << currentNode->getName() << flush;
+ if (!ui.radioButtonVelocityControl->isChecked())
+ {
+ return;
+ }
+ NameControlModeMap jointModes;
+ NameValueMap jointVelocities;
- if (kinematicUnitInterfacePrx)
+ if (currentNode)
{
+ jointModes[currentNode->getName()] = eVelocityControl;
+
+ // set the velocity to zero to stop any previous controller (e.g. torque controller)
+ jointVelocities[currentNode->getName()] = 0;
+
+
+ const QString unit = [&]() -> QString
+ {
+ if (currentNode->isRotationalJoint() or currentNode->isHemisphereJoint())
+ {
+ 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;
+
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint();
+
+ const float lo = isRot ? (isDeg ? -90 : -M_PI * 100) : -1000;
+ const float hi = isRot ? (isDeg ? +90 : +M_PI * 100) : 1000;
+
try
{
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ if (kinematicUnitInterfacePrx)
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
+ }
}
catch (...)
{
-
}
- }
- ui.horizontalSliderKinematicUnitPos->blockSignals(true);
- ui.horizontalSliderKinematicUnitPos->setMaximum(20000.0);
- ui.horizontalSliderKinematicUnitPos->setMinimum(-20000.0);
+ ui.widgetSliderFactor->setVisible(true);
- ui.widgetSliderFactor->setVisible(true);
-
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
+ ui.horizontalSliderKinematicUnitPos->blockSignals(true);
+ ui.horizontalSliderKinematicUnitPos->setMaximum(hi);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
+ ui.horizontalSliderKinematicUnitPos->blockSignals(false);
+ resetSlider();
+ }
}
-}
-VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string fileName)
-{
- VirtualRobot::RobotPtr robot;
-
- if (verbose)
+ ControlMode
+ KinematicUnitWidgetController::getSelectedControlMode() const
{
- ARMARX_INFO << "Loading KinematicUnit " << kinematicUnitName << " from " << kinematicUnitFile << " ..." << flush;
+ if (ui.radioButtonPositionControl->isChecked())
+ {
+ return ControlMode::ePositionControl;
+ }
+
+ if (ui.radioButtonVelocityControl->isChecked())
+ {
+ return ControlMode::eVelocityControl;
+ }
+
+ if (ui.radioButtonTorqueControl->isChecked())
+ {
+ return ControlMode::eTorqueControl;
+ }
+
+ // if no button is checked, then the joint is likely initialized but no controller has been loaded yet
+ // (well, the no movement controller should be active)
+ return ControlMode::eUnknown;
}
- if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
+ void
+ KinematicUnitWidgetController::setControlModeTorque()
{
- ARMARX_INFO << "Could not find Robot XML file with name " << fileName << flush;
- }
+ if (!ui.radioButtonTorqueControl->isChecked())
+ {
+ return;
+ }
+ NameControlModeMap jointModes;
- robot = VirtualRobot::RobotIO::loadRobot(fileName);
+ if (currentNode)
+ {
+ jointModes[currentNode->getName()] = eTorqueControl;
+ ui.labelUnit->setText("Ncm");
+ ARMARX_INFO << "setting torque control for current Node Name: "
+ << currentNode->getName() << flush;
- if (!robot)
- {
- ARMARX_INFO << "Could not find Robot XML file with name " << fileName << "(" << kinematicUnitName << ")" << flush;
- }
+ if (kinematicUnitInterfacePrx)
+ {
+ try
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ }
+ catch (...)
+ {
+ }
+ }
- return robot;
-}
+ ui.horizontalSliderKinematicUnitPos->blockSignals(true);
+ ui.horizontalSliderKinematicUnitPos->setMaximum(20000.0);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(-20000.0);
-VirtualRobot::CoinVisualizationPtr KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
-{
- VirtualRobot::CoinVisualizationPtr coinVisualization;
+ ui.widgetSliderFactor->setVisible(true);
- if (robot != NULL)
+ ui.horizontalSliderKinematicUnitPos->blockSignals(false);
+ resetSlider();
+ }
+ }
+
+ VirtualRobot::RobotPtr
+ KinematicUnitWidgetController::loadRobotFile(std::string fileName)
{
- ARMARX_VERBOSE << "getting coin visualization" << flush;
- coinVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
+ VirtualRobot::RobotPtr robot;
- if (!coinVisualization || !coinVisualization->getCoinVisualization())
+ if (verbose)
{
- ARMARX_INFO << "could not get coin visualization" << flush;
+ ARMARX_INFO << "Loading KinematicUnit " << kinematicUnitName << " from "
+ << kinematicUnitFile << " ..." << flush;
}
- }
- return coinVisualization;
-}
+ if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
+ {
+ ARMARX_INFO << "Could not find Robot XML file with name " << fileName << flush;
+ }
-VirtualRobot::RobotNodeSetPtr KinematicUnitWidgetController::getRobotNodeSet(VirtualRobot::RobotPtr robot, std::string nodeSetName)
-{
- VirtualRobot::RobotNodeSetPtr nodeSetPtr;
+ robot = VirtualRobot::RobotIO::loadRobot(fileName);
+
+ if (!robot)
+ {
+ ARMARX_INFO << "Could not find Robot XML file with name " << fileName << "("
+ << kinematicUnitName << ")" << flush;
+ }
+
+ return robot;
+ }
- if (robot)
+ VirtualRobot::CoinVisualizationPtr
+ KinematicUnitWidgetController::getCoinVisualization(VirtualRobot::RobotPtr robot)
{
- nodeSetPtr = robot->getRobotNodeSet(nodeSetName);
+ VirtualRobot::CoinVisualizationPtr coinVisualization;
- if (!nodeSetPtr)
+ if (robot != NULL)
{
- ARMARX_INFO << "RobotNodeSet with name " << nodeSetName << " is not defined" << flush;
+ ARMARX_VERBOSE << "getting coin visualization" << flush;
+ coinVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
+ if (!coinVisualization || !coinVisualization->getCoinVisualization())
+ {
+ ARMARX_INFO << "could not get coin visualization" << flush;
+ }
}
+
+ return coinVisualization;
}
- return nodeSetPtr;
-}
+ VirtualRobot::RobotNodeSetPtr
+ KinematicUnitWidgetController::getRobotNodeSet(VirtualRobot::RobotPtr robot,
+ std::string nodeSetName)
+ {
+ VirtualRobot::RobotNodeSetPtr nodeSetPtr;
+ if (robot)
+ {
+ nodeSetPtr = robot->getRobotNodeSet(nodeSetName);
-bool KinematicUnitWidgetController::initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet)
-{
- ui.nodeListComboBox->clear();
+ if (!nodeSetPtr)
+ {
+ ARMARX_INFO << "RobotNodeSet with name " << nodeSetName << " is not defined"
+ << flush;
+ }
+ }
+
+ return nodeSetPtr;
+ }
- if (robotNodeSet)
+ bool
+ KinematicUnitWidgetController::initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet)
{
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+ ui.nodeListComboBox->clear();
- for (unsigned int i = 0; i < rn.size(); i++)
+ if (robotNodeSet)
{
- // ARMARX_INFO << "adding item to joint combo box" << rn[i]->getName() << flush;
- QString name(rn[i]->getName().c_str());
- ui.nodeListComboBox->addItem(name);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ // ARMARX_INFO << "adding item to joint combo box" << rn[i]->getName() << flush;
+ QString name(rn[i]->getName().c_str());
+ ui.nodeListComboBox->addItem(name);
+ }
+ ui.nodeListComboBox->setCurrentIndex(-1);
+ return true;
}
- ui.nodeListComboBox->setCurrentIndex(-1);
- return true;
+ return false;
}
- return false;
-}
+ bool
+ KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet)
+ {
+ uint numberOfColumns = 10;
-bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet)
-{
- uint numberOfColumns = 10;
-
- //dont use clear! It is not required here and somehow causes the tabel to have
- //numberOfColumns additional empty columns and rn.size() additional empty rows.
- //Somehow columncount (rowcount) stay at numberOfColumns (rn.size())
- //ui.tableJointList->clear();
+ //dont use clear! It is not required here and somehow causes the tabel to have
+ //numberOfColumns additional empty columns and rn.size() additional empty rows.
+ //Somehow columncount (rowcount) stay at numberOfColumns (rn.size())
+ //ui.tableJointList->clear();
- if (robotNodeSet)
- {
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
-
- //set dimension of table
- //ui.tableJointList->setColumnWidth(0,110);
-
- //ui.tableJointList->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
- ui.tableJointList->setRowCount(rn.size());
- ui.tableJointList->setColumnCount(eTabelColumnCount);
-
-
- //ui.tableJointList->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Expanding);
-
- // set table header
- // if the order is changed dont forget to update the order in the enum JointTabelColumnIndex
- // in theheader file
- QStringList s;
- s << "Joint Name"
- << "Control Mode"
- << "Angle [deg]/Position [mm]"
- << "Velocity [deg/s]/[mm/s]"
- << "Torque [Nm] / PWM"
- << "Current [A]"
- << "Temperature [C]"
- << "Operation"
- << "Error"
- << "Enabled"
- << "Emergency Stop";
- ui.tableJointList->setHorizontalHeaderLabels(s);
- ARMARX_CHECK_EXPRESSION(ui.tableJointList->columnCount() == eTabelColumnCount) << "Current table size: " << ui.tableJointList->columnCount();
-
-
- // fill in joint names
- for (unsigned int i = 0; i < rn.size(); i++)
+ if (robotNodeSet)
{
- // ARMARX_INFO << "adding item to joint table" << rn[i]->getName() << flush;
- QString name(rn[i]->getName().c_str());
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+
+ //set dimension of table
+ //ui.tableJointList->setColumnWidth(0,110);
+
+ //ui.tableJointList->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
+ ui.tableJointList->setRowCount(rn.size());
+ ui.tableJointList->setColumnCount(eTabelColumnCount);
+
+
+ //ui.tableJointList->setSizePolicy(QSizePolicy::Expanding,QSizePolicy::Expanding);
+
+ // set table header
+ // if the order is changed dont forget to update the order in the enum JointTabelColumnIndex
+ // in theheader file
+ QStringList s;
+ s << "Joint Name"
+ << "Control Mode"
+ << "Angle [deg]/Position [mm]"
+ << "Velocity [deg/s]/[mm/s]"
+ << "Torque [Nm] / PWM"
+ << "Current [A]"
+ << "Temperature [C]"
+ << "Operation"
+ << "Error"
+ << "Enabled"
+ << "Emergency Stop";
+ ui.tableJointList->setHorizontalHeaderLabels(s);
+ ARMARX_CHECK_EXPRESSION(ui.tableJointList->columnCount() == eTabelColumnCount)
+ << "Current table size: " << ui.tableJointList->columnCount();
+
+
+ // fill in joint names
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ // ARMARX_INFO << "adding item to joint table" << rn[i]->getName() << flush;
+ QString name(rn[i]->getName().c_str());
- QTableWidgetItem* newItem = new QTableWidgetItem(name);
- ui.tableJointList->setItem(i, eTabelColumnName, newItem);
- }
+ QTableWidgetItem* newItem = new QTableWidgetItem(name);
+ ui.tableJointList->setItem(i, eTabelColumnName, newItem);
+ }
- // init missing table fields with default values
- for (unsigned int i = 0; i < rn.size(); i++)
- {
- for (unsigned int j = 1; j < numberOfColumns; j++)
+ // init missing table fields with default values
+ for (unsigned int i = 0; i < rn.size(); i++)
{
- QString state = "--";
- QTableWidgetItem* newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, j, newItem);
+ for (unsigned int j = 1; j < numberOfColumns; j++)
+ {
+ QString state = "--";
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, j, newItem);
+ }
}
- }
- //hide columns Operation, Error, Enabled and Emergency Stop
- //they will be shown when changes occur
- ui.tableJointList->setColumnHidden(eTabelColumnTemperature, true);
- ui.tableJointList->setColumnHidden(eTabelColumnOperation, true);
- ui.tableJointList->setColumnHidden(eTabelColumnError, true);
- ui.tableJointList->setColumnHidden(eTabelColumnEnabled, true);
- ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, true);
+ //hide columns Operation, Error, Enabled and Emergency Stop
+ //they will be shown when changes occur
+ ui.tableJointList->setColumnHidden(eTabelColumnTemperature, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnOperation, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnError, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnEnabled, true);
+ ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, true);
+
+ return true;
+ }
- return true;
+ return false;
}
- return false;
-}
+ void
+ KinematicUnitWidgetController::selectJoint(int i)
+ {
+ std::unique_lock lock(mutexNodeSet);
+ ARMARX_INFO << "Selected index: " << ui.nodeListComboBox->currentIndex();
-void KinematicUnitWidgetController::selectJoint(int i)
-{
- std::unique_lock lock(mutexNodeSet);
+ if (!robotNodeSet || i < 0 || i >= static_cast<int>(robotNodeSet->getSize()))
+ {
+ return;
+ }
- ARMARX_INFO << "Selected index: " << ui.nodeListComboBox->currentIndex();
+ currentNode = robotNodeSet->getAllRobotNodes()[i];
+ ARMARX_IMPORTANT << "Selected joint is `" << currentNode->getName() << "`.";
- if (!robotNodeSet || i < 0 || i >= static_cast<int>(robotNodeSet->getSize()))
- {
- return;
- }
+ const auto controlModes = kinematicUnitInterfacePrx->getControlModes();
+ if (controlModes.count(currentNode->getName()) == 0)
+ {
+ ARMARX_ERROR << "Could not retrieve control mode for joint `" << currentNode->getName()
+ << "` from kinematic unit!";
+ return;
+ }
- currentNode = robotNodeSet->getAllRobotNodes()[i];
- ARMARX_IMPORTANT << "Selected joint is `" << currentNode->getName() << "`.";
+ const auto controlMode = controlModes.at(currentNode->getName());
+ setControlModeRadioButtonGroup(controlMode);
- const auto controlModes = kinematicUnitInterfacePrx->getControlModes();
- if(controlModes.count(currentNode->getName()) == 0)
- {
- ARMARX_ERROR << "Could not retrieve control mode for joint `" << currentNode->getName() << "` from kinematic unit!";
- return;
+ if (controlMode == ePositionControl)
+ {
+ setControlModePosition();
+ }
+ else if (controlMode == eVelocityControl)
+ {
+ setControlModeVelocity();
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ }
+ else if (controlMode == eTorqueControl)
+ {
+ setControlModeTorque();
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ }
}
- const auto controlMode = controlModes.at(currentNode->getName());
- setControlModeRadioButtonGroup(controlMode);
-
- if (controlMode == ePositionControl)
+ void
+ KinematicUnitWidgetController::selectJointFromTableWidget(int row, int column)
{
- setControlModePosition();
- }
- else if (controlMode == eVelocityControl)
- {
- setControlModeVelocity();
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
- }
- else if (controlMode == eTorqueControl)
- {
- setControlModeTorque();
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ if (column == eTabelColumnName)
+ {
+ ui.nodeListComboBox->setCurrentIndex(row);
+ // selectJoint(row);
+ }
}
-}
-void KinematicUnitWidgetController::selectJointFromTableWidget(int row, int column)
-{
- if (column == eTabelColumnName)
+ void
+ KinematicUnitWidgetController::sliderValueChanged(int pos)
{
- ui.nodeListComboBox->setCurrentIndex(row);
- // selectJoint(row);
- }
-}
-
-void KinematicUnitWidgetController::sliderValueChanged(int pos)
-{
- std::unique_lock lock(mutexNodeSet);
+ std::unique_lock lock(mutexNodeSet);
- if (!currentNode)
- {
- return;
- }
+ if (!currentNode)
+ {
+ return;
+ }
- const float value = static_cast<float>(ui.horizontalSliderKinematicUnitPos->value());
+ const float value = static_cast<float>(ui.horizontalSliderKinematicUnitPos->value());
- const ControlMode currentControlMode = getSelectedControlMode();
+ const ControlMode currentControlMode = getSelectedControlMode();
- const bool isDeg = ui.checkBoxUseDegree->isChecked();
- const bool isRot = currentNode->isRotationalJoint();
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint() or currentNode->isHemisphereJoint();
- if (currentControlMode == ePositionControl)
- {
- const auto factor = isRot && ! isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
- float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
+ if (currentControlMode == ePositionControl)
+ {
+ const auto factor =
+ isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
+ float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
- NameValueMap jointAngles;
+ NameValueMap jointAngles;
- jointAngles[currentNode->getName()] = value / conversionFactor / factor;
- ui.lcdNumberKinematicUnitJointValue->display(value / factor);
- if (kinematicUnitInterfacePrx)
- {
- try
- {
- kinematicUnitInterfacePrx->setJointAngles(jointAngles);
- }
- catch (...)
+ jointAngles[currentNode->getName()] = value / conversionFactor / factor;
+ ui.lcdNumberKinematicUnitJointValue->display(value / factor);
+ if (kinematicUnitInterfacePrx)
{
+ try
+ {
+ kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ }
+ catch (...)
+ {
+ }
}
}
- }
- else if (currentControlMode == eVelocityControl)
- {
- float conversionFactor = isRot ? (isDeg ? 180.0 / M_PI : 100.f) : 1.0f;
- NameValueMap jointVelocities;
- jointVelocities[currentNode->getName()] = value / conversionFactor * static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
- ui.lcdNumberKinematicUnitJointValue->display(value);
-
- if (kinematicUnitInterfacePrx)
+ else if (currentControlMode == eVelocityControl)
{
- try
- {
- kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
- }
- catch (...)
- {
- }
- }
- }
- else if (currentControlMode == eTorqueControl)
- {
- NameValueMap jointTorques;
- float torqueTargetValue = value / 100.0f * static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
- jointTorques[currentNode->getName()] = torqueTargetValue;
- ui.lcdNumberKinematicUnitJointValue->display(torqueTargetValue);
+ float conversionFactor = isRot ? (isDeg ? 180.0 / M_PI : 100.f) : 1.0f;
+ NameValueMap jointVelocities;
+ jointVelocities[currentNode->getName()] =
+ value / conversionFactor *
+ static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
+ ui.lcdNumberKinematicUnitJointValue->display(value);
- if (kinematicUnitInterfacePrx)
- {
- try
+ if (kinematicUnitInterfacePrx)
{
- kinematicUnitInterfacePrx->setJointTorques(jointTorques);
+ try
+ {
+ kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
+ }
+ catch (...)
+ {
+ }
}
- catch (...)
+ }
+ else if (currentControlMode == eTorqueControl)
+ {
+ NameValueMap jointTorques;
+ float torqueTargetValue =
+ value / 100.0f *
+ static_cast<float>(ui.doubleSpinBoxKinematicUnitPosFactor->value());
+ jointTorques[currentNode->getName()] = torqueTargetValue;
+ ui.lcdNumberKinematicUnitJointValue->display(torqueTargetValue);
+
+ if (kinematicUnitInterfacePrx)
{
+ try
+ {
+ kinematicUnitInterfacePrx->setJointTorques(jointTorques);
+ }
+ catch (...)
+ {
+ }
}
}
+ else
+ {
+ ARMARX_INFO << "current ControlModes unknown" << flush;
+ }
}
- else
- {
- ARMARX_INFO << "current ControlModes unknown" << flush;
- }
-}
-
-
-
-void KinematicUnitWidgetController::updateControlModesTable(const NameControlModeMap& reportedJointControlModes)
-{
- if (!getWidget() || !robotNodeSet)
- {
- return;
- }
-
- std::unique_lock lock(mutexNodeSet);
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateControlModesTable(
+ const NameControlModeMap& reportedJointControlModes)
{
- NameControlModeMap::const_iterator it;
- it = reportedJointControlModes.find(rn[i]->getName());
- QString state;
-
- if (it == reportedJointControlModes.end())
+ if (!getWidget() || !robotNodeSet)
{
- state = "unknown";
+ return;
}
- else
- {
- ControlMode currentMode = it->second;
+ std::unique_lock lock(mutexNodeSet);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ NameControlModeMap::const_iterator it;
+ it = reportedJointControlModes.find(rn[i]->getName());
+ QString state;
- switch (currentMode)
+ if (it == reportedJointControlModes.end())
+ {
+ state = "unknown";
+ }
+ else
{
- /*case eNoMode:
+ ControlMode currentMode = it->second;
+
+
+ switch (currentMode)
+ {
+ /*case eNoMode:
state = "None";
break;
@@ -1111,531 +1292,567 @@ void KinematicUnitWidgetController::updateControlModesTable(const NameControlMod
state = "Unknown";
break;
*/
- case eDisabled:
- state = "Disabled";
- break;
+ case eDisabled:
+ state = "Disabled";
+ break;
- case eUnknown:
- state = "Unknown";
- break;
+ case eUnknown:
+ state = "Unknown";
+ break;
- case ePositionControl:
- state = "Position";
- break;
+ case ePositionControl:
+ state = "Position";
+ break;
- case eVelocityControl:
- state = "Velocity";
- break;
+ case eVelocityControl:
+ state = "Velocity";
+ break;
- case eTorqueControl:
- state = "Torque";
- break;
+ case eTorqueControl:
+ state = "Torque";
+ break;
- case ePositionVelocityControl:
- state = "Position + Velocity";
- break;
+ case ePositionVelocityControl:
+ state = "Position + Velocity";
+ break;
- default:
- //show the value of the mode so it can be implemented
- state = QString("<nyi Mode: %1>").arg(static_cast<int>(currentMode));
- break;
+ default:
+ //show the value of the mode so it can be implemented
+ state = QString("<nyi Mode: %1>").arg(static_cast<int>(currentMode));
+ break;
+ }
}
- }
- QTableWidgetItem* newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnControlMode, newItem);
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnControlMode, newItem);
+ }
}
-}
-void KinematicUnitWidgetController::updateJointStatusesTable(const NameStatusMap& reportedJointStatuses)
-{
- if (!getWidget() || !robotNodeSet || reportedJointStatuses.empty())
+ void
+ KinematicUnitWidgetController::updateJointStatusesTable(
+ const NameStatusMap& reportedJointStatuses)
{
- return;
- }
+ if (!getWidget() || !robotNodeSet || reportedJointStatuses.empty())
+ {
+ return;
+ }
- std::unique_lock lock(mutexNodeSet);
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+ std::unique_lock lock(mutexNodeSet);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
- for (unsigned int i = 0; i < rn.size(); i++)
- {
- NameStatusMap::const_iterator it;
- it = reportedJointStatuses.find(rn[i]->getName());
- JointStatus currentStatus = it->second;
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
- QString state = translateStatus(currentStatus.operation);
- QTableWidgetItem* newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnOperation, newItem);
+ auto it = reportedJointStatuses.find(rn[i]->getName());
+ if (it == reportedJointStatuses.end())
+ {
+ ARMARX_WARNING << deactivateSpam(5) << "Joint Status for " << rn[i]->getName()
+ << " was not reported!";
+ continue;
+ }
+ JointStatus currentStatus = it->second;
- state = translateStatus(currentStatus.error);
- newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnError, newItem);
+ QString state = translateStatus(currentStatus.operation);
+ QTableWidgetItem* newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnOperation, newItem);
- state = currentStatus.enabled ? "X" : "-";
- newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnEnabled, newItem);
+ state = translateStatus(currentStatus.error);
+ newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnError, newItem);
- state = currentStatus.emergencyStop ? "X" : "-";
- newItem = new QTableWidgetItem(state);
- ui.tableJointList->setItem(i, eTabelColumnEmergencyStop, newItem);
- }
+ state = currentStatus.enabled ? "X" : "-";
+ newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnEnabled, newItem);
- //show columns
- ui.tableJointList->setColumnHidden(eTabelColumnOperation, false);
- ui.tableJointList->setColumnHidden(eTabelColumnError, false);
- ui.tableJointList->setColumnHidden(eTabelColumnEnabled, false);
- ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, false);
-}
+ state = currentStatus.emergencyStop ? "X" : "-";
+ newItem = new QTableWidgetItem(state);
+ ui.tableJointList->setItem(i, eTabelColumnEmergencyStop, newItem);
+ }
-QString KinematicUnitWidgetController::translateStatus(OperationStatus status)
-{
- switch (status)
+ //show columns
+ ui.tableJointList->setColumnHidden(eTabelColumnOperation, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnError, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnEnabled, false);
+ ui.tableJointList->setColumnHidden(eTabelColumnEmergencyStop, false);
+ }
+
+ QString
+ KinematicUnitWidgetController::translateStatus(OperationStatus status)
{
- case eOffline:
- return "Offline";
+ switch (status)
+ {
+ case eOffline:
+ return "Offline";
- case eOnline:
- return "Online";
+ case eOnline:
+ return "Online";
- case eInitialized:
- return "Initialized";
+ case eInitialized:
+ return "Initialized";
- default:
- return "?";
+ default:
+ return "?";
+ }
}
-}
-QString KinematicUnitWidgetController::translateStatus(ErrorStatus status)
-{
- switch (status)
+ QString
+ KinematicUnitWidgetController::translateStatus(ErrorStatus status)
{
- case eOk:
- return "Ok";
-
- case eWarning:
- return "Wr";
-
- case eError:
- return "Er";
+ switch (status)
+ {
+ case eOk:
+ return "Ok";
- default:
- return "?";
- }
-}
+ case eWarning:
+ return "Wr";
-void KinematicUnitWidgetController::updateJointAnglesTable(const NameValueMap& reportedJointAngles)
-{
- std::unique_lock lock(mutexNodeSet);
+ case eError:
+ return "Er";
- if (!robotNodeSet)
- {
- return;
+ default:
+ return "?";
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
-
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointAnglesTable(const NameValueMap& reportedJointAngles)
{
- NameValueMap::const_iterator it;
- VirtualRobot::RobotNodePtr node = rn[i];
- it = reportedJointAngles.find(node->getName());
+ std::unique_lock lock(mutexNodeSet);
- if (it == reportedJointAngles.end())
+ if (!robotNodeSet)
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
- const float currentValue = it->second;
- QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
- float conversionFactor = ui.checkBoxUseDegree->isChecked() &&
- node->isRotationalJoint() ? 180.0 / M_PI : 1;
- ui.tableJointList->model()->setData(index, (int)(cutJitter(currentValue * conversionFactor) * 100) / 100.0f, eJointAngleRole);
- ui.tableJointList->model()->setData(index, node->getJointLimitHigh() * conversionFactor, eJointHiRole);
- ui.tableJointList->model()->setData(index, node->getJointLimitLow() * conversionFactor, eJointLoRole);
- }
-}
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ NameValueMap::const_iterator it;
+ VirtualRobot::RobotNodePtr node = rn[i];
+ it = reportedJointAngles.find(node->getName());
-void KinematicUnitWidgetController::updateJointVelocitiesTable(const NameValueMap& reportedJointVelocities)
-{
- if (!getWidget())
- {
- return;
- }
+ if (it == reportedJointAngles.end())
+ {
+ continue;
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!robotNodeSet)
- {
- return;
+ const float currentValue = it->second;
+
+ QModelIndex index = ui.tableJointList->model()->index(i, eTabelColumnAngleProgressbar);
+ float conversionFactor =
+ ui.checkBoxUseDegree->isChecked() &&
+ (node->isRotationalJoint() or node->isHemisphereJoint())
+ ? 180.0 / M_PI
+ : 1;
+ ui.tableJointList->model()->setData(
+ index,
+ (int)(cutJitter(currentValue * conversionFactor) * 100) / 100.0f,
+ eJointAngleRole);
+ ui.tableJointList->model()->setData(
+ index, node->getJointLimitHigh() * conversionFactor, eJointHiRole);
+ ui.tableJointList->model()->setData(
+ index, node->getJointLimitLow() * conversionFactor, eJointLoRole);
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointVelocitiesTable(
+ const NameValueMap& reportedJointVelocities)
{
- NameValueMap::const_iterator it;
- it = reportedJointVelocities.find(rn[i]->getName());
-
- if (it == reportedJointVelocities.end())
+ if (!getWidget())
{
- continue;
+ return;
}
- float currentValue = it->second;
- if (ui.checkBoxUseDegree->isChecked() && rn[i]->isRotationalJoint())
+ std::unique_lock lock(mutexNodeSet);
+ if (!robotNodeSet)
{
- currentValue *= 180.0 / M_PI;
+ return;
}
- const QString Text = QString::number(cutJitter(currentValue), 'g', 2);
- newItem = new QTableWidgetItem(Text);
- ui.tableJointList->setItem(i, eTabelColumnVelocity, newItem);
- }
-}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
-void KinematicUnitWidgetController::updateJointTorquesTable(const NameValueMap& reportedJointTorques)
-{
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ NameValueMap::const_iterator it;
+ it = reportedJointVelocities.find(rn[i]->getName());
+ if (it == reportedJointVelocities.end())
+ {
+ continue;
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!getWidget() || !robotNodeSet)
- {
- return;
+ float currentValue = it->second;
+ if (ui.checkBoxUseDegree->isChecked() &&
+ (rn[i]->isRotationalJoint() or rn[i]->isHemisphereJoint()))
+ {
+ currentValue *= 180.0 / M_PI;
+ }
+ const QString Text = QString::number(cutJitter(currentValue), 'g', 2);
+ newItem = new QTableWidgetItem(Text);
+ ui.tableJointList->setItem(i, eTabelColumnVelocity, newItem);
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- NameValueMap::const_iterator it;
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointTorquesTable(const NameValueMap& reportedJointTorques)
{
- it = reportedJointTorques.find(rn[i]->getName());
- if (it == reportedJointTorques.end())
+
+ std::unique_lock lock(mutexNodeSet);
+ if (!getWidget() || !robotNodeSet)
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
+ NameValueMap::const_iterator it;
- const float currentValue = it->second;
- newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
- ui.tableJointList->setItem(i, eTabelColumnTorque, newItem);
- }
-}
-
-void KinematicUnitWidgetController::updateJointCurrentsTable(const NameValueMap& reportedJointCurrents, const NameStatusMap& reportedJointStatuses)
-{
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ it = reportedJointTorques.find(rn[i]->getName());
+ if (it == reportedJointTorques.end())
+ {
+ continue;
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!getWidget() || !robotNodeSet || jointCurrentHistory.size() == 0)
- {
- return;
+ const float currentValue = it->second;
+ newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
+ ui.tableJointList->setItem(i, eTabelColumnTorque, newItem);
+ }
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- // FIXME history!
- // NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
- NameValueMap::const_iterator it;
-
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateJointCurrentsTable(
+ const NameValueMap& reportedJointCurrents,
+ const NameStatusMap& reportedJointStatuses)
{
- it = reportedJointCurrents.find(rn[i]->getName());
- if (it == reportedJointCurrents.end())
+
+ std::unique_lock lock(mutexNodeSet);
+ if (!getWidget() || !robotNodeSet || jointCurrentHistory.size() == 0)
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
- const float currentValue = it->second;
- newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
- ui.tableJointList->setItem(i, eTabelColumnCurrent, newItem);
- }
+ // FIXME history!
+ // NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
+ NameValueMap::const_iterator it;
- highlightCriticalValues(reportedJointStatuses);
-}
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ it = reportedJointCurrents.find(rn[i]->getName());
-void KinematicUnitWidgetController::updateMotorTemperaturesTable(const NameValueMap& reportedJointTemperatures)
-{
+ if (it == reportedJointCurrents.end())
+ {
+ continue;
+ }
+ const float currentValue = it->second;
+ newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
+ ui.tableJointList->setItem(i, eTabelColumnCurrent, newItem);
+ }
- std::unique_lock lock(mutexNodeSet);
- if (!getWidget() || !robotNodeSet || reportedJointTemperatures.empty())
- {
- return;
+ highlightCriticalValues(reportedJointStatuses);
}
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
- QTableWidgetItem* newItem;
- NameValueMap::const_iterator it;
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::updateMotorTemperaturesTable(
+ const NameValueMap& reportedJointTemperatures)
{
- it = reportedJointTemperatures.find(rn[i]->getName());
- if (it == reportedJointTemperatures.end())
+
+ std::unique_lock lock(mutexNodeSet);
+ if (!getWidget() || !robotNodeSet || reportedJointTemperatures.empty())
{
- continue;
+ return;
}
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ QTableWidgetItem* newItem;
+ NameValueMap::const_iterator it;
- const float currentValue = it->second;
- newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
- ui.tableJointList->setItem(i, eTabelColumnTemperature, newItem);
- }
- ui.tableJointList->setColumnHidden(eTabelColumnTemperature, false);
-
-}
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ it = reportedJointTemperatures.find(rn[i]->getName());
+ if (it == reportedJointTemperatures.end())
+ {
+ continue;
+ }
-void KinematicUnitWidgetController::updateModel(const NameValueMap& reportedJointAngles)
-{
- // ARMARX_INFO << "updateModel()" << flush;
- std::unique_lock lock(mutexNodeSet);
- if (!robotNodeSet)
- {
- return;
+ const float currentValue = it->second;
+ newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
+ ui.tableJointList->setItem(i, eTabelColumnTemperature, newItem);
+ }
+ ui.tableJointList->setColumnHidden(eTabelColumnTemperature, false);
}
- robot->setJointValues(reportedJointAngles);
-}
-std::optional<float> mean(const boost::circular_buffer<NameValueMap>& buffer, const std::string& key)
-{
- float sum = 0;
- std::size_t count = 0;
-
- for(const auto& element: buffer)
+ void
+ KinematicUnitWidgetController::updateModel(const NameValueMap& reportedJointAngles)
{
- if(element.count(key) > 0)
+ // ARMARX_INFO << "updateModel()" << flush;
+ std::unique_lock lock(mutexNodeSet);
+ if (!robotNodeSet)
{
- sum += element.at(key);
+ return;
}
+ robot->setJointValues(reportedJointAngles);
}
- if(count == 0)
+ std::optional<float>
+ mean(const boost::circular_buffer<NameValueMap>& buffer, const std::string& key)
{
- return std::nullopt;
- }
-
- return sum / static_cast<float>(count);
-}
-
-void KinematicUnitWidgetController::highlightCriticalValues(const NameStatusMap& reportedJointStatuses)
-{
- if (!enableValueValidator)
- {
- return;
- }
+ float sum = 0;
+ std::size_t count = 0;
- std::unique_lock lock(mutexNodeSet);
-
- std::vector< VirtualRobot::RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
+ for (const auto& element : buffer)
+ {
+ if (element.count(key) > 0)
+ {
+ sum += element.at(key);
+ }
+ }
- // get standard line colors
- static std::vector<QBrush> standardColors;
- if (standardColors.size() == 0)
- {
- for (unsigned int i = 0; i < rn.size(); i++)
+ if (count == 0)
{
- // all cells of a row have the same color
- standardColors.push_back(ui.tableJointList->item(i, eTabelColumnCurrent)->background());
+ return std::nullopt;
}
+
+ return sum / static_cast<float>(count);
}
- // check robot current value of nodes
- for (unsigned int i = 0; i < rn.size(); i++)
+ void
+ KinematicUnitWidgetController::highlightCriticalValues(
+ const NameStatusMap& reportedJointStatuses)
{
- const auto& jointName = rn[i]->getName();
-
- const auto currentSmoothValOpt = mean(jointCurrentHistory, jointName);
- if(not currentSmoothValOpt.has_value())
+ if (!enableValueValidator)
{
- continue;
+ return;
}
- const float smoothValue = std::fabs(currentSmoothValOpt.value());
+ std::unique_lock lock(mutexNodeSet);
+
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
- if(jointCurrentHistory.front().count(jointName) == 0)
+ // get standard line colors
+ static std::vector<QBrush> standardColors;
+ if (standardColors.size() == 0)
{
- continue;
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ // all cells of a row have the same color
+ standardColors.push_back(
+ ui.tableJointList->item(i, eTabelColumnCurrent)->background());
+ }
}
- const float startValue = jointCurrentHistory.front().at(jointName);
- const bool isStatic = (smoothValue == startValue);
+ // check robot current value of nodes
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ const auto& jointName = rn[i]->getName();
+
+ const auto currentSmoothValOpt = mean(jointCurrentHistory, jointName);
+ if (not currentSmoothValOpt.has_value())
+ {
+ continue;
+ }
+
+ const float smoothValue = std::fabs(currentSmoothValOpt.value());
+
+ if (jointCurrentHistory.front().count(jointName) == 0)
+ {
+ continue;
+ }
- NameStatusMap::const_iterator it;
- it = reportedJointStatuses.find(rn[i]->getName());
- JointStatus currentStatus = it->second;
+ const float startValue = jointCurrentHistory.front().at(jointName);
+ const bool isStatic = (smoothValue == startValue);
- if (isStatic)
- {
- if (currentStatus.operation != eOffline)
+ NameStatusMap::const_iterator it;
+ it = reportedJointStatuses.find(rn[i]->getName());
+ JointStatus currentStatus = it->second;
+
+ if (isStatic)
+ {
+ if (currentStatus.operation != eOffline)
+ {
+ // current value is zero, but joint is not offline
+ ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::yellow);
+ }
+ }
+ else if (std::abs(smoothValue) > currentValueMax)
+ {
+ // current value is too high
+ ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
+ }
+ else
{
- // current value is zero, but joint is not offline
- ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::yellow);
+ // everything seems to work as expected
+ ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(standardColors[i]);
}
+ }
+ }
+
+ void
+ KinematicUnitWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
+ {
+ this->mutex3D = mutex3D;
+ if (debugDrawer)
+ {
+ debugDrawer->setMutex(mutex3D);
}
- else if (std::abs(smoothValue) > currentValueMax)
+ }
+
+ QPointer<QWidget>
+ KinematicUnitWidgetController::getCustomTitlebarWidget(QWidget* parent)
+ {
+ if (customToolbar)
{
- // current value is too high
- ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
+ customToolbar->setParent(parent);
}
else
{
- // everything seems to work as expected
- ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(standardColors[i]);
+ customToolbar = new QToolBar(parent);
+ customToolbar->addAction("ZeroVelocity", this, SLOT(kinematicUnitZeroVelocity()));
}
+ return customToolbar.data();
}
-}
-
-void KinematicUnitWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
-{
- this->mutex3D = mutex3D;
- if (debugDrawer)
+ float
+ KinematicUnitWidgetController::cutJitter(float value)
{
- debugDrawer->setMutex(mutex3D);
+ return (abs(value) < static_cast<float>(ui.jitterThresholdSpinBox->value())) ? 0 : value;
}
-}
-QPointer<QWidget> KinematicUnitWidgetController::getCustomTitlebarWidget(QWidget* parent)
-{
- if (customToolbar)
+ void
+ KinematicUnitWidgetController::fetchData()
{
- customToolbar->setParent(parent);
- }
- else
- {
- customToolbar = new QToolBar(parent);
- customToolbar->addAction("ZeroVelocity", this, SLOT(kinematicUnitZeroVelocity()));
- }
- return customToolbar.data();
-}
+ ARMARX_DEBUG << "updateGui";
-float KinematicUnitWidgetController::cutJitter(float value)
-{
- return (abs(value) < static_cast<float>(ui.jitterThresholdSpinBox->value())) ? 0 : value;
-}
+ if (not kinematicUnitInterfacePrx)
+ {
+ ARMARX_WARNING << "KinematicUnit is not available!";
+ return;
+ }
-void KinematicUnitWidgetController::fetchData()
-{
- ARMARX_DEBUG << "updateGui";
+ const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
- if(not kinematicUnitInterfacePrx)
- {
- ARMARX_WARNING << "KinematicUnit is not available!";
- return;
+ emit onDebugInfoReceived(debugInfo);
}
- const auto debugInfo = kinematicUnitInterfacePrx->getDebugInfo();
-
- emit onDebugInfoReceived(debugInfo);
-}
-
-void KinematicUnitWidgetController::debugInfoReceived(const DebugInfo& debugInfo)
-{
- ARMARX_DEBUG << "debug info received";
-
- updateModel(debugInfo.jointAngles);
+ void
+ KinematicUnitWidgetController::debugInfoReceived(const DebugInfo& debugInfo)
+ {
+ ARMARX_DEBUG << "debug info received";
- updateJointAnglesTable(debugInfo.jointAngles);
- updateJointVelocitiesTable(debugInfo.jointVelocities);
- updateJointTorquesTable(debugInfo.jointTorques);
- updateJointCurrentsTable(debugInfo.jointCurrents, debugInfo.jointStatus);
- updateControlModesTable(debugInfo.jointModes);
- updateJointStatusesTable(debugInfo.jointStatus);
- updateMotorTemperaturesTable(debugInfo.jointMotorTemperatures);
+ updateModel(debugInfo.jointAngles);
-}
+ updateJointAnglesTable(debugInfo.jointAngles);
+ updateJointVelocitiesTable(debugInfo.jointVelocities);
+ updateJointTorquesTable(debugInfo.jointTorques);
+ updateJointCurrentsTable(debugInfo.jointCurrents, debugInfo.jointStatus);
+ updateControlModesTable(debugInfo.jointModes);
+ updateJointStatusesTable(debugInfo.jointStatus);
+ updateMotorTemperaturesTable(debugInfo.jointMotorTemperatures);
+ }
-void RangeValueDelegate::paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const
-{
- if (index.column() == KinematicUnitWidgetController::eTabelColumnAngleProgressbar)
+ void
+ RangeValueDelegate::paint(QPainter* painter,
+ const QStyleOptionViewItem& option,
+ const QModelIndex& index) const
{
- float jointValue = index.data(KinematicUnitWidgetController:: eJointAngleRole).toFloat();
- float loDeg = index.data(KinematicUnitWidgetController::eJointLoRole).toFloat();
- float hiDeg = index.data(KinematicUnitWidgetController::eJointHiRole).toFloat();
+ if (index.column() == KinematicUnitWidgetController::eTabelColumnAngleProgressbar)
+ {
+ float jointValue = index.data(KinematicUnitWidgetController::eJointAngleRole).toFloat();
+ float loDeg = index.data(KinematicUnitWidgetController::eJointLoRole).toFloat();
+ float hiDeg = index.data(KinematicUnitWidgetController::eJointHiRole).toFloat();
+
+ if (hiDeg - loDeg <= 0)
+ {
+ QStyledItemDelegate::paint(painter, option, index);
+ return;
+ }
- if (hiDeg - loDeg <= 0)
+ QStyleOptionProgressBar progressBarOption;
+ progressBarOption.rect = option.rect;
+ progressBarOption.minimum = loDeg;
+ progressBarOption.maximum = hiDeg;
+ progressBarOption.progress = jointValue;
+ progressBarOption.text = QString::number(jointValue);
+ progressBarOption.textVisible = true;
+ QPalette pal;
+ pal.setColor(QPalette::Background, Qt::red);
+ progressBarOption.palette = pal;
+ QApplication::style()->drawControl(QStyle::CE_ProgressBar, &progressBarOption, painter);
+ }
+ else
{
QStyledItemDelegate::paint(painter, option, index);
- return;
}
-
- QStyleOptionProgressBar progressBarOption;
- progressBarOption.rect = option.rect;
- progressBarOption.minimum = loDeg;
- progressBarOption.maximum = hiDeg;
- progressBarOption.progress = jointValue;
- progressBarOption.text = QString::number(jointValue);
- progressBarOption.textVisible = true;
- QPalette pal;
- pal.setColor(QPalette::Background, Qt::red);
- progressBarOption.palette = pal;
- QApplication::style()->drawControl(QStyle::CE_ProgressBar,
- &progressBarOption, painter);
-
}
- else
+
+ KinematicUnitWidgetController::~KinematicUnitWidgetController()
{
- QStyledItemDelegate::paint(painter, option, index);
- }
-}
+ kinematicUnitInterfacePrx = nullptr;
-KinematicUnitWidgetController::~KinematicUnitWidgetController()
-{
- kinematicUnitInterfacePrx = nullptr;
+ if (updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
+ }
- if(updateTask)
+ void
+ KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
{
- updateTask->stop();
- updateTask->join();
- updateTask = nullptr;
- }
-}
+ bool ok;
+ const auto text = QInputDialog::getMultiLineText(
+ __widget, tr("JSON Joint values"), tr("Json:"), "{\n}", &ok)
+ .toStdString();
-void KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
-{
- bool ok;
- const auto text = QInputDialog::getMultiLineText(
- __widget,
- tr("JSON Joint values"),
- tr("Json:"), "{\n}", &ok).toStdString();
+ if (!ok || text.empty())
+ {
+ return;
+ }
- if (!ok || text.empty())
- {
- return;
- }
+ NameValueMap jointAngles;
+ try
+ {
+ jointAngles = simox::json::json2NameValueMap(text);
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "invalid json";
+ }
- NameValueMap jointAngles;
- try
- {
- jointAngles = simox::json::json2NameValueMap(text);
- }
- catch (...)
- {
- ARMARX_ERROR << "invalid json";
- }
+ NameControlModeMap jointModes;
+ for (const auto& [key, _] : jointAngles)
+ {
+ jointModes[key] = ePositionControl;
+ }
- NameControlModeMap jointModes;
- for (const auto& [key, _] : jointAngles)
- {
- jointModes[key] = ePositionControl;
+ try
+ {
+ kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "failed to switch mode or set angles";
+ }
}
- try
+ void
+ KinematicUnitWidgetController::synchronizeRobotJointAngles()
{
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointAngles(jointAngles);
+ const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+ robot->setJointValues(currentJointAngles);
}
- catch (...)
- {
- ARMARX_ERROR << "failed to switch mode or set angles";
- }
-}
-
-void KinematicUnitWidgetController::synchronizeRobotJointAngles()
-{
- const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
- robot->setJointValues(currentJointAngles);
-}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index b40d5a2fcf9c480a71f0576d049ad0a311813815..a6d2ff63c1462557b5c55fc9538228dc46509ef1 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
@@ -63,6 +63,7 @@ set(COMPONENT_LIBS
RobotAPIInterfaces
aron
RobotAPISkills
+ SkillsMemory
aronjsonconverter
SimpleConfigDialog
)
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index 7c25a1b848daab3347a7247199aa9a54eefb99bc..b96d3850e2c7181892e22030c600157963fb41af 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -20,8 +20,38 @@
<string>SkillManagerMonitorWidget</string>
</property>
<layout class="QGridLayout" name="gridLayout_3">
- <item row="0" column="0">
+ <item row="3" column="0">
+ <widget class="QCheckBox" name="checkBoxAutoUpdate">
+ <property name="text">
+ <string>Auto Update</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="3">
+ <widget class="QPushButton" name="pushButtonRefreshNow">
+ <property name="text">
+ <string>Refresh Now</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Update Frequency:</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0" colspan="4">
<widget class="QSplitter" name="splitter_2">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
@@ -34,25 +64,59 @@
<property name="childrenCollapsible">
<bool>false</bool>
</property>
- <widget class="QGroupBox" name="groupBoxActiveSkills">
+ <widget class="QGroupBox" name="groupBoxSkillExecutions">
<property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
- <string>Active Skills</string>
+ <string>Executions</string>
</property>
<layout class="QGridLayout" name="gridLayout_4">
- <item row="0" column="0">
- <widget class="QListWidget" name="listWidgetActiveSkills">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
+ <item row="1" column="0" colspan="3">
+ <widget class="QTreeWidget" name="treeWidgetSkillExecutions">
+ <column>
+ <property name="text">
+ <string>ExecutionID</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Executor</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>SkillID</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>IsConstructing</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>IsInitializing</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>IsPreparing</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>IsRunning</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Finished</string>
+ </property>
+ </column>
</widget>
</item>
</layout>
@@ -78,17 +142,7 @@
<string>Manager</string>
</property>
<layout class="QGridLayout" name="gridLayout">
- <item row="3" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Update Frequency:</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0" colspan="3">
+ <item row="5" column="0" colspan="3">
<widget class="QTreeWidget" name="treeWidgetSkills">
<column>
<property name="text">
@@ -97,33 +151,36 @@
</column>
<column>
<property name="text">
- <string>HasType</string>
+ <string>HasInputType</string>
</property>
</column>
<column>
<property name="text">
- <string>State</string>
+ <string>HasOutputType</string>
</property>
</column>
</widget>
</item>
- <item row="2" column="0">
- <widget class="QCheckBox" name="autoUpdateCheckBox">
+ <item row="4" column="1">
+ <widget class="QPushButton" name="pushButtonSearch">
<property name="text">
- <string>Auto Update</string>
+ <string>Search</string>
</property>
</widget>
</item>
- <item row="2" column="2">
- <widget class="QPushButton" name="refreshNowPushButton">
+ <item row="4" column="0">
+ <widget class="QLineEdit" name="lineEditSearch">
<property name="text">
- <string>Refresh Now</string>
+ <string>Search...</string>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QGroupBox" name="groupBoxSkillDetails">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Expanding">
<horstretch>0</horstretch>
@@ -134,7 +191,28 @@
<string>Skill Details</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="1" column="0" colspan="4">
+ <item row="8" column="3">
+ <widget class="QPushButton" name="pushButtonExecuteSkill">
+ <property name="text">
+ <string>Request Execution</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QPushButton" name="pushButtonPaste">
+ <property name="text">
+ <string>Set args from clipboard</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QPushButton" name="pushButtonReset">
+ <property name="text">
+ <string>Reset args to profile</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0" colspan="4">
<widget class="QTreeWidget" name="treeWidgetSkillDetails">
<property name="contextMenuPolicy">
<enum>Qt::CustomContextMenu</enum>
@@ -159,39 +237,11 @@
</column>
<column>
<property name="text">
- <string>defaultValue</string>
+ <string>defaultValue (hidden in GUI)</string>
</property>
</column>
</widget>
</item>
- <item row="6" column="3">
- <widget class="QPushButton" name="pushButtonExecuteSkill">
- <property name="text">
- <string>Request Execution</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QPushButton" name="pushButtonPaste">
- <property name="text">
- <string>Set from clipboard</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QPushButton" name="pushButtonStopSkill">
- <property name="text">
- <string>Stop current skill</string>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QPushButton" name="pushButtonReset">
- <property name="text">
- <string>Reset args</string>
- </property>
- </widget>
- </item>
<item row="0" column="1">
<widget class="QPushButton" name="pushButtonCopy">
<property name="text">
@@ -199,6 +249,15 @@
</property>
</widget>
</item>
+ <item row="1" column="1" colspan="3">
+ <widget class="QComboBox" name="comboBoxProfiles">
+ <item>
+ <property name="text">
+ <string><No Profile selected. Using root></string>
+ </property>
+ </item>
+ </widget>
+ </item>
</layout>
</widget>
</widget>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index f9fa41252ad6a0f1144bf7eb043f62771061b5fc..191c160194207e561a1be7582c038361299a81ac 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -24,7 +24,7 @@
#include <string>
-#include <RobotAPI/libraries/skills/provider/Skill.h>
+#include <RobotAPI/libraries/skills/core/Skill.h>
#include "aronTreeWidget/visitors/AronTreeWidgetConverter.h"
#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
@@ -38,10 +38,11 @@
#include <QDoubleSpinBox>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
#include "aronTreeWidget/Data.h"
-//config
+//configSk
namespace armarx
{
QPointer<QDialog>
@@ -50,8 +51,8 @@ namespace armarx
if (!dialog)
{
dialog = new SimpleConfigDialog(parent);
- dialog->addProxyFinder<skills::manager::dti::SkillManagerInterfacePrx>(
- "SkillManager", "", "Skill*");
+ dialog->addProxyFinder<skills::dti::SkillMemoryInterfacePrx>(
+ "SkillMemory", "", "SkillMem*");
}
return qobject_cast<SimpleConfigDialog*>(dialog);
}
@@ -59,37 +60,60 @@ namespace armarx
void
SkillManagerMonitorWidgetController::configured()
{
- observerName = dialog->getProxyName("SkillManager");
+ observerName = dialog->getProxyName("SkillMemory");
}
void
SkillManagerMonitorWidgetController::loadSettings(QSettings* settings)
{
- observerName = settings->value("SkillManager", "SkillManager").toString().toStdString();
+ observerName = settings->value("SkillMemory", "SkillMemory").toString().toStdString();
}
void
SkillManagerMonitorWidgetController::saveSettings(QSettings* settings)
{
- settings->setValue("SkillManager", QString::fromStdString(observerName));
+ settings->setValue("SkillMemory", QString::fromStdString(observerName));
}
} // namespace armarx
// Others
namespace armarx
{
+ SkillExecutionInfoTreeWidgetItem*
+ SkillExecutionInfoTreeWidgetItem::SearchRecursiveForMatch(
+ SkillExecutionInfoTreeWidgetItem* haystack,
+ const skills::SkillExecutionID& needle)
+ {
+ if (!haystack)
+ {
+ return nullptr;
+ }
+
+ if (needle == haystack->executionId)
+ {
+ return haystack;
+ }
+ for (int i = 0; i < haystack->childCount(); ++i)
+ {
+ auto el = static_cast<SkillExecutionInfoTreeWidgetItem*>(haystack->child(i));
+ return SkillExecutionInfoTreeWidgetItem::SearchRecursiveForMatch(el, needle);
+ }
+ return nullptr;
+ }
+
SkillManagerMonitorWidgetController::SkillManagerMonitorWidgetController()
{
widget.setupUi(getWidget());
- widget.doubleSpinBoxUpdateFreq->setValue(5.0);
- widget.doubleSpinBoxUpdateFreq->setMinimum(0);
- widget.doubleSpinBoxUpdateFreq->setMaximum(100);
+ double default_hz = 2;
+ widget.doubleSpinBoxUpdateFreq->setValue(default_hz);
+ widget.doubleSpinBoxUpdateFreq->setMinimum(0.5);
+ widget.doubleSpinBoxUpdateFreq->setMaximum(20);
widget.doubleSpinBoxUpdateFreq->setSingleStep(0.5);
widget.doubleSpinBoxUpdateFreq->setSuffix(" Hz");
refreshSkillsResultTimer = new QTimer(this);
- refreshSkillsResultTimer->setInterval(1000 / 2); // Keep this stable.
+ updateTimerFrequency();
refreshSkillsResultTimer->start();
connect(widget.doubleSpinBoxUpdateFreq,
@@ -99,7 +123,7 @@ namespace armarx
connect(refreshSkillsResultTimer,
&QTimer::timeout,
this,
- &SkillManagerMonitorWidgetController::refreshSkillsPeriodically);
+ &SkillManagerMonitorWidgetController::refreshSkillsAndExecutions);
connect(widget.pushButtonCopy,
&QPushButton::clicked,
@@ -114,17 +138,17 @@ namespace armarx
&QPushButton::clicked,
this,
&SkillManagerMonitorWidgetController::executeSkill);
- connect(widget.pushButtonStopSkill,
- &QPushButton::clicked,
- this,
- &SkillManagerMonitorWidgetController::stopSkill);
+ // connect(widget.pushButtonStopSkill,
+ // &QPushButton::clicked,
+ // this,
+ // &SkillManagerMonitorWidgetController::stopSkill);
connect(widget.treeWidgetSkills,
&QTreeWidget::currentItemChanged,
this,
&SkillManagerMonitorWidgetController::skillSelectionChanged);
- connect(widget.refreshNowPushButton,
+ connect(widget.pushButtonRefreshNow,
&QPushButton::clicked,
this,
&SkillManagerMonitorWidgetController::refreshSkills);
@@ -148,7 +172,7 @@ namespace armarx
QAbstractItemView::EditTrigger::NoEditTriggers);
widget.treeWidgetSkillDetails->setColumnHidden(3, true);
- getProxy(manager, observerName, 1000);
+ getProxy(memory, observerName, 1000);
connected = true;
}
@@ -156,15 +180,15 @@ namespace armarx
SkillManagerMonitorWidgetController::onDisconnectComponent()
{
connected = false;
- manager = nullptr;
+ memory = nullptr;
// reset all
skills.clear();
widget.treeWidgetSkills->clear();
widget.treeWidgetSkillDetails->clear();
skillsArgumentsTreeWidgetItem = nullptr;
- selectedSkill.providerName = "";
- selectedSkill.skillName = "";
+ selectedSkill.skillId.providerId->providerName = "";
+ selectedSkill.skillId.skillName = "";
}
void
@@ -175,28 +199,21 @@ namespace armarx
}
void
- SkillManagerMonitorWidgetController::refreshSkillsPeriodically()
+ SkillManagerMonitorWidgetController::refreshSkillsAndExecutions()
{
- if (widget.autoUpdateCheckBox->isChecked())
+ if (widget.checkBoxAutoUpdate->isChecked())
{
refreshSkills();
+ refreshExecutions();
}
}
void
SkillManagerMonitorWidgetController::refreshSkills()
{
- static std::map<skills::provider::dto::Execution::Status, std::string>
- ExecutionStatus2String = {
- {skills::provider::dto::Execution::Status::Aborted, "Aborted"},
- {skills::provider::dto::Execution::Status::Failed, "Failed"},
- {skills::provider::dto::Execution::Status::Idle, "Not yet started"},
- {skills::provider::dto::Execution::Status::Running, "Running"},
- {skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
- {skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}};
-
- if (!manager)
+ if (!memory)
{
+ // check if null
return;
}
@@ -204,149 +221,210 @@ namespace armarx
// remove non-existing ones
try
{
- ARMARX_DEBUG << "GET REMOVED PROVIDERS AND REMOVE FROM MAP";
- std::scoped_lock l(skillMutex);
- auto managerSkills = manager->getSkillDescriptions();
- std::vector<std::string> removedProviders;
- for (auto it = skills.begin(); it != skills.end();)
- {
- // TODO: iterate over skills, not just over providers!
- std::string providerName = it->first;
- if (managerSkills.find(providerName) == managerSkills.end())
- {
- ARMARX_DEBUG << "REMOVE " << providerName;
- removedProviders.push_back(providerName);
- it = skills.erase(it);
- }
- else
- {
- it++;
- }
- }
+ std::scoped_lock l(updateMutex);
- // add new ones
- ARMARX_DEBUG << "GET NEW PROVIDERS AND ADD THEM TO MAP";
- std::vector<std::string> newProviders;
- for (const auto& [providerName, providerSkills] : managerSkills)
+ auto managerSkills = memory->getSkillDescriptions();
+
+ // completely recreate internal skills map
+ skills.clear();
+ for (const auto& [sid, desc] : managerSkills)
{
- if (skills.find(providerName) == skills.end())
- {
- ARMARX_DEBUG << "ADD " << providerName;
- skills.insert(std::make_pair(providerName, providerSkills));
- newProviders.push_back(providerName);
- }
+ auto description = skills::SkillDescription::FromIce(desc);
+ auto skillId = skills::SkillID::FromIce(sid);
+ auto providerId = skillId.providerId.value_or(
+ skills::ProviderID{.providerName = "UNKNOWN PROVIDER NAME"});
+
+ ARMARX_CHECK(skillId.isFullySpecified());
+
+ auto& providedSkillsMap = skills[providerId]; // create new if not existent
+ providedSkillsMap.insert({skillId, description});
}
- /* CHECK TREE VIEW */
- // remove providers from tree
- ARMARX_DEBUG << "REMOVE PROVIDERS FROM TREE VIEW";
+ // update tree view. Remove non-existing elements
int i = 0;
while (i < widget.treeWidgetSkills->topLevelItemCount())
{
- QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
- if (auto it = std::find(removedProviders.begin(),
- removedProviders.end(),
- item->text(0).toStdString());
- it != removedProviders.end())
+ auto* providerItem = widget.treeWidgetSkills->topLevelItem(i);
+ auto providerName = providerItem->text(0).toStdString();
+ skills::ProviderID providerId{.providerName = providerName};
+
+ if (skills.find(providerId) == skills.end())
{
- ARMARX_DEBUG << "REMOVE PROVIDER " << *it;
- delete widget.treeWidgetSkills->takeTopLevelItem(i);
+ providerItem = nullptr; // reset
+ auto remove = widget.treeWidgetSkills->takeTopLevelItem(i);
+ delete remove;
+ continue;
}
- else
+
+ ++i;
+ auto& providedSkills = skills.at(providerId);
+
+ int j = 0;
+ while (j < providerItem->childCount())
{
- ++i;
+ auto* skillItem = providerItem->child(j);
+ auto skillName = skillItem->text(0).toStdString();
+
+ skills::SkillID skillId{.providerId =
+ skills::ProviderID{.providerName = providerName},
+ .skillName = skillName};
+
+ if (providedSkills.find(skillId) == providedSkills.end())
+ {
+ skillItem = nullptr;
+ auto remove = providerItem->takeChild(j);
+ delete remove;
+ continue;
+ }
+
+ ++j;
}
}
- // add new providers
- ARMARX_DEBUG << "ADD NEW PROVIDERS TO TREE VIEW";
- for (const auto& [providerName, providerSkills] : skills)
+ // update tree view. Add new elements
+ for (const auto& [providerId, providedSkills] : skills)
{
- if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName);
- it != newProviders.end())
+ QTreeWidgetItem* providerItem = nullptr;
+ for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
{
- auto item = new QTreeWidgetItem(widget.treeWidgetSkills);
- item->setText(0, QString::fromStdString(providerName));
- for (const auto& [name, sk] : providerSkills)
+ auto el = widget.treeWidgetSkills->topLevelItem(i);
+ auto providerName = el->text(0).toStdString();
+ skills::ProviderID elProviderId{.providerName = providerName};
+
+ if (providerId == elProviderId)
{
- ARMARX_DEBUG << "ADD PROVIDER " << *it;
- auto itsk = new QTreeWidgetItem(item);
- item->addChild(itsk);
- itsk->setText(0, QString::fromStdString(name));
+ providerItem = el;
+ break;
}
}
- }
- // update status and active skills window
- ARMARX_DEBUG << "UPDATE STATI AND ACTIVE SKILL";
- std::map<skills::SkillID, std::string> activeSkillsAndPrefixes;
- auto managerStatuses = manager->getSkillExecutionStatuses();
- for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
- {
- try
+ if (!providerItem)
{
- QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
- auto providerName = item->text(0).toStdString();
-
- ARMARX_DEBUG << "UPDATE STATI FOR PROVIDER " << providerName;
- auto allStatusesForProvider = managerStatuses.at(providerName);
+ providerItem = new QTreeWidgetItem(widget.treeWidgetSkills);
+ providerItem->setText(0, QString::fromStdString(providerId.providerName));
+ }
- for (int j = 0; j < item->childCount(); ++j)
+ for (const auto& [skillId, skill] : providedSkills)
+ {
+ QTreeWidgetItem* skillItem = nullptr;
+ for (int i = 0; i < providerItem->childCount(); ++i)
{
- QTreeWidgetItem* skillItem = item->child(j);
- skills::SkillID currentSkillId(providerName,
- skillItem->text(0).toStdString());
-
- ARMARX_DEBUG << "UPDATE STATI FOR SKILL " << currentSkillId.skillName;
- auto statusForSkill = allStatusesForProvider.at(currentSkillId.skillName);
- skillItem->setText(2,
- QString::fromStdString(ExecutionStatus2String.at(
- statusForSkill.header.status)));
-
- if (not statusForSkill.header.executorName
- .empty()) // it means that the skill was called by someone
+ auto el = providerItem->child(i);
+ auto skillName = el->text(0).toStdString();
+ skills::SkillID elSkillId{providerId, skillName};
+
+ if (skillId == elSkillId)
{
- ARMARX_DEBUG << "ADD SKILL TO ACTIVE " << currentSkillId.skillName;
- activeSkillsAndPrefixes.insert(
- {currentSkillId, statusForSkill.header.executorName});
+ skillItem = el;
+ break;
}
}
- }
- catch (...)
- {
- // Perhaps the skill provider died after the check at the beginning of this method
- continue;
+
+ if (!skillItem)
+ {
+ skillItem = new SkillInfoTreeWidgetItem(skill, providerItem);
+ skillItem->setText(0, QString::fromStdString(skillId.skillName));
+ }
}
}
+ }
+ catch (...)
+ {
+ // perhaps the manager died during the method?
+ }
+ }
+
+ void
+ SkillManagerMonitorWidgetController::refreshExecutions()
+ {
+ static std::map<skills::SkillStatus, std::string> ExecutionStatus2String = {
+ // Terminating
+ {skills::SkillStatus::Aborted, "Aborted"},
+ {skills::SkillStatus::Failed, "Failed"},
+ {skills::SkillStatus::Succeeded, "Succeeded"},
+
+ // Others
+ {skills::SkillStatus::Constructing, "Under construction"},
+ {skills::SkillStatus::Running, "Running"},
+ {skills::SkillStatus::Initializing, "Initializing"},
+ {skills::SkillStatus::Preparing, "Preparing"}};
+
+ if (!memory)
+ {
+ // check if null
+ return;
+ }
+
+ try
+ {
+ std::scoped_lock l(updateMutex);
- // finally update the view of active skills
- ARMARX_DEBUG << "UPDATE ACTIVE SKILLS";
- widget.listWidgetActiveSkills->clear();
- for (const auto& [id, prefix] : activeSkillsAndPrefixes)
+ auto currentManagerStatuses =
+ memory
+ ->getSkillExecutionStatuses(); // we assume that there are no more than 100 new skills..
+
+ for (const auto& [k, v] : currentManagerStatuses)
{
- auto prefixedStr = id.toString(prefix);
- bool longest = true;
- for (
- const auto& [id2, prefix2] :
- activeSkillsAndPrefixes) // check if there is a deeper skill currently executing
+ auto executionId = skills::SkillExecutionID::FromIce(k);
+ auto statusUpdate = skills::SkillStatusUpdate::FromIce(v);
+
+ SkillExecutionInfoTreeWidgetItem* found = nullptr;
+ for (int i = 0; i < widget.treeWidgetSkillExecutions->topLevelItemCount(); ++i)
{
- auto prefixedStr2 = id.toString(prefix2);
- if (prefixedStr == prefixedStr2)
- {
- continue;
- }
+ auto c = static_cast<SkillExecutionInfoTreeWidgetItem*>(
+ widget.treeWidgetSkillExecutions->topLevelItem(i));
+
+ found =
+ SkillExecutionInfoTreeWidgetItem::SearchRecursiveForMatch(c, executionId);
- if (simox::alg::starts_with(prefixedStr2, prefixedStr))
+ if (found)
{
- longest = false;
+ // update values
+ found->setText(3,
+ QString::fromStdString(
+ statusUpdate.hasBeenConstructed() ? " yes " : " no "));
+ found->setText(4,
+ QString::fromStdString(
+ statusUpdate.hasBeenInitialized() ? " yes " : " no "));
+ found->setText(5,
+ QString::fromStdString(
+ statusUpdate.hasBeenPrepared() ? " yes " : " no "));
+ found->setText(6,
+ QString::fromStdString(
+ statusUpdate.hasBeenRunning() ? " yes " : " no "));
+ found->setText(7,
+ QString::fromStdString(
+ statusUpdate.hasBeenTerminated() ? " yes " : " no "));
break;
}
}
- if (longest)
+ if (!found)
{
- widget.listWidgetActiveSkills->addItem(
- QString::fromStdString(id.toString() + ": " + id.toString(prefix)));
+ // TODO: Sort to executor!
+ auto item = new SkillExecutionInfoTreeWidgetItem(
+ executionId, widget.treeWidgetSkillExecutions);
+
+ item->setText(0,
+ QString::fromStdString(
+ executionId.executionStartedTime.toDateTimeString()));
+ item->setText(1, QString::fromStdString(executionId.executorName));
+ item->setText(2, QString::fromStdString(executionId.skillId.toString()));
+ item->setText(3,
+ QString::fromStdString(
+ statusUpdate.hasBeenConstructed() ? " yes " : " no "));
+ item->setText(4,
+ QString::fromStdString(
+ statusUpdate.hasBeenInitialized() ? " yes " : " no "));
+ item->setText(
+ 5,
+ QString::fromStdString(statusUpdate.hasBeenPrepared() ? " yes " : " no "));
+ item->setText(
+ 6,
+ QString::fromStdString(statusUpdate.hasBeenRunning() ? " yes " : " no "));
+ item->setText(7,
+ QString::fromStdString(
+ statusUpdate.hasBeenTerminated() ? " yes " : " no "));
}
}
}
@@ -359,14 +437,16 @@ namespace armarx
void
SkillManagerMonitorWidgetController::executeSkill()
{
- if (selectedSkill.providerName.empty() or selectedSkill.skillName.empty())
+ if (not selectedSkill.skillId.isFullySpecified())
{
return;
}
- std::scoped_lock l(skillMutex);
- const auto& skillDescriptions = skills.at(selectedSkill.providerName);
- if (!skillDescriptions.count(selectedSkill.skillName))
+ std::scoped_lock l(updateMutex);
+
+ auto providerId = *selectedSkill.skillId.providerId;
+ const auto& skillDescriptions = skills.at(providerId);
+ if (skillDescriptions.find(selectedSkill.skillId) == skillDescriptions.end())
{
return;
}
@@ -374,45 +454,44 @@ namespace armarx
auto data = getConfigAsAron();
char hostname[HOST_NAME_MAX];
-
gethostname(hostname, HOST_NAME_MAX);
- skills::manager::dto::SkillExecutionRequest exInfo;
- exInfo.executorName = "Skills.Manager GUI (hostname: " + std::string(hostname) + ")";
- exInfo.skillId = {selectedSkill.providerName, selectedSkill.skillName};
- exInfo.params = aron::data::Dict::ToAronDictDTO(data);
+ skills::SkillExecutionRequest req{selectedSkill.skillId,
+ "Skills.Manager GUI (hostname: " + std::string(hostname) +
+ ")",
+ data};
- ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.providerName << "/"
- << selectedSkill.skillName << ". The data was: " << data;
+ 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.
- manager->begin_executeSkill(exInfo);
+ memory->begin_executeSkill(req.toManagerIce());
}
void
SkillManagerMonitorWidgetController::stopSkill()
{
- std::scoped_lock l(skillMutex);
- if (selectedSkill.providerName.empty() or selectedSkill.skillName.empty())
- {
- return;
- }
+ // std::scoped_lock l(updateMutex);
+ // if (selectedSkill.skillId.isFullySpecified())
+ // {
+ // return;
+ // }
- const auto& skillDescriptions = skills.at(selectedSkill.providerName);
- if (!skillDescriptions.count(selectedSkill.skillName))
- {
- return;
- }
+ // const auto& skillDescriptions = skills.at(*selectedSkill.skillId.providerId);
+ // if (!skillDescriptions.count(selectedSkill.skillId.skillName))
+ // {
+ // return;
+ // }
- ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.providerName << "/"
- << selectedSkill.skillName;
- manager->abortSkill(selectedSkill.providerName, selectedSkill.skillName);
+ // ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.skillId;
+
+ // manager->abortSkill(selectedSkill.skillId);
}
void
SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current,
QTreeWidgetItem*)
{
- std::scoped_lock l(skillMutex);
+ std::scoped_lock l(updateMutex);
widget.groupBoxSkillDetails->setEnabled(false);
if (!current)
@@ -423,48 +502,40 @@ namespace armarx
if (!current->parent())
{
- // no parent available. Should not happen
+ // no parent available. Perhaps provider clicked?
return;
}
- SelectedSkill newSelectedSkill;
-
- // setup selected skill
- newSelectedSkill.providerName = current->parent()->text(0).toStdString();
- newSelectedSkill.skillName = current->text(0).toStdString();
-
- // setup groupBox
- widget.groupBoxSkillDetails->setTitle(QString::fromStdString(
- newSelectedSkill.providerName + "/" + newSelectedSkill.skillName));
- widget.groupBoxSkillDetails->setEnabled(true);
+ auto c = static_cast<SkillInfoTreeWidgetItem*>(current);
+ auto skillDescription = c->skillDescription;
- if (newSelectedSkill.providerName == selectedSkill.providerName and
- newSelectedSkill.skillName == selectedSkill.skillName)
+ if (selectedSkill.skillId == skillDescription.skillId)
{
+ // no change
return;
}
- selectedSkill = newSelectedSkill;
+ selectedSkill.skillId = skillDescription.skillId;
+
+ // setup groupBox
+ widget.groupBoxSkillDetails->setTitle(
+ QString::fromStdString(selectedSkill.skillId.toString()));
+ widget.groupBoxSkillDetails->setEnabled(true);
// setup table view
widget.treeWidgetSkillDetails->clear();
aronTreeWidgetController = nullptr;
skillsArgumentsTreeWidgetItem = nullptr;
- auto skillDesc = skills.at(selectedSkill.providerName).at(selectedSkill.skillName);
+ // We assert that the skill exists
+ 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);
{
- auto it = new QTreeWidgetItem(
- widget.treeWidgetSkillDetails,
- {QString::fromStdString("Name"), QString::fromStdString(skillDesc.skillName)});
- widget.treeWidgetSkillDetails->addTopLevelItem(it);
- }
-
- {
- auto it = new QTreeWidgetItem(
- widget.treeWidgetSkillDetails,
- {QString::fromStdString("Robot"),
- QString::fromStdString(simox::alg::join(skillDesc.robots, ", "))});
+ auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Name"),
+ QString::fromStdString(skillDesc.skillId.skillName)});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
@@ -479,17 +550,36 @@ namespace armarx
auto it = new QTreeWidgetItem(
widget.treeWidgetSkillDetails,
{QString::fromStdString("Timeout"),
- QString::fromStdString(std::to_string(skillDesc.timeoutMs)) + " ms"});
+ QString::fromStdString(std::to_string(skillDesc.timeout.toMilliSeconds())) +
+ " ms"});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
+ // select root profile
+ widget.comboBoxProfiles->setCurrentIndex(0);
+
+ // remove any profile
+ while (widget.comboBoxProfiles->count() > 1)
+ {
+ widget.comboBoxProfiles->removeItem(1);
+ }
+
+ // add new profiles for this skill
+ // TODO: Where stored?
+
skillsArgumentsTreeWidgetItem = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
{QString::fromStdString("Arguments")});
- auto aron_args = aron::type::Object::FromAronObjectDTO(skillDesc.acceptedType);
- auto default_args = aron::data::Dict::FromAronDictDTO(skillDesc.defaultParams);
+ auto aron_args = skillDesc.parametersType;
+ auto default_args_of_profile = skillDesc.rootProfileDefaults;
+
+ aronTreeWidgetController =
+ std::make_shared<AronTreeWidgetController>(widget.treeWidgetSkillDetails,
+ skillsArgumentsTreeWidgetItem,
+ aron_args,
+ default_args_of_profile);
- aronTreeWidgetController = std::make_shared<AronTreeWidgetController>(
- widget.treeWidgetSkillDetails, skillsArgumentsTreeWidgetItem, aron_args, default_args);
+ // automatically expand args
+ skillsArgumentsTreeWidgetItem->setExpanded(true);
}
aron::data::DictPtr
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index f1ad1bb720ea69503b86b00f75a6e6f09f6058c5..1fd2e2f110fe8d34890f8d685c8faa5bc2031b70 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -38,11 +38,51 @@
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
+#include <RobotAPI/libraries/skills/core/ProviderID.h>
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include "aronTreeWidget/AronTreeWidgetController.h"
namespace armarx
{
+ class SkillInfoTreeWidgetItem : public QTreeWidgetItem
+ {
+ public:
+ SkillInfoTreeWidgetItem(const skills::SkillDescription& desc, QTreeWidgetItem* parent) :
+ QTreeWidgetItem(parent), skillDescription(desc)
+ {
+ }
+
+ SkillInfoTreeWidgetItem(const skills::SkillDescription& desc, QTreeWidget* parent) :
+ QTreeWidgetItem(parent), skillDescription(desc)
+ {
+ }
+
+ skills::SkillDescription skillDescription;
+ };
+
+ class SkillExecutionInfoTreeWidgetItem : public QTreeWidgetItem
+ {
+ public:
+ SkillExecutionInfoTreeWidgetItem(const skills::SkillExecutionID& id,
+ QTreeWidgetItem* parent) :
+ QTreeWidgetItem(parent), executionId(id)
+ {
+ }
+
+ SkillExecutionInfoTreeWidgetItem(const skills::SkillExecutionID& id, QTreeWidget* parent) :
+ QTreeWidgetItem(parent), executionId(id)
+ {
+ }
+
+ static SkillExecutionInfoTreeWidgetItem*
+ SearchRecursiveForMatch(SkillExecutionInfoTreeWidgetItem* el,
+ const skills::SkillExecutionID& needle);
+
+ skills::SkillExecutionID executionId;
+ };
+
class ARMARXCOMPONENT_IMPORT_EXPORT SkillManagerMonitorWidgetController :
public armarx::ArmarXComponentWidgetControllerTemplate<SkillManagerMonitorWidgetController>
{
@@ -81,7 +121,8 @@ namespace armarx
void updateTimerFrequency();
void refreshSkills();
- void refreshSkillsPeriodically();
+ void refreshExecutions();
+ void refreshSkillsAndExecutions();
void copyCurrentConfig();
void pasteCurrentConfig();
@@ -99,20 +140,25 @@ namespace armarx
QPointer<SimpleConfigDialog> dialog;
std::string observerName = "SkillManager";
- skills::manager::dti::SkillManagerInterfacePrx manager = nullptr;
-
- struct SelectedSkill
- {
- std::string providerName;
- std::string skillName;
- };
+ skills::dti::SkillMemoryInterfacePrx memory = nullptr;
// Data taken from observer (snapshot of it)
- mutable std::mutex skillMutex;
- skills::manager::dto::SkillDescriptionMapMap skills = {};
+ mutable std::mutex updateMutex;
+ std::map<skills::ProviderID, std::map<skills::SkillID, skills::SkillDescription>> skills =
+ {};
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> skillStatusUpdates = {};
// User Input
- SelectedSkill selectedSkill;
+ struct SelectedSkill
+ {
+ skills::SkillID skillId;
+
+ // make default constructable
+ SelectedSkill() :
+ skillId{.providerId = skills::ProviderID{.providerName = ""}, .skillName = ""}
+ {
+ }
+ } selectedSkill;
// Helper to get the treeWidgetItem easily
QTreeWidgetItem* skillsArgumentsTreeWidgetItem = nullptr;
@@ -121,9 +167,6 @@ namespace armarx
// others
QTimer* refreshSkillsResultTimer;
- // skillExecutions
- std::vector<std::thread> executions;
-
// connected flag
std::atomic_bool connected = false;
};
diff --git a/source/RobotAPI/interface/skills/SkillManagerInterface.ice b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
index 088d28561684e1a604595f63af4aad6903e4caaf..f22738bda8b71cac09faf71620e226135ac93532 100644
--- a/source/RobotAPI/interface/skills/SkillManagerInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
@@ -32,40 +32,103 @@ module armarx
{
module dto
{
- // Inputs
- struct SkillExecutionRequest
+ struct ProviderID
+ {
+ string providerName;
+ };
+
+ // A skill ID. Must be unique within one SkillManager
+ struct SkillID
+ {
+ ProviderID providerId;
+ string skillName;
+ };
+
+ struct SkillDescription
{
- string executorName; // Who called the request
- provider::dto::SkillID skillId; // The id of a skill to request. Note that the skill or provider name can be regexes.
- aron::data::dto::Dict params; // the parameters for the skill. Can be nullptr if the skill does not require parameters
+ SkillID skillId;
+ string description;
+ armarx::core::time::dto::Duration timeout;
+ aron::type::dto::AronObject parametersType;
+ aron::type::dto::AronObject resultType;
+ aron::data::dto::Dict rootProfileDefaults;
};
+
+ dictionary<SkillID, dto::SkillDescription> SkillDescriptionMap;
+
struct ProviderInfo
{
- string providerName;
- provider::dti::SkillProviderInterface* provider;
- provider::dto::SkillDescriptionMap providedSkills;
+ ProviderID providerId;
+ provider::dti::SkillProviderInterface* providerInterface;
+ dto::SkillDescriptionMap providedSkills;
+ };
+
+ struct SkillExecutionID
+ {
+ SkillID skillId;
+ string executorName;
+ armarx::core::time::dto::DateTime executionStartedTime;
+ string uuid;
};
- // Provider data types
- dictionary<string, provider::dto::SkillDescriptionMap> SkillDescriptionMapMap;
- dictionary<string, provider::dto::SkillStatusUpdateMap> SkillStatusUpdateMapMap;
+ struct SkillStatusUpdate
+ {
+ SkillExecutionID executionId;
+ aron::data::dto::Dict parameters;
+ callback::dti::SkillProviderCallbackInterface* callbackInterface;
+ core::dto::Execution::Status status;
+ aron::data::dto::Dict result;
+ };
+
+ dictionary<SkillExecutionID, SkillStatusUpdate> SkillStatusUpdateMap;
+
+ struct SkillExecutionRequest
+ {
+ SkillID skillId;
+ string executorName;
+ aron::data::dto::Dict parameters;
+ };
}
module dti
{
- interface SkillManagerInterface extends callback::dti::SkillProviderCallbackInterface
+ interface SkillManagerInterface extends
+ callback::dti::SkillProviderCallbackInterface
{
- // There is by design no method to get the ProviderInfo. You should only communicate with the manager
+ // !! There is by design no method to get a proxy of the provider. You should only communicate with the manager !!
void addProvider(dto::ProviderInfo providerInfo);
- void removeProvider(string providerName);
- dto::SkillDescriptionMapMap getSkillDescriptions();
- dto::SkillStatusUpdateMapMap getSkillExecutionStatuses();
+ void removeProvider(dto::ProviderID providerId);
+
+ optional(1) dto::SkillDescription getSkillDescription(
+ dto::SkillID skillId); // get one skilldescriptions from a provider
+
+ dto::SkillDescriptionMap
+ getSkillDescriptions(); // get all skilldescriptions from all providers
+
+ optional(2) dto::SkillStatusUpdate
+ getSkillExecutionStatus(dto::SkillExecutionID executionId);
+
+ dto::SkillStatusUpdateMap
+ getSkillExecutionStatuses(); // returns the current executions from all providers
+
+ dto::SkillStatusUpdate
+ executeSkill(dto::SkillExecutionRequest
+ skillExecutionRequest); // blocks until skill is finished.
+
+ dto::SkillExecutionID executeSkillAsync(
+ dto::SkillExecutionRequest
+ skillExecutionRequest); // directly returns the execution id
- provider::dto::SkillStatusUpdate executeSkill(dto::SkillExecutionRequest skillExecutionInfo);
- void abortSkill(string providerName, string skillName);
+ provider::dto::ParameterUpdateResult updateSkillParameters(
+ dto::SkillExecutionID executionId,
+ aron::data::dto::Dict parameters); // add params to a skill
+ // 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 c8e3212efa1b0ae3bf2768a124a53441898cd6de..3d0d8ee61b23a5e184514b87792323d379c81173 100644
--- a/source/RobotAPI/interface/skills/SkillMemoryInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillMemoryInterface.ice
@@ -24,10 +24,10 @@
#include <ArmarXCore/interface/core/Profiler.ice>
-#include <RobotAPI/interface/skills/SkillProviderInterface.ice>
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
#include <RobotAPI/interface/skills/SkillManagerInterface.ice>
+#include <RobotAPI/interface/skills/SkillProviderInterface.ice>
#include <RobotAPI/interface/skills/StatechartListenerInterface.ice>
-#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
module armarx
{
@@ -35,9 +35,8 @@ module armarx
{
module dti
{
- interface SkillMemoryInterface extends armem::server::MemoryInterface, dti::StatechartListenerInterface, manager::dti::SkillManagerInterface
- {
- };
+ interface SkillMemoryInterface extends armem::server::MemoryInterface,
+ dti::StatechartListenerInterface, manager::dti::SkillManagerInterface{};
}
}
}
diff --git a/source/RobotAPI/interface/skills/SkillProviderInterface.ice b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
index ed40611c800bfccba6f44310ca5d88639d83c02f..3066c962e45326fecfdf8f08c6985bb89bf6c3d7 100644
--- a/source/RobotAPI/interface/skills/SkillProviderInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
@@ -22,6 +22,8 @@
#pragma once
+#include <ArmarXCore/interface/core/time.ice>
+
#include <RobotAPI/interface/aron.ice>
@@ -37,6 +39,43 @@ module armarx
interface SkillProviderCallbackInterface;
}
}
+
+ module provider
+ {
+ module dti
+ {
+ interface SkillProviderInterface;
+ }
+ }
+ }
+}
+
+module armarx
+{
+ module skills
+ {
+ module core
+ {
+ module dto
+ {
+ // The status enum of a skill
+ module Execution
+ {
+ enum Status
+ {
+ Constructing,
+ Initializing,
+ Preparing,
+ Running,
+
+ // terminating values
+ Failed,
+ Succeeded,
+ Aborted
+ };
+ }
+ }
+ }
}
}
@@ -49,88 +88,112 @@ module armarx
{
module dto
{
- sequence<string> StringList;
+
+ // A profile is something that is client side!
+ // // A parameterization profile
+ // struct SkillProfile
+ // {
+ // string profileName;
+ // string predecessorProfileName; // may be empty
+ // aron::data::dto::Dict
+ // parameterization; // may be only a partial set of accepted type
+ // };
+
+ // // A list of parameterization profiles. Note that the first element is recognized as root. Following elements overwrite the previous.
+ // dictionary<string, SkillProfile> SkillProfileDict;
// A skill ID. Must be unique within one SkillManager
struct SkillID
{
- string providerName;
string skillName;
};
// Description of a skill, independant of a provider
- // A skill is nothing but a executable thing, which can be executed on one or more 'robots' (empty means all)
+ // A skill is nothing but a executable thing, which can be executed
struct SkillDescription
{
- string skillName; // the name of the skill
- string description; // a human readable description of what the skill does. Used in GUI
- StringList robots; // the names of the robots that are able to execute that skill
- long timeoutMs; // in milliseconds, can be set to -1 for infinite
- aron::type::dto::AronObject acceptedType; // the name of the object is irrelevant and only used in GUI. nullptr if not set
- aron::data::dto::Dict defaultParams; // the default parameterization used in GUI. nullptr if not set
+ SkillID skillId;
+ string description;
+ armarx::core::time::dto::Duration timeout;
+ aron::type::dto::AronObject parametersType;
+ aron::type::dto::AronObject resultType;
+ aron::data::dto::Dict rootProfileDefaults;
};
- dictionary<string, SkillDescription> SkillDescriptionMap;
+
+ dictionary<SkillID, SkillDescription> SkillDescriptionMap;
// Input to a provider to execute a skill
struct SkillExecutionRequest
{
- string skillName; // the id of the skill
- string executorName; // the name of the component/lib/skill that called the execution of the skill
- aron::data::dto::Dict params; // the used parameterization
- callback::dti::SkillProviderCallbackInterface* callbackInterface; // use nullptr if you do not want to have callbacks
+ SkillID skillId;
+ string executorName;
+ aron::data::dto::Dict parameters;
+ callback::dti::SkillProviderCallbackInterface* callbackInterface;
};
- // The status enum of a skill
- module Execution
+ // The minimum information that is needed to uniquely identifying a past skill execution
+ struct SkillExecutionID
{
- enum Status
- {
- Idle, // a skill can only be idled if it has never been scheduled
- Scheduled,
- Running,
-
- // terminating values
- Failed,
- Succeeded,
- Aborted
- };
- }
+ SkillID skillId;
+ string executorName;
+ armarx::core::time::dto::DateTime executionStartedTime;
+ string uuid;
+ };
// Status updates of a skill
- struct SkillStatusUpdateHeader
+ struct SkillStatusUpdate
{
- SkillID skillId; // the id of the skill
- string executorName; // the name of the component/lib/skill that called the execution of the skill
- aron::data::dto::Dict usedParams; // the used parameterization
- callback::dti::SkillProviderCallbackInterface* usedCallbackInterface; // the used callback interface. Probably a prx to the manager
- Execution::Status status; // the current status of the skill
+ SkillExecutionID executionId;
+ aron::data::dto::Dict parameters;
+ callback::dti::SkillProviderCallbackInterface* callbackInterface;
+ core::dto::Execution::Status status;
+ aron::data::dto::Dict result;
+ /// @todo maybe add in future:
+ // aron::data::dto::Dict feedback;
};
- struct SkillStatusUpdate
+ dictionary<SkillExecutionID, SkillStatusUpdate> SkillStatusUpdateMap;
+
+ struct ParameterUpdateResult
{
- SkillStatusUpdateHeader header;
- aron::data::dto::Dict data; // data, attached to the status update. If send via a callback, this data may be used by the callback interface
+ bool success; // false if skill is not running (anymore) or already prepared
};
- dictionary<string, SkillStatusUpdate> SkillStatusUpdateMap;
+ struct AbortSkillResult
+ {
+ bool success; // false if skill is not running (anymore)
+ };
}
module dti
{
interface SkillProviderInterface
{
- dto::SkillDescription getSkillDescription(string name);
+ optional(1) dto::SkillDescription getSkillDescription(dto::SkillID skill);
+
dto::SkillDescriptionMap getSkillDescriptions();
- dto::SkillStatusUpdate getSkillExecutionStatus(string name);
- dto::SkillStatusUpdateMap getSkillExecutionStatuses();
+
+ optional(2) dto::SkillStatusUpdate
+ getSkillExecutionStatus(dto::SkillExecutionID executionId);
+
+ dto::SkillStatusUpdateMap
+ getSkillExecutionStatuses(); // returns all current skill executions
// execute skill will ALWAYS fully execute the skill and wait, until the skill is finished.
- // Use the _begin() method, if you want to call a skill async
+ // TODO: Explain skill phases
// This method returns a status update where the status is ALWAYS one of the terminating values
dto::SkillStatusUpdate executeSkill(dto::SkillExecutionRequest executionInfo);
+ dto::SkillExecutionID
+ executeSkillAsync(dto::SkillExecutionRequest executionInfo);
+
+ 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(string skill);
+ dto::AbortSkillResult abortSkill(dto::SkillExecutionID skill);
+ dto::AbortSkillResult abortSkillAsync(dto::SkillExecutionID skill);
};
}
}
@@ -144,15 +207,23 @@ module armarx
{
module callback
{
+ module dto
+ {
+ struct ProviderID
+ {
+ string providerName;
+ };
+ }
+
module dti
{
interface SkillProviderCallbackInterface
{
// used for callbacks from providers to update their skill execution status
- void updateStatusForSkill(provider::dto::SkillStatusUpdate statusUpdate);
+ void updateStatusForSkill(provider::dto::SkillStatusUpdate statusUpdate,
+ dto::ProviderID providerId);
}
}
}
}
}
-
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/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
index a85ae2c2c5531450d3f9c0440e31d8c1695884ba..e577eb166ebc77bbbb66dc4ecfe47b9728b2a2be 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -62,7 +62,8 @@ namespace armarx::objpose
std::optional<ObjectPose>
ObjectPoseClient::fetchObjectPose(const ObjectID& objectID) const
{
- const auto *object = findObjectPoseByID(fetchObjectPoses(), objectID);
+ const auto objectPoses = fetchObjectPoses();
+ const auto *object = findObjectPoseByID(objectPoses, objectID);
if(object != nullptr)
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
index 433fc67ec7890cbb7fede64d2edda22533e46eb8..4a775da39159a8e0a8435f88679368b3cf39a0dc 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
@@ -24,13 +24,15 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
#include "../ArmarXObjects.h"
#include <iostream>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <RobotAPI/Test.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
namespace fs = std::filesystem;
@@ -38,7 +40,6 @@ namespace fs = std::filesystem;
BOOST_AUTO_TEST_SUITE(arondto_ObjectPose_test)
-
BOOST_AUTO_TEST_CASE(test_ObjectType_copy_assignment)
{
BOOST_TEST_MESSAGE("Constructor");
@@ -76,17 +77,25 @@ BOOST_AUTO_TEST_CASE(test_ObjectPose_copy_assignment)
BOOST_AUTO_TEST_SUITE_END()
-
BOOST_AUTO_TEST_SUITE(ObjectFinderTest)
-
BOOST_AUTO_TEST_CASE(test_find)
{
using namespace armarx;
+ {
+ armarx::CMakePackageFinder packageFinder(ObjectFinder::DefaultObjectsPackageName);
+ if (not packageFinder.packageFound())
+ {
+ // Do not test further.
+ BOOST_CHECK(true);
+ return;
+ }
+ }
+
ObjectFinder finder;
- bool checkPaths = false;
+ const bool checkPaths = false;
std::vector<ObjectInfo> objects = finder.findAllObjects(checkPaths);
BOOST_CHECK_GT(objects.size(), 0);
@@ -106,5 +115,4 @@ BOOST_AUTO_TEST_CASE(test_find)
}
-
BOOST_AUTO_TEST_SUITE_END()
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 3dac8e38bdeb8bbc6679ffa809bbf670d10f166c..4b7bba78baae8c78c7c37fdc77ebdaff8e6fd154 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
@@ -12,35 +12,35 @@ 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);
- grasping::GraspCandidatePtr
+ ::armarx::grasping::GraspCandidatePtr
queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
- grasping::GraspCandidateDict
+ ::armarx::grasping::GraspCandidateDict
queryGraspCandidateInstancesByID(std::vector<armem::MemoryID> const& ids) const;
- grasping::BimanualGraspCandidatePtr
+ ::armarx::grasping::BimanualGraspCandidatePtr
queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
- grasping::GraspCandidateDict
+ ::armarx::grasping::GraspCandidateDict
queryLatestGraspCandidateEntity(std::string const& provider,
std::string const& entity) const;
- std::map<std::string, grasping::BimanualGraspCandidatePtr>
+ std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
queryLatestBimanualGraspCandidateEntity(std::string const& provider,
std::string const& entity) const;
- grasping::GraspCandidateDict
+ ::armarx::grasping::GraspCandidateDict
queryLatestGraspCandidates(std::string const& provider = "") const;
- grasping::GraspCandidateDict queryGraspCandidatesNewerThan(
+ ::armarx::grasping::GraspCandidateDict queryGraspCandidatesNewerThan(
std::string const& provider = "",
const armarx::DateTime& timestamp = armarx::DateTime::Now()) const;
- std::map<std::string, grasping::BimanualGraspCandidatePtr>
+ std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
queryLatestBimanualGraspCandidates(std::string const& provider = "") const;
@@ -48,10 +48,10 @@ namespace armarx::armem
private:
- grasping::GraspCandidateDict
+ ::armarx::grasping::GraspCandidateDict
getGraspCandidatesFromResultSet(armem::client::QueryResult const& qResult) const;
- std::map<std::string, grasping::BimanualGraspCandidatePtr>
+ std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
getBimanualGraspCandidatesFromResultSet(armem::client::QueryResult const& qResult) const;
armem::client::Reader memoryReader;
@@ -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/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
index c4b170313f7d9d71d09b055ffd497b9b658af5ac..045026b0eb62f1a1ce95210acc555045fd2e0c82 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -21,6 +21,10 @@
* GNU General Public License
*/
+#include "GraspTrajectory.h"
+
+#include <memory>
+
#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/exceptions/Exception.h>
@@ -34,14 +38,23 @@
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
-#include "GraspTrajectory.h"
-
namespace armarx
{
+ Eigen::VectorXf
+ mapValuesToVector(const armarx::NameValueMap& map)
+ {
+ ARMARX_TRACE;
+ Eigen::VectorXf vector(map.size());
+ std::transform(
+ map.begin(), map.end(), vector.data(), [](const auto& item) { return item.second; });
+ return vector;
+ }
+
void
GraspTrajectory::writeToFile(const std::string& filename)
{
+ ARMARX_TRACE;
RapidXmlWriter writer;
RapidXmlWriterNode root = writer.createRootNode("GraspTrajectory");
for (const KeypointPtr& keypoint : keypoints)
@@ -49,7 +62,12 @@ namespace armarx
SimpleJsonLoggerEntry e;
e.Add("dt", keypoint->dt);
e.AddAsArr("Pose", keypoint->tcpTarget);
- e.AddAsArr("HandValues", keypoint->handJointsTarget);
+ JsonObjectPtr const obj = std::make_shared<JsonObject>();
+ for (const auto& [name, value] : keypoint->handJointsTarget)
+ {
+ obj->add(name, JsonValue::Create(value));
+ }
+ e.obj->add("HandValues", obj);
root.append_string_node("Keypoint", e.obj->toJsonString(0, "", true));
}
writer.saveToFile(filename, true);
@@ -58,7 +76,8 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
{
- RapidXmlReaderNode root = reader->getRoot();
+ ARMARX_TRACE;
+ RapidXmlReaderNode const root = reader->getRoot();
GraspTrajectoryPtr traj;
for (const RapidXmlReaderNode& kpNode : root.nodes("Keypoint"))
@@ -79,16 +98,17 @@ namespace armarx
}
}
- Eigen::Vector3f handValues;
+ armarx::NameValueMap handValues;
std::vector<JPathNavigator> cells = nav.select("HandValues/*");
- for (int j = 0; j < 3; j++)
+ for (const auto& cell : cells)
{
- handValues(j) = cells.at(j).asFloat();
+ handValues[cell.getKey()] = cell.asFloat();
}
+
if (!traj)
{
- traj = GraspTrajectoryPtr(new GraspTrajectory(pose, handValues));
+ traj = std::make_shared<GraspTrajectory>(pose, handValues);
}
else
{
@@ -101,25 +121,32 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromFile(const std::string& filename)
{
+ ARMARX_TRACE;
return ReadFromReader(RapidXmlReader::FromFile(filename));
}
GraspTrajectoryPtr
GraspTrajectory::ReadFromString(const std::string& xml)
{
+ ARMARX_TRACE;
return ReadFromReader(RapidXmlReader::FromXmlString(xml));
}
- GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f &tcpStart,
- const Eigen::Vector3f &handJointsStart) {
+ GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f& tcpStart,
+ const armarx::NameValueMap& handJointsStart)
+ {
+ ARMARX_TRACE;
KeypointPtr keypoint(new Keypoint(tcpStart, handJointsStart));
keypointMap[0] = keypoints.size();
keypoints.push_back(keypoint);
}
void
- GraspTrajectory::addKeypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget,
- float dt) {
+ GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt)
+ {
+ ARMARX_TRACE;
KeypointPtr prev = lastKeypoint();
KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
keypoint->updateVelocities(prev, dt);
@@ -129,13 +156,19 @@ namespace armarx
}
size_t
- GraspTrajectory::getKeypointCount() const {
+ GraspTrajectory::getKeypointCount() const
+ {
+ ARMARX_TRACE;
return keypoints.size();
}
void
- GraspTrajectory::insertKeypoint(size_t index, const Eigen::Matrix4f &tcpTarget,
- const Eigen::Vector3f &handJointsTarget, float dt) {
+ GraspTrajectory::insertKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index > keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -153,7 +186,9 @@ namespace armarx
}
void
- GraspTrajectory::removeKeypoint(size_t index) {
+ GraspTrajectory::removeKeypoint(size_t index)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index >= keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -161,6 +196,7 @@ namespace armarx
keypoints.erase(keypoints.begin() + index);
if (index < keypoints.size())
{
+ ARMARX_TRACE;
KeypointPtr prev = keypoints.at(index - 1);
KeypointPtr next = keypoints.at(index);
next->updateVelocities(prev, next->dt);
@@ -169,8 +205,12 @@ namespace armarx
}
void
- GraspTrajectory::replaceKeypoint(size_t index, const Eigen::Matrix4f &tcpTarget,
- const Eigen::Vector3f &handJointsTarget, float dt) {
+ GraspTrajectory::replaceKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index >= keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -183,7 +223,9 @@ namespace armarx
}
void
- GraspTrajectory::setKeypointDt(size_t index, float dt) {
+ GraspTrajectory::setKeypointDt(size_t index, float dt)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index >= keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -194,23 +236,31 @@ namespace armarx
updateKeypointMap();
}
- GraspTrajectory::KeypointPtr &
- GraspTrajectory::lastKeypoint() {
+ GraspTrajectory::KeypointPtr&
+ GraspTrajectory::lastKeypoint()
+ {
+ ARMARX_TRACE;
return keypoints.at(keypoints.size() - 1);
}
- GraspTrajectory::KeypointPtr &
- GraspTrajectory::getKeypoint(int i) {
+ GraspTrajectory::KeypointPtr&
+ GraspTrajectory::getKeypoint(int i)
+ {
+ ARMARX_TRACE;
return keypoints.at(i);
}
Eigen::Matrix4f
- GraspTrajectory::getStartPose() {
+ GraspTrajectory::getStartPose()
+ {
+ ARMARX_TRACE;
return getKeypoint(0)->getTargetPose();
}
void
- GraspTrajectory::getIndex(float t, int &i1, int &i2, float &f) {
+ GraspTrajectory::getIndex(float t, int& i1, int& i2, float& f)
+ {
+ ARMARX_TRACE;
if (t <= 0)
{
i1 = 0;
@@ -234,15 +284,20 @@ namespace armarx
}
Eigen::Vector3f
- GraspTrajectory::GetPosition(float t) {
+ GraspTrajectory::GetPosition(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
- return ::math::Helpers::Lerp(getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
+ return ::math::Helpers::Lerp(
+ getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
}
Eigen::Matrix3f
- GraspTrajectory::GetOrientation(float t) {
+ GraspTrajectory::GetOrientation(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -257,12 +312,16 @@ namespace armarx
}
Eigen::Matrix4f
- GraspTrajectory::GetPose(float t) {
+ GraspTrajectory::GetPose(float t)
+ {
+ ARMARX_TRACE;
return ::math::Helpers::CreatePose(GetPosition(t), GetOrientation(t));
}
std::vector<Eigen::Matrix4f>
- GraspTrajectory::getAllKeypointPoses() {
+ GraspTrajectory::getAllKeypointPoses()
+ {
+ ARMARX_TRACE;
std::vector<Eigen::Matrix4f> res;
for (const KeypointPtr& keypoint : keypoints)
{
@@ -272,7 +331,9 @@ namespace armarx
}
std::vector<Eigen::Vector3f>
- GraspTrajectory::getAllKeypointPositions() {
+ GraspTrajectory::getAllKeypointPositions()
+ {
+ ARMARX_TRACE;
std::vector<Eigen::Vector3f> res;
for (const KeypointPtr& keypoint : keypoints)
{
@@ -282,7 +343,9 @@ namespace armarx
}
std::vector<Eigen::Matrix3f>
- GraspTrajectory::getAllKeypointOrientations() {
+ GraspTrajectory::getAllKeypointOrientations()
+ {
+ ARMARX_TRACE;
std::vector<Eigen::Matrix3f> res;
for (const KeypointPtr& keypoint : keypoints)
{
@@ -291,16 +354,29 @@ namespace armarx
return res;
}
- Eigen::Vector3f
- GraspTrajectory::GetHandValues(float t) {
+ armarx::NameValueMap
+ GraspTrajectory::GetHandValues(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
- return ::math::Helpers::Lerp(getKeypoint(i1)->handJointsTarget, getKeypoint(i2)->handJointsTarget, f);
+ auto handTargetsMap = getKeypoint(i1)->handJointsTarget;
+ const auto handTargets1 = mapValuesToVector(handTargetsMap);
+ const auto handTargets2 = mapValuesToVector(getKeypoint(i2)->handJointsTarget);
+ const Eigen::VectorXf lerpTargets = handTargets1 * (1 - f) + handTargets2 * f;
+ auto* lerpTargetsIt = lerpTargets.data();
+ for (auto& [name, value] : handTargetsMap)
+ {
+ value = *(lerpTargetsIt++);
+ }
+ return handTargetsMap;
}
Eigen::Vector3f
- GraspTrajectory::GetPositionDerivative(float t) {
+ GraspTrajectory::GetPositionDerivative(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -308,7 +384,9 @@ namespace armarx
}
Eigen::Vector3f
- GraspTrajectory::GetOrientationDerivative(float t) {
+ GraspTrajectory::GetOrientationDerivative(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -316,7 +394,9 @@ namespace armarx
}
Eigen::Matrix<float, 6, 1>
- GraspTrajectory::GetTcpDerivative(float t) {
+ GraspTrajectory::GetTcpDerivative(float t)
+ {
+ ARMARX_TRACE;
Eigen::Matrix<float, 6, 1> ffVel;
ffVel.block<3, 1>(0, 0) = GetPositionDerivative(t);
ffVel.block<3, 1>(3, 0) = GetOrientationDerivative(t);
@@ -324,7 +404,9 @@ namespace armarx
}
Eigen::Vector3f
- GraspTrajectory::GetHandJointsDerivative(float t) {
+ GraspTrajectory::GetHandJointsDerivative(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -332,12 +414,16 @@ namespace armarx
}
float
- GraspTrajectory::getDuration() const {
+ GraspTrajectory::getDuration() const
+ {
+ ARMARX_TRACE;
return keypointMap.rbegin()->first;
}
GraspTrajectory::Length
- GraspTrajectory::calculateLength() const {
+ GraspTrajectory::calculateLength() const
+ {
+ ARMARX_TRACE;
Length l;
for (size_t i = 1; i < keypoints.size(); i++)
{
@@ -357,19 +443,31 @@ namespace armarx
}
GraspTrajectoryPtr
- GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f &translation, const Eigen::Matrix3f &rotation) {
- GraspTrajectoryPtr traj(new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(getKeypoint(0)->getTargetPose(), translation, rotation), getKeypoint(0)->handJointsTarget));
+ GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f& translation,
+ const Eigen::Matrix3f& rotation)
+ {
+ ARMARX_TRACE;
+ GraspTrajectoryPtr traj(
+ new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(
+ getKeypoint(0)->getTargetPose(), translation, rotation),
+ getKeypoint(0)->handJointsTarget));
for (size_t i = 1; i < keypoints.size(); i++)
{
KeypointPtr& kp = keypoints.at(i);
- traj->addKeypoint(::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation), kp->handJointsTarget, kp->dt);
+ traj->addKeypoint(
+ ::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation),
+ kp->handJointsTarget,
+ kp->dt);
}
return traj;
}
GraspTrajectoryPtr
- GraspTrajectory::getTransformed(const Eigen::Matrix4f &transform) {
- GraspTrajectoryPtr traj(new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
+ GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform)
+ {
+ ARMARX_TRACE;
+ GraspTrajectoryPtr traj(
+ new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
for (size_t i = 1; i < keypoints.size(); i++)
{
KeypointPtr& kp = keypoints.at(i);
@@ -379,32 +477,44 @@ namespace armarx
}
GraspTrajectoryPtr
- GraspTrajectory::getClone() {
+ GraspTrajectory::getClone()
+ {
+ ARMARX_TRACE;
return getTransformed(Eigen::Matrix4f::Identity());
}
GraspTrajectoryPtr
- GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f &target, const Eigen::Vector3f &handForward) {
+ GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f& target,
+ const Eigen::Vector3f& handForward)
+ {
+ ARMARX_TRACE;
Eigen::Matrix4f startPose = getStartPose();
- Eigen::Vector3f targetHandForward = ::math::Helpers::TransformDirection(target, handForward);
- Eigen::Vector3f trajHandForward = ::math::Helpers::TransformDirection(startPose, handForward);
+ Eigen::Vector3f targetHandForward =
+ ::math::Helpers::TransformDirection(target, handForward);
+ Eigen::Vector3f trajHandForward =
+ ::math::Helpers::TransformDirection(startPose, handForward);
Eigen::Vector3f up(0, 0, 1);
float angle = ::math::Helpers::Angle(targetHandForward, trajHandForward, up);
Eigen::AngleAxisf aa(angle, up);
- Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(-::math::Helpers::GetPosition(startPose), aa.toRotationMatrix(), ::math::Helpers::GetPosition(target));
+ Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(
+ -::math::Helpers::GetPosition(startPose),
+ aa.toRotationMatrix(),
+ ::math::Helpers::GetPosition(target));
return getTransformed(transform);
}
GraspTrajectoryPtr
- GraspTrajectory::getTransformedToOtherHand() {
+ GraspTrajectory::getTransformedToOtherHand()
+ {
+ ARMARX_TRACE;
Eigen::Matrix3f flip_yz = Eigen::Matrix3f::Identity();
flip_yz(0, 0) *= -1.0;
Eigen::Matrix4f start_pose = getStartPose();
start_pose.block<3, 3>(0, 0) = flip_yz * start_pose.block<3, 3>(0, 0) * flip_yz;
GraspTrajectoryPtr output_trajectory(
- new GraspTrajectory(start_pose, getKeypoint(0)->handJointsTarget));
+ new GraspTrajectory(start_pose, getKeypoint(0)->handJointsTarget));
for (size_t i = 1; i < getKeypointCount(); i++)
{
GraspTrajectory::KeypointPtr& kp = getKeypoint(i);
@@ -416,13 +526,19 @@ namespace armarx
}
SimpleDiffIK::Reachability
- GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp,
- SimpleDiffIK::Parameters params) {
- return SimpleDiffIK::CalculateReachability(getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
+ GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp,
+ SimpleDiffIK::Parameters params)
+ {
+ ARMARX_TRACE;
+ return SimpleDiffIK::CalculateReachability(
+ getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
}
void
- GraspTrajectory::updateKeypointMap() {
+ GraspTrajectory::updateKeypointMap()
+ {
+ ARMARX_TRACE;
keypointMap.clear();
float t = 0;
for (size_t i = 0; i < keypoints.size(); i++)
@@ -432,54 +548,72 @@ namespace armarx
}
}
-
-
void
- GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr &prev, float dt) {
+ GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr& prev,
+ float deltat)
+ {
+ ARMARX_TRACE;
Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(prev->tcpTarget);
Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(prev->tcpTarget);
- Eigen::Vector3f hnd0 = prev->handJointsTarget;
+ auto hnd0 = mapValuesToVector(prev->handJointsTarget);
Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(tcpTarget);
Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(tcpTarget);
- Eigen::Vector3f hnd1 = handJointsTarget;
+ auto hnd1 = mapValuesToVector(handJointsTarget);
Eigen::Vector3f dpos = pos1 - pos0;
Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1);
- Eigen::Vector3f dhnd = hnd1 - hnd0;
+ Eigen::VectorXf dhnd = hnd1 - hnd0;
- this->dt = dt;
- feedForwardPosVelocity = dpos / dt;
- feedForwardOriVelocity = dori / dt;
- feedForwardHandJointsVelocity = dhnd / dt;
+ this->dt = deltat;
+ feedForwardPosVelocity = dpos / deltat;
+ feedForwardOriVelocity = dori / deltat;
+ feedForwardHandJointsVelocity = dhnd / deltat;
}
Eigen::Vector3f
- GraspTrajectory::Keypoint::getTargetPosition() const {
+ GraspTrajectory::Keypoint::getTargetPosition() const
+ {
+ ARMARX_TRACE;
return ::math::Helpers::GetPosition(tcpTarget);
}
Eigen::Matrix3f
- GraspTrajectory::Keypoint::getTargetOrientation() const {
+ GraspTrajectory::Keypoint::getTargetOrientation() const
+ {
+ ARMARX_TRACE;
return ::math::Helpers::GetOrientation(tcpTarget);
}
Eigen::Matrix4f
- GraspTrajectory::Keypoint::getTargetPose() const {
+ GraspTrajectory::Keypoint::getTargetPose() const
+ {
+ ARMARX_TRACE;
return tcpTarget;
}
- GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget,
- float dt, const Eigen::Vector3f &feedForwardPosVelocity,
- const Eigen::Vector3f &feedForwardOriVelocity,
- const Eigen::Vector3f &feedForwardHandJointsVelocity)
- : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(dt),
- feedForwardPosVelocity(feedForwardPosVelocity), feedForwardOriVelocity(feedForwardOriVelocity),
- feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
- { }
-
- GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget)
- : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(0),
- feedForwardPosVelocity(0, 0, 0), feedForwardOriVelocity(0, 0, 0), feedForwardHandJointsVelocity(0, 0, 0)
- { }
-}
+ GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt,
+ const Eigen::Vector3f& feedForwardPosVelocity,
+ const Eigen::Vector3f& feedForwardOriVelocity,
+ const Eigen::VectorXf& feedForwardHandJointsVelocity) :
+ tcpTarget(tcpTarget),
+ handJointsTarget(handJointsTarget),
+ dt(dt),
+ feedForwardPosVelocity(feedForwardPosVelocity),
+ feedForwardOriVelocity(feedForwardOriVelocity),
+ feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
+ {
+ }
+
+ GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget) :
+ tcpTarget(tcpTarget),
+ handJointsTarget(handJointsTarget),
+ dt(0),
+ feedForwardPosVelocity(0, 0, 0),
+ feedForwardOriVelocity(0, 0, 0)
+ {
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
index 18990ca21ae05f41fbe520c0da94374d4f2ae11e..697727b2274b6fee76a285fa392094043496f60e 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
@@ -23,12 +23,15 @@
#pragma once
-#include <Eigen/Core>
-#include <memory>
#include <map>
+#include <memory>
+
+#include <Eigen/Core>
+
#include <VirtualRobot/VirtualRobot.h>
-#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
namespace armarx
{
@@ -48,20 +51,23 @@ namespace armarx
{
public:
Eigen::Matrix4f tcpTarget;
- Eigen::Vector3f handJointsTarget;
+ armarx::NameValueMap handJointsTarget;
float dt;
Eigen::Vector3f feedForwardPosVelocity;
Eigen::Vector3f feedForwardOriVelocity;
- Eigen::Vector3f feedForwardHandJointsVelocity;
-
- Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget);
- Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt,
- const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity
- , const Eigen::Vector3f& feedForwardHandJointsVelocity);
+ Eigen::VectorXf feedForwardHandJointsVelocity;
+
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const armarx::NameValueMap& handJointsTarget);
+ Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt,
+ const Eigen::Vector3f& feedForwardPosVelocity,
+ const Eigen::Vector3f& feedForwardOriVelocity,
+ const Eigen::VectorXf& feedForwardHandJointsVelocity);
Eigen::Vector3f getTargetPosition() const;
Eigen::Matrix3f getTargetOrientation() const;
Eigen::Matrix4f getTargetPose() const;
- void updateVelocities(const KeypointPtr& prev, float dt);
+ void updateVelocities(const KeypointPtr& prev, float deltat);
};
struct Length
@@ -73,17 +79,26 @@ namespace armarx
public:
GraspTrajectory() = default;
- GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::Vector3f& handJointsStart);
+ GraspTrajectory(const Eigen::Matrix4f& tcpStart,
+ const armarx::NameValueMap& handJointsStart);
- void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+ void addKeypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt);
size_t getKeypointCount() const;
- void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+ void insertKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt);
void removeKeypoint(size_t index);
- void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+ void replaceKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt);
void setKeypointDt(size_t index, float dt);
@@ -103,7 +118,7 @@ namespace armarx
std::vector<Eigen::Vector3f> getAllKeypointPositions();
std::vector<Eigen::Matrix3f> getAllKeypointOrientations();
- Eigen::Vector3f GetHandValues(float t);
+ armarx::NameValueMap GetHandValues(float t);
Eigen::Vector3f GetPositionDerivative(float t);
@@ -118,18 +133,21 @@ namespace armarx
Length calculateLength() const;
- GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation);
+ GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation,
+ const Eigen::Matrix3f& rotation);
GraspTrajectoryPtr getTransformed(const Eigen::Matrix4f& transform);
GraspTrajectoryPtr getTransformedToOtherHand();
GraspTrajectoryPtr getClone();
GraspTrajectoryPtr
- getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
-
- SimpleDiffIK::Reachability calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
-
+ getTransformedToGraspPose(const Eigen::Matrix4f& target,
+ const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
+ SimpleDiffIK::Reachability
+ calculateReachability(VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(),
+ SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
void writeToFile(const std::string& filename);
@@ -139,13 +157,10 @@ namespace armarx
static GraspTrajectoryPtr ReadFromString(const std::string& xml);
private:
-
void updateKeypointMap();
private:
std::vector<KeypointPtr> keypoints;
std::map<float, size_t> keypointMap;
-
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
index 73984b1d1c3f6539e384e02668e76e264f30565c..03ff54827ffe615760598e6e4d76bfb0e44b7d30 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
+++ b/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
@@ -2,6 +2,7 @@ add_subdirectory(core)
add_subdirectory(util/LocationLoader)
add_subdirectory(util/AffordanceLoader)
+add_subdirectory(util/CommonPlaceLoader)
add_subdirectory(motions)
add_subdirectory(objects)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/README b/source/RobotAPI/libraries/PriorKnowledge/README
new file mode 100644
index 0000000000000000000000000000000000000000..bf66dd168ff03dd38799072d053c7b96b7b270a3
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/README
@@ -0,0 +1 @@
+This one should be moved to prior knowledge data itself, shouldnt it? (as optional dependency for robotapi)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp
index 7283ac4abae3f8a64d659c9b92430c9b610715f7..2e9db269c52a2e418e609ed0e0f79f95734f50bf 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp
@@ -6,42 +6,41 @@
namespace armarx::priorknowledge::util
{
std::vector<StaticAffordance>
- AffordanceLoader::LoadStaticAffordances(const std::string& sourceId, const nlohmann::json& js)
+ AffordanceLoader::LoadAffordances(const std::string& source, const nlohmann::json& js)
{
std::vector<StaticAffordance> ret;
- if (not js.contains("static_affordances"))
+ if (not js.contains("affordances"))
{
ARMARX_WARNING << "The affordances file has the wrong structure. Missing key "
- "'static_affordances'.";
+ "'affordances'.";
return ret;
}
- for (const auto& affordance : js["static_affordances"].get<std::vector<std::string>>())
+ for (const auto& affordance : js["affordances"].get<std::vector<std::string>>())
{
- StaticAffordance a(sourceId, affordance);
+ StaticAffordance a{{source, affordance}};
ret.push_back(a);
}
return ret;
}
- std::vector<LocatableAffordance>
- AffordanceLoader::LoadLocatableAffordances(const std::string& sourceId,
- const nlohmann::json& js)
+ std::vector<LocationAffordance>
+ AffordanceLoader::LoadLocationAffordances(const std::string& source, const nlohmann::json& js)
{
- std::vector<LocatableAffordance> ret;
- if (not js.contains("locatable_affordances"))
+ std::vector<LocationAffordance> ret;
+ if (not js.contains("location_affordances"))
{
ARMARX_WARNING << "The affordances file has the wrong structure. Missing key "
- "'locatable_affordances'.";
+ "'location_affordances'.";
return ret;
}
- for (const auto& [affordance, locationIds] :
- js["locatable_affordances"].get<std::map<std::string, std::vector<std::string>>>())
+ for (const auto& [locationName, affordances] :
+ js["location_affordances"].get<std::map<std::string, std::vector<std::string>>>())
{
- for (const auto& locationId : locationIds)
+ for (const auto& affordance : affordances)
{
- LocatableAffordance a(sourceId, affordance, locationId);
+ LocationAffordance a{{source, affordance}, locationName};
ret.push_back(a);
}
}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
index 71696abc97db46be35c9475d9ccc7506ed0933dd..b8f0b9f6e0cf7a4ec7d64eab6a650c7afb90ad1b 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
@@ -14,13 +14,14 @@ namespace armarx::priorknowledge::util
{
public:
static const constexpr auto DEFAULT_FILE_NAME = "affordances.json";
+ static const constexpr auto DEFAULT_LOCATION_FILE_NAME = "location_affordances.json";
AffordanceLoader() = delete;
- static std::vector<StaticAffordance> LoadStaticAffordances(const std::string& sourceId,
- const nlohmann::json&);
+ static std::vector<StaticAffordance> LoadAffordances(const std::string& sourceId,
+ const nlohmann::json&);
- static std::vector<LocatableAffordance>
- LoadLocatableAffordances(const std::string& sourceId, const nlohmann::json&);
+ static std::vector<LocationAffordance> LoadLocationAffordances(const std::string& sourceId,
+ const nlohmann::json&);
};
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp
index 621ce74f0e41668382bf65ecc992e92890e8d3f5..f28e60c80c3932c083feb25db5b0f93b40266148 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp
@@ -6,28 +6,10 @@
namespace armarx::priorknowledge::util
{
-
- AffordanceId::AffordanceId(const std::string& dataset, const std::string& name) :
- sourceId(dataset), name(name)
- {
- }
-
std::string
AffordanceId::toString() const
{
- return sourceId + "/" + name;
- }
-
- StaticAffordance::StaticAffordance(const std::string& dataset, const std::string& name) :
- id(dataset, name)
- {
- }
-
- LocatableAffordance::LocatableAffordance(const std::string& dataset,
- const std::string& name,
- const std::string& locationId) :
- id(dataset, name), locationId(locationId)
- {
+ return source + "/" + name;
}
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.h b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.h
index ce030b534a4471c1ff33d89478972b3477fe71e0..39eaedc0c86c1fe7a8f3993264c2bd9987ca1601 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.h
@@ -6,28 +6,21 @@ namespace armarx::priorknowledge::util
{
struct AffordanceId
{
- std::string sourceId;
+ std::string source;
std::string name;
- AffordanceId(const std::string& dataset, const std::string& name);
-
std::string toString() const;
};
struct StaticAffordance
{
AffordanceId id;
-
- StaticAffordance(const std::string& dataset, const std::string& name);
};
- struct LocatableAffordance
+ struct LocationAffordance
{
AffordanceId id;
- std::string locationId;
-
- LocatableAffordance(const std::string& dataset,
- const std::string& name,
- const std::string& locationId);
+ std::string locationName;
};
+
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ce15a37df98b729bd0a0dc875ccbac13aae2108f
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
@@ -0,0 +1,27 @@
+set(LIB_NAME ${PROJECT_NAME}PriorKnowledgeCommonPlaceLoaderUtil)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ 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)
+
+# add unit tests
+#add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f8c05ffe2e56883568f00621c8661a7a631d722f
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp
@@ -0,0 +1,28 @@
+#include "CommonPlaceLoader.h"
+
+#include <ArmarXCore/core/eigen/ice_conversions.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+namespace armarx::priorknowledge::util
+{
+
+ std::vector<CommonPlace>
+ CommonPlaceLoader::LoadCommonPlaces(const std::string& source, const nlohmann::json& js)
+ {
+ std::vector<CommonPlace> ret;
+ if (not js.contains("common_places"))
+ {
+ ARMARX_WARNING << "The common_places file has the wrong structure. Missing key "
+ "'common_places'.";
+ return ret;
+ }
+
+ for (const auto& [locationName, priority] :
+ js["common_places"].get<std::map<std::string, int>>())
+ {
+ CommonPlace a{source, locationName, priority};
+ ret.push_back(a);
+ }
+ return ret;
+ }
+} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.h b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.h
new file mode 100644
index 0000000000000000000000000000000000000000..f6ee9021210c5698008f578deb6f54d232eb4385
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include <fstream>
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/json.h>
+
+#include "datatypes/CommonPlace.h"
+
+namespace armarx::priorknowledge::util
+{
+ class CommonPlaceLoader
+ {
+ public:
+ static const constexpr auto DEFAULT_FILE_NAME = "common_places.json";
+
+ CommonPlaceLoader() = delete;
+
+ static std::vector<CommonPlace> LoadCommonPlaces(const std::string& source,
+ const nlohmann::json&);
+ };
+} // namespace armarx::priorknowledge::util
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/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de43258cf4040b1aa95fdd3ae9a930d767f66a06
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.cpp
@@ -0,0 +1,10 @@
+#include "CommonPlace.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+namespace armarx::priorknowledge::util
+{
+
+} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.h b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.h
new file mode 100644
index 0000000000000000000000000000000000000000..b95e5a236be3647eebc5b5c70868d27b21c1cccd
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <string>
+
+namespace armarx::priorknowledge::util
+{
+ struct CommonPlace
+ {
+ std::string source;
+ std::string locationName;
+ int priority;
+ };
+
+} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
index 47b8601280e5191e99a7f1a4d56fec5aeaf75ab5..cfeb43db57231ff85188667c60773c6d0e30390c 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
@@ -3,79 +3,154 @@
#include <SimoxUtility/algorithm/string.h>
#include <ArmarXCore/core/eigen/ice_conversions.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
namespace armarx::priorknowledge::util
{
- std::vector<Location>
- LocationLoader::LoadLocations(const std::string& dataset, const nlohmann::json& js)
+ FramedLocationPtr
+ LocationLoader::LoadFramedLocation(const std::string& source,
+ const std::string& locationName,
+ const nlohmann::json& j)
{
- std::vector<Location> ret;
- if (not js.contains("locations"))
- {
- ARMARX_WARNING
- << "The locations file has the wrong structure. Missing key 'locations'.";
- return ret;
- }
+ ARMARX_CHECK(j.find("framedPose") != j.end());
- for (const auto& [locationIdStr, j] :
- js["locations"].get<std::map<std::string, nlohmann::json>>())
+ const auto framedPose = j.at("framedPose").get<std::map<std::string, nlohmann::json>>();
+
+ std::string frame = framedPose.at("frame");
+ std::string agent = framedPose.at("agent");
+ Eigen::Matrix4f pose;
+
+ // sanitize frame if not set
+ if (frame.empty())
{
- if (j.find("framedPose") == j.end())
+ if (agent.empty())
{
- ARMARX_WARNING << "The element '" << locationIdStr
- << "' has no 'framedPose' member. Skipping "
- "this entity.";
- continue;
+ ARMARX_WARNING << "Got empty frame for location '" + locationName +
+ "'. Sanitizing it to '" + GlobalFrame + "'.";
+ frame = GlobalFrame;
+ }
+ else
+ {
+ ARMARX_WARNING << "Got empty frame for location '" + locationName +
+ "'. Sanitizing it to 'root' because "
+ "an agent name '" +
+ agent + "' was set.";
+ frame = "root";
}
+ }
+
+ if (frame != GlobalFrame && agent.empty())
+ {
+ ARMARX_WARNING << "Got an empty agent name but a set frame '" + frame +
+ "' for location '" + locationName +
+ "'. This may lead to problems...";
+ // TODO: What to do in that case?
+ }
- const auto framedPose = j.at("framedPose").get<std::map<std::string, nlohmann::json>>();
+ // Utilize ice structure of eigen
+ armarx::core::eigen::fromIce(
+ pose,
+ framedPose.at("pose").get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
- std::string frame = framedPose.at("frame");
- std::string agent = framedPose.at("agent");
- Eigen::Matrix4f pose;
+ FramedLocationPtr loc(new FramedLocation(
+ LocationId(source, locationName), LocationType::FRAMED_LOCATION, frame, agent, pose));
+ return loc;
+ }
- // sanitize frame if not set
- if (frame.empty())
+ FramedBoxedLocationPtr
+ LocationLoader::LoadBoxedLocation(const std::string& source,
+ const std::string& locationName,
+ const nlohmann::json& j)
+ {
+ ARMARX_CHECK(j.find("framedOrientedBox") != j.end());
+
+ const auto framedOrientedBox =
+ j.at("framedOrientedBox").get<std::map<std::string, nlohmann::json>>();
+
+ std::string frame = framedOrientedBox.at("frame");
+ std::string agent = framedOrientedBox.at("agent");
+ Eigen::Matrix4f pose;
+ Eigen::Vector3f extents;
+
+ // sanitize frame if not set
+ if (frame.empty())
+ {
+ if (agent.empty())
{
- if (agent.empty())
- {
- ARMARX_WARNING << "Got empty frame for location '" + locationIdStr +
- "'. Sanitizing it to '" + GlobalFrame + "'.";
- frame = GlobalFrame;
- }
- else
- {
- ARMARX_WARNING << "Got empty frame for location '" + locationIdStr +
- "'. Sanitizing it to 'root' because "
- "an agent name '" +
- agent + "' was set.";
- frame = "root";
- }
+ ARMARX_WARNING << "Got empty frame for location '" + locationName +
+ "'. Sanitizing it to '" + GlobalFrame + "'.";
+ frame = GlobalFrame;
}
-
- if (frame != GlobalFrame && agent.empty())
+ else
{
- ARMARX_WARNING << "Got an empty agent name but a set frame '" + frame +
- "' for location '" + locationIdStr +
- "'. This may lead to problems...";
- // TODO: What to do in that case?
+ ARMARX_WARNING << "Got empty frame for location '" + locationName +
+ "'. Sanitizing it to 'root' because "
+ "an agent name '" +
+ agent + "' was set.";
+ frame = "root";
}
+ }
+
+ if (frame != GlobalFrame && agent.empty())
+ {
+ ARMARX_WARNING << "Got an empty agent name but a set frame '" + frame +
+ "' for location '" + locationName +
+ "'. This may lead to problems...";
+ // TODO: What to do in that case?
+ }
+
+ // Utilize ice structure of eigen
+ armarx::core::eigen::fromIce(
+ pose,
+ framedOrientedBox.at("pose")
+ .get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
- // Utilize ice structure of eigen
- armarx::core::eigen::fromIce(
- pose,
- framedPose.at("pose")
- .get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
+ // Utilize ice structure of eigen
+ armarx::core::eigen::fromIce(
+ extents,
+ framedOrientedBox.at("extents").get<std::vector<float>>()); // load the 4x4 matrix
- armarx::FramedPose fp(pose, frame, agent);
+ FramedBoxedLocationPtr loc(new FramedBoxedLocation(LocationId(source, locationName),
+ LocationType::FRAMED_BOXED_LOCATION,
+ frame,
+ agent,
+ pose,
+ extents));
+ return loc;
+ }
- // escape locationIdStr
- auto locationIdStrSplitted = simox::alg::split(locationIdStr, "#");
- std::string locationIdStrEscaped = simox::alg::trim_copy(locationIdStrSplitted[0]);
+ std::vector<LocationPtr>
+ LocationLoader::LoadLocations(const std::string& source /* e.g. dataset/class or graph */,
+ const nlohmann::json& js)
+ {
+ std::vector<LocationPtr> ret;
+ if (not js.contains("locations"))
+ {
+ ARMARX_WARNING
+ << "The locations file has the wrong structure. Missing key 'locations'.";
+ return ret;
+ }
- Location loc{{dataset, fp.agent, locationIdStrEscaped}, fp};
- ret.push_back(loc);
+ for (const auto& [locationName, j] :
+ js["locations"].get<std::map<std::string, nlohmann::json>>())
+ {
+ if (j.find("framedPose") != j.end())
+ {
+ ret.push_back(LocationLoader::LoadFramedLocation(source, locationName, j));
+ }
+ else if (j.find("framedOrientedBox") != j.end())
+ {
+ ret.push_back(LocationLoader::LoadBoxedLocation(source, locationName, j));
+ }
+ else
+ {
+ ARMARX_WARNING
+ << "The element '" << locationName
+ << "' has no 'framedPose' member or no 'framedOrientedBox' member. Skipping "
+ "this entity.";
+ continue;
+ }
}
return ret;
}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h
index 70f66236bc4d8c4f286a669c6199caf64cf2ae97..b480f98922f78127aa395a34439ec13f8fb2279d 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h
@@ -17,7 +17,20 @@ namespace armarx::priorknowledge::util
LocationLoader() = delete;
- static std::vector<Location> LoadLocations(const std::string& dataset,
- const nlohmann::json&);
+ // static std::vector<FramedLocation> LoadFramedLocations(const std::string& dataset,
+ // const nlohmann::json&);
+ // static std::vector<FramedBoxedLocation> LoadBoxedLocations(const std::string& dataset,
+ // const nlohmann::json&);
+ static std::vector<LocationPtr> LoadLocations(const std::string& dataset,
+ const nlohmann::json&);
+
+ private:
+ static FramedLocationPtr LoadFramedLocation(const std::string& source,
+ const std::string& locationName,
+ const nlohmann::json& j);
+
+ static FramedBoxedLocationPtr LoadBoxedLocation(const std::string& source,
+ const std::string& locationName,
+ const nlohmann::json& j);
};
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
index 6935d58403c62e1b74dd9361da955f6236519554..80dfd636de50f94003f673cb47114c86afdcb844 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
@@ -1,23 +1,83 @@
#include "Visu.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx::priorknowledge::util::location
{
- armarx::viz::Pose
- Visu::getLocationVertex(const std::string& id, const Eigen::Matrix4f& locationGlobalPose) const
+ void
+ Visu::addFramedLocationToLayer(viz::Layer& layer,
+ const std::string& id,
+ const Eigen::Matrix4f& locationGlobalPose) const
+ {
+ // Add global location to layer
+ layer.add(armarx::viz::Pose(id).pose(locationGlobalPose));
+ layer.add(armarx::viz::Arrow(id + "_dir")
+ .pose(locationGlobalPose)
+ .length(110)
+ .width(7.5)
+ .color(this->settings.framedLocationArrowColor));
+ }
+
+ void
+ Visu::addFramedBoxedLocationToLayer(viz::Layer& layer,
+ const std::string& id,
+ const Eigen::Matrix4f& locationGlobalPose,
+ const Eigen::Vector3f& extends) const
{
// Add global location to layer
- return armarx::viz::Pose(id).pose(locationGlobalPose).scale(settings.vertexScale);
+ layer.add(armarx::viz::Box(id)
+ .pose(locationGlobalPose)
+ .size(extends)
+ .color(this->settings.framedBoxedLocationColor));
}
viz::Layer
Visu::locationsToLayer(const std::string& layerName,
- const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const
+ const std::map<std::string, FramedLocationData>& locationData) const
+ {
+ auto layer = arviz.layer(layerName);
+ for (auto& [id, data] : locationData)
+ {
+ if (data.extents.has_value())
+ {
+ addFramedBoxedLocationToLayer(layer, id, data.globalPose, data.extents.value());
+ }
+ else
+ {
+ addFramedLocationToLayer(layer, id, data.globalPose);
+ }
+ }
+ return layer;
+ }
+
+ viz::Layer
+ Visu::framedLocationsToLayer(
+ const std::string& layerName,
+ const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const
{
auto layer = arviz.layer(layerName);
for (auto& [id, pose] : locationGlobalPoses)
{
// Add global location to layer
- layer.add(getLocationVertex(id, pose));
+ addFramedLocationToLayer(layer, id, pose);
+ }
+ return layer;
+ }
+
+ viz::Layer
+ Visu::framedBoxedLocationsToLayer(
+ const std::string& layerName,
+ const std::map<std::string, std::pair<Eigen::Matrix4f, Eigen::Vector3f>>&
+ locationGlobalPosesAndExtends) const
+ {
+ auto layer = arviz.layer(layerName);
+ for (auto& [id, pair] : locationGlobalPosesAndExtends)
+ {
+ const auto& pose = pair.first;
+ const auto& extends = pair.second;
+
+ // Add global location to layer
+ addFramedBoxedLocationToLayer(layer, id, pose, extends);
}
return layer;
}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
index 33abc90e7d3e8560f93b96f226f8542692b33a7c..a237c48b3ee1ae78bce651ab887f37c3aa2569df 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
@@ -7,25 +7,55 @@
namespace armarx::priorknowledge::util::location
{
+
class Visu
{
public:
+ struct FramedLocationData
+ {
+ Eigen::Matrix4f globalPose;
+ std::optional<Eigen::Vector3f> extents;
+ };
+
Visu(viz::Client& arviz) : arviz(arviz)
+
{
}
~Visu() = default;
- armarx::viz::Pose getLocationVertex(const std::string& id,
- const Eigen::Matrix4f& locationGlobalPose) const;
-
viz::Layer
locationsToLayer(const std::string& layerName,
- const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const;
+ const std::map<std::string, FramedLocationData>& locationData) const;
+
+ viz::Layer framedLocationsToLayer(
+ const std::string& layerName,
+ const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const;
+
+ viz::Layer framedBoxedLocationsToLayer(
+ const std::string& layerName,
+ const std::map<std::string, std::pair<Eigen::Matrix4f, Eigen::Vector3f>>&
+ locationGlobalPosesAndExtends) const;
+ protected:
+ void addFramedLocationToLayer(viz::Layer&,
+ const std::string& id,
+ const Eigen::Matrix4f& locationGlobalPose) const;
+
+ void addFramedBoxedLocationToLayer(viz::Layer&,
+ const std::string& id,
+ const Eigen::Matrix4f& locationGlobalPose,
+ const Eigen::Vector3f& extends) const;
+
+ public:
struct Settings
{
- float vertexScale = 1.0;
+ // FramedLocation
+ viz::Color framedLocationArrowColor = viz::Color::green();
+
+ // FramedBoxedLocation
+ viz::Color framedBoxedLocationColor = viz::Color(0, 150, 130, 40);
+
} settings;
protected:
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
index ea08e20229bb09989f2b4239a529d9b107a3e0f2..56617061dfc387e3bc74d6247134a00dfae799fa 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
@@ -9,17 +9,7 @@ namespace armarx::priorknowledge::util
std::string
LocationId::toString() const
{
- return sourceId + "/" + toStringWithoutSourceId();
- }
-
- std::string
- LocationId::toStringWithoutSourceId() const
- {
- if (! prefix.empty())
- {
- return prefix + "/" + name;
- }
- return name;
+ return source + "/" + name;
}
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
index 202b7734a2b7dea90e37d1df1f4a6886ba162b18..2a1eccb4cbe72f30ac0da1582c294eb6508ef0f2 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
@@ -1,24 +1,85 @@
#pragma once
+#include <memory>
#include <string>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::priorknowledge::util
{
+ enum class LocationType
+ {
+ FRAMED_LOCATION,
+ FRAMED_BOXED_LOCATION
+ };
+
struct LocationId
{
- std::string sourceId;
- std::string prefix;
+ std::string source;
std::string name;
+ LocationId(const std::string& s, const std::string& n) : source(s), name(n)
+ {
+ }
+
std::string toString() const;
- std::string toStringWithoutSourceId() const;
};
struct Location
{
LocationId id;
- armarx::FramedPose pose;
+ LocationType type;
+
+ Location(const LocationId& i, const LocationType t) : id(i), type(t)
+ {
+ }
+
+ virtual ~Location() = default;
};
+
+ struct FramedLocation : public Location
+ {
+ std::string frame;
+ std::string agent;
+ Eigen::Matrix4f pose;
+
+ FramedLocation(const LocationId& i,
+ const LocationType t,
+ const std::string& f,
+ const std::string& a,
+ const Eigen::Matrix4f& p) :
+ Location(i, t), frame(f), agent(a), pose(p)
+ {
+ }
+
+ virtual ~FramedLocation() = default;
+
+ armarx::FramedPose toFramedPose();
+ };
+
+ struct FramedBoxedLocation : public FramedLocation
+ {
+ Eigen::Vector3f extents;
+
+ FramedBoxedLocation(const LocationId& i,
+ const LocationType t,
+ const std::string& f,
+ const std::string& a,
+ const Eigen::Matrix4f& p,
+ const Eigen::Vector3f& e) :
+ FramedLocation(i, t, f, a, p), extents(e)
+ {
+ }
+
+ virtual ~FramedBoxedLocation() = default;
+
+ simox::OrientedBox<float> toOrientedBox();
+ };
+
+ using FramedBoxedLocationPtr = std::unique_ptr<FramedBoxedLocation>;
+ using FramedLocationPtr = std::unique_ptr<FramedLocation>;
+ using LocationPtr = std::unique_ptr<Location>;
+
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/armem/client/Query.h b/source/RobotAPI/libraries/armem/client/Query.h
index bee999f073bc57cd53db0b9031cba9feee1cfbb7..8e14c5b5a7aa22ea260779c95bb63a24891cc29b 100644
--- a/source/RobotAPI/libraries/armem/client/Query.h
+++ b/source/RobotAPI/libraries/armem/client/Query.h
@@ -1,44 +1,60 @@
#pragma once
-// RobotAPI
#include <RobotAPI/interface/armem/query.h>
-
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/SuccessHeader.h>
#include <RobotAPI/libraries/armem/core/query/DataMode.h>
#include <RobotAPI/libraries/armem/core/query/QueryTarget.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-
namespace armarx::armem::client::query
{
// #include <RobotAPI/libraries/armem/client/query/Builder.h>
class Builder;
-}
+} // namespace armarx::armem::client::query
namespace armarx::armem::client
{
using QueryBuilder = query::Builder;
-
/**
- * @brief An update of an entity for a specific point in time.
+ * @brief A query for parts of a memory.
*/
struct QueryInput
{
+ /**
+ * @brief The queries.
+ */
armem::query::data::MemoryQuerySeq memoryQueries;
+ /**
+ * @brief Whether entity instance data (i.e., their payload) should be transferred.
+ */
armem::query::DataMode dataMode;
static QueryInput fromIce(const armem::query::data::Input& ice);
armem::query::data::Input toIce() const;
};
-
/**
- * @brief Result of an `EntityUpdate`.
+ * @brief Result of a `QueryInput`.
+ *
+ * If `success` is false, an error occurred during the query. In this case, `errorMessage` can
+ * contain more information.
+ *
+ * @note An empty result is valid, i.e. successful. In other words, an empty result is not a
+ * failure. To check whether any entity snapshot matched the query, use `memory.hasSnapshots()`
+ * or `memory.hasInstances()`.
+ *
+ * @see wm::Memory::hasInstances(), wm::Memory::hasSnapshots()
*/
struct QueryResult : public detail::SuccessHeader
{
+ /**
+ * @brief The slice of the memory that matched the query.
+ *
+ * To check whether the memory contains any snapshots or instances, use
+ * `memory.hasSnapshots()` or `memory.hasInstances()`.
+ */
wm::Memory memory;
@@ -48,11 +64,10 @@ namespace armarx::armem::client
friend std::ostream& operator<<(std::ostream& os, const QueryResult& rhs);
};
-
void toIce(armem::query::data::Input& ice, const QueryInput& input);
void fromIce(const armem::query::data::Input& ice, QueryInput& input);
void toIce(armem::query::data::Result& ice, const QueryResult& result);
void fromIce(const armem::query::data::Result& ice, QueryResult& result);
-}
+} // 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/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
index b0bbf2869134f92cdf15e45bde4dd264dda2d1b0..e14ce8848fd1fe8242d681770563bf9cddefd7cd 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -1,20 +1,17 @@
#include "SimpleReaderBase.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
{
- SimpleReaderBase::SimpleReaderBase(MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
+ SimpleReaderBase::SimpleReaderBase()
{
}
-
- void SimpleReaderBase::registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def)
+ void
+ SimpleReaderBase::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
@@ -26,16 +23,16 @@ namespace armarx::armem::client::util
def->optional(props.coreSegmentName, prefix + "CoreSegment");
}
-
- void SimpleReaderBase::connect()
+ void
+ 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
- << "' ...";
+ ARMARX_IMPORTANT << "SimpleReaderBase: Waiting for memory '" << props.memoryName << "' ...";
try
{
- memoryReaderClient = memoryNameSystem.useReader(MemoryID().withMemoryName(props.memoryName));
- ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName << "'";
+ memoryReaderClient = mns.useReader(MemoryID().withMemoryName(props.memoryName));
+ ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '" << props.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -44,20 +41,20 @@ namespace armarx::armem::client::util
}
}
-
- std::mutex& SimpleReaderBase::memoryReaderMutex()
+ std::mutex&
+ SimpleReaderBase::memoryReaderMutex()
{
return memoryMutex;
}
-
- const armem::client::Reader& SimpleReaderBase::memoryReader() const
+ const armem::client::Reader&
+ SimpleReaderBase::memoryReader() const
{
return memoryReaderClient;
}
-
- const SimpleReaderBase::Properties& SimpleReaderBase::properties() const
+ const SimpleReaderBase::Properties&
+ SimpleReaderBase::properties() const
{
return props;
}
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/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index 7b6972a93bf7dcad9d804a03136869472b364082..1dbfeaebfa8eae9cfcf1cb54fc6017d6963b3ecf 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -17,7 +17,6 @@
#include "detail/lookup_mixins.h"
#include "detail/negative_index_semantics.h"
-
namespace armarx::armem::base
{
@@ -25,7 +24,7 @@ namespace armarx::armem::base
* @brief An entity over a period of time.
*
* An entity should be a physical thing or abstract concept existing
- * (and potentially evolving) over some time.
+ * (and potentially evolving) over time.
*
* Examples are:
* - objects (the green box)
@@ -42,16 +41,17 @@ namespace armarx::armem::base
*/
template <class _EntitySnapshotT, class _Derived>
class EntityBase :
- public detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>
- , public detail::ForEachEntityInstanceMixin<_Derived>
- , public detail::GetFindInstanceMixin<_Derived>
+ public detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>,
+ public detail::ForEachEntityInstanceMixin<_Derived>,
+ public detail::GetFindInstanceMixin<_Derived>,
+ public detail::GetLatestInstanceMixin<_Derived>,
+ public detail::GetLatestSnapshotMixin<_Derived>
{
using Base = detail::MemoryContainerBase<std::map<Time, _EntitySnapshotT>, _Derived>;
public:
-
- using typename Base::DerivedT;
using typename Base::ContainerT;
+ using typename Base::DerivedT;
using EntitySnapshotT = _EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
@@ -68,58 +68,69 @@ namespace armarx::armem::base
};
public:
-
EntityBase()
{
}
+
explicit EntityBase(const std::string& name, const MemoryID& parentID = {}) :
EntityBase(parentID.withEntityName(name))
{
}
- explicit EntityBase(const MemoryID& id) :
- Base(id)
+
+ explicit EntityBase(const MemoryID& id) : Base(id)
{
}
-
EntityBase(const EntityBase& other) = default;
EntityBase(EntityBase&& other) = default;
EntityBase& operator=(const EntityBase& other) = default;
EntityBase& operator=(EntityBase&& other) = default;
-
// READING
// Get key
- inline std::string& name()
+ inline std::string&
+ name()
{
return this->id().entityName;
}
- inline const std::string& name() const
+
+ inline const std::string&
+ name() const
{
return this->id().entityName;
}
+ /// Indicate whether the entity has any snapshots.
+ bool
+ hasSnapshots() const
+ {
+ return not this->empty();
+ }
- // Has child by key
- /// Indicates whether a history entry for the given time exists.
- bool hasSnapshot(const Time& time) const
+ // Has child with key
+ /// Indicate whether a snapshot at the given time exists.
+ bool
+ hasSnapshot(const Time& time) const
{
return this->findSnapshot(time) != nullptr;
}
- // Has child by MemoryID
- bool hasSnapshot(const MemoryID& snapshotID) const
+
+ // Has child with MemoryID
+ /// Indicate whether a snapshot with the given ID exists.
+ bool
+ hasSnapshot(const MemoryID& snapshotID) const
{
return this->findSnapshot(snapshotID) != nullptr;
}
-
// Find child via key
EntitySnapshotT*
findSnapshot(const Time& timestamp)
{
return detail::findChildByKey(timestamp, this->_container);
}
+
const EntitySnapshotT*
findSnapshot(const Time& timestamp) const
{
@@ -137,18 +148,21 @@ namespace armarx::armem::base
EntitySnapshotT&
getSnapshot(const Time& time)
{
- return detail::getChildByKey(time, this->_container, *this, [](const Time & time)
- {
- return toDateTimeMilliSeconds(time);
- });
+ return detail::getChildByKey(time,
+ this->_container,
+ *this,
+ [](const Time& time)
+ { return toDateTimeMilliSeconds(time); });
}
+
const EntitySnapshotT&
getSnapshot(const Time& time) const
{
- return detail::getChildByKey(time, this->_container, *this, [](const Time & time)
- {
- return toDateTimeMilliSeconds(time);
- });
+ return detail::getChildByKey(time,
+ this->_container,
+ *this,
+ [](const Time& time)
+ { return toDateTimeMilliSeconds(time); });
}
// Find child via MemoryID
@@ -158,6 +172,7 @@ namespace armarx::armem::base
detail::checkHasTimestamp(snapshotID);
return this->findSnapshot(snapshotID.timestamp);
}
+
const EntitySnapshotT*
findSnapshot(const MemoryID& snapshotID) const
{
@@ -172,12 +187,12 @@ namespace armarx::armem::base
detail::checkHasTimestamp(snapshotID);
return this->getSnapshot(snapshotID.timestamp);
}
+
const EntitySnapshotT&
getSnapshot(const MemoryID& snapshotID) const
{
detail::checkHasTimestamp(snapshotID);
return this->getSnapshot(snapshotID.timestamp);
-
}
// get/findInstance are provided by GetFindInstanceMixin
@@ -189,15 +204,18 @@ namespace armarx::armem::base
* @brief Get the latest timestamp.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- Time getLatestTimestamp() const
+ Time
+ getLatestTimestamp() const
{
return this->getLatestSnapshot().time();
}
+
/**
* @brief Get the oldest timestamp.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- Time getFirstTimestamp() const
+ Time
+ getFirstTimestamp() const
{
return this->getFirstSnapshot().time();
}
@@ -206,46 +224,30 @@ namespace armarx::armem::base
* @brief Return the snapshot with the most recent timestamp.
* @return The latest snapshot or nullptr if the entity is empty.
*/
- EntitySnapshotT* findLatestSnapshot()
- {
- return this->empty() ? nullptr : &this->_container.rbegin()->second;
- }
- const EntitySnapshotT* findLatestSnapshot() const
+ EntitySnapshotT*
+ findLatestSnapshot()
{
return this->empty() ? nullptr : &this->_container.rbegin()->second;
}
- /**
- * @brief Return the snapshot with the most recent timestamp.
- * @return The latest snapshot.
- * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- */
- EntitySnapshotT& getLatestSnapshot()
- {
- return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getLatestSnapshot());
- }
- const EntitySnapshotT& getLatestSnapshot() const
+ const EntitySnapshotT*
+ findLatestSnapshot() const
{
- if (const EntitySnapshotT* snapshot = this->findLatestSnapshot())
- {
- return *snapshot;
- }
- else
- {
- throw armem::error::EntityHistoryEmpty(name(), "when getting the latest snapshot.");
- }
+ return this->empty() ? nullptr : &this->_container.rbegin()->second;
}
-
/**
* @brief Return the snapshot with the least recent timestamp.
* @return The first snapshot or nullptr if the entity is empty.
*/
- EntitySnapshotT* findFirstSnapshot()
+ EntitySnapshotT*
+ findFirstSnapshot()
{
return this->empty() ? nullptr : &this->_container.begin()->second;
}
- const EntitySnapshotT* findFirstSnapshot() const
+
+ const EntitySnapshotT*
+ findFirstSnapshot() const
{
return this->empty() ? nullptr : &this->_container.begin()->second;
}
@@ -255,11 +257,15 @@ namespace armarx::armem::base
* @return The first snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
- EntitySnapshotT& getFirstSnapshot()
+ EntitySnapshotT&
+ getFirstSnapshot()
{
- return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getFirstSnapshot());
+ return const_cast<EntitySnapshotT&>(
+ const_cast<const EntityBase*>(this)->getFirstSnapshot());
}
- const EntitySnapshotT& getFirstSnapshot() const
+
+ const EntitySnapshotT&
+ getFirstSnapshot() const
{
if (const EntitySnapshotT* snapshot = this->findFirstSnapshot())
{
@@ -271,13 +277,13 @@ namespace armarx::armem::base
}
}
-
/**
* @brief Return the lastest snapshot before time.
* @param time The time.
* @return The latest snapshot < time or nullptr if none was found.
*/
- const EntitySnapshotT* findLatestSnapshotBefore(const Time& time) const
+ const EntitySnapshotT*
+ findLatestSnapshotBefore(const Time& time) const
{
if (this->empty())
{
@@ -309,7 +315,8 @@ namespace armarx::armem::base
* @param time The time.
* @return The latest snapshot <= time or nullptr if none was found.
*/
- const EntitySnapshotT* findLatestSnapshotBeforeOrAt(const Time& time) const
+ const EntitySnapshotT*
+ findLatestSnapshotBeforeOrAt(const Time& time) const
{
return findLatestSnapshotBefore(time + Duration::MicroSeconds(1));
}
@@ -319,7 +326,8 @@ namespace armarx::armem::base
* @param time The time.
* @return The first snapshot >= time or nullptr if none was found.
*/
- const EntitySnapshotT* findFirstSnapshotAfterOrAt(const Time& time) const
+ const EntitySnapshotT*
+ findFirstSnapshotAfterOrAt(const Time& time) const
{
// We want the leftmost element >= time.
// That's lower bound.
@@ -337,24 +345,28 @@ namespace armarx::armem::base
* @param time The time.
* @return The first snapshot >= time or nullptr if none was found.
*/
- const EntitySnapshotT* findFirstSnapshotAfter(const Time& time) const
+ const EntitySnapshotT*
+ findFirstSnapshotAfter(const Time& time) const
{
return findFirstSnapshotAfter(time - Duration::MicroSeconds(1));
}
-
- auto* findLatestInstance(int instanceIndex = 0)
+ auto*
+ findLatestInstance(int instanceIndex = 0)
{
auto* snapshot = this->findLatestSnapshot();
return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
}
- const auto* findLatestInstance(int instanceIndex = 0) const
+
+ const auto*
+ findLatestInstance(int instanceIndex = 0) const
{
auto* snapshot = this->findLatestSnapshot();
return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
}
-#if 0 // Do not offer this yet.
+
+#if 0 // Do not offer this yet.
auto* findLatestInstanceData(int instanceIndex = 0)
{
auto* instance = this->findLatestInstance(instanceIndex);
@@ -374,31 +386,38 @@ namespace armarx::armem::base
* @param func Function like: bool process(EntitySnapshotT& snapshot)
*/
template <class SnapshotFunctionT>
- bool forEachSnapshot(SnapshotFunctionT&& func)
+ bool
+ forEachSnapshot(SnapshotFunctionT&& func)
{
return this->forEachChild(func);
}
+
/**
* @param func Function like void process(const EntitySnapshotT& snapshot)
*/
template <class SnapshotFunctionT>
- bool forEachSnapshot(SnapshotFunctionT&& func) const
+ bool
+ forEachSnapshot(SnapshotFunctionT&& func) const
{
return this->forEachChild(func);
}
+
/**
* @param func Function like: bool process(EntitySnapshotT& snapshot)
*/
template <class SnapshotFunctionT>
- bool forEachSnapshotIn(const MemoryID& id, SnapshotFunctionT&& func)
+ bool
+ forEachSnapshotIn(const MemoryID& id, SnapshotFunctionT&& func)
{
return this->forEachChild(func);
}
+
/**
* @param func Function like void process(const EntitySnapshotT& snapshot)
*/
template <class SnapshotFunctionT>
- bool forEachSnapshotIn(const MemoryID& id, SnapshotFunctionT&& func) const
+ bool
+ forEachSnapshotIn(const MemoryID& id, SnapshotFunctionT&& func) const
{
if (id.hasTimestamp())
{
@@ -409,6 +428,7 @@ namespace armarx::armem::base
}
return this->forEachChild(func);
}
+
// forEachInstance() is provided by ForEachEntityInstanceMixin.
@@ -418,7 +438,8 @@ namespace armarx::armem::base
* @return The latest snapshots.
*/
template <class FunctionT>
- void forEachSnapshotBefore(const Time& time, FunctionT&& func) const
+ void
+ forEachSnapshotBefore(const Time& time, FunctionT&& func) const
{
for (const auto& [timestamp, snapshot] : this->_container)
{
@@ -439,12 +460,12 @@ namespace armarx::armem::base
* @return The latest snapshots.
*/
template <class FunctionT>
- void forEachSnapshotBeforeOrAt(const Time& time, FunctionT&& func) const
+ void
+ forEachSnapshotBeforeOrAt(const Time& time, FunctionT&& func) const
{
getSnapshotsBefore(time + Duration::MicroSeconds(1), func);
}
-
/**
* @brief Return all snapshots between, including, min and max.
* @param min The lowest time to include.
@@ -452,12 +473,15 @@ namespace armarx::armem::base
* @return The snapshots in [min, max].
*/
template <class FunctionT>
- void forEachSnapshotInTimeRange(const Time& min, const Time& max, FunctionT&& func) const
+ void
+ forEachSnapshotInTimeRange(const Time& min, const Time& max, FunctionT&& func) const
{
// Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
- auto begin = min.toMicroSecondsSinceEpoch() > 0 ? this->_container.lower_bound(min) : this->_container.begin();
+ auto begin = min.toMicroSecondsSinceEpoch() > 0 ? this->_container.lower_bound(min)
+ : this->_container.begin();
// Returns an iterator pointing to the first element that is *greater than* key.
- auto end = max.toMicroSecondsSinceEpoch() > 0 ? this->_container.upper_bound(max) : this->_container.end();
+ auto end = max.toMicroSecondsSinceEpoch() > 0 ? this->_container.upper_bound(max)
+ : this->_container.end();
for (auto it = begin; it != end && it != this->_container.end(); ++it)
{
@@ -479,7 +503,8 @@ namespace armarx::armem::base
* @return The snapshots in [first, last].
*/
template <class FunctionT>
- void forEachSnapshotInIndexRange(long first, long last, FunctionT&& func) const
+ void
+ forEachSnapshotInIndexRange(long first, long last, FunctionT&& func) const
{
if (this->empty())
{
@@ -494,7 +519,7 @@ namespace armarx::armem::base
auto it = this->_container.begin();
std::advance(it, first_);
- size_t num = last_ - first_ + 1; // +1 to make last inclusive
+ size_t num = last_ - first_ + 1; // +1 to make last inclusive
for (size_t i = 0; i < num; ++i, ++it)
{
if (not call(func, it->second))
@@ -505,39 +530,44 @@ namespace armarx::armem::base
}
}
-
/**
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
{
- return detail::forEachInstanceIn(
- id, func, *this,
- id.hasTimestamp(),
- id.hasTimestamp() ? this->findSnapshot(id.timestamp) : nullptr);
+ return detail::forEachInstanceIn(id,
+ func,
+ *this,
+ id.hasTimestamp(),
+ id.hasTimestamp() ? this->findSnapshot(id.timestamp)
+ : nullptr);
}
+
/**
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
{
- return detail::forEachInstanceIn(
- id, func, *this,
- id.hasTimestamp(),
- id.hasTimestamp() ? this->findSnapshot(id.timestamp) : nullptr);
+ return detail::forEachInstanceIn(id,
+ func,
+ *this,
+ id.hasTimestamp(),
+ id.hasTimestamp() ? this->findSnapshot(id.timestamp)
+ : nullptr);
}
-
// Get child keys
/// @brief Get all timestamps in the history.
- std::vector<Time> getTimestamps() const
+ std::vector<Time>
+ getTimestamps() const
{
return simox::alg::get_keys(this->_container);
}
-
// MODIFICATION
/**
@@ -545,7 +575,8 @@ namespace armarx::armem::base
* @param update The update.
* @return The snapshot ID of the update.
*/
- UpdateResult update(const EntityUpdate& update)
+ UpdateResult
+ update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.entityName, this->name());
UpdateResult ret;
@@ -572,59 +603,63 @@ namespace armarx::armem::base
return ret;
}
-
template <class OtherDerivedT>
- void append(const OtherDerivedT& other)
+ void
+ append(const OtherDerivedT& other)
{
- other.forEachSnapshot([this](const auto& snapshot)
- {
- auto it = this->_container.find(snapshot.time());
- if (it == this->_container.end())
+ other.forEachSnapshot(
+ [this](const auto& snapshot)
{
- EntitySnapshotT copy { snapshot };
- copy.id() = this->id().withTimestamp(snapshot.time()); // update id (e.g. memory name) if necessary
- this->_container.emplace(snapshot.time(), copy);
- }
- // else: snapshot already exists
- // We assume that a snapshot does not change, so ignore
- return true;
- });
+ auto it = this->_container.find(snapshot.time());
+ if (it == this->_container.end())
+ {
+ EntitySnapshotT copy{snapshot};
+ copy.id() = this->id().withTimestamp(
+ snapshot.time()); // update id (e.g. memory name) if necessary
+ this->_container.emplace(snapshot.time(), copy);
+ }
+ // else: snapshot already exists
+ // We assume that a snapshot does not change, so ignore
+ return true;
+ });
}
-
/// Add a snapshot at the given time.
- EntitySnapshotT& addSnapshot(const Time& timestamp)
+ EntitySnapshotT&
+ addSnapshot(const Time& timestamp)
{
return this->addSnapshot(timestamp, EntitySnapshotT(timestamp));
}
/// Copy and insert a snapshot
- EntitySnapshotT& addSnapshot(const EntitySnapshotT& snapshot)
+ EntitySnapshotT&
+ addSnapshot(const EntitySnapshotT& snapshot)
{
return this->addSnapshot(snapshot.time(), EntitySnapshotT(snapshot));
}
/// Move and insert a snapshot
- EntitySnapshotT& addSnapshot(EntitySnapshotT&& snapshot)
+ EntitySnapshotT&
+ addSnapshot(EntitySnapshotT&& snapshot)
{
- Time timestamp = snapshot.time(); // Copy before move.
+ Time timestamp = snapshot.time(); // Copy before move.
return this->addSnapshot(timestamp, std::move(snapshot));
}
/// Insert a snapshot in-place.
- template <class ...Args>
- EntitySnapshotT& addSnapshot(const Time& timestamp, Args... args)
+ template <class... Args>
+ EntitySnapshotT&
+ addSnapshot(const Time& timestamp, Args... args)
{
auto it = this->_container.emplace_hint(this->_container.end(), timestamp, args...);
it->second.id() = this->id().withTimestamp(timestamp);
return it->second;
}
-
-
// MISC
- bool equalsDeep(const DerivedT& other) const
+ bool
+ equalsDeep(const DerivedT& other) const
{
//std::cout << "Entity::equalsDeep" << std::endl;
if (this->size() != other.size())
@@ -645,15 +680,17 @@ namespace armarx::armem::base
return true;
}
- std::string getKeyString() const
+ std::string
+ getKeyString() const
{
return this->id().entityName;
}
- static std::string getLevelName()
+
+ static std::string
+ getLevelName()
{
return "entity";
}
-
};
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
index 0bd03af1448d99536640c0b9f4a2f83036afc307..892c5db2ada839176eb42ff682e948d1d57b53a0 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
@@ -5,7 +5,6 @@
#include "detail/MemoryItem.h"
-
namespace armarx::armem::base
{
@@ -16,17 +15,19 @@ namespace armarx::armem::base
{
virtual ~NoData() = default;
- bool operator==(const NoData& other)
+ bool
+ operator==(const NoData& other)
{
return true;
}
- bool operator!=(const NoData& other)
+
+ bool
+ operator!=(const NoData& other)
{
return false;
}
};
-
/**
* @brief Metadata of an entity instance.
*/
@@ -62,7 +63,9 @@ namespace armarx::armem::base
void access() const;
bool operator==(const EntityInstanceMetadata& other) const;
- inline bool operator!=(const EntityInstanceMetadata& other) const
+
+ inline bool
+ operator!=(const EntityInstanceMetadata& other) const
{
return !(*this == other);
}
@@ -70,109 +73,123 @@ namespace armarx::armem::base
std::ostream& operator<<(std::ostream& os, const EntityInstanceMetadata& rhs);
-
-
/**
* @brief Data of a single entity instance.
*/
template <class _DataT = NoData, class _MetadataT = EntityInstanceMetadata>
- class EntityInstanceBase :
- public detail::MemoryItem
+ class EntityInstanceBase : public detail::MemoryItem
{
using Base = detail::MemoryItem;
public:
-
using MetadataT = _MetadataT;
using DataT = _DataT;
-
EntityInstanceBase()
{
}
+
explicit EntityInstanceBase(int index, const MemoryID& parentID = {}) :
EntityInstanceBase(parentID.withInstanceIndex(index))
{
}
- explicit EntityInstanceBase(const MemoryID& id) :
- Base(id)
+
+ explicit EntityInstanceBase(const MemoryID& id) : Base(id)
{
}
-
// Key
- inline int& index()
+ inline int&
+ index()
{
return id().instanceIndex;
}
- inline int index() const
+
+ inline int
+ index() const
{
return id().instanceIndex;
}
-
// Data
- MetadataT& metadata()
+ MetadataT&
+ metadata()
{
return _metadata;
}
- const MetadataT& metadata() const
+
+ const MetadataT&
+ metadata() const
{
return _metadata;
}
- const DataT& data() const
+ const DataT&
+ data() const
{
return _data;
}
- DataT& data()
+ DataT&
+ data()
{
return _data;
}
- void setData(DataT& d)
+ void
+ setData(const DataT& data)
{
- _data = d;
+ _data = data;
}
- void setMetadata(MetadataT& m)
+ void
+ setMetadata(const MetadataT& metadata)
{
- _metadata = m;
+ _metadata = metadata;
}
/**
* @brief Get the data converted to a generated Aron DTO class.
*/
template <class AronDtoT>
- AronDtoT dataAs() const
+ AronDtoT
+ dataAs() const
{
return AronDtoT::FromAron(_data);
}
+ template <class AronDtoT>
+ EntityInstanceBase<AronDtoT, MetadataT>
+ withDataAs() const
+ {
+ EntityInstanceBase<AronDtoT, MetadataT> instance{_id};
+ instance.data() = dataAs<AronDtoT>();
+ instance.metadata() = _metadata;
+ return instance;
+ }
// Misc
- static std::string getLevelName()
+ static std::string
+ getLevelName()
{
return "entity instance";
}
- std::string getKeyString() const
+ std::string
+ getKeyString() const
{
return std::to_string(index());
}
protected:
-
/// The metadata.
MetadataT _metadata;
/// The data. May be nullptr.
DataT _data;
-
};
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index 6a069bae3ce3153e1524cac273369e21baa22551..8a7a329753ee84ac1cccf4f70e552f4a7f4f1b1b 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -10,11 +10,11 @@
#include "detail/iteration_mixins.h"
#include "detail/lookup_mixins.h"
-
namespace armarx::armem::base::detail
{
void throwIfNotEqual(const Time& ownTime, const Time& updateTime);
}
+
namespace armarx::armem::base
{
/**
@@ -27,24 +27,23 @@ namespace armarx::armem::base
using Base = detail::MemoryContainerBase<std::vector<_EntityInstanceT>, _Derived>;
public:
-
- using typename Base::DerivedT;
using typename Base::ContainerT;
+ using typename Base::DerivedT;
using EntityInstanceT = _EntityInstanceT;
public:
-
EntitySnapshotBase()
{
}
+
explicit EntitySnapshotBase(Time time, const MemoryID& parentID = {}) :
EntitySnapshotBase(parentID.withTimestamp(time))
{
}
- explicit EntitySnapshotBase(const MemoryID& id) :
- Base(id)
+
+ explicit EntitySnapshotBase(const MemoryID& id) : Base(id)
{
}
@@ -53,43 +52,55 @@ namespace armarx::armem::base
EntitySnapshotBase& operator=(const EntitySnapshotBase& other) = default;
EntitySnapshotBase& operator=(EntitySnapshotBase&& other) = default;
-
// READING
// Get key
- inline Time& time()
+ inline Time&
+ time()
{
return this->id().timestamp;
}
- inline const Time& time() const
+
+ inline const Time&
+ time() const
{
return this->id().timestamp;
}
+ /// Indicate whether this snapshot has any instances.
+ bool
+ hasInstances() const
+ {
+ return not this->empty();
+ }
// Has child by key
- bool hasInstance(int index) const
+ bool
+ hasInstance(int index) const
{
return this->findInstance(index) != nullptr;
}
+
// Has child by ID
- bool hasInstance(const MemoryID& instanceID) const
+ bool
+ hasInstance(const MemoryID& instanceID) const
{
return this->findInstance(instanceID) != nullptr;
}
-
// Find child by key
- EntityInstanceT* findInstance(int index)
+ EntityInstanceT*
+ findInstance(int index)
{
- return const_cast<EntityInstanceT*>(const_cast<const EntitySnapshotBase*>(this)->findInstance(index));
+ return const_cast<EntityInstanceT*>(
+ const_cast<const EntitySnapshotBase*>(this)->findInstance(index));
}
- const EntityInstanceT* findInstance(int index) const
+
+ const EntityInstanceT*
+ findInstance(int index) const
{
const size_t si = static_cast<size_t>(index);
- return (index >= 0 && si < this->_container.size())
- ? &this->_container[si]
- : nullptr;
+ return (index >= 0 && si < this->_container.size()) ? &this->_container[si] : nullptr;
}
// Get child by key
@@ -102,8 +113,10 @@ namespace armarx::armem::base
EntityInstanceT&
getInstance(int index)
{
- return const_cast<EntityInstanceT&>(const_cast<const EntitySnapshotBase*>(this)->getInstance(index));
+ return const_cast<EntityInstanceT&>(
+ const_cast<const EntitySnapshotBase*>(this)->getInstance(index));
}
+
const EntityInstanceT&
getInstance(int index) const
{
@@ -113,7 +126,8 @@ namespace armarx::armem::base
}
else
{
- throw armem::error::MissingEntry::create<EntityInstanceT>(std::to_string(index), *this);
+ throw armem::error::MissingEntry::create<EntityInstanceT>(std::to_string(index),
+ *this);
}
}
@@ -124,6 +138,7 @@ namespace armarx::armem::base
detail::checkHasInstanceIndex(instanceID);
return this->findInstance(instanceID.instanceIndex);
}
+
const EntityInstanceT*
findInstance(const MemoryID& instanceID) const
{
@@ -145,6 +160,7 @@ namespace armarx::armem::base
detail::checkHasInstanceIndex(instanceID);
return this->getInstance(instanceID.instanceIndex);
}
+
const EntityInstanceT&
getInstance(const MemoryID& instanceID) const
{
@@ -152,22 +168,24 @@ namespace armarx::armem::base
return this->getInstance(instanceID.instanceIndex);
}
-
// ITERATION
/**
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstance(InstanceFunctionT&& func)
+ bool
+ forEachInstance(InstanceFunctionT&& func)
{
return this->forEachChild(func);
}
+
/**
* @param func Function like void process (const EntityInstanceT& instance)
*/
template <class InstanceFunctionT>
- bool forEachInstance(InstanceFunctionT&& func) const
+ bool
+ forEachInstance(InstanceFunctionT&& func) const
{
return this->forEachChild(func);
}
@@ -176,7 +194,8 @@ namespace armarx::armem::base
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
{
if (id.hasInstanceIndex())
{
@@ -188,11 +207,13 @@ namespace armarx::armem::base
return this->forEachInstance(func);
}
}
+
/**
* @param func Function like void process (const EntityInstanceT& instance)
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
{
if (id.hasInstanceIndex())
{
@@ -205,9 +226,9 @@ namespace armarx::armem::base
}
}
-
// Get child keys
- std::vector<int> getInstanceIndices() const
+ std::vector<int>
+ getInstanceIndices() const
{
std::vector<int> indices;
indices.reserve(this->size());
@@ -218,10 +239,10 @@ namespace armarx::armem::base
return indices;
}
-
// MODIFICATION
- void update(const EntityUpdate& update)
+ void
+ update(const EntityUpdate& update)
{
detail::throwIfNotEqual(time(), update.referencedTime);
@@ -239,26 +260,23 @@ namespace armarx::armem::base
* @return The stored instance.
* @throw `armem::error::InvalidArgument` If the given index is invalid. Must be equal to container.size() or -1 (meaning push_back)
*/
- EntityInstanceT& addInstance(const EntityInstanceT& instance)
+ EntityInstanceT&
+ addInstance(const EntityInstanceT& instance)
{
return addInstance(EntityInstanceT(instance));
}
- EntityInstanceT& addInstance(EntityInstanceT&& instance)
+ EntityInstanceT&
+ addInstance(EntityInstanceT&& instance)
{
- if (instance.index() > 0 && static_cast<size_t>(instance.index()) < this->_container.size())
- {
- throw error::InvalidArgument(std::to_string(instance.index()), "EntitySnapshot::addInstance",
- "Cannot add an EntityInstance because its index already exists.");
- }
- /*
if (instance.index() > 0 && static_cast<size_t>(instance.index()) > this->_container.size())
{
- throw error::InvalidArgument(std::to_string(instance.index()), "EntitySnapshot::addInstance",
- "Cannot add an EntityInstance because its index is too big.");
+ throw error::InvalidArgument(
+ std::to_string(instance.index()),
+ "EntitySnapshot::addInstance",
+ "Cannot add an EntityInstance because its index is too big.");
}
- */
int index = instance.index();
//int index = static_cast<int>(this->_container.size());
EntityInstanceT& added = this->_container.emplace_back(std::move(instance));
@@ -267,7 +285,8 @@ namespace armarx::armem::base
return added;
}
- EntityInstanceT& addInstance()
+ EntityInstanceT&
+ addInstance()
{
//ARMARX_INFO << "Trying to add an instance without instance yet generated";
int index = static_cast<int>(this->size()); //this is the problem, because instances have names 0 and 11 (I do not know why)
@@ -276,10 +295,10 @@ namespace armarx::armem::base
return added;
}
-
// MISC
- bool equalsDeep(const DerivedT& other) const
+ bool
+ equalsDeep(const DerivedT& other) const
{
//std::cout << "EntitySnapshot::equalsDeep" << std::endl;
if (this->size() != other.size())
@@ -298,17 +317,17 @@ namespace armarx::armem::base
return true;
}
-
- std::string getKeyString() const
+ std::string
+ getKeyString() const
{
return toDateTimeMilliSeconds(this->time());
}
- static std::string getLevelName()
+ static std::string
+ getLevelName()
{
return "entity snapshot";
}
-
};
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index 6cd70391660d6f61fe073837052a9b06fb57da1c..ce5b8d7ea9a0ab258dabe04a3266f102ee9f4dbf 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -7,12 +7,11 @@
#include "EntityBase.h"
#include "detail/AronTyped.h"
-#include "detail/Predictive.h"
#include "detail/MemoryContainerBase.h"
+#include "detail/Predictive.h"
#include "detail/iteration_mixins.h"
#include "detail/lookup_mixins.h"
-
namespace armarx::armem::base
{
@@ -21,20 +20,19 @@ namespace armarx::armem::base
*/
template <class _EntityT, class _Derived>
class ProviderSegmentBase :
- public detail::MemoryContainerBase<std::map<std::string, _EntityT>, _Derived>
- , public detail::AronTyped
- , public detail::Predictive<_Derived>
- , public detail::ForEachEntityInstanceMixin<_Derived>
- , public detail::ForEachEntitySnapshotMixin<_Derived>
- , public detail::GetFindInstanceMixin<_Derived>
- , public detail::GetFindSnapshotMixin<_Derived>
+ public detail::MemoryContainerBase<std::map<std::string, _EntityT>, _Derived>,
+ public detail::AronTyped,
+ public detail::Predictive<_Derived>,
+ public detail::ForEachEntityInstanceMixin<_Derived>,
+ public detail::ForEachEntitySnapshotMixin<_Derived>,
+ public detail::GetFindInstanceMixin<_Derived>,
+ public detail::GetFindSnapshotMixin<_Derived>
{
using Base = detail::MemoryContainerBase<std::map<std::string, _EntityT>, _Derived>;
public:
-
- using typename Base::DerivedT;
using typename Base::ContainerT;
+ using typename Base::DerivedT;
using EntityT = _EntityT;
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
@@ -50,39 +48,37 @@ namespace armarx::armem::base
std::vector<EntitySnapshotT> removedSnapshots;
UpdateResult() = default;
+
UpdateResult(const typename EntityT::UpdateResult& c) :
- entityUpdateType(c.entityUpdateType),
- id(c.id),
- removedSnapshots(c.removedSnapshots)
- {}
+ entityUpdateType(c.entityUpdateType), id(c.id), removedSnapshots(c.removedSnapshots)
+ {
+ }
};
public:
-
ProviderSegmentBase()
{
}
- explicit ProviderSegmentBase(
- const std::string& name,
- aron::type::ObjectPtr aronType = nullptr,
- const std::vector<PredictionEngine>& predictionEngines = {}) :
+ explicit ProviderSegmentBase(const std::string& name,
+ aron::type::ObjectPtr aronType = nullptr,
+ const std::vector<PredictionEngine>& predictionEngines = {}) :
ProviderSegmentBase(name, MemoryID(), aronType, predictionEngines)
{
}
- explicit ProviderSegmentBase(
- const std::string& name,
- const MemoryID parentID,
- aron::type::ObjectPtr aronType = nullptr,
- const std::vector<PredictionEngine>& predictionEngines = {}) :
+
+ explicit ProviderSegmentBase(const std::string& name,
+ const MemoryID parentID,
+ aron::type::ObjectPtr aronType = nullptr,
+ const std::vector<PredictionEngine>& predictionEngines = {}) :
ProviderSegmentBase(parentID.withProviderSegmentName(name), aronType, predictionEngines)
{
}
- explicit ProviderSegmentBase(
- const MemoryID id,
- aron::type::ObjectPtr aronType = nullptr,
- const std::vector<PredictionEngine>& predictionEngines = {}) :
+
+ explicit ProviderSegmentBase(const MemoryID id,
+ aron::type::ObjectPtr aronType = nullptr,
+ const std::vector<PredictionEngine>& predictionEngines = {}) :
Base(id), AronTyped(aronType), detail::Predictive<_Derived>(predictionEngines)
{
}
@@ -92,71 +88,86 @@ namespace armarx::armem::base
ProviderSegmentBase& operator=(const ProviderSegmentBase& other) = default;
ProviderSegmentBase& operator=(ProviderSegmentBase&& other) = default;
-
// READ ACCESS
// Get key
- inline std::string& name()
+ inline std::string&
+ name()
{
return this->id().providerSegmentName;
}
- inline const std::string& name() const
+
+ inline const std::string&
+ name() const
{
return this->id().providerSegmentName;
}
-
// Has child by key
- bool hasEntity(const std::string& name) const
+ bool
+ hasEntity(const std::string& name) const
{
return this->findEntity(name) != nullptr;
}
+
// Has child by ID
- bool hasEntity(const MemoryID& entityID) const
+ bool
+ hasEntity(const MemoryID& entityID) const
{
return this->findEntity(entityID) != nullptr;
}
-
// Find child by key
- EntityT* findEntity(const std::string& name)
+ EntityT*
+ findEntity(const std::string& name)
{
return detail::findChildByKey(name, this->_container);
}
- const EntityT* findEntity(const std::string& name) const
+
+ const EntityT*
+ findEntity(const std::string& name) const
{
return detail::findChildByKey(name, this->_container);
}
// Get child by key
- EntityT& getEntity(const std::string& name)
+ EntityT&
+ getEntity(const std::string& name)
{
return detail::getChildByKey(name, this->_container, *this);
}
- const EntityT& getEntity(const std::string& name) const
+
+ const EntityT&
+ getEntity(const std::string& name) const
{
return detail::getChildByKey(name, this->_container, *this);
}
// Find child by MemoryID
- EntityT* findEntity(const MemoryID& entityID)
+ EntityT*
+ findEntity(const MemoryID& entityID)
{
detail::checkHasEntityName(entityID);
return this->findEntity(entityID.entityName);
}
- const EntityT* findEntity(const MemoryID& entityID) const
+
+ const EntityT*
+ findEntity(const MemoryID& entityID) const
{
detail::checkHasEntityName(entityID);
return this->findEntity(entityID.entityName);
}
// Get child by MemoryID
- EntityT& getEntity(const MemoryID& entityID)
+ EntityT&
+ getEntity(const MemoryID& entityID)
{
detail::checkHasEntityName(entityID);
return this->getEntity(entityID.entityName);
}
- const EntityT& getEntity(const MemoryID& entityID) const
+
+ const EntityT&
+ getEntity(const MemoryID& entityID) const
{
detail::checkHasEntityName(entityID);
return this->getEntity(entityID.entityName);
@@ -174,15 +185,18 @@ namespace armarx::armem::base
* @param func Function like: bool process(EntityT& entity)
*/
template <class EntityFunctionT>
- bool forEachEntity(EntityFunctionT&& func)
+ bool
+ forEachEntity(EntityFunctionT&& func)
{
return this->forEachChild(func);
}
+
/**
* @param func Function like: bool process(const EntityT& entity)
*/
template <class EntityFunctionT>
- bool forEachEntity(EntityFunctionT&& func) const
+ bool
+ forEachEntity(EntityFunctionT&& func) const
{
return this->forEachChild(func);
}
@@ -194,33 +208,39 @@ namespace armarx::armem::base
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
{
- return detail::forEachInstanceIn(
- id, func, *this,
- id.hasEntityName(),
- id.hasEntityName() ? this->findEntity(id.entityName) : nullptr);
+ return detail::forEachInstanceIn(id,
+ func,
+ *this,
+ id.hasEntityName(),
+ id.hasEntityName() ? this->findEntity(id.entityName)
+ : nullptr);
}
+
/**
* @param func Function like void process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
+ bool
+ forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
{
- return detail::forEachInstanceIn(
- id, func, *this,
- id.hasEntityName(),
- id.hasEntityName() ? this->findEntity(id.entityName) : nullptr);
+ return detail::forEachInstanceIn(id,
+ func,
+ *this,
+ id.hasEntityName(),
+ id.hasEntityName() ? this->findEntity(id.entityName)
+ : nullptr);
}
-
// Get child keys
- std::vector<std::string> getEntityNames() const
+ std::vector<std::string>
+ getEntityNames() const
{
return simox::alg::get_keys(this->_container);
}
-
// MODIFICATION
/**
@@ -228,7 +248,8 @@ namespace armarx::armem::base
*
* Missing entity entries are added before updating.
*/
- UpdateResult update(const EntityUpdate& update)
+ UpdateResult
+ update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.providerSegmentName, this->name());
@@ -253,55 +274,61 @@ namespace armarx::armem::base
return ret;
}
-
template <class OtherDerivedT>
- void append(const OtherDerivedT& other)
+ void
+ append(const OtherDerivedT& other)
{
- other.forEachEntity([this](const auto& entity)
- {
- auto it = this->_container.find(entity.name());
- if (it == this->_container.end())
+ other.forEachEntity(
+ [this](const auto& entity)
{
- it = this->_container.emplace(entity.name(), this->id().withEntityName(entity.name())).first;
- }
- it->second.append(entity);
- return true;
- });
+ auto it = this->_container.find(entity.name());
+ if (it == this->_container.end())
+ {
+ it = this->_container
+ .emplace(entity.name(), this->id().withEntityName(entity.name()))
+ .first;
+ }
+ it->second.append(entity);
+ return true;
+ });
}
-
/// Add an empty entity with the given name.
- EntityT& addEntity(const std::string& name)
+ EntityT&
+ addEntity(const std::string& name)
{
return this->_derived().addEntity(name, name);
}
/// Copy and insert an entity.
- EntityT& addEntity(const EntityT& entity)
+ EntityT&
+ addEntity(const EntityT& entity)
{
return this->_derived().addEntity(entity.name(), EntityT(entity));
}
/// Move and insert an entity.
- EntityT& addEntity(EntityT&& entity)
+ EntityT&
+ addEntity(EntityT&& entity)
{
- const std::string name = entity.name(); // Copy before move.
+ const std::string name = entity.name(); // Copy before move.
return this->_derived().addEntity(name, std::move(entity));
}
/// Insert an entity in-place.
- template <class ...Args>
- EntityT& addEntity(const std::string& name, Args... args)
+ template <class... Args>
+ EntityT&
+ addEntity(const std::string& name, Args... args)
{
ChildT& child = this->template _addChild<ChildT>(name, args...);
child.id() = this->id().withEntityName(name);
return child;
}
-
// MISC
- bool equalsDeep(const DerivedT& other) const
+ bool
+ equalsDeep(const DerivedT& other) const
{
if (this->size() != other.size())
{
@@ -322,17 +349,17 @@ namespace armarx::armem::base
return true;
}
-
- static std::string getLevelName()
+ static std::string
+ getLevelName()
{
return "provider segment";
}
- std::string getKeyString() const
+ std::string
+ getKeyString() const
{
return this->name();
}
-
};
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
index 2b00c96f14b42aa470d35e8ad9b49934bbe1d23c..07930b46e931513cd42f4a6dcdaacd5a3f80a56d 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
@@ -6,13 +6,14 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include "derived.h"
namespace
{
- template<typename F, typename Ret, typename A, typename... Rest>
+ template <typename F, typename Ret, typename A, typename... Rest>
A helper(Ret (F::*)(A, Rest...));
- template<typename F, typename Ret, typename A, typename... Rest>
+ template <typename F, typename Ret, typename A, typename... Rest>
A helper(Ret (F::*)(A, Rest...) const);
// volatile or lvalue/rvalue *this not required for lambdas (phew)
@@ -20,12 +21,12 @@ namespace
template <typename FuncT>
struct first_argument
{
- using type = decltype( helper(&FuncT::operator()) );
+ using type = decltype(helper(&FuncT::operator()));
};
template <typename FuncT>
using first_argument_t = typename first_argument<FuncT>::type;
-}
+} // namespace
namespace armarx::armem::base::detail
{
@@ -34,9 +35,10 @@ namespace armarx::armem::base::detail
// Handle functions with different return type.
template <class FunctionT, class ChildT>
- bool call(FunctionT&& func, ChildT&& child)
+ bool
+ call(FunctionT&& func, ChildT&& child)
{
- if constexpr(std::is_same_v<decltype(func(child)), bool>)
+ if constexpr (std::is_same_v<decltype(func(child)), bool>)
{
if (!func(child))
{
@@ -51,10 +53,10 @@ namespace armarx::armem::base::detail
}
}
-
// Single-valued containers.
template <class ContainerT, class FunctionT>
- bool forEachChildSingle(ContainerT& container, FunctionT&& func)
+ bool
+ forEachChildSingle(ContainerT& container, FunctionT&& func)
{
for (auto& child : container)
{
@@ -66,10 +68,10 @@ namespace armarx::armem::base::detail
return true;
}
-
// Pair-valued containers.
template <class ContainerT, class FunctionT>
- bool forEachChildPair(ContainerT& container, FunctionT&& func)
+ bool
+ forEachChildPair(ContainerT& container, FunctionT&& func)
{
for (auto& [_, child] : container)
{
@@ -81,22 +83,25 @@ namespace armarx::armem::base::detail
return true;
}
-
// see: https://en.cppreference.com/w/cpp/types/void_t
// primary template handles types that have no nested ::type member:
- template< class, class = void >
- struct has_mapped_type : std::false_type { };
+ template <class, class = void>
+ struct has_mapped_type : std::false_type
+ {
+ };
// specialization recognizes types that do have a nested ::type member:
- template< class T >
- struct has_mapped_type<T, std::void_t<typename T::mapped_type>> : std::true_type { };
-
+ template <class T>
+ struct has_mapped_type<T, std::void_t<typename T::mapped_type>> : std::true_type
+ {
+ };
template <class ContainerT, class FunctionT>
- bool forEachChild(ContainerT& container, FunctionT&& func)
+ bool
+ forEachChild(ContainerT& container, FunctionT&& func)
{
- if constexpr(has_mapped_type<ContainerT>::value)
+ if constexpr (has_mapped_type<ContainerT>::value)
{
return forEachChildPair(container, func);
}
@@ -106,20 +111,15 @@ namespace armarx::armem::base::detail
}
}
-
template <class FunctionT, class ParentT, class ChildT>
- bool forEachInstanceIn(
- const MemoryID& id,
- FunctionT&& func,
- ParentT& parent,
- bool single,
- ChildT* child
- )
+ bool
+ forEachInstanceIn(const MemoryID& id,
+ FunctionT&& func,
+ ParentT& parent,
+ bool single,
+ ChildT* child)
{
- auto childFn = [&id,&func](auto& child)
- {
- return child.forEachInstanceIn(id, func);
- };
+ auto childFn = [&id, &func](auto& child) { return child.forEachInstanceIn(id, func); };
if (single)
{
return child ? childFn(*child) : true;
@@ -130,8 +130,6 @@ namespace armarx::armem::base::detail
}
}
-
-
// We use auto instead of, e.g. DerivedT::EntitySnapshotT,
// as we cannot use the typedef before DerivedT was completely defined.
@@ -144,33 +142,29 @@ namespace armarx::armem::base::detail
* @param func Function like: bool process(EntityInstanceT& instance)>
*/
template <class InstanceFunctionT>
- bool forEachInstance(InstanceFunctionT&& func)
+ bool
+ forEachInstance(InstanceFunctionT&& func)
{
- return static_cast<DerivedT*>(this)->forEachSnapshot(
- [&func](auto & snapshot) -> bool
- {
- return snapshot.forEachInstance(func);
- });
+ return derived<DerivedT>(this).forEachSnapshot(
+ [&func](auto& snapshot) -> bool { return snapshot.forEachInstance(func); });
}
/**
* @param func Function like: bool process(const EntityInstanceT& instance)
*/
template <class InstanceFunctionT>
- bool forEachInstance(InstanceFunctionT&& func) const
+ bool
+ forEachInstance(InstanceFunctionT&& func) const
{
- return static_cast<const DerivedT*>(this)->forEachSnapshot(
- [&func](const auto & snapshot) -> bool
- {
- return snapshot.forEachInstance(func);
- });
+ return derived<DerivedT>(this).forEachSnapshot(
+ [&func](const auto& snapshot) -> bool { return snapshot.forEachInstance(func); });
}
/**
- * Call `func` on the data of each instances converted to Aron DTO class.
+ * @brief Call `func` on the data of each instance converted to Aron DTO class.
*
* @code
- * ().forEachEntityInstanceAs([](const my::arondto::CoolData& data)
+ * ().forEachEntityInstanceAs([](my::arondto::CoolData data)
* {
* ...
* });
@@ -178,21 +172,54 @@ namespace armarx::armem::base::detail
*
* The Aron DTO type is deduced from the passed function's first argument.
*
- * @param func Function like: `bool process(const my::arondto::CoolData& data)`
+ * @param func Function like: `bool process(my::arondto::CoolData data)`
*/
template <class AronDtoFunctionT>
- bool forEachInstanceAs(AronDtoFunctionT&& func) const
+ bool
+ forEachInstanceAs(AronDtoFunctionT&& func) const
{
- return static_cast<const DerivedT*>(this)->forEachInstance(
- [&func](const auto & instance) -> bool
- {
- using AronDtoT = typename std::remove_reference_t<first_argument_t<AronDtoFunctionT>>;
- return func(instance.template dataAs<AronDtoT>());
- });
+ using AronDtoT = typename std::remove_const_t<
+ std::remove_reference_t<first_argument_t<AronDtoFunctionT>>>;
+
+ return derived<DerivedT>(this).forEachInstance(
+ [&func](const auto& instance)
+ { return func(instance.template dataAs<AronDtoT>()); });
}
- };
+ /**
+ * @brief Call `func` on each instance with its data converted to Aron DTO class.
+ *
+ * @code
+ * ().forEachEntityInstanceWithDataAs(
+ * [](armem::wm::EntityInstanceBase<my::arondto::CoolData> instance)
+ * {
+ * const armarx::armem::MemoryID id = instance.id();
+ * const armarx::DateTime timestamp = instance.id().timestamp;
+ * const armarx::arondto::Duration& data = instance.data();
+ * ...
+ * });
+ * @endcode
+ *
+ * Compared to forEachInstanceAs(), this function allows accessing the full metadata of the
+ * instances in addition to the payload data converted to the ARON DTO.
+ *
+ * The Aron DTO type is deduced from the passed function's signature.
+ *
+ * @param func Function like:
+ * `bool process(armem::wm::EntityInstanceBase<my::arondto::CoolData> instance)`
+ */
+ template <class EntityInstanceBaseAronDtoFunctionT>
+ bool
+ forEachInstanceWithDataAs(EntityInstanceBaseAronDtoFunctionT&& func) const
+ {
+ using AronDtoT = typename std::remove_reference_t<
+ first_argument_t<EntityInstanceBaseAronDtoFunctionT>>::DataT;
+ return derived<DerivedT>(this).forEachInstance(
+ [&func](const auto& instance)
+ { return func(instance.template withDataAs<AronDtoT>()); });
+ }
+ };
template <class DerivedT>
struct ForEachEntitySnapshotMixin
@@ -201,30 +228,25 @@ namespace armarx::armem::base::detail
* @param func Function like: bool process(EntitySnapshotT& snapshot)>
*/
template <class SnapshotFunctionT>
- bool forEachSnapshot(SnapshotFunctionT&& func)
+ bool
+ forEachSnapshot(SnapshotFunctionT&& func)
{
- return static_cast<DerivedT*>(this)->forEachEntity(
- [&func](auto & entity) -> bool
- {
- return entity.forEachSnapshot(func);
- });
+ return derived<DerivedT>(this).forEachEntity([&func](auto& entity) -> bool
+ { return entity.forEachSnapshot(func); });
}
/**
* @param func Function like: bool process(const EntitySnapshotT& snapshot)
*/
template <class SnapshotFunctionT>
- bool forEachSnapshot(SnapshotFunctionT&& func) const
+ bool
+ forEachSnapshot(SnapshotFunctionT&& func) const
{
- return static_cast<const DerivedT*>(this)->forEachEntity(
- [&func](const auto & entity) -> bool
- {
- return entity.forEachSnapshot(func);
- });
+ return derived<DerivedT>(this).forEachEntity([&func](const auto& entity) -> bool
+ { return entity.forEachSnapshot(func); });
}
};
-
template <class DerivedT>
struct ForEachEntityMixin
{
@@ -232,30 +254,27 @@ namespace armarx::armem::base::detail
* @param func Function like: bool process(EntityT& entity)>
*/
template <class FunctionT>
- bool forEachEntity(FunctionT&& func)
+ bool
+ forEachEntity(FunctionT&& func)
{
- return static_cast<DerivedT*>(this)->forEachProviderSegment(
- [&func](auto & providerSegment) -> bool
- {
- return providerSegment.forEachEntity(func);
- });
+ return derived<DerivedT>(this).forEachProviderSegment(
+ [&func](auto& providerSegment) -> bool
+ { return providerSegment.forEachEntity(func); });
}
/**
* @param func Function like: bool process(const EntityT& entity)
*/
template <class FunctionT>
- bool forEachEntity(FunctionT&& func) const
+ bool
+ forEachEntity(FunctionT&& func) const
{
- return static_cast<const DerivedT*>(this)->forEachProviderSegment(
- [&func](const auto & providerSegment) -> bool
- {
- return providerSegment.forEachEntity(func);
- });
+ return derived<DerivedT>(this).forEachProviderSegment(
+ [&func](const auto& providerSegment) -> bool
+ { return providerSegment.forEachEntity(func); });
}
};
-
template <class DerivedT>
struct ForEachProviderSegmentMixin
{
@@ -263,27 +282,25 @@ namespace armarx::armem::base::detail
* @param func Function like: bool process(ProviderSegmentT& providerSegment)>
*/
template <class FunctionT>
- bool forEachProviderSegment(FunctionT&& func)
+ bool
+ forEachProviderSegment(FunctionT&& func)
{
- return static_cast<DerivedT*>(this)->forEachCoreSegment(
- [&func](auto & coreSegment) -> bool
- {
- return coreSegment.forEachProviderSegment(func);
- });
+ return derived<DerivedT>(this).forEachCoreSegment(
+ [&func](auto& coreSegment) -> bool
+ { return coreSegment.forEachProviderSegment(func); });
}
/**
* @param func Function like: bool process(const ProviderSegmentT& providerSegment)
*/
template <class FunctionT>
- bool forEachProviderSegment(FunctionT&& func) const
+ bool
+ forEachProviderSegment(FunctionT&& func) const
{
- return static_cast<const DerivedT*>(this)->forEachCoreSegment(
- [&func](const auto & coreSegment) -> bool
- {
- return coreSegment.forEachProviderSegment(func);
- });
+ return derived<DerivedT>(this).forEachCoreSegment(
+ [&func](const auto& coreSegment) -> bool
+ { return coreSegment.forEachProviderSegment(func); });
}
};
-}
+} // namespace armarx::armem::base::detail
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h b/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h
index ce63b1ee78d4dbc97fe99b9efddd783dc6e5e315..204b1644d15217b028592c2bebb94cb874993ccf 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h
@@ -8,13 +8,25 @@
namespace armarx::armem::base::detail
{
+ /// Throw armem::error::InvalidMemoryID if the given ID has no instance index.
void checkHasInstanceIndex(const MemoryID& instanceID);
+ /// Throw armem::error::InvalidMemoryID if the given ID has no timestamp.
void checkHasTimestamp(const MemoryID& snapshotID);
+ /// Throw armem::error::InvalidMemoryID if the given ID has no entity name.
void checkHasEntityName(const MemoryID& entityID);
+ /// Throw armem::error::InvalidMemoryID if the given ID has provider segment name.
void checkHasProviderSegmentName(const MemoryID& providerSegmentID);
+ /// Throw armem::error::InvalidMemoryID if the given ID has core segment name.
void checkHasCoreSegmentName(const MemoryID& coreSegmentID);
+ /// Throw armem::error::InvalidMemoryID if the given ID has memory name.
void checkHasMemoryName(const MemoryID& memory);
+ /**
+ * @brief Find a child in a container by its key.
+ * @param key The child's key.
+ * @param container The child's container.
+ * @return A pointer to the child, or nullptr if it was not found.
+ */
template <class KeyT, class ContainerT>
auto*
findChildByKey(const KeyT& key, ContainerT&& container)
@@ -23,11 +35,20 @@ namespace armarx::armem::base::detail
return it != container.end() ? &it->second : nullptr;
}
+ /**
+ * @brief Retrieve a child in a container by its key.
+ * @param key The child's key.
+ * @param container The container.
+ * @param parent The container's owner. Used for the error message.
+ * @param keyStringFn A function which turns key into a string. Used for the error message.
+ * @return A reference to the child.
+ * @throw armem::error::ArMemError If the child was not found.
+ */
template <class KeyT, class ContainerT, class ParentT, class KeyStringFn>
auto&
getChildByKey(const KeyT& key,
ContainerT&& container,
- const ParentT& parent,
+ const ParentT& owner,
KeyStringFn&& keyStringFn)
{
if (auto* child = findChildByKey(key, container))
@@ -37,10 +58,18 @@ namespace armarx::armem::base::detail
else
{
throw armem::error::MissingEntry::create<typename ParentT::ChildT>(keyStringFn(key),
- parent);
+ owner);
}
}
+ /**
+ * @brief Retrieve a child in a container by its key.
+ * @param key The child's key.
+ * @param container The container.
+ * @param parent The container's owner. Used for the error message.
+ * @return A reference to the child.
+ * @throw armem::error::ArMemError If the child was not found.
+ */
template <class KeyT, class ContainerT, class ParentT>
auto&
getChildByKey(const KeyT& key, ContainerT&& container, const ParentT& parent)
@@ -51,8 +80,30 @@ namespace armarx::armem::base::detail
template <class DerivedT>
struct GetFindInstanceMixin
{
- // Relies on this->find/getSnapshot()
+ // Relies on this->find/get/forEachInstance()
+ /**
+ * @brief Indicate whether this container contains at least one entity instance.
+ * @return True if there is at least one entity instance.
+ */
+ bool
+ hasInstances() const
+ {
+ bool has = false;
+ derived<DerivedT>(this).forEachInstance(
+ [&has](const auto& snapshot)
+ {
+ has = true;
+ return false;
+ });
+ return has;
+ }
+
+ /**
+ * @brief Indicate whether this container has an instance with the given ID.
+ * @param instanceID The instance ID.
+ * @return ... WIP
+ */
bool
hasInstance(const MemoryID& instanceID) const
{
@@ -82,7 +133,7 @@ namespace armarx::armem::base::detail
* @brief Retrieve an entity instance.
* @param id The instance ID.
* @return The instance if it is found.
- * @throw `armem::error::ArMemError` if it is missing.
+ * @throw armem::error::ArMemError if it is missing.
*/
auto&
getInstance(const MemoryID& instanceID)
@@ -98,20 +149,118 @@ namespace armarx::armem::base::detail
};
template <class DerivedT>
- struct GetFindSnapshotMixin
+ struct GetLatestInstanceMixin
{
- // Relies on this->find/getEntity()
+ // Relies on findLatestInstance()
+
+ /**
+ * @brief Retrieve the latest entity instance.
+ * @param instanceIndex The instance's index in the latest snapshot.
+ * @return The latest entity instance.
+ * @throw armem::error::ArMemError If there is no entity instance.
+ */
+ auto&
+ getLatestInstance(int instanceIndex = 0)
+ {
+ auto* instance = derived<DerivedT>(this).findLatestInstance();
+ if (not instance)
+ {
+ throw armem::error::NoSuchEntries(
+ "entity instances", DerivedT::getLevelName(), derived<DerivedT>(this).id());
+ }
+ return *instance;
+ }
+
+ const auto&
+ getLatestInstance(int instanceIndex = 0) const
+ {
+ const auto* instance = derived<DerivedT>(this).findLatestInstance();
+ if (not instance)
+ {
+ throw armem::error::NoSuchEntries(
+ "entity instances", DerivedT::getLevelName(), derived<DerivedT>(this).id());
+ }
+ return *instance;
+ }
+ };
+
+ template <class DerivedT>
+ struct GetLatestSnapshotMixin
+ {
+ // Relies on findLatestSnapshot()
+
+ /**
+ * @brief Retrieve the latest entity snapshot.
+ * @param snapshotIndex The snapshot's index in the latest snapshot.
+ * @return The latest entity snapshot.
+ * @throw armem::error::ArMemError If there is no entity snapshot.
+ */
+ auto&
+ getLatestSnapshot(int snapshotIndex = 0)
+ {
+ auto* snapshot = derived<DerivedT>(this).findLatestSnapshot();
+ if (not snapshot)
+ {
+ throw armem::error::NoSuchEntries(
+ "entity snapshots", DerivedT::getLevelName(), derived<DerivedT>(this).id());
+ }
+ return *snapshot;
+ }
+
+ const auto&
+ getLatestSnapshot(int snapshotIndex = 0) const
+ {
+ const auto* snapshot = derived<DerivedT>(this).findLatestSnapshot();
+ if (not snapshot)
+ {
+ throw armem::error::NoSuchEntries(
+ "entity snapshots", DerivedT::getLevelName(), derived<DerivedT>(this).id());
+ }
+ return *snapshot;
+ }
+ };
+
+ template <class DerivedT>
+ struct GetFindSnapshotMixin :
+ public GetLatestInstanceMixin<DerivedT>,
+ public GetLatestSnapshotMixin<DerivedT>
+ {
+ // Relies on this->find/getEntity, forEachSnapshot()
+
+ /**
+ * @brief Indicate whether this container contains at least one entity snapshot.
+ * @return True if there is at least one entity snapshot.
+ */
+ bool
+ hasSnapshots() const
+ {
+ bool has = false;
+ derived<DerivedT>(this).forEachSnapshot(
+ [&has](const auto& snapshot)
+ {
+ has = true;
+ return false;
+ });
+ return has;
+ }
+ /**
+ * @brief Indicates whether a snapshot with the given ID exists.
+ * @param snapshotID The snapshot ID.
+ * @return True if the snapshot exists, false otherwise.
+ */
bool
hasSnapshot(const MemoryID& snapshotID) const
{
return derived<DerivedT>(this).findSnapshot(snapshotID) != nullptr;
}
+ // Snapshot by ID
+
/**
* @brief Find an entity snapshot.
* @param id The snapshot ID.
- * @return The snapshot or nullptr if it is missing.
+ * @return The snapshot, or nullptr if it is missing.
*/
auto*
findSnapshot(const MemoryID& snapshotID)
@@ -131,7 +280,7 @@ namespace armarx::armem::base::detail
* @brief Retrieve an entity snapshot.
* @param id The snapshot ID.
* @return The snapshot if it is found.
- * @throw `armem::error::ArMemError` if it is missing.
+ * @throw armem::error::ArMemError if it is missing.
*/
auto&
getSnapshot(const MemoryID& snapshotID)
@@ -145,8 +294,60 @@ namespace armarx::armem::base::detail
return derived<DerivedT>(this).getEntity(snapshotID).getSnapshot(snapshotID);
}
- // More elaborate cases
+ // Latest snapshot in container.
+
+ /**
+ * @brief Find the latest entity snapshot.
+ * @return A pointer to the latest instance, or nullptr if there is no snapshot.
+ */
+ const auto*
+ findLatestSnapshot() const
+ {
+ const typename DerivedT::EntitySnapshotT* latestSnapshot = nullptr;
+ derived<DerivedT>(this).forEachEntity(
+ [&latestSnapshot](const auto& entity)
+ {
+ const auto* snapshot = entity.findLatestSnapshot();
+ if (latestSnapshot == nullptr)
+ {
+ latestSnapshot = snapshot;
+ }
+ else if (snapshot and snapshot->time() > latestSnapshot->time())
+ {
+ latestSnapshot = snapshot;
+ }
+ });
+ return latestSnapshot;
+ }
+ auto*
+ findLatestSnapshot()
+ {
+ typename DerivedT::EntitySnapshotT* latestSnapshot = nullptr;
+ derived<DerivedT>(this).forEachEntity(
+ [&latestSnapshot](auto& entity)
+ {
+ auto* snapshot = entity.findLatestSnapshot();
+ if (latestSnapshot == nullptr)
+ {
+ latestSnapshot = snapshot;
+ }
+ else if (snapshot and snapshot->time() > latestSnapshot->time())
+ {
+ latestSnapshot = snapshot;
+ }
+ });
+ return latestSnapshot;
+ }
+
+ // Latest snapshot in entity
+
+ /**
+ * @brief Find the latest entity snapshot in the given entity.
+ * @param entityID The entity's ID.
+ * @return A pointer to the latest snapshot in the specified entity, or nullptr if the
+ * entity does not exist or it has no snapshot.
+ */
auto*
findLatestSnapshot(const MemoryID& entityID)
{
@@ -161,6 +362,34 @@ namespace armarx::armem::base::detail
return entity ? entity->findLatestSnapshot() : nullptr;
}
+ // Latest instance in container.
+
+ /**
+ * @brief Find the latest entity instance.
+ * @return A pointer to the latest instance, or nullptr if there is no instance.
+ */
+ const auto*
+ findLatestInstance(int instanceIndex = 0) const
+ {
+ auto* snapshot = derived<DerivedT>(this).findLatestSnapshot();
+ return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
+ }
+
+ auto*
+ findLatestInstance(int instanceIndex = 0)
+ {
+ auto* snapshot = derived<DerivedT>(this).findLatestSnapshot();
+ return snapshot ? snapshot->findInstance(instanceIndex) : nullptr;
+ }
+
+ // Latest instance in entity.
+
+ /**
+ * @brief Find the latest entity instance in the given entity.
+ * @param entityID The entity's ID.
+ * @return A pointer to the latest instance in the specified entity, or nullptr if the
+ * entity does not exist or it has no instance.
+ */
auto*
findLatestInstance(const MemoryID& entityID, int instanceIndex = 0)
{
@@ -210,7 +439,7 @@ namespace armarx::armem::base::detail
* @brief Retrieve an entity.
* @param id The entity ID.
* @return The entity if it is found.
- * @throw `armem::error::ArMemError` if it is missing.
+ * @throw armem::error::ArMemError if it is missing.
*/
auto&
getEntity(const MemoryID& entityID)
@@ -259,7 +488,7 @@ namespace armarx::armem::base::detail
* @brief Retrieve a provider segment.
* @param id The provider segment ID.
* @return The provider segment if it is found.
- * @throw `armem::error::ArMemError` if it is missing.
+ * @throw armem::error::ArMemError if it is missing.
*/
auto&
getProviderSegment(const MemoryID& providerSegmentID)
diff --git a/source/RobotAPI/libraries/armem/core/base/ice_conversions.h b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
index e808e01ace5165eb3fb38109cc850138e20061df..344073dc9361546d47e815638d054ea632ac6100 100644
--- a/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.h
@@ -1,21 +1,19 @@
#pragma once
-#include "EntityInstanceBase.h"
-#include "EntitySnapshotBase.h"
-#include "EntityBase.h"
-#include "ProviderSegmentBase.h"
-#include "CoreSegmentBase.h"
-#include "MemoryBase.h"
-
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
-
-#include <RobotAPI/interface/armem/memory.h>
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/ice_conversions.h>
+#include <RobotAPI/interface/armem/memory.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
+
+#include "CoreSegmentBase.h"
+#include "EntityBase.h"
+#include "EntityInstanceBase.h"
+#include "EntitySnapshotBase.h"
+#include "MemoryBase.h"
+#include "ProviderSegmentBase.h"
namespace armarx::armem::base::detail
{
@@ -27,7 +25,8 @@ namespace armarx::armem::base::detail
void toIce(aron::type::dto::GenericTypePtr& ice, const aron::type::ObjectPtr& bo);
void fromIce(const aron::type::dto::GenericTypePtr& ice, aron::type::ObjectPtr& bo);
-}
+} // namespace armarx::armem::base::detail
+
namespace armarx::armem::base
{
void toIce(data::EntityInstanceMetadata& ice, const EntityInstanceMetadata& metadata);
@@ -36,9 +35,9 @@ namespace armarx::armem::base
void toIce(data::EntityInstanceMetadataPtr& ice, const EntityInstanceMetadata& metadata);
void fromIce(const data::EntityInstanceMetadataPtr& ice, EntityInstanceMetadata& metadata);
-
- template <class ...Args>
- void toIce(data::EntityInstance& ice, const EntityInstanceBase<Args...>& data)
+ template <class... Args>
+ void
+ toIce(data::EntityInstance& ice, const EntityInstanceBase<Args...>& data)
{
detail::toIceItem(ice, data);
@@ -46,8 +45,10 @@ namespace armarx::armem::base
detail::toIce(ice.data, data.data());
toIce(ice.metadata, data.metadata());
}
- template <class ...Args>
- void fromIce(const data::EntityInstance& ice, EntityInstanceBase<Args...>& data)
+
+ template <class... Args>
+ void
+ fromIce(const data::EntityInstance& ice, EntityInstanceBase<Args...>& data)
{
detail::fromIceItem(ice, data);
@@ -55,55 +56,61 @@ namespace armarx::armem::base
fromIce(ice.metadata, data.metadata());
}
- template <class ...Args>
- void toIce(data::EntitySnapshot& ice, const EntitySnapshotBase<Args...>& snapshot)
+ template <class... Args>
+ void
+ toIce(data::EntitySnapshot& ice, const EntitySnapshotBase<Args...>& snapshot)
{
detail::toIceItem(ice, snapshot);
ice.instances.clear();
- snapshot.forEachInstance([&ice](const auto & instance)
- {
- armarx::toIce(ice.instances.emplace_back(), instance);
- });
+ snapshot.forEachInstance([&ice](const auto& instance)
+ { armarx::toIce(ice.instances.emplace_back(), instance); });
}
- template <class ...Args>
- void fromIce(const data::EntitySnapshot& ice, EntitySnapshotBase<Args...>& snapshot)
+
+ template <class... Args>
+ void
+ fromIce(const data::EntitySnapshot& ice, EntitySnapshotBase<Args...>& snapshot)
{
detail::fromIceItem(ice, snapshot);
snapshot.clear();
for (const data::EntityInstancePtr& iceInstance : ice.instances)
{
- snapshot.addInstance(armarx::fromIce<typename EntitySnapshotBase<Args...>::EntityInstanceT>(iceInstance));
+ snapshot.addInstance(
+ armarx::fromIce<typename EntitySnapshotBase<Args...>::EntityInstanceT>(
+ iceInstance));
}
}
- template <class ...Args>
- void toIce(data::Entity& ice, const EntityBase<Args...>& entity)
+ template <class... Args>
+ void
+ toIce(data::Entity& ice, const EntityBase<Args...>& entity)
{
detail::toIceItem(ice, entity);
ice.history.clear();
- entity.forEachSnapshot([&ice](const auto & snapshot)
- {
- armarx::toIce(ice.history[armarx::toIce<dto::Time>(snapshot.time())], snapshot);
- });
+ entity.forEachSnapshot(
+ [&ice](const auto& snapshot)
+ { armarx::toIce(ice.history[armarx::toIce<dto::Time>(snapshot.time())], snapshot); });
}
- template <class ...Args>
- void fromIce(const data::Entity& ice, EntityBase<Args...>& entity)
+
+ template <class... Args>
+ void
+ fromIce(const data::Entity& ice, EntityBase<Args...>& entity)
{
detail::fromIceItem(ice, entity);
entity.clear();
for (const auto& [key, snapshot] : ice.history)
{
- entity.addSnapshot(armarx::fromIce<typename EntityBase<Args...>::EntitySnapshotT>(snapshot));
+ entity.addSnapshot(
+ armarx::fromIce<typename EntityBase<Args...>::EntitySnapshotT>(snapshot));
}
}
-
- template <class ...Args>
- void toIce(data::ProviderSegment& ice, const ProviderSegmentBase<Args...>& providerSegment)
+ template <class... Args>
+ void
+ toIce(data::ProviderSegment& ice, const ProviderSegmentBase<Args...>& providerSegment)
{
detail::toIceItem(ice, providerSegment);
@@ -111,13 +118,13 @@ namespace armarx::armem::base
ARMARX_CHECK(!providerSegment.aronType() || ice.aronType);
ice.entities.clear();
- providerSegment.forEachEntity([&ice](const auto & entity)
- {
- armarx::toIce(ice.entities[entity.name()], entity);
- });
+ providerSegment.forEachEntity([&ice](const auto& entity)
+ { armarx::toIce(ice.entities[entity.name()], entity); });
}
- template <class ...Args>
- void fromIce(const data::ProviderSegment& ice, ProviderSegmentBase<Args...>& providerSegment)
+
+ template <class... Args>
+ void
+ fromIce(const data::ProviderSegment& ice, ProviderSegmentBase<Args...>& providerSegment)
{
detail::fromIceItem(ice, providerSegment);
@@ -132,8 +139,9 @@ namespace armarx::armem::base
}
}
- template <class ...Args>
- void toIce(data::CoreSegment& ice, const CoreSegmentBase<Args...>& coreSegment)
+ template <class... Args>
+ void
+ toIce(data::CoreSegment& ice, const CoreSegmentBase<Args...>& coreSegment)
{
detail::toIceItem(ice, coreSegment);
@@ -141,13 +149,14 @@ namespace armarx::armem::base
ARMARX_CHECK(!coreSegment.aronType() || ice.aronType);
ice.providerSegments.clear();
- coreSegment.forEachProviderSegment([&ice](const auto & providerSegment)
- {
- armarx::toIce(ice.providerSegments[providerSegment.name()], providerSegment);
- });
+ coreSegment.forEachProviderSegment(
+ [&ice](const auto& providerSegment)
+ { armarx::toIce(ice.providerSegments[providerSegment.name()], providerSegment); });
}
- template <class ...Args>
- void fromIce(const data::CoreSegment& ice, CoreSegmentBase<Args...>& coreSegment)
+
+ template <class... Args>
+ void
+ fromIce(const data::CoreSegment& ice, CoreSegmentBase<Args...>& coreSegment)
{
detail::fromIceItem(ice, coreSegment);
@@ -159,23 +168,26 @@ namespace armarx::armem::base
for (const auto& [key, providerSegment] : ice.providerSegments)
{
coreSegment.addProviderSegment(
- armarx::fromIce<typename CoreSegmentBase<Args...>::ProviderSegmentT>(providerSegment));
+ armarx::fromIce<typename CoreSegmentBase<Args...>::ProviderSegmentT>(
+ providerSegment));
}
}
- template <class ...Args>
- void toIce(data::Memory& ice, const MemoryBase<Args...>& memory)
+ template <class... Args>
+ void
+ toIce(data::Memory& ice, const MemoryBase<Args...>& memory)
{
base::detail::toIceItem(ice, memory);
ice.coreSegments.clear();
- memory.forEachCoreSegment([&ice](const auto & coreSegment)
- {
- armarx::toIce(ice.coreSegments[coreSegment.name()], coreSegment);
- });
+ memory.forEachCoreSegment(
+ [&ice](const auto& coreSegment)
+ { armarx::toIce(ice.coreSegments[coreSegment.name()], coreSegment); });
}
- template <class ...Args>
- void fromIce(const data::Memory& ice, MemoryBase<Args...>& memory)
+
+ template <class... Args>
+ void
+ fromIce(const data::Memory& ice, MemoryBase<Args...>& memory)
{
base::detail::fromIceItem(ice, memory);
@@ -187,4 +199,4 @@ namespace armarx::armem::base
}
}
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index 87b687447b7277cbd3861d7c29e3df566e3e26b3..354b95712761f6a14d3fb6deb5baeff6b2a18aba 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -6,7 +6,6 @@
#include "../MemoryID.h"
-
namespace armarx::armem::error
{
@@ -15,7 +14,6 @@ namespace armarx::armem::error
{
}
-
InvalidArgument::InvalidArgument(const std::string& argument,
const std::string& function,
const std::string& message) :
@@ -41,7 +39,6 @@ namespace armarx::armem::error
return ss.str();
}
-
ContainerNameMismatch::ContainerNameMismatch(const std::string& gottenName,
const std::string& ownTerm,
const std::string& containerName) :
@@ -60,7 +57,6 @@ namespace armarx::armem::error
return ss.str();
}
-
ContainerEntryAlreadyExists::ContainerEntryAlreadyExists(const std::string& existingTerm,
const std::string& existingName,
const std::string& ownTerm,
@@ -81,7 +77,6 @@ namespace armarx::armem::error
return ss.str();
}
-
MissingEntry::MissingEntry(const std::string& missingTerm,
const std::string& missingName,
const std::string& containerTerm,
@@ -104,6 +99,24 @@ namespace armarx::armem::error
return ss.str();
}
+ NoSuchEntries::NoSuchEntries(const std::string& missingTerm,
+ const std::string& containerTerm,
+ const MemoryID& containerID,
+ const std::string& message) :
+ ArMemError(makeMsg(missingTerm, containerTerm, containerID, message))
+ {
+ }
+
+ std::string
+ NoSuchEntries::makeMsg(const std::string& missingTerm,
+ const std::string& containerTerm,
+ const MemoryID& containerID,
+ const std::string& message)
+ {
+ std::stringstream ss;
+ ss << "No " << missingTerm << " in " << containerTerm << " " << containerID << ".";
+ return ss.str();
+ }
MissingData::MissingData(const std::string& missingTerm,
const std::string& missingName,
@@ -125,7 +138,6 @@ namespace armarx::armem::error
return ss.str();
}
-
ParseIntegerError::ParseIntegerError(std::string string, std::string semanticName) :
ArMemError(makeMsg(string, semanticName))
{
@@ -139,7 +151,6 @@ namespace armarx::armem::error
return ss.str();
}
-
InvalidMemoryID::InvalidMemoryID(const MemoryID& id, const std::string& message) :
ArMemError(makeMsg(id, message))
{
@@ -153,7 +164,6 @@ namespace armarx::armem::error
return ss.str();
}
-
EntityHistoryEmpty::EntityHistoryEmpty(const std::string& entityName,
const std::string& message) :
ArMemError(makeMsg(entityName, message))
@@ -176,7 +186,6 @@ namespace armarx::armem::error
return ss.str();
}
-
UnknownQueryType::UnknownQueryType(const std::string& term, const std::string& typeName) :
ArMemError(makeMsg(term, typeName))
{
@@ -195,7 +204,8 @@ namespace armarx::armem::error
{
}
- std::string QueryFailed::makeMsg(const std::string& memory, const std::string& message)
+ std::string
+ QueryFailed::makeMsg(const std::string& memory, const std::string& message)
{
std::stringstream ss;
ss << "Query from memory " << memory << " failed with message: " << message;
@@ -207,7 +217,6 @@ namespace armarx::armem::error
{
}
-
std::string
IOError::makeMsg(const std::string& path, const std::string& message)
{
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 6f5be02a243e9e953d56b67f5f1726be214b2745..69ab7d4ddff8e22791272c520386e2fdff3d6e6f 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -4,7 +4,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::armem
{
class MemoryID;
@@ -19,85 +18,103 @@ namespace armarx::armem::error
class ArMemError : public std::runtime_error
{
public:
-
ArMemError(const std::string& msg);
-
};
-
/**
* @brief Indicates that an argument was invalid.
*/
class InvalidArgument : public ArMemError
{
public:
-
- InvalidArgument(const std::string& argument, const std::string& function,
+ InvalidArgument(const std::string& argument,
+ const std::string& function,
const std::string& message);
- static std::string makeMsg(const std::string& argument, const std::string& function,
+ static std::string makeMsg(const std::string& argument,
+ const std::string& function,
const std::string& message);
-
};
-
-
/**
* @brief Indicates that a name in a given ID does not match a container's own name.
*/
class ContainerNameMismatch : public ArMemError
{
public:
-
ContainerNameMismatch(const std::string& gottenName,
- const std::string& containerTerm, const std::string& containerName);
+ const std::string& containerTerm,
+ const std::string& containerName);
static std::string makeMsg(const std::string& gottenName,
- const std::string& containerTerm, const std::string& containerName);
-
+ const std::string& containerTerm,
+ const std::string& containerName);
};
-
/**
* @brief Indicates that a name in a given ID does not match a container's own name.
*/
class ContainerEntryAlreadyExists : public ArMemError
{
public:
-
- ContainerEntryAlreadyExists(const std::string& existingTerm, const std::string& existingName,
- const std::string& ownTerm, const std::string& ownName);
-
- static std::string makeMsg(const std::string& existingTerm, const std::string& existingName,
- const std::string& ownTerm, const std::string& ownName);
-
+ ContainerEntryAlreadyExists(const std::string& existingTerm,
+ const std::string& existingName,
+ const std::string& ownTerm,
+ const std::string& ownName);
+
+ static std::string makeMsg(const std::string& existingTerm,
+ const std::string& existingName,
+ const std::string& ownTerm,
+ const std::string& ownName);
};
-
/**
* @brief Indicates that a container did not have an entry under a given name.
*/
class MissingEntry : public ArMemError
{
public:
-
template <class MissingT, class ContainerT>
- static MissingEntry create(const std::string& missingKey, const ContainerT& container)
+ static MissingEntry
+ create(const std::string& missingKey, const ContainerT& container)
{
- return MissingEntry(MissingT::getLevelName(), missingKey,
- ContainerT::getLevelName(), container.getKeyString(), container.size());
+ return MissingEntry(MissingT::getLevelName(),
+ missingKey,
+ ContainerT::getLevelName(),
+ container.getKeyString(),
+ container.size());
}
-
- MissingEntry(const std::string& missingTerm, const std::string& missingName,
- const std::string& containerTerm, const std::string& containerName,
+ MissingEntry(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& containerTerm,
+ const std::string& containerName,
size_t containerSize);
- static std::string makeMsg(const std::string& missingTerm, const std::string& missingName,
- const std::string& containerTerm, const std::string& containerName,
+ static std::string makeMsg(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& containerTerm,
+ const std::string& containerName,
size_t size);
};
+ /**
+ * @brief Indicates that an operation requiring at least one element to exist
+ * failed because there were no such entries.
+ */
+ class NoSuchEntries : public ArMemError
+ {
+ public:
+ NoSuchEntries(const std::string& missingTerm,
+ const std::string& containerTerm,
+ const MemoryID& containerID,
+ const std::string& message = "");
+
+ static std::string makeMsg(const std::string& missingTerm,
+ const std::string& containerTerm,
+ const MemoryID& containerID,
+ const std::string& message = "");
+ };
/**
* @brief Indicates that a container did have an entry, but the entry's data was
@@ -106,14 +123,17 @@ namespace armarx::armem::error
class MissingData : public ArMemError
{
public:
- MissingData(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName);
-
- static std::string makeMsg(const std::string& missingTerm, const std::string& missingName,
- const std::string& ownTerm, const std::string& ownName);
+ MissingData(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& ownTerm,
+ const std::string& ownName);
+
+ static std::string makeMsg(const std::string& missingTerm,
+ const std::string& missingName,
+ const std::string& ownTerm,
+ const std::string& ownName);
};
-
/**
* @brief Indicates that a string could not be parsed as integer.
*/
@@ -125,52 +145,43 @@ namespace armarx::armem::error
static std::string makeMsg(std::string string, std::string semanticName);
};
-
-
/**
* @brief Indicates that a memory ID is invalid, e.g. does not contain necessary information.
*/
class InvalidMemoryID : public ArMemError
{
public:
-
InvalidMemoryID(const MemoryID& id, const std::string& message);
static std::string makeMsg(const MemoryID& id, const std::string& message);
-
};
-
/**
* @brief Indicates that an entity's history was queried, but is empty.
*/
class EntityHistoryEmpty : public ArMemError
{
public:
-
EntityHistoryEmpty(const std::string& entityName, const std::string& message = "");
static std::string makeMsg(const std::string& entityName, const std::string& message = "");
-
};
-
/**
* @brief Indicates that an entity's history was queried, but is empty.
*/
class UnknownQueryType : public ArMemError
{
public:
-
template <class QueryType>
UnknownQueryType(const std::string& term, const QueryType& query) :
UnknownQueryType(term, simox::meta::get_type_name(query))
{
}
+
UnknownQueryType(const std::string& term, const std::string& typeName);
static std::string makeMsg(const std::string& term, const std::string& typeName);
-
};
/**
@@ -179,28 +190,23 @@ namespace armarx::armem::error
class QueryFailed : public ArMemError
{
public:
-
QueryFailed(const std::string& memory, const std::string& message = "");
static std::string makeMsg(const std::string& memory, const std::string& message = "");
-
};
-
/**
* @brief Indicates that something went wrong when accessing the filesystem.
*/
class IOError : public ArMemError
{
public:
-
IOError(const std::string& path, const std::string& message = "");
static std::string makeMsg(const std::string& path, const std::string& message = "");
std::string path;
-
};
/**
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
index bfc408ac99a98a5bf1826f37334e1c331196ed1b..1da7db6bc21c98a33f93c2c52f5f38da4a505a88 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.cpp
@@ -1,25 +1,23 @@
#include "memory_definitions.h"
-#include "error.h"
-
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include <map>
#include <vector>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include "error.h"
namespace armarx::armem::wm
{
- bool EntityInstance::equalsDeep(const EntityInstance& other) const
+ bool
+ EntityInstance::equalsDeep(const EntityInstance& other) const
{
if (_data and other.data())
{
- return id() == other.id()
- && _metadata == other.metadata()
- && *_data == *other.data();
+ return id() == other.id() && _metadata == other.metadata() && *_data == *other.data();
}
if (_data or other.data())
{
@@ -28,8 +26,8 @@ namespace armarx::armem::wm
return id() == other.id() && _metadata == other.metadata();
}
-
- void EntityInstance::update(const EntityUpdate& update)
+ void
+ EntityInstance::update(const EntityUpdate& update)
{
ARMARX_CHECK_FITS_SIZE(this->index(), update.instancesData.size());
@@ -42,4 +40,4 @@ namespace armarx::armem::wm
this->_metadata.arrivedTime = update.arrivedTime;
}
-}
+} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
index 642cc50eda5e34a90e482c7bc5b26d48e3847081..dcc82878286cf3387e0381ca21312f76bff89bcb 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_definitions.h
@@ -1,35 +1,42 @@
#pragma once
-#include "detail/data_lookup_mixins.h"
-
+#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
#include <RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h>
-#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
-#include <RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h>
-#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
-
+#include <RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h>
#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
+#include "detail/data_lookup_mixins.h"
namespace armarx::armem::wm
{
+ /**
+ * @brief Client-side working memory entity instance metadata.
+ */
using EntityInstanceMetadata = base::EntityInstanceMetadata;
+ /**
+ * @brief Client-side working memory entity instance data (payload).
+ */
using EntityInstanceData = armarx::aron::data::Dict;
+ /**
+ * @brief Pointer type of EntityInstanceData.
+ */
using EntityInstanceDataPtr = armarx::aron::data::DictPtr;
-
- /// @see base::EntityInstanceBase
+ /**
+ * @brief Client-side working entity instance.
+ * @see base::EntityInstanceBase
+ */
class EntityInstance :
public base::EntityInstanceBase<EntityInstanceDataPtr, EntityInstanceMetadata>
{
using Base = base::EntityInstanceBase<EntityInstanceDataPtr, EntityInstanceMetadata>;
public:
-
using Base::EntityInstanceBase;
-
/**
* @brief Fill `*this` with the update's values.
* @param update The update.
@@ -38,78 +45,99 @@ namespace armarx::armem::wm
void update(const EntityUpdate& update);
bool equalsDeep(const EntityInstance& other) const;
-
};
-
-
- /// @see base::EntitySnapshotBase
+ /**
+ * @brief Entity instance with a concrete ARON DTO type as data.
+ *
+ * This is the return type of EntityInstance::withDataAs<AronDtoT>().
+ *
+ * Usage example:
+ * @code
+ * #include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
+ *
+ * armarx::arondto::Duration duration;
+ * duration.microSeconds = 1000;
+ *
+ * armarx::armem::wm::EntityInstance instance;
+ * instance.data() = duration.toAron();
+ *
+ * const armarx::armem::wm::EntityInstanceBase<armarx::arondto::Duration> cast =
+ * instance.withDataAs<armarx::arondto::Duration>();
+ *
+ * const armarx::arondto::Duration& durationOut = cast.data();
+ *
+ * assert(durationOut.microseconds == 1000);
+ * @endcode
+ */
+ template <class AronDtoT>
+ using EntityInstanceBase = base::EntityInstanceBase<AronDtoT, EntityInstanceMetadata>;
+
+ /**
+ * @brief Client-side working memory entity snapshot.
+ * @see base::EntitySnapshotBase
+ */
class EntitySnapshot :
- public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>
- , public detail::FindInstanceDataMixinForSnapshot<EntitySnapshot>
+ public base::EntitySnapshotBase<EntityInstance, EntitySnapshot>,
+ public detail::FindInstanceDataMixinForSnapshot<EntitySnapshot>
+
{
public:
-
using base::EntitySnapshotBase<EntityInstance, EntitySnapshot>::EntitySnapshotBase;
-
};
-
- /// @see base::EntityBase
+ /**
+ * @brief Client-side working memory entity.
+ * @see base::EntityBase
+ */
class Entity :
- public base::EntityBase<EntitySnapshot, Entity>
- , public detail::FindInstanceDataMixinForEntity<Entity>
+ public base::EntityBase<EntitySnapshot, Entity>,
+ public detail::FindInstanceDataMixinForEntity<Entity>
{
public:
-
using base::EntityBase<EntitySnapshot, Entity>::EntityBase;
-
};
-
-
- /// @see base::ProviderSegmentBase
+ /**
+ * @brief Client-side working memory provider segment.
+ * @see base::ProviderSegmentBase
+ */
class ProviderSegment :
- public base::ProviderSegmentBase<Entity, ProviderSegment>
- , public detail::FindInstanceDataMixin<ProviderSegment>
+ public base::ProviderSegmentBase<Entity, ProviderSegment>,
+ public detail::FindInstanceDataMixin<ProviderSegment>
{
using Base = base::ProviderSegmentBase<Entity, ProviderSegment>;
public:
-
using Base::ProviderSegmentBase;
-
};
-
-
- /// @see base::CoreSegmentBase
+ /**
+ * @brief Client-side working memory core segment.
+ * @see base::CoreSegmentBase
+ */
class CoreSegment :
- public base::CoreSegmentBase<ProviderSegment, CoreSegment>
- , public detail::FindInstanceDataMixin<CoreSegment>
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
+ public detail::FindInstanceDataMixin<CoreSegment>
{
using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
public:
-
using Base::CoreSegmentBase;
-
};
-
-
- /// @see base::MemoryBase
+ /**
+ * @brief Client-side working memory.
+ * @see base::MemoryBase
+ */
class Memory :
- public base::MemoryBase<CoreSegment, Memory>
- , public detail::FindInstanceDataMixin<Memory>
+ public base::MemoryBase<CoreSegment, Memory>,
+ public detail::FindInstanceDataMixin<Memory>
{
using Base = base::MemoryBase<CoreSegment, Memory>;
public:
-
using Base::MemoryBase;
-
};
-}
-
+} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/test/ArMemEntityInstanceDataTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemEntityInstanceDataTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..24193227415c833e91251a03208e830738ca1b31
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/test/ArMemEntityInstanceDataTest.cpp
@@ -0,0 +1,72 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::armem
+
+#define ARMARX_BOOST_TEST
+
+#include <iostream>
+#include <set>
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/common/aron/Color.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
+
+BOOST_AUTO_TEST_CASE(test_EntityInstance_withDataAs_Color)
+{
+ simox::arondto::Color color;
+ color.r = 0;
+ color.g = 100;
+ color.b = 200;
+ color.a = 255;
+
+ armarx::armem::wm::EntityInstance instance;
+ instance.data() = color.toAron();
+
+ const armarx::armem::wm::EntityInstanceBase<simox::arondto::Color> w =
+ instance.withDataAs<simox::arondto::Color>();
+ const simox::arondto::Color& colorOut = w.data();
+
+ BOOST_CHECK(colorOut == color);
+ BOOST_CHECK_EQUAL(colorOut.r, 0);
+ BOOST_CHECK_EQUAL(colorOut.g, 100);
+ BOOST_CHECK_EQUAL(colorOut.b, 200);
+ BOOST_CHECK_EQUAL(colorOut.a, 255);
+}
+
+BOOST_AUTO_TEST_CASE(test_EntityInstance_withDataAs_Time)
+{
+ armarx::arondto::Duration duration;
+ duration.microSeconds = 1000;
+
+ armarx::armem::wm::EntityInstance instance;
+ instance.data() = duration.toAron();
+
+ const armarx::armem::wm::EntityInstanceBase<armarx::arondto::Duration> w =
+ instance.withDataAs<armarx::arondto::Duration>();
+ const armarx::arondto::Duration& durationOut = w.data();
+
+ BOOST_CHECK(durationOut == duration);
+ BOOST_CHECK_EQUAL(durationOut.microSeconds, 1000);
+ assert(durationOut.microSeconds == 1000);
+}
diff --git a/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
index bf97734a52d7d9db9198101307270c23d8b07383..53771403181a1afcba608d94979faa54db61404b 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
@@ -24,33 +24,30 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
-#include <RobotAPI/libraries/armem/core/Commit.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/operations.h>
-
#include <iostream>
#include <set>
+#include <RobotAPI/Test.h>
+#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
namespace armem = armarx::armem;
-
template <class ValueT>
-static
-std::vector<ValueT> toVector(const std::set<ValueT>& set)
+static std::vector<ValueT>
+toVector(const std::set<ValueT>& set)
{
return {set.begin(), set.end()};
}
-
-
BOOST_AUTO_TEST_CASE(test_forEach)
{
using namespace armarx::armem;
const MemoryID mid("memory");
- const std::set<MemoryID> emptySet; // For comparison.
+ const std::set<MemoryID> emptySet; // For comparison.
wm::Memory memory(mid);
std::set<MemoryID> cids, pids, eids, sids, iids;
@@ -101,75 +98,79 @@ BOOST_AUTO_TEST_CASE(test_forEach)
BOOST_TEST_MESSAGE("Memory: \n" << armem::print(memory));
- memory.forEachInstance([&](const wm::EntityInstance & i) -> bool
- {
- BOOST_TEST_CONTEXT(wm::EntityInstance::getLevelName() << " " << i.id())
+ memory.forEachInstance(
+ [&](const wm::EntityInstance& i) -> bool
{
- BOOST_TEST_INFO(toVector(iids));
- BOOST_CHECK_EQUAL(iids.count(i.id()), 1);
- iids.erase(i.id());
- }
- return true;
- });
+ BOOST_TEST_CONTEXT(wm::EntityInstance::getLevelName() << " " << i.id())
+ {
+ BOOST_TEST_INFO(toVector(iids));
+ BOOST_CHECK_EQUAL(iids.count(i.id()), 1);
+ iids.erase(i.id());
+ }
+ return true;
+ });
BOOST_TEST_CONTEXT(toVector(iids))
{
BOOST_CHECK_EQUAL(iids.size(), 0);
}
- memory.forEachSnapshot([&](const wm::EntitySnapshot & s) -> bool
- {
- BOOST_TEST_CONTEXT(wm::EntitySnapshot::getLevelName() << " " << s.id())
+ memory.forEachSnapshot(
+ [&](const wm::EntitySnapshot& s) -> bool
{
- BOOST_TEST_INFO(toVector(sids));
- BOOST_CHECK_EQUAL(sids.count(s.id()), 1);
- sids.erase(s.id());
- }
- return true;
- });
+ BOOST_TEST_CONTEXT(wm::EntitySnapshot::getLevelName() << " " << s.id())
+ {
+ BOOST_TEST_INFO(toVector(sids));
+ BOOST_CHECK_EQUAL(sids.count(s.id()), 1);
+ sids.erase(s.id());
+ }
+ return true;
+ });
BOOST_TEST_INFO(toVector(sids));
BOOST_CHECK_EQUAL(sids.size(), 0);
- memory.forEachEntity([&](const wm::Entity & e) -> bool
- {
- BOOST_TEST_CONTEXT(wm::Entity::getLevelName() << " " << e.id())
+ memory.forEachEntity(
+ [&](const wm::Entity& e) -> bool
{
- BOOST_TEST_INFO(toVector(eids));
- BOOST_CHECK_EQUAL(eids.count(e.id()), 1);
- eids.erase(e.id());
- }
- return true;
- });
+ BOOST_TEST_CONTEXT(wm::Entity::getLevelName() << " " << e.id())
+ {
+ BOOST_TEST_INFO(toVector(eids));
+ BOOST_CHECK_EQUAL(eids.count(e.id()), 1);
+ eids.erase(e.id());
+ }
+ return true;
+ });
BOOST_TEST_INFO(toVector(eids));
BOOST_CHECK_EQUAL(eids.size(), 0);
- memory.forEachProviderSegment([&](const wm::ProviderSegment & p) // -> bool
- {
- BOOST_TEST_CONTEXT(wm::ProviderSegment::getLevelName() << " " << p.id())
+ memory.forEachProviderSegment(
+ [&](const wm::ProviderSegment& p) // -> bool
{
- BOOST_TEST_INFO(toVector(pids));
- BOOST_CHECK_EQUAL(pids.count(p.id()), 1);
- pids.erase(p.id());
- }
- return true;
- });
+ BOOST_TEST_CONTEXT(wm::ProviderSegment::getLevelName() << " " << p.id())
+ {
+ BOOST_TEST_INFO(toVector(pids));
+ BOOST_CHECK_EQUAL(pids.count(p.id()), 1);
+ pids.erase(p.id());
+ }
+ return true;
+ });
BOOST_TEST_INFO(toVector(pids));
BOOST_CHECK_EQUAL(pids.size(), 0);
- memory.forEachCoreSegment([&](const wm::CoreSegment & c) // -> bool
- {
- BOOST_TEST_CONTEXT(wm::CoreSegment::getLevelName() << " " << c.id())
+ memory.forEachCoreSegment(
+ [&](const wm::CoreSegment& c) // -> bool
{
- BOOST_TEST_INFO(toVector(cids));
- BOOST_CHECK_EQUAL(cids.count(c.id()), 1);
- cids.erase(c.id());
- }
- return true;
- });
+ BOOST_TEST_CONTEXT(wm::CoreSegment::getLevelName() << " " << c.id())
+ {
+ BOOST_TEST_INFO(toVector(cids));
+ BOOST_CHECK_EQUAL(cids.count(c.id()), 1);
+ cids.erase(c.id());
+ }
+ return true;
+ });
BOOST_TEST_INFO(toVector(cids));
BOOST_CHECK_EQUAL(cids.size(), 0);
}
-
BOOST_AUTO_TEST_CASE(test_forEach_non_bool_func)
{
// Check whether this compiles + runs without break.
@@ -179,15 +180,10 @@ BOOST_AUTO_TEST_CASE(test_forEach_non_bool_func)
entity.addSnapshot(armem::Time(armem::Duration::MicroSeconds(1500)));
int i = 0;
- entity.forEachSnapshot([&i](const armem::wm::EntitySnapshot&) -> void
- {
- ++i;
- });
+ entity.forEachSnapshot([&i](const armem::wm::EntitySnapshot&) -> void { ++i; });
BOOST_CHECK_EQUAL(i, 2);
}
-
-
BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
{
armem::wm::EntitySnapshot snapshot(armem::Time::Now());
@@ -195,10 +191,7 @@ BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
snapshot.addInstance();
int i = 0;
- auto count = [&i](const auto&) -> void
- {
- ++i;
- };
+ auto count = [&i](const auto&) -> void { ++i; };
{
auto test = [&](auto&& snapshot)
@@ -273,9 +266,7 @@ BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
armem::wm::ProviderSegment provSeg("provider");
provSeg.addEntity(entity);
- provSeg.addEntity(entity.name() + " 2")
- .addSnapshot(armem::Time::Now())
- .addInstance();
+ provSeg.addEntity(entity.name() + " 2").addSnapshot(armem::Time::Now()).addInstance();
{
auto test = [&](auto&& provSeg)
@@ -320,9 +311,9 @@ BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
armem::wm::CoreSegment coreSeg("core");
coreSeg.addProviderSegment(provSeg);
coreSeg.addProviderSegment("other prov segment")
- .addEntity("other entity")
- .addSnapshot(armem::Time::Now())
- .addInstance();
+ .addEntity("other entity")
+ .addSnapshot(armem::Time::Now())
+ .addInstance();
{
auto test = [&](auto&& coreSeg)
@@ -370,10 +361,10 @@ BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
armem::wm::Memory memory("memory");
memory.addCoreSegment(coreSeg);
memory.addCoreSegment("other memory")
- .addProviderSegment("other provider segment")
- .addEntity("other entity")
- .addSnapshot(armem::Time::Now())
- .addInstance();
+ .addProviderSegment("other provider segment")
+ .addEntity("other entity")
+ .addSnapshot(armem::Time::Now())
+ .addInstance();
{
auto test = [&](auto&& memory)
@@ -422,3 +413,212 @@ BOOST_AUTO_TEST_CASE(test_forEachInstanceIn)
test(const_cast<const armem::wm::Memory&>(memory));
}
}
+
+namespace forEachInstanceAs
+{
+ struct Fixture
+ {
+ armem::wm::Entity entity;
+ armem::wm::EntitySnapshot* snapshotA = nullptr;
+ armem::wm::EntitySnapshot* snapshotB = nullptr;
+
+ armem::wm::EntityInstance* instanceA0 = nullptr;
+ armem::wm::EntityInstance* instanceA1 = nullptr;
+ armem::wm::EntityInstance* instanceB0 = nullptr;
+ armem::wm::EntityInstance* instanceB1 = nullptr;
+
+ Fixture()
+ {
+ snapshotA = &entity.addSnapshot(armem::Time(armem::Duration::MicroSeconds(100)));
+ snapshotB = &entity.addSnapshot(armem::Time(armem::Duration::MicroSeconds(200)));
+
+ BOOST_REQUIRE_EQUAL(entity.size(), 2);
+ BOOST_REQUIRE_EQUAL(snapshotA->size(), 0);
+ BOOST_REQUIRE_EQUAL(snapshotB->size(), 0);
+
+ instanceA0 = &snapshotA->addInstance();
+ instanceA1 = &snapshotA->addInstance();
+ instanceB0 = &snapshotB->addInstance();
+ instanceB1 = &snapshotB->addInstance();
+
+ if (false)
+ {
+ BOOST_REQUIRE_EQUAL(snapshotA->findInstance(0), instanceA0);
+ BOOST_REQUIRE_EQUAL(snapshotA->findInstance(1), instanceA1);
+ BOOST_REQUIRE_EQUAL(snapshotB->findInstance(0), instanceB0);
+ BOOST_REQUIRE_EQUAL(snapshotB->findInstance(1), instanceB1);
+ }
+
+ int i = 0;
+
+ i = 0;
+ entity.forEachSnapshot(
+ [&i](const armem::wm::EntitySnapshot& snapshot)
+ {
+ BOOST_REQUIRE_EQUAL(snapshot.size(), 2);
+ ++i;
+ });
+ BOOST_REQUIRE_EQUAL(i, 2);
+
+ i = 0;
+ entity.forEachInstance(
+ [&i](armem::wm::EntityInstance& instance)
+ {
+ instance.setData(makeDuration(i * 1000).toAron());
+ BOOST_REQUIRE(instance.data());
+ ++i;
+ });
+ BOOST_REQUIRE_EQUAL(i, 4);
+
+ if (false)
+ {
+ instanceA0->setData(makeDuration(1000).toAron());
+ instanceA1->setData(makeDuration(2000).toAron());
+ instanceB0->setData(makeDuration(3000).toAron());
+ instanceB1->setData(makeDuration(4000).toAron());
+
+ BOOST_REQUIRE(instanceA0->data());
+ BOOST_REQUIRE(instanceA1->data());
+ BOOST_REQUIRE(instanceB0->data());
+ BOOST_REQUIRE(instanceB1->data());
+ BOOST_REQUIRE_NO_THROW(armarx::arondto::Duration::FromAron(instanceA0->data()));
+ BOOST_REQUIRE_NO_THROW(armarx::arondto::Duration::FromAron(instanceA1->data()));
+ BOOST_REQUIRE_NO_THROW(armarx::arondto::Duration::FromAron(instanceB0->data()));
+ BOOST_REQUIRE_NO_THROW(armarx::arondto::Duration::FromAron(instanceB1->data()));
+ }
+
+ i = 0;
+ entity.forEachInstance(
+ [&i](const armem::wm::EntityInstance& instance)
+ {
+ BOOST_REQUIRE(instance.data());
+ ++i;
+ return true;
+ });
+ BOOST_REQUIRE_EQUAL(i, 4);
+ }
+
+ static armarx::arondto::Duration
+ makeDuration(int microSeconds)
+ {
+ armarx::arondto::Duration duration;
+ duration.microSeconds = microSeconds;
+ return duration;
+ };
+ };
+} // namespace forEachInstanceAs
+
+
+BOOST_FIXTURE_TEST_SUITE(forEachInstanceAs, Fixture)
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceAs_const_ref_return_true)
+{
+ int i = 0;
+
+ i = 0;
+ entity.forEachInstanceAs(
+ [&i](const armarx::arondto::Duration& duration)
+ {
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+
+ return true;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceAs_const_ref)
+{
+ int i = 0;
+ entity.forEachInstanceAs(
+ [&i](const armarx::arondto::Duration& duration)
+ {
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceAs_value_return_true)
+{
+ int i = 0;
+ entity.forEachInstanceAs(
+ [&i](armarx::arondto::Duration duration)
+ {
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+
+ return true;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceAs_value)
+{
+ int i = 0;
+ entity.forEachInstanceAs(
+ [&i](armarx::arondto::Duration duration)
+ {
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceWithDataAs_const_ref_return_true)
+{
+ int i = 0;
+ entity.forEachInstanceWithDataAs(
+ [&i](const armem::wm::EntityInstanceBase<armarx::arondto::Duration>& instance)
+ {
+ const armarx::arondto::Duration& duration = instance.data();
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+
+ return true;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceWithDataAs_const_ref)
+{
+ int i = 0;
+ entity.forEachInstanceWithDataAs(
+ [&i](const armem::wm::EntityInstanceBase<armarx::arondto::Duration>& instance)
+ {
+ const armarx::arondto::Duration& duration = instance.data();
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceWithDataAs_value_return_true)
+{
+ int i = 0;
+ entity.forEachInstanceWithDataAs(
+ [&i](armem::wm::EntityInstanceBase<armarx::arondto::Duration> instance)
+ {
+ const armarx::arondto::Duration& duration = instance.data();
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+
+ return true;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_CASE(test_forEachInstanceWithDataAs_value)
+{
+ int i = 0;
+ entity.forEachInstanceWithDataAs(
+ [&i](armem::wm::EntityInstanceBase<armarx::arondto::Duration> instance)
+ {
+ const armarx::arondto::Duration& duration = instance.data();
+ BOOST_CHECK_EQUAL(duration.microSeconds, i * 1000);
+ ++i;
+ });
+ BOOST_CHECK_EQUAL(i, 4);
+}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
index f03b02052ef9956aab079dda96677acaf5aca48d..4c133b5ac32e58eaf573e3b26f334b148c6a17cb 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
@@ -14,7 +14,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::ArmarXObjects::armem
- * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @author Rainer Kartmann ( raier dot kartmann at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
@@ -24,29 +24,36 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/operations.h>
+#include <iostream>
#include <RobotAPI/Test.h>
-
-#include <iostream>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armem = armarx::armem;
namespace wm = armarx::armem::wm;
namespace aron = armarx::aron;
-
namespace ArMemGetFindTest
{
struct Fixture
{
wm::EntitySnapshot snapshot;
+ wm::EntitySnapshot snapshotEmpty;
+
wm::Entity entity;
+ wm::Entity entityEmpty;
+
wm::ProviderSegment provSeg;
+ wm::ProviderSegment provSegEmpty;
+
wm::CoreSegment coreSeg;
+ wm::CoreSegment coreSegEmpty;
+
wm::Memory memory;
+ wm::Memory memoryEmpty;
armem::MemoryID instanceID;
armem::MemoryID snapshotID;
@@ -58,24 +65,38 @@ namespace ArMemGetFindTest
Fixture()
{
{
+ snapshotEmpty.time() = armem::Time(armem::Duration::MicroSeconds(500));
+
snapshot.time() = armem::Time(armem::Duration::MicroSeconds(1000));
snapshot.addInstance();
}
{
+ entityEmpty.name() = "entity empty";
+
entity.name() = "entity";
entity.addSnapshot(snapshot);
+ entity.addSnapshot(snapshotEmpty);
}
{
+ provSegEmpty.name() = "provider segment empty";
+
provSeg.name() = "provider segment";
provSeg.addEntity(entity);
+ provSeg.addEntity(entityEmpty);
}
{
+ coreSegEmpty.name() = "core segment empty";
+
coreSeg.name() = "core segment";
coreSeg.addProviderSegment(provSeg);
+ coreSeg.addProviderSegment(provSegEmpty);
}
{
+ memoryEmpty.name() = "memory empty";
+
memory.name() = "memory";
memory.addCoreSegment(coreSeg);
+ memory.addCoreSegment(coreSegEmpty);
}
memoryID = memory.id();
@@ -85,22 +106,18 @@ namespace ArMemGetFindTest
snapshotID = entityID.withTimestamp(snapshot.time());
instanceID = snapshotID.withInstanceIndex(0);
}
+
~Fixture()
{
}
-
template <class ParentT>
- void test_get_find_instance_by_id(ParentT&& parent)
- {
- _test_get_find_instance_by_id(const_cast<const ParentT&>(parent));
- _test_get_find_instance_by_id(const_cast<ParentT&>(parent));
- }
- template <class ParentT>
- void _test_get_find_instance_by_id(ParentT&& parent)
+ void
+ _test_get_find_instance_by_id(ParentT&& parent)
{
BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
{
+ BOOST_CHECK_EQUAL(parent.hasInstances(), true);
BOOST_CHECK_EQUAL(parent.hasInstance(snapshotID.withInstanceIndex(0)), true);
BOOST_CHECK_EQUAL(parent.hasInstance(snapshotID.withInstanceIndex(1)), false);
@@ -108,106 +125,240 @@ namespace ArMemGetFindTest
BOOST_CHECK_EQUAL(parent.findInstance(snapshotID.withInstanceIndex(1)), nullptr);
BOOST_CHECK_NO_THROW(parent.getInstance(snapshotID.withInstanceIndex(0)));
- BOOST_CHECK_THROW(parent.getInstance(snapshotID.withInstanceIndex(1)), armem::error::MissingEntry);
+ BOOST_CHECK_THROW(parent.getInstance(snapshotID.withInstanceIndex(1)),
+ armem::error::MissingEntry);
}
}
template <class ParentT>
- void test_get_find_snapshot_by_id(ParentT&& parent)
+ void
+ test_get_find_instance_by_id(ParentT&& parent)
{
- _test_get_find_snapshot_by_id(const_cast<const ParentT&>(parent));
- _test_get_find_snapshot_by_id(const_cast<ParentT&>(parent));
+ _test_get_find_instance_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_instance_by_id(const_cast<ParentT&>(parent));
}
+
template <class ParentT>
- void _test_get_find_snapshot_by_id(ParentT&& parent)
+ void
+ _test_get_find_latest_instance(ParentT&& parent)
{
BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
{
- BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))), true);
- BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(2000)))), false);
+ BOOST_CHECK_EQUAL(parent.hasInstances(), true);
+ BOOST_CHECK_NE(parent.findLatestInstance(), nullptr);
+ BOOST_CHECK_NO_THROW(parent.getLatestInstance());
+ }
+ }
- BOOST_CHECK_NE(parent.findSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))), nullptr);
- BOOST_CHECK_EQUAL(parent.findSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(2000)))), nullptr);
+ template <class ParentT>
+ void
+ test_get_find_latest_instance(ParentT&& parent)
+ {
+ _test_get_find_latest_instance(const_cast<const ParentT&>(parent));
+ _test_get_find_latest_instance(const_cast<ParentT&>(parent));
+ }
- BOOST_CHECK_NO_THROW(parent.getSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))));
- BOOST_CHECK_THROW(parent.getSnapshot(entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(2000)))), armem::error::MissingEntry);
+ template <class ParentT>
+ void
+ _test_get_find_latest_instance_when_empty(ParentT&& parent)
+ {
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasInstances(), false);
+ BOOST_CHECK_EQUAL(parent.findLatestInstance(), nullptr);
+ BOOST_CHECK_THROW(parent.getLatestInstance(), armem::error::NoSuchEntries);
}
}
+ template <class ParentT>
+ void
+ test_get_find_latest_instance_when_empty(ParentT&& parent)
+ {
+ _test_get_find_latest_instance_when_empty(const_cast<const ParentT&>(parent));
+ _test_get_find_latest_instance_when_empty(const_cast<ParentT&>(parent));
+ }
template <class ParentT>
- void test_get_find_entity_by_id(ParentT&& parent)
+ void
+ _test_get_find_snapshot_by_id(ParentT&& parent)
{
- _test_get_find_entity_by_id(const_cast<const ParentT&>(parent));
- _test_get_find_entity_by_id(const_cast<ParentT&>(parent));
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasSnapshots(), true);
+
+ BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(
+ armem::Time(armem::Duration::MicroSeconds(1000)))),
+ true);
+ BOOST_CHECK_EQUAL(parent.hasSnapshot(entityID.withTimestamp(
+ armem::Time(armem::Duration::MicroSeconds(2000)))),
+ false);
+
+ BOOST_CHECK_NE(parent.findSnapshot(entityID.withTimestamp(
+ armem::Time(armem::Duration::MicroSeconds(1000)))),
+ nullptr);
+ BOOST_CHECK_EQUAL(parent.findSnapshot(entityID.withTimestamp(
+ armem::Time(armem::Duration::MicroSeconds(2000)))),
+ nullptr);
+
+ BOOST_CHECK_NO_THROW(parent.getSnapshot(
+ entityID.withTimestamp(armem::Time(armem::Duration::MicroSeconds(1000)))));
+ BOOST_CHECK_THROW(parent.getSnapshot(entityID.withTimestamp(
+ armem::Time(armem::Duration::MicroSeconds(2000)))),
+ armem::error::MissingEntry);
+ }
+ }
+
+ template <class ParentT>
+ void
+ test_get_find_snapshot_by_id(ParentT&& parent)
+ {
+ _test_get_find_snapshot_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_snapshot_by_id(const_cast<ParentT&>(parent));
}
+
template <class ParentT>
- void _test_get_find_entity_by_id(ParentT&& parent)
+ void
+ _test_get_find_entity_by_id(ParentT&& parent)
{
BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
{
BOOST_CHECK_EQUAL(parent.hasEntity(provSegID.withEntityName("entity")), true);
- BOOST_CHECK_EQUAL(parent.hasEntity(provSegID.withEntityName("other entity")), false);
+ BOOST_CHECK_EQUAL(parent.hasEntity(provSegID.withEntityName("other entity")),
+ false);
BOOST_CHECK_NE(parent.findEntity(provSegID.withEntityName("entity")), nullptr);
- BOOST_CHECK_EQUAL(parent.findEntity(provSegID.withEntityName("other entity")), nullptr);
+ BOOST_CHECK_EQUAL(parent.findEntity(provSegID.withEntityName("other entity")),
+ nullptr);
BOOST_CHECK_NO_THROW(parent.getEntity(provSegID.withEntityName("entity")));
- BOOST_CHECK_THROW(parent.getEntity(provSegID.withEntityName("other entity")), armem::error::MissingEntry);
+ BOOST_CHECK_THROW(parent.getEntity(provSegID.withEntityName("other entity")),
+ armem::error::MissingEntry);
}
}
template <class ParentT>
- void test_get_find_provider_segment_by_id(ParentT&& parent)
+ void
+ _test_get_find_latest_snapshot(ParentT&& parent)
{
- _test_get_find_provider_segment_by_id(const_cast<const ParentT&>(parent));
- _test_get_find_provider_segment_by_id(const_cast<ParentT&>(parent));
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(parent.hasSnapshots(), true);
+ BOOST_CHECK_NE(parent.findLatestSnapshot(), nullptr);
+ BOOST_CHECK_NO_THROW(parent.getLatestSnapshot());
+ }
}
+
template <class ParentT>
- void _test_get_find_provider_segment_by_id(ParentT&& parent)
+ void
+ test_get_find_latest_snapshot(ParentT&& parent)
+ {
+ _test_get_find_latest_snapshot(const_cast<const ParentT&>(parent));
+ _test_get_find_latest_snapshot(const_cast<ParentT&>(parent));
+ }
+
+ template <class ParentT>
+ void
+ _test_get_find_latest_snapshot_when_empty(ParentT&& parent)
{
BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
{
- BOOST_CHECK_EQUAL(parent.hasProviderSegment(provSegID.withProviderSegmentName("provider segment")), true);
- BOOST_CHECK_EQUAL(parent.hasProviderSegment(provSegID.withProviderSegmentName("other provider segment")), false);
-
- BOOST_CHECK_NE(parent.findProviderSegment(provSegID.withProviderSegmentName("provider segment")), nullptr);
- BOOST_CHECK_EQUAL(parent.findProviderSegment(provSegID.withProviderSegmentName("other provider segment")), nullptr);
-
- BOOST_CHECK_NO_THROW(parent.getProviderSegment(provSegID.withProviderSegmentName("provider segment")));
- BOOST_CHECK_THROW(parent.getProviderSegment(provSegID.withProviderSegmentName("other provider segment")), armem::error::MissingEntry);
+ BOOST_CHECK_EQUAL(parent.hasSnapshots(), false);
+ BOOST_CHECK_EQUAL(parent.findLatestSnapshot(), nullptr);
+ BOOST_CHECK_THROW(parent.getLatestSnapshot(), armem::error::NoSuchEntries);
}
}
template <class ParentT>
- void test_get_find_core_segment_by_id(ParentT&& parent)
+ void
+ test_get_find_latest_snapshot_when_empty(ParentT&& parent)
{
- _test_get_find_core_segment_by_id(const_cast<const ParentT&>(parent));
- _test_get_find_core_segment_by_id(const_cast<ParentT&>(parent));
+ _test_get_find_latest_snapshot_when_empty(const_cast<const ParentT&>(parent));
+ _test_get_find_latest_snapshot_when_empty(const_cast<ParentT&>(parent));
}
+
template <class ParentT>
- void _test_get_find_core_segment_by_id(ParentT&& parent)
+ void
+ test_get_find_entity_by_id(ParentT&& parent)
+ {
+ _test_get_find_entity_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_entity_by_id(const_cast<ParentT&>(parent));
+ }
+
+ template <class ParentT>
+ void
+ _test_get_find_provider_segment_by_id(ParentT&& parent)
{
BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
{
- BOOST_CHECK_EQUAL(parent.hasCoreSegment(provSegID.withCoreSegmentName("core segment")), true);
- BOOST_CHECK_EQUAL(parent.hasCoreSegment(provSegID.withCoreSegmentName("other core segment")), false);
+ BOOST_CHECK_EQUAL(parent.hasProviderSegment(
+ provSegID.withProviderSegmentName("provider segment")),
+ true);
+ BOOST_CHECK_EQUAL(parent.hasProviderSegment(
+ provSegID.withProviderSegmentName("other provider segment")),
+ false);
+
+ BOOST_CHECK_NE(parent.findProviderSegment(
+ provSegID.withProviderSegmentName("provider segment")),
+ nullptr);
+ BOOST_CHECK_EQUAL(parent.findProviderSegment(
+ provSegID.withProviderSegmentName("other provider segment")),
+ nullptr);
+
+ BOOST_CHECK_NO_THROW(parent.getProviderSegment(
+ provSegID.withProviderSegmentName("provider segment")));
+ BOOST_CHECK_THROW(parent.getProviderSegment(
+ provSegID.withProviderSegmentName("other provider segment")),
+ armem::error::MissingEntry);
+ }
+ }
- BOOST_CHECK_NE(parent.findCoreSegment(provSegID.withCoreSegmentName("core segment")), nullptr);
- BOOST_CHECK_EQUAL(parent.findCoreSegment(provSegID.withCoreSegmentName("other core segment")), nullptr);
+ template <class ParentT>
+ void
+ test_get_find_provider_segment_by_id(ParentT&& parent)
+ {
+ _test_get_find_provider_segment_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_provider_segment_by_id(const_cast<ParentT&>(parent));
+ }
- BOOST_CHECK_NO_THROW(parent.getCoreSegment(provSegID.withCoreSegmentName("core segment")));
- BOOST_CHECK_THROW(parent.getCoreSegment(provSegID.withCoreSegmentName("other core segment")), armem::error::MissingEntry);
+ template <class ParentT>
+ void
+ _test_get_find_core_segment_by_id(ParentT&& parent)
+ {
+ BOOST_TEST_CONTEXT("Parent: " << armem::print(parent))
+ {
+ BOOST_CHECK_EQUAL(
+ parent.hasCoreSegment(provSegID.withCoreSegmentName("core segment")), true);
+ BOOST_CHECK_EQUAL(
+ parent.hasCoreSegment(provSegID.withCoreSegmentName("other core segment")),
+ false);
+
+ BOOST_CHECK_NE(
+ parent.findCoreSegment(provSegID.withCoreSegmentName("core segment")), nullptr);
+ BOOST_CHECK_EQUAL(
+ parent.findCoreSegment(provSegID.withCoreSegmentName("other core segment")),
+ nullptr);
+
+ BOOST_CHECK_NO_THROW(
+ parent.getCoreSegment(provSegID.withCoreSegmentName("core segment")));
+ BOOST_CHECK_THROW(
+ parent.getCoreSegment(provSegID.withCoreSegmentName("other core segment")),
+ armem::error::MissingEntry);
}
}
+
+ template <class ParentT>
+ void
+ test_get_find_core_segment_by_id(ParentT&& parent)
+ {
+ _test_get_find_core_segment_by_id(const_cast<const ParentT&>(parent));
+ _test_get_find_core_segment_by_id(const_cast<ParentT&>(parent));
+ }
};
-}
+} // namespace ArMemGetFindTest
BOOST_FIXTURE_TEST_SUITE(ArMemGetFindTest, Fixture)
-
-
BOOST_AUTO_TEST_CASE(test_snapshot_get_find_instance_by_key)
{
BOOST_CHECK_EQUAL(snapshot.hasInstance(0), true);
@@ -220,20 +371,20 @@ BOOST_AUTO_TEST_CASE(test_snapshot_get_find_instance_by_key)
BOOST_CHECK_THROW(snapshot.getInstance(1), armem::error::MissingEntry);
}
-
BOOST_AUTO_TEST_CASE(test_entity_get_find_snapshot_by_key)
{
BOOST_CHECK_EQUAL(entity.hasSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))), true);
BOOST_CHECK_EQUAL(entity.hasSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))), false);
BOOST_CHECK_NE(entity.findSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))), nullptr);
- BOOST_CHECK_EQUAL(entity.findSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))), nullptr);
+ BOOST_CHECK_EQUAL(entity.findSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))),
+ nullptr);
BOOST_CHECK_NO_THROW(entity.getSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))));
- BOOST_CHECK_THROW(entity.getSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))), armem::error::MissingEntry);
+ BOOST_CHECK_THROW(entity.getSnapshot(armem::Time(armem::Duration::MicroSeconds(2000))),
+ armem::error::MissingEntry);
}
-
BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_entity_by_key)
{
BOOST_CHECK_EQUAL(provSeg.hasEntity("entity"), true);
@@ -246,7 +397,6 @@ BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_entity_by_key)
BOOST_CHECK_THROW(provSeg.getEntity("other entity"), armem::error::MissingEntry);
}
-
BOOST_AUTO_TEST_CASE(test_core_segment_get_find_provider_segment_by_key)
{
BOOST_CHECK_EQUAL(coreSeg.hasProviderSegment("provider segment"), true);
@@ -256,10 +406,10 @@ BOOST_AUTO_TEST_CASE(test_core_segment_get_find_provider_segment_by_key)
BOOST_CHECK_EQUAL(coreSeg.findProviderSegment("other provider segment"), nullptr);
BOOST_CHECK_NO_THROW(coreSeg.getProviderSegment("provider segment"));
- BOOST_CHECK_THROW(coreSeg.getProviderSegment("other provider segment"), armem::error::MissingEntry);
+ BOOST_CHECK_THROW(coreSeg.getProviderSegment("other provider segment"),
+ armem::error::MissingEntry);
}
-
BOOST_AUTO_TEST_CASE(test_memory_get_find_core_segment_by_key)
{
BOOST_CHECK_EQUAL(memory.hasCoreSegment("core segment"), true);
@@ -272,66 +422,119 @@ BOOST_AUTO_TEST_CASE(test_memory_get_find_core_segment_by_key)
BOOST_CHECK_THROW(memory.getCoreSegment("other core segment"), armem::error::MissingEntry);
}
-
-
BOOST_AUTO_TEST_CASE(test_snapshot_get_find_instance_by_id)
{
test_get_find_instance_by_id(snapshot);
}
+
BOOST_AUTO_TEST_CASE(test_entity_get_find_instance_by_id)
{
test_get_find_instance_by_id(entity);
}
+
BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_instance_by_id)
{
test_get_find_instance_by_id(provSeg);
}
+
BOOST_AUTO_TEST_CASE(test_core_segment_get_find_instance_by_id)
{
test_get_find_instance_by_id(coreSeg);
}
+
BOOST_AUTO_TEST_CASE(test_memory_get_find_instance_by_id)
{
test_get_find_instance_by_id(memory);
}
+BOOST_AUTO_TEST_CASE(test_entity_get_find_latest_instance)
+{
+ test_get_find_latest_instance(entity);
+ test_get_find_latest_instance_when_empty(entityEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_latest_instance)
+{
+ test_get_find_latest_instance(provSeg);
+ test_get_find_latest_instance_when_empty(provSegEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_core_segment_get_find_latest_instance)
+{
+ test_get_find_latest_instance(coreSeg);
+ test_get_find_latest_instance_when_empty(coreSegEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_memory_get_find_latest_instance)
+{
+ test_get_find_latest_instance(memory);
+ test_get_find_latest_instance_when_empty(memoryEmpty);
+}
BOOST_AUTO_TEST_CASE(test_entity_get_find_snapshot_by_id)
{
test_get_find_snapshot_by_id(entity);
}
+
BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_snapshot_by_id)
{
test_get_find_snapshot_by_id(provSeg);
}
+
BOOST_AUTO_TEST_CASE(test_core_segment_get_find_snapshot_by_id)
{
test_get_find_snapshot_by_id(coreSeg);
}
+
BOOST_AUTO_TEST_CASE(test_memory_get_find_snapshot_by_id)
{
test_get_find_snapshot_by_id(memory);
}
+BOOST_AUTO_TEST_CASE(test_entity_get_find_latest_snapshot)
+{
+ test_get_find_latest_snapshot(entity);
+ test_get_find_latest_snapshot_when_empty(entityEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_latest_snapshot)
+{
+ test_get_find_latest_snapshot(provSeg);
+ test_get_find_latest_snapshot_when_empty(provSegEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_core_segment_get_find_latest_snapshot)
+{
+ test_get_find_latest_snapshot(coreSeg);
+ test_get_find_latest_snapshot_when_empty(coreSegEmpty);
+}
+
+BOOST_AUTO_TEST_CASE(test_memory_get_find_latest_snapshot)
+{
+ test_get_find_latest_snapshot(memory);
+ test_get_find_latest_snapshot_when_empty(memoryEmpty);
+}
BOOST_AUTO_TEST_CASE(test_provider_segment_get_find_entity_by_id)
{
test_get_find_entity_by_id(provSeg);
}
+
BOOST_AUTO_TEST_CASE(test_core_segment_get_find_entity_by_id)
{
test_get_find_entity_by_id(coreSeg);
}
+
BOOST_AUTO_TEST_CASE(test_memory_get_find_entity_by_id)
{
test_get_find_entity_by_id(memory);
}
-
BOOST_AUTO_TEST_CASE(test_core_segment_get_find_provider_segment_by_id)
{
test_get_find_provider_segment_by_id(coreSeg);
}
+
BOOST_AUTO_TEST_CASE(test_memory_get_find_provider_segment_by_id)
{
test_get_find_provider_segment_by_id(memory);
@@ -342,6 +545,4 @@ BOOST_AUTO_TEST_CASE(test_memory_get_find_core_segment_by_id)
test_get_find_core_segment_by_id(memory);
}
-
-
BOOST_AUTO_TEST_SUITE_END()
diff --git a/source/RobotAPI/libraries/armem/test/CMakeLists.txt b/source/RobotAPI/libraries/armem/test/CMakeLists.txt
index da77a43b82a6174b756ddf1705e8c29ea3acabb2..688f0009e90f7540708e8e02fde2f7336944a471 100644
--- a/source/RobotAPI/libraries/armem/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/test/CMakeLists.txt
@@ -2,6 +2,7 @@
# Libs required for the tests
SET(LIBS ${LIBS} ArmarXCore ${LIB_NAME})
+armarx_add_test(ArMemEntityInstanceDataTest ArMemEntityInstanceDataTest.cpp "${LIBS}")
armarx_add_test(ArMemForEachTest ArMemForEachTest.cpp "${LIBS}")
armarx_add_test(ArMemGetFindTest ArMemGetFindTest.cpp "${LIBS}")
armarx_add_test(ArMemIceConversionsTest ArMemIceConversionsTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
index 76112d64cfa0788fadfbcc5e86f923b8bf1d978b..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,17 +60,18 @@ 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_WARNING << "No entity snapshots found";
+ ARMARX_VERBOSE << "No entity snapshots found";
return std::nullopt;
}
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;
@@ -88,7 +86,7 @@ namespace armarx::armem::grasping::known_grasps
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_INFO << "Lookup result in reader: " << qResult;
+ ARMARX_VERBOSE << "Lookup result in reader: " << qResult;
if (not qResult.success) /* c++20 [[unlikely]] */
{
@@ -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_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index 00bd50a3d4721765f2287482bd31f09540d480a3..a3a55ac0fafa59c5b0997dc08b4ac2065b616a15 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -1,4 +1,6 @@
#include "KnownGraspProviderSegment.h"
+#include <VirtualRobot/Grasping/GraspSet.h>
+#include <VirtualRobot/XML/ObjectIO.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
@@ -26,83 +28,57 @@ namespace armarx::armem::grasping::segment
if (std::filesystem::is_regular_file(graspFilePath))
{
- auto reader = RapidXmlReader::FromFile(graspFilePath);
- RapidXmlReaderNode root = reader->getRoot();
-
- std::string objectClassNameXML = root.attribute_value("name");
- ARMARX_CHECK_EQUAL(objectClassName, objectClassNameXML);
+ ARMARX_INFO << "loading " << graspFilePath;
+ try {
+ auto manipulationObject = VirtualRobot::ObjectIO::loadManipulationObject(graspFilePath);
+
+ if(manipulationObject == nullptr)
+ {
+ ARMARX_WARNING << "Invalid file content: " << graspFilePath;
+ return std::nullopt;
+ }
- arondto::KnownGraspInfo ret;
- ret.correspondingObject.memoryName = "Object";
- ret.correspondingObject.coreSegmentName = "Class";
- ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
- ret.correspondingObject.entityName = info.idStr();
- ret.xml.package = fileLocInfo.package;
- ret.xml.path = fileLocInfo.relativePath;
+ arondto::KnownGraspInfo ret;
+ ret.correspondingObject.memoryName = "Object";
+ ret.correspondingObject.coreSegmentName = "Class";
+ ret.correspondingObject.providerSegmentName = "PriorKnowledgeData";
+ ret.correspondingObject.entityName = info.idStr();
+ ret.xml.package = fileLocInfo.package;
+ ret.xml.path = fileLocInfo.relativePath;
- for (const auto& graspSetNode : root.nodes())
- {
- if (graspSetNode.name() != "GraspSet")
+ for (const VirtualRobot::GraspSetPtr& graspSet : manipulationObject->getAllGraspSets())
{
- continue;
- }
+ ARMARX_CHECK_NOT_NULL(graspSet);
- arondto::KnownGraspSet retGraspSet;
+ arondto::KnownGraspSet retGraspSet;
- retGraspSet.name = graspSetNode.attribute_value("name");
- retGraspSet.robot = graspSetNode.attribute_value("RobotType");
- retGraspSet.endeffector = graspSetNode.attribute_value("EndEffector");
+ retGraspSet.name = graspSet->getName();
+ retGraspSet.robot = graspSet->getRobotType();
+ retGraspSet.endeffector = graspSet->getEndEffector();
- for (const auto& graspNode : graspSetNode.nodes())
- {
- if (graspNode.name() != "Grasp")
+ for (const VirtualRobot::GraspPtr& grasp : graspSet->getGrasps())
{
- continue;
- }
+ ARMARX_CHECK_NOT_NULL(grasp);
+
+ arondto::KnownGrasp retGrasp;
+
+ retGrasp.name = grasp->getName();
+ retGrasp.quality = grasp->getQuality();
+ retGrasp.creator = grasp->getCreationMethod();
+ retGrasp.pose = grasp->getTransformation();
- arondto::KnownGrasp retGrasp;
-
- retGrasp.name = graspNode.attribute_value("name");
- retGrasp.quality = std::stof(graspNode.attribute_value("quality"));
- retGrasp.creator = graspNode.attribute_value("Creation");
-
- ARMARX_CHECK(graspNode.nodes().size() == 2 or graspNode.nodes().size() == 1);
- RapidXmlReaderNode transformNode = graspNode.nodes()[0];
-
- ARMARX_CHECK_EQUAL(transformNode.name(), "Transform");
- ARMARX_CHECK_EQUAL(transformNode.nodes().size(), 1);
- RapidXmlReaderNode matrixNode = transformNode.nodes()[0];
-
- ARMARX_CHECK_EQUAL(matrixNode.nodes().size(), 4);
- RapidXmlReaderNode row0 = matrixNode.nodes()[0];
- RapidXmlReaderNode row1 = matrixNode.nodes()[1];
- RapidXmlReaderNode row2 = matrixNode.nodes()[2];
- RapidXmlReaderNode row3 = matrixNode.nodes()[3];
-
- retGrasp.pose(0, 0) = std::stof(row0.attribute_value("c1"));
- retGrasp.pose(0, 1) = std::stof(row0.attribute_value("c2"));
- retGrasp.pose(0, 2) = std::stof(row0.attribute_value("c3"));
- retGrasp.pose(0, 3) = std::stof(row0.attribute_value("c4"));
- retGrasp.pose(1, 0) = std::stof(row1.attribute_value("c1"));
- retGrasp.pose(1, 1) = std::stof(row1.attribute_value("c2"));
- retGrasp.pose(1, 2) = std::stof(row1.attribute_value("c3"));
- retGrasp.pose(1, 3) = std::stof(row1.attribute_value("c4"));
- retGrasp.pose(2, 0) = std::stof(row2.attribute_value("c1"));
- retGrasp.pose(2, 1) = std::stof(row2.attribute_value("c2"));
- retGrasp.pose(2, 2) = std::stof(row2.attribute_value("c3"));
- retGrasp.pose(2, 3) = std::stof(row2.attribute_value("c4"));
- retGrasp.pose(3, 0) = std::stof(row3.attribute_value("c1"));
- retGrasp.pose(3, 1) = std::stof(row3.attribute_value("c2"));
- retGrasp.pose(3, 2) = std::stof(row3.attribute_value("c3"));
- retGrasp.pose(3, 3) = std::stof(row3.attribute_value("c4"));
-
- ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
-
- retGraspSet.grasps.push_back(retGrasp);
+ ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
+
+ retGraspSet.grasps.push_back(retGrasp);
+ }
+ ret.graspSets[retGraspSet.name] = retGraspSet;
}
- ret.graspSets[retGraspSet.name] = retGraspSet;
+ return ret;
+
+ } catch (...) {
+ ARMARX_WARNING << graspFilePath << " is not a manipulation object!";
+ return std::nullopt;
}
- return ret;
}
return std::nullopt;
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
index e9bb7bc4441bb2246ca2d360dc747a3398b4fdfd..b4c59389a918789222beb3595a29ec49320775df 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
@@ -90,6 +90,17 @@ namespace armarx::aron
value << DataDisplayVisitor::getValue(data);
}
+ void
+ TypedDataDisplayVisitor::visitIntEnum(const data::VariantPtr& data,
+ const type::VariantPtr& type)
+ {
+ type::IntEnumPtr enumType = type::IntEnum::DynamicCast(type);
+ data::IntPtr enumData = data::Int::DynamicCast(data);
+
+ std::string name = enumType->getValueName(enumData->getValue());
+ value << name;
+ }
+
/*void TypedDataDisplayVisitor::visitDateTime(const data::VariantPtr& data, const type::VariantPtr& type)
{
auto l = data::Long::DynamicCastAndCheck(data);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
index cdbcf69129ff9a3224c3f14d8f87a168d54238c4..c5a6fefc266f3bdda9dfde3942554334473f02ea 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
@@ -37,6 +37,8 @@ namespace armarx::aron
void visitLong(const data::VariantPtr& data, const type::VariantPtr& type) override;
void visitString(const data::VariantPtr& data, const type::VariantPtr& type) override;
+ void visitIntEnum(const data::VariantPtr& data, const type::VariantPtr& type) override;
+
void visitMatrix(const data::VariantPtr& data, const type::VariantPtr& type) override;
void visitQuaternion(const data::VariantPtr& data, const type::VariantPtr& type) override;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
index 8b2b4f1d70cc65a416c8aea58c3bd165b1bcdb3f..1447684d0e223660c00703f3ea0403fcf15c74d0 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -2,18 +2,16 @@
#include <QTreeWidgetItem>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
-
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
-#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
-#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
-#include <RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h>
#include <RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h>
+#include <RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h>
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
#include <RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h>
-
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::gui::instance
{
@@ -22,135 +20,138 @@ namespace armarx::armem::gui::instance
{
}
-
- void TypedDataTreeBuilder::updateTree(
- QTreeWidgetItem* parent,
- const aron::type::Dict& type,
- const aron::data::Dict& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::Dict& type,
+ const aron::data::Dict& data)
{
auto childType = type.getAcceptedType();
DictBuilder builder = getDictBuilder();
- builder.setUpdateItemFn([this, &childType, &data](const std::string & key, QTreeWidgetItem * item)
- {
- auto childData = data.getElement(key);
- if (childData)
+ builder.setUpdateItemFn(
+ [this, &childType, &data](const std::string& key, QTreeWidgetItem* item)
{
- this->updateDispatch(item, key, childType, childData);
- }
- return true;
- });
+ auto childData = data.getElement(key);
+ if (childData)
+ {
+ this->updateDispatch(item, key, childType, childData);
+ }
+ return true;
+ });
builder.updateTreeWithContainer(parent, data.getAllKeys());
}
- void TypedDataTreeBuilder::updateTree(
- QTreeWidgetItem* parent,
- const aron::type::AnyObject& type,
- const aron::data::Dict& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::AnyObject& type,
+ const aron::data::Dict& data)
{
DictBuilder builder = getDictBuilder();
- builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
- {
- auto childData = data.getElement(key);
- if (childData)
+ builder.setUpdateItemFn(
+ [this, &data](const std::string& key, QTreeWidgetItem* item)
{
- this->updateDispatch(item, key, nullptr, childData);
- }
- return true;
- });
+ auto childData = data.getElement(key);
+ if (childData)
+ {
+ this->updateDispatch(item, key, nullptr, childData);
+ }
+ return true;
+ });
builder.updateTreeWithContainer(parent, data.getAllKeys());
}
-
- void TypedDataTreeBuilder::updateTree(
- QTreeWidgetItem* parent,
- const aron::type::Object& type,
- const aron::data::Dict& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::Object& type,
+ const aron::data::Dict& data)
{
DictBuilder builder = getDictBuilder();
- builder.setMakeItemFn([this, &type](const std::string & key) -> QTreeWidgetItem*
- {
- if (type.hasMemberType(key)
- && type.getMemberType(key)->getFullName() == instance::rawMemoryIDTypeName)
+ builder.setMakeItemFn(
+ [this, &type](const std::string& key) -> QTreeWidgetItem*
{
- MemoryIDTreeWidgetItem* item = new MemoryIDTreeWidgetItem({QString::fromStdString(key)});
- item->addKeyChildren();
- return item;
- }
- else
+ if (type.hasMemberType(key) &&
+ type.getMemberType(key)->getFullName() == instance::rawMemoryIDTypeName)
+ {
+ MemoryIDTreeWidgetItem* item =
+ new MemoryIDTreeWidgetItem({QString::fromStdString(key)});
+ item->addKeyChildren();
+ return item;
+ }
+ else
+ {
+ return this->makeItem(key);
+ }
+ });
+ builder.setUpdateItemFn(
+ [this, &type, &data](const std::string& key, QTreeWidgetItem* item)
{
- return this->makeItem(key);
- }
- });
- builder.setUpdateItemFn([this, &type, &data](const std::string & key, QTreeWidgetItem * item)
- {
- auto childData = data.getElement(key);
-
- // We need this check here because getMemberType(key) throws
- // instead of returning nullptr if the type doesn't have the key.
- if (type.hasMemberType(key))
- {
- this->updateDispatch(item, key, type.getMemberType(key), childData);
- }
- else
- {
- this->updateDispatch(item, key, nullptr, childData);
- }
- return true;
- });
+ auto childData = data.getElement(key);
+
+ // We need this check here because getMemberType(key) throws
+ // instead of returning nullptr if the type doesn't have the key.
+ if (type.hasMemberType(key))
+ {
+ this->updateDispatch(item, key, type.getMemberType(key), childData);
+ }
+ else
+ {
+ this->updateDispatch(item, key, nullptr, childData);
+ }
+ return true;
+ });
builder.updateTreeWithContainer(parent, data.getAllKeys());
}
-
- void TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
- const aron::type::List& type,
- const aron::data::List& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::List& type,
+ const aron::data::List& data)
{
auto childType = type.getAcceptedType();
auto children = data.getChildren();
ListBuilder builder = getListBuilder();
- builder.setUpdateItemFn([this, &children, &childType](size_t key, QTreeWidgetItem * item)
- {
- if (auto childData = children.at(key))
+ builder.setUpdateItemFn(
+ [this, &children, &childType](size_t key, QTreeWidgetItem* item)
{
- this->updateDispatch(item, std::to_string(key), childType, childData);
- }
- return true;
- });
+ if (auto childData = children.at(key))
+ {
+ this->updateDispatch(item, std::to_string(key), childType, childData);
+ }
+ return true;
+ });
builder.updateTreeWithContainer(parent, getIndex(children.size()));
}
-
- void TypedDataTreeBuilder::updateTree(
- QTreeWidgetItem* parent,
- const aron::type::Pair& type,
- const aron::data::List& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::Pair& type,
+ const aron::data::List& data)
{
ARMARX_CHECK_EQUAL(data.childrenSize(), 2);
auto childTypes = type.getAcceptedTypes();
ListBuilder builder = getListBuilder();
- builder.setUpdateItemFn([this, &data, &childTypes](size_t i, QTreeWidgetItem * item)
- {
- auto childType = i == 0 ? childTypes.first : childTypes.second;
- auto childData = data.getElement(static_cast<unsigned int>(i));
+ builder.setUpdateItemFn(
+ [this, &data, &childTypes](size_t i, QTreeWidgetItem* item)
+ {
+ auto childType = i == 0 ? childTypes.first : childTypes.second;
+ auto childData = data.getElement(static_cast<unsigned int>(i));
- this->updateDispatch(item, std::to_string(i), childType, childData);
- return true;
- });
+ this->updateDispatch(item, std::to_string(i), childType, childData);
+ return true;
+ });
builder.updateTreeWithContainer(parent, getIndex(data.childrenSize()));
}
-
- void TypedDataTreeBuilder::updateTree(
- QTreeWidgetItem* parent,
- const aron::type::Tuple& type,
- const aron::data::List& data)
+ void
+ TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ const aron::type::Tuple& type,
+ const aron::data::List& data)
{
// Allows tuples where the data list is longer than the type tuple -
// is that desired behavior?
@@ -158,28 +159,28 @@ namespace armarx::armem::gui::instance
auto childTypes = type.getAcceptedTypes();
ListBuilder builder = getListBuilder();
- builder.setUpdateItemFn([this, &data, &childTypes](size_t i, QTreeWidgetItem * item)
- {
- auto childType = (i < childTypes.size()) ? childTypes.at(i) : nullptr;
- auto childData = data.getElement(static_cast<unsigned int>(i));
+ builder.setUpdateItemFn(
+ [this, &data, &childTypes](size_t i, QTreeWidgetItem* item)
+ {
+ auto childType = (i < childTypes.size()) ? childTypes.at(i) : nullptr;
+ auto childData = data.getElement(static_cast<unsigned int>(i));
- this->updateDispatch(item, std::to_string(i), childType, childData);
- return true;
- });
+ this->updateDispatch(item, std::to_string(i), childType, childData);
+ return true;
+ });
builder.updateTreeWithContainer(parent, getIndex(data.childrenSize()));
}
-
/*! Used so that elements in the data that don't appear in the type
* can still be shown in the GUI if type information is enabled
* (otherwise, they would be hidden).
*/
- void TypedDataTreeBuilder::updateDispatch(
- QTreeWidgetItem* item,
- const std::string& key,
- const aron::type::VariantPtr& type,
- const aron::data::VariantPtr& data)
+ void
+ TypedDataTreeBuilder::updateDispatch(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::type::VariantPtr& type,
+ const aron::data::VariantPtr& data)
{
if (type)
{
@@ -191,17 +192,27 @@ namespace armarx::armem::gui::instance
}
}
-
- void TypedDataTreeBuilder::update(
- QTreeWidgetItem* item,
- const std::string& key,
- const aron::type::VariantPtr& type,
- const aron::data::VariantPtr& data)
+ void
+ TypedDataTreeBuilder::update(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::type::VariantPtr& type,
+ const aron::data::VariantPtr& data)
{
using namespace aron;
- const std::string value = data ? aron::TypedDataDisplayVisitor::getValue(type, data) : "(none)";
- std::string typeName = instance::sanitizeTypeName(type->getFullName());
+ const std::string value =
+ data ? aron::TypedDataDisplayVisitor::getValue(type, data) : "(none)";
+
+ std::string typeName;
+ if (type::IntEnumPtr enumType = type::IntEnum::DynamicCast(type))
+ {
+ typeName = enumType->getEnumName();
+ }
+ else
+ {
+ typeName = type->getFullName();
+ }
+ typeName = instance::sanitizeTypeName(typeName);
switch (type->getMaybe())
{
case aron::type::Maybe::OPTIONAL:
@@ -213,7 +224,9 @@ namespace armarx::armem::gui::instance
setRowTexts(item, key, value, typeName);
- item->setData(columnKey, Qt::UserRole, data ? instance::serializePath(data->getPath()) : QStringList());
+ item->setData(columnKey,
+ Qt::UserRole,
+ data ? instance::serializePath(data->getPath()) : QStringList());
item->setData(columnType, Qt::UserRole, static_cast<int>(type->getDescriptor()));
if (typeName == sanitizedMemoryIDTypeName)
@@ -228,50 +241,57 @@ namespace armarx::armem::gui::instance
MemoryID id = aron::fromAron<MemoryID>(dto);
memoryIDItem->setInstanceID(id);
- return; // Done, no recursion.
+ return; // Done, no recursion.
}
}
// We pass empty containers if data is null so that subitems of the data are deleted.
auto emptyDict = aron::data::Dict(type->getPath());
auto emptyList = aron::data::List(type->getPath());
- if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Object::DynamicCast(type))
+ if (const auto d = aron::data::Dict::DynamicCast(data);
+ const auto t = type::Object::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyDict);
}
- else if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::Dict::DynamicCast(type))
+ else if (const auto d = aron::data::Dict::DynamicCast(data);
+ const auto t = type::Dict::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyDict);
}
- else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::List::DynamicCast(type))
+ else if (const auto d = aron::data::List::DynamicCast(data);
+ const auto t = type::List::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyList);
}
- else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Pair::DynamicCast(type))
+ else if (const auto d = aron::data::List::DynamicCast(data);
+ const auto t = type::Pair::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyList);
}
- else if (const auto d = aron::data::List::DynamicCast(data); const auto t = type::Tuple::DynamicCast(type))
+ else if (const auto d = aron::data::List::DynamicCast(data);
+ const auto t = type::Tuple::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyList);
}
- else if (const auto d = aron::data::Dict::DynamicCast(data); const auto t = type::AnyObject::DynamicCast(type))
+ else if (const auto d = aron::data::Dict::DynamicCast(data);
+ const auto t = type::AnyObject::DynamicCast(type))
{
_updateTree(item, *t, d ? *d : emptyDict);
}
}
-
- void TypedDataTreeBuilder::update(QTreeWidgetItem* item,
- const std::string& key,
- const aron::data::VariantPtr& data)
+ void
+ TypedDataTreeBuilder::update(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::data::VariantPtr& data)
{
if (data)
{
this->setRowTexts(item, key, data);
- item->setData(columnKey, Qt::UserRole,
- data ? instance::serializePath(data->getPath()) : QStringList());
+ item->setData(columnKey,
+ Qt::UserRole,
+ data ? instance::serializePath(data->getPath()) : QStringList());
if (auto cast = aron::data::Dict::DynamicCast(data))
{
@@ -290,11 +310,11 @@ namespace armarx::armem::gui::instance
}
}
-
template <class DataT, class TypeT>
- void TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, DataT& data)
+ void
+ TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, DataT& data)
{
updateTree(item, type, data);
}
-}
+} // namespace armarx::armem::gui::instance
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/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 0b466d404fbc41d5eee48819418dd5b5cae35d1f..5e5b15f15d8135b7de89386c7ba95a7d65f29b9e 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -10,7 +10,6 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-
namespace armarx::armem::articulated_object
{
armem::articulated_object::ArticulatedObject
@@ -37,13 +36,14 @@ namespace armarx::armem::articulated_object
return armem::articulated_object::ArticulatedObject{
.description = {.name = obj.getType(),
- .xml = PackagePath(armarx::ArmarXDataPath::getProject(
- {package}, fileRelPath),
- obj.getFilename())},
+ .xml = PackagePath(
+ armarx::ArmarXDataPath::getProject({package}, fileRelPath),
+ obj.getFilename())},
.instance = obj.getName(),
.config = {.timestamp = timestamp,
.globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
- .jointMap = obj.getJointValues()},
+ .jointMap = obj.getJointValues(),
+ .proprioception = std::nullopt},
.timestamp = timestamp};
}
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 dd8a8473c2561c058c5754e59fb5997fdddec40e..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};
+ .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_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index a5822667f61138a40a9f581de8dc68fb7fd812d6..d6635696acd4f72d3c3342423233a459baf89af0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1119,6 +1119,12 @@ namespace armarx::armem::server::obj::instance
for (const auto& object : scene.objects)
{
+ if (simox::alg::starts_with(object.className, "#"))
+ {
+ // marked to be ignored
+ continue;
+ }
+
const ObjectID classID = object.getClassID(objectFinder);
objpose::ObjectPose& pose = objectPoses.emplace_back();
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
index 4ef29cf75b7e9a86727a4ab62041b16714742eb7..38ea8a81548cbb2fe52c02627493309f6339772f 100644
--- a/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
+++ b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
@@ -4,6 +4,9 @@
<CodeIncludes>
<SystemInclude include="<Eigen/Core>" />
</CodeIncludes>
+ <AronIncludes>
+ <PackagePath package="RobotAPI" path="libraries/armem_robot_state/aron/Proprioception.xml" />
+ </AronIncludes>
<GenerateTypes>
<Object name="armarx::armem::arondto::RobotState">
@@ -22,6 +25,9 @@
</Dict>
</ObjectChild>
+ <ObjectChild key='proprioception'>
+ <armarx::armem::arondto::Proprioception optional="true"/>
+ </ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem_robot/client/interfaces.h b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
index 4975e53e597f54b3f451c7075e368f00f692d343..64d68ed6a3549fa7b0e6fa5745a1cc5731158a38 100644
--- a/source/RobotAPI/libraries/armem_robot/client/interfaces.h
+++ b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
@@ -12,10 +12,10 @@ namespace armarx::armem::robot
public:
virtual ~ReaderInterface() = default;
- virtual bool synchronize(Robot& obj, const armem::Time& timestamp) = 0;
+ virtual bool synchronize(Robot& obj, const armem::Time& timestamp) const = 0;
- virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) = 0;
- virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) = 0;
+ virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) const = 0;
+ virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) const = 0;
};
class WriterInterface
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 2eeff803f8bdd243aeb275c004c6d08b0f31e701..9caf84f24574be547036e94fc0f07e50680d0fec 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -10,7 +10,7 @@
#include <ArmarXCore/core/time/DateTime.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::robot
{
@@ -60,6 +60,8 @@ namespace armarx::armem::robot
Pose globalPose;
JointMap jointMap;
+
+ std::optional<armarx::armem::arondto::Proprioception> proprioception;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index 8dda90015cd030197cd4af43fd2b9d534968fc19..c71d1d839e5511f73cb67b13d8c80c28def503d4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -63,7 +63,6 @@ armarx_enable_aron_file_generation_for_target(
"${LIB_NAME}"
ARON_FILES
- aron/JointState.xml
aron/Proprioception.xml
aron/Exteroception.xml
aron/TransformHeader.xml
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml b/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
deleted file mode 100644
index 5d18e8f78b6c99d58aab69ac2a154d5cb580bd63..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_state/aron/JointState.xml
+++ /dev/null
@@ -1,14 +0,0 @@
-<!--This class contains the data structure for ObjectPose -->
-<?xml version="1.0" encoding="UTF-8" ?>
-<AronTypeDefinition>
- <GenerateTypes>
- <Object name="armarx::armem::arondto::JointState">
- <ObjectChild key='name'>
- <String/>
- </ObjectChild>
- <ObjectChild key='position'>
- <float32 />
- </ObjectChild>
- </Object>
- </GenerateTypes>
-</AronTypeDefinition>
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/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 901d98dcb8f649b0968694f2eefc13888bde91cb..78014ea76bf070f08f01212719da1c1be7483fa3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -4,7 +4,6 @@
#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
@@ -51,23 +50,6 @@ namespace armarx::armem
aron::fromAron(dto.timestamp, bo.timestamp);
}
- /* JointState */
-
- void
- fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
- {
- aron::fromAron(dto.name, bo.name);
- aron::fromAron(dto.position, bo.position);
- }
-
- void
- toAron(arondto::JointState& dto, const robot_state::JointState& bo)
- {
- aron::toAron(dto.name, bo.name);
- aron::toAron(dto.position, bo.position);
- }
-
-
void
fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index f78efc200ee18205523ddf7ea5c60e8c6949bf9f..a3e821caac1379d7413160583822b792f8f7c453 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -61,9 +61,6 @@ namespace armarx::armem
void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo);
void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo);
- void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
- void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
-
void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
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 7699417369cb2dae5d6cc59b8a1d7fabcd0b9251..a8280d7e93ee5513449571a8c13d8ee086c9843d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -23,7 +23,6 @@
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
@@ -32,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)
{
}
@@ -46,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)
{
@@ -66,7 +65,7 @@ namespace armarx::armem::robot_state
}
std::optional<robot::Robot>
- RobotReader::get(const std::string& name, const armem::Time& timestamp)
+ RobotReader::get(const std::string& name, const armem::Time& timestamp) const
{
const auto description = queryDescription(name, timestamp);
@@ -80,14 +79,14 @@ namespace armarx::armem::robot_state
}
robot::Robot
- RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp)
+ RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp) const
{
robot::Robot robot{.description = description,
.instance = "", // TODO(fabian.reister):
.config = {}, // will be populated by synchronize
.timestamp = timestamp};
- synchronize(robot, timestamp);
+ ARMARX_CHECK(synchronize(robot, timestamp));
return robot;
}
@@ -106,7 +105,7 @@ namespace armarx::armem::robot_state
}
bool
- RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
+ RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp) const
{
const auto tsStartFunctionInvokation = armem::Time::Now();
@@ -136,7 +135,7 @@ namespace armarx::armem::robot_state
}
std::optional<robot::RobotDescription>
- RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp) const
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
@@ -184,14 +183,17 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState>
RobotReader::queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp)
+ const armem::Time& timestamp) const
{
- const auto jointMap = queryJointState(description, timestamp);
- if (not jointMap)
+ const auto proprioception = queryProprioception(description, timestamp);
+
+ if (not proprioception.has_value())
{
- ARMARX_VERBOSE << "Failed to query joint state for robot '" << description.name << "'.";
+ ARMARX_VERBOSE << "Failed to query proprioception for robot '" << description.name
+ << "'.";
return std::nullopt;
}
+ const auto jointMap = proprioception->joints.position;
const auto globalPose = queryGlobalPose(description, timestamp);
if (not globalPose)
@@ -200,8 +202,10 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return robot::RobotState{
- .timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
+ return robot::RobotState{.timestamp = timestamp,
+ .globalPose = *globalPose,
+ .jointMap = jointMap,
+ .proprioception = proprioception};
}
std::optional<::armarx::armem::robot_state::Transform>
@@ -231,10 +235,9 @@ namespace armarx::armem::robot_state
// }
}
-
- std::optional<robot::RobotState::JointMap>
- RobotReader::queryJointState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const // Why timestamp?!?!
+ std::optional<armarx::armem::arondto::Proprioception>
+ RobotReader::queryProprioception(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const // Why timestamp?!?!
{
// TODO(fabian.reister): how to deal with multiple providers?
@@ -263,7 +266,7 @@ namespace armarx::armem::robot_state
return std::nullopt;
}
- return getRobotJointState(qResult.memory, description.name);
+ return getRobotProprioception(qResult.memory, description.name);
}
catch (...)
{
@@ -304,7 +307,6 @@ namespace armarx::armem::robot_state
return getRobotJointStates(qResult.memory, description.name);
}
-
std::optional<robot::PlatformState>
RobotReader::queryPlatformState(const robot::RobotDescription& description,
const armem::Time& timestamp) const
@@ -415,19 +417,19 @@ namespace armarx::armem::robot_state
}
}
- std::optional<robot::RobotState::JointMap>
- RobotReader::getRobotJointState(const armarx::armem::wm::Memory& memory,
- const std::string& name) const
+ std::optional<armarx::armem::arondto::Proprioception>
+ RobotReader::getRobotProprioception(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const
{
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
.getCoreSegment(constants::proprioceptionCoreSegment);
// clang-format on
- std::optional<robot::RobotState::JointMap> jointMap;
+ std::optional<armarx::armem::arondto::Proprioception> proprioception;
coreSegment.forEachEntity(
- [&jointMap](const wm::Entity& entity)
+ [&proprioception](const wm::Entity& entity)
{
if (not entity.getLatestSnapshot().hasInstance(0))
{
@@ -436,25 +438,10 @@ namespace armarx::armem::robot_state
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto proprioception =
- tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
- ARMARX_CHECK(proprioception.has_value());
-
- const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
-
- // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
- // if (not jointState)
- // {
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
- // return;
- // }
-
- jointMap = joints.position;
-
- // jointMap.emplace(jointState->name, jointState->position);
+ proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
});
- return jointMap;
+ return proprioception;
}
RobotReader::JointTrajectory
@@ -497,7 +484,6 @@ namespace armarx::armem::robot_state
return jointTrajectory;
}
-
// force torque for left and right
std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
RobotReader::queryForceTorque(const robot::RobotDescription& description,
@@ -563,7 +549,6 @@ namespace armarx::armem::robot_state
return getForceTorques(qResult.memory, description.name);
}
-
std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
RobotReader::queryToF(const robot::RobotDescription& description,
const armem::Time& timestamp) const
@@ -594,7 +579,6 @@ namespace armarx::armem::robot_state
return getToF(qResult.memory, description.name);
}
-
std::optional<robot::PlatformState>
RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
@@ -755,7 +739,6 @@ namespace armarx::armem::robot_state
return tofs;
}
-
std::optional<robot::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
const std::string& name) const
@@ -800,7 +783,7 @@ namespace armarx::armem::robot_state
}
std::vector<robot::RobotDescription>
- RobotReader::queryDescriptions(const armem::Time& timestamp)
+ RobotReader::queryDescriptions(const armem::Time& timestamp) const
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
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 ee1274fab20cadb62231bc7fdd685c408104e705..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,31 +43,33 @@ 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) 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) override;
+ const armem::Time& timestamp) const override;
robot::Robot get(const robot::RobotDescription& description,
- const armem::Time& timestamp) override;
+ const armem::Time& timestamp) const override;
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp);
+ const armem::Time& timestamp) const;
- std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
+ std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp) const;
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp);
+ const armem::Time& timestamp) const;
- std::optional<robot::RobotState::JointMap>
- queryJointState(const robot::RobotDescription& description,
- const armem::Time& timestamp) const;
+ std::optional<armarx::armem::arondto::Proprioception>
+ queryProprioception(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
@@ -134,8 +135,9 @@ namespace armarx::armem::robot_state
std::vector<robot::RobotDescription>
getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
- std::optional<robot::RobotState::JointMap>
- getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::optional<armarx::armem::arondto::Proprioception>
+ 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 cfa84fb3f50ad95db2f82b29114cd2606250a2c4..e92e13d6421c71313bdd04a488e1f43397d1f222 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
@@ -24,7 +24,6 @@
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/TransformHeader.aron.generated.h>
@@ -35,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 << "' ...";
@@ -66,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 416585111bfbd96cca816211285faf9ea3715792..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)
+ 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 a80d9b02871c65998c75c3ee5522ef4f71af9a06..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,14 +40,14 @@ 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,
- const armem::Time& timestamp);
+ const armem::Time& timestamp) const;
[[nodiscard]] VirtualRobot::RobotPtr
getRobot(const std::string& name,
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 47519748cad8fe1c5a0ca37db35f215e0359197b..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()};
-
- 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 abd89ebd82a7d30d19c1945444d1dd8f5d650bfd..9b247cb3973cf28befe37289a3ff42160133f21b 100644
--- a/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
+++ b/source/RobotAPI/libraries/armem_skills/aron/Skill.xml
@@ -11,72 +11,118 @@ The memory should look like the following:
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<GenerateTypes>
- <Object name='armarx::skills::arondto::SkillDescription'>
+ <Object name='armarx::skills::arondto::ProviderID'>
+ <ObjectChild key='providerName'>
+ <String />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::skills::arondto::SkillID'>
+ <ObjectChild key='providerId'>
+ <armarx::skills::arondto::ProviderID />
+ </ObjectChild>
<ObjectChild key='skillName'>
<String />
</ObjectChild>
+ </Object>
- <ObjectChild key='robots'>
- <List>
- <String />
- </List>
+
+ <!-- TODO -->
+ <Object name='armarx::skills::arondto::SkillProfile'>
+
+ <ObjectChild key='profileName'>
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key='predecessorProfileName'>
+ <String />
+ </ObjectChild>
+
+ <ObjectChild key='parameterization'>
+ <AnyObject shared_ptr="1" />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::skills::arondto::SkillDescription'>
+ <ObjectChild key='skillId'>
+ <armarx::skills::arondto::SkillID />
</ObjectChild>
<ObjectChild key='description'>
<String />
</ObjectChild>
+ <ObjectChild key='rootProfileParameterization'>
+ <AnyObject shared_ptr="1" />
+ </ObjectChild>
+
<ObjectChild key='iceInfo'>
<String />
</ObjectChild>
- <ObjectChild key='timeoutMs'>
- <int64 />
+ <ObjectChild key='timeout'>
+ <Duration />
+ </ObjectChild>
+
+ <ObjectChild key='parametersType'>
+ <AnyObject shared_ptr="1" />
</ObjectChild>
- <ObjectChild key='acceptedType'>
+ <ObjectChild key='resultType'>
<AnyObject shared_ptr="1" />
</ObjectChild>
</Object>
- <Object name='armarx::skills::arondto::SkillExecutionRequest'>
+ <!-- TODO -->
+ <Object name='armarx::skills::arondto::CompositeSkillExecutionParams'>
<ObjectChild key='providerName'>
- <String />
+ <List>
+ <String /> <!--<armarx::skills::arondto::SkillExecutionInfo />-->
+ </List>
</ObjectChild>
+ </Object>
- <ObjectChild key='skillName'>
- <String />
+ <Object name='armarx::skills::arondto::SkillExecutionRequest'>
+
+ <ObjectChild key='skillId'>
+ <armarx::skills::arondto::SkillID />
</ObjectChild>
<ObjectChild key='executorName'>
<String />
</ObjectChild>
- <ObjectChild key='params'>
+ <ObjectChild key='parameters'>
<AnyObject shared_ptr="1" />
</ObjectChild>
-
</Object>
- <Object name='armarx::skills::arondto::SkillExecutionEvent'>
- <ObjectChild key='providerName'>
- <String />
+ <Object name='armarx::skills::arondto::SkillStatusUpdate'>
+
+ <ObjectChild key='skillId'>
+ <armarx::skills::arondto::SkillID />
</ObjectChild>
- <ObjectChild key='skillName'>
+ <ObjectChild key='executorName'>
<String />
</ObjectChild>
+ <ObjectChild key='executionStartedTimestamp'>
+ <DateTime />
+ </ObjectChild>
+
<ObjectChild key='status'>
<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 1d8383b68a8ce2620a48718ed7584814c29b276a..3b1d8925fc6694ba7117a4ce3e0db07ecab68586 100644
--- a/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_skills/aron_conversions.cpp
@@ -1,26 +1,50 @@
#include "aron_conversions.h"
+#include <RobotAPI/libraries/aron/converter/datatype/DatatypeConverterVisitor.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
namespace armarx::armem
{
- std::map<armarx::eStateType, armarx::skills::arondto::Statechart::StateType> toAronStateTypeMap =
- {
- {eNormalState, (armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::NORMAL},
- {eFinalState, (armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::FINAL},
- {eRemoteState, (armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::REMOTE},
- {eDynamicRemoteState, (armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::DYNAMIC_REMOTE},
- {eUndefined, (armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::UNDEFINED},
+ std::map<armarx::eStateType, armarx::skills::arondto::Statechart::StateType>
+ toAronStateTypeMap = {
+ {eNormalState,
+ (armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::NORMAL},
+ {eFinalState,
+ (armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::FINAL},
+ {eRemoteState,
+ (armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::REMOTE},
+ {eDynamicRemoteState,
+ (armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::DYNAMIC_REMOTE},
+ {eUndefined,
+ (armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::UNDEFINED},
};
- std::map<armarx::skills::arondto::Statechart::StateType, armarx::eStateType> fromAronStateTypeMap =
- {
- {(armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::NORMAL, eNormalState},
- {(armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::FINAL, eFinalState},
- {(armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::REMOTE, eRemoteState},
- {(armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::DYNAMIC_REMOTE, eDynamicRemoteState},
- {(armarx::skills::arondto::Statechart::StateType) armarx::skills::arondto::Statechart::StateType::UNDEFINED, eUndefined},
+ std::map<armarx::skills::arondto::Statechart::StateType, armarx::eStateType>
+ fromAronStateTypeMap = {
+ {(armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::NORMAL,
+ eNormalState},
+ {(armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::FINAL,
+ eFinalState},
+ {(armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::REMOTE,
+ eRemoteState},
+ {(armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::DYNAMIC_REMOTE,
+ eDynamicRemoteState},
+ {(armarx::skills::arondto::Statechart::StateType)
+ armarx::skills::arondto::Statechart::StateType::UNDEFINED,
+ eUndefined},
};
- void fromAron(const skills::arondto::Statechart::StateType& dto, eStateType& bo)
+ void
+ fromAron(const skills::arondto::Statechart::StateType& dto, eStateType& bo)
{
if (fromAronStateTypeMap.find(dto) != fromAronStateTypeMap.end())
{
@@ -32,7 +56,8 @@ namespace armarx::armem
}
}
- void toAron(skills::arondto::Statechart::StateType& dto, const eStateType& bo)
+ void
+ toAron(skills::arondto::Statechart::StateType& dto, const eStateType& bo)
{
if (toAronStateTypeMap.find(bo) != toAronStateTypeMap.end())
{
@@ -44,7 +69,8 @@ namespace armarx::armem
}
}
- void fromAron(const skills::arondto::Statechart::ParameterMap& dto, StateParameterMap& bo)
+ void
+ fromAron(const skills::arondto::Statechart::ParameterMap& dto, StateParameterMap& bo)
{
// todo: implement
// for (auto const& [key, val] : dto.parameters)
@@ -54,7 +80,8 @@ namespace armarx::armem
// }
}
- void toAron(skills::arondto::Statechart::ParameterMap& dto, const StateParameterMap& bo)
+ void
+ toAron(skills::arondto::Statechart::ParameterMap& dto, const StateParameterMap& bo)
{
for (auto const& [key, val] : bo)
{
@@ -62,7 +89,9 @@ namespace armarx::armem
}
}
- void fromAron(const skills::arondto::Statechart::Transition& dto, ProfilerStatechartTransitionWithParameters& bo)
+ void
+ fromAron(const skills::arondto::Statechart::Transition& dto,
+ ProfilerStatechartTransitionWithParameters& bo)
{
bo.processId = dto.processId;
bo.sourceStateIdentifier = dto.sourceStateIdentifier;
@@ -74,7 +103,9 @@ namespace armarx::armem
fromAron(dto.outputParameters, bo.outputParameters);
}
- void toAron(skills::arondto::Statechart::Transition& dto, const ProfilerStatechartTransitionWithParameters& bo)
+ void
+ toAron(skills::arondto::Statechart::Transition& dto,
+ const ProfilerStatechartTransitionWithParameters& bo)
{
dto.processId = bo.processId;
dto.sourceStateIdentifier = bo.sourceStateIdentifier;
@@ -85,4 +116,122 @@ namespace armarx::armem
toAron(dto.localParameters, bo.localParameters);
toAron(dto.outputParameters, bo.outputParameters);
}
-}
+
+ void
+ fromAron(const armarx::skills::arondto::ProviderID& dto, skills::ProviderID& bo)
+ {
+ bo.providerName = dto.providerName;
+ }
+
+ void
+ toAron(armarx::skills::arondto::ProviderID& dto, const skills::ProviderID& bo)
+ {
+ dto.providerName = bo.providerName;
+ }
+
+ void
+ fromAron(const armarx::skills::arondto::SkillID& dto, skills::SkillID& bo)
+ {
+ bo.skillName = dto.skillName;
+ bo.providerId = skills::ProviderID{.providerName = ""};
+ fromAron(dto.providerId, *bo.providerId);
+ }
+
+ void
+ toAron(armarx::skills::arondto::SkillID& dto, const skills::SkillID& bo)
+ {
+ dto.skillName = bo.skillName;
+ dto.providerId.resetHard();
+ if (bo.providerId.has_value())
+ {
+ toAron(dto.providerId, *bo.providerId);
+ }
+ }
+
+ void
+ fromAron(const armarx::skills::arondto::SkillDescription& dto, skills::SkillDescription& bo)
+ {
+ fromAron(dto.skillId, bo.skillId);
+ bo.description = dto.description;
+ bo.timeout = dto.timeout;
+ bo.rootProfileDefaults = dto.rootProfileParameterization;
+ if (dto.parametersType)
+ {
+ throw armarx::LocalException("Not implemented yet");
+ }
+ }
+
+ void
+ toAron(armarx::skills::arondto::SkillDescription& dto, const skills::SkillDescription& bo)
+ {
+ toAron(dto.skillId, bo.skillId);
+ dto.description = bo.description;
+ dto.timeout = bo.timeout;
+ dto.rootProfileParameterization = bo.rootProfileDefaults;
+ if (bo.parametersType)
+ {
+ aron::type::converter::AronDatatypeConverterVisitor c;
+ aron::type::visit(c, bo.parametersType);
+
+ dto.parametersType = aron::data::Dict::DynamicCastAndCheck(c.latest);
+ }
+ }
+
+ void
+ fromAron(const armarx::skills::arondto::SkillExecutionRequest& dto,
+ skills::SkillExecutionRequest& bo)
+ {
+ fromAron(dto.skillId, bo.skillId);
+ bo.executorName = dto.executorName;
+ bo.parameters = dto.parameters;
+ }
+
+ void
+ toAron(armarx::skills::arondto::SkillExecutionRequest& dto,
+ const skills::SkillExecutionRequest& bo)
+ {
+ toAron(dto.skillId, bo.skillId);
+ dto.executorName = bo.executorName;
+ dto.parameters = bo.parameters;
+ }
+
+ void
+ fromAron(const armarx::skills::arondto::SkillStatusUpdate& dto, skills::SkillStatusUpdate& bo)
+ {
+ static std::map<std::string, skills::SkillStatus> map{
+ {"Constructing", skills::SkillStatus::Constructing},
+ {"Initializing", skills::SkillStatus::Initializing},
+ {"Preparing", skills::SkillStatus::Preparing},
+ {"Running", skills::SkillStatus::Running},
+ {"Failed", skills::SkillStatus::Failed},
+ {"Aborted", skills::SkillStatus::Aborted},
+ {"Succeeded", skills::SkillStatus::Succeeded}};
+
+ fromAron(dto.skillId, bo.executionId.skillId);
+ bo.executionId.executionStartedTime = dto.executionStartedTimestamp;
+ bo.executionId.executorName = dto.executorName;
+ bo.parameters = dto.parameters;
+ bo.result = dto.result;
+ bo.status = map.at(dto.status);
+ }
+
+ void
+ toAron(armarx::skills::arondto::SkillStatusUpdate& dto, const skills::SkillStatusUpdate& bo)
+ {
+ static std::map<skills::SkillStatus, std::string> map{
+ {skills::SkillStatus::Constructing, "Constructing"},
+ {skills::SkillStatus::Initializing, "Initializing"},
+ {skills::SkillStatus::Preparing, "Preparing"},
+ {skills::SkillStatus::Running, "Running"},
+ {skills::SkillStatus::Failed, "Failed"},
+ {skills::SkillStatus::Aborted, "Aborted"},
+ {skills::SkillStatus::Succeeded, "Succeeded"}};
+
+ toAron(dto.skillId, bo.executionId.skillId);
+ dto.executorName = bo.executionId.executorName;
+ dto.executionStartedTimestamp = bo.executionId.executionStartedTime;
+ 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 37092255c9dd47c5204907a23128be857456d146..38ffbe0c43ecc492cf76398abdd2f3cad122db7c 100644
--- a/source/RobotAPI/libraries/armem_skills/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_skills/aron_conversions.h
@@ -3,16 +3,45 @@
#include <ArmarXCore/interface/core/Profiler.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
+#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
namespace armarx::armem
{
- void fromAron(const armarx::skills::arondto::Statechart::StateType& dto, armarx::eStateType& bo);
+ void fromAron(const armarx::skills::arondto::Statechart::StateType& dto,
+ armarx::eStateType& bo);
void toAron(armarx::skills::arondto::Statechart::StateType& dto, const armarx::eStateType& bo);
- void fromAron(const armarx::skills::arondto::Statechart::ParameterMap& dto, armarx::StateParameterMap& bo);
- void toAron(armarx::skills::arondto::Statechart::ParameterMap& dto, const armarx::StateParameterMap& bo);
+ void fromAron(const armarx::skills::arondto::Statechart::ParameterMap& dto,
+ armarx::StateParameterMap& bo);
+ void toAron(armarx::skills::arondto::Statechart::ParameterMap& dto,
+ const armarx::StateParameterMap& bo);
- void fromAron(const armarx::skills::arondto::Statechart::Transition& dto, armarx::ProfilerStatechartTransitionWithParameters& bo);
- void toAron(armarx::skills::arondto::Statechart::Transition& dto, const armarx::ProfilerStatechartTransitionWithParameters& bo);
-}
+ void fromAron(const armarx::skills::arondto::Statechart::Transition& dto,
+ armarx::ProfilerStatechartTransitionWithParameters& bo);
+ void toAron(armarx::skills::arondto::Statechart::Transition& dto,
+ const armarx::ProfilerStatechartTransitionWithParameters& bo);
+
+ void fromAron(const armarx::skills::arondto::ProviderID& dto, skills::ProviderID& bo);
+ void toAron(armarx::skills::arondto::ProviderID& dto, const skills::ProviderID& bo);
+
+ void fromAron(const armarx::skills::arondto::SkillID& dto, skills::SkillID& bo);
+ void toAron(armarx::skills::arondto::SkillID& dto, const skills::SkillID& bo);
+
+ void fromAron(const armarx::skills::arondto::SkillDescription& dto,
+ skills::SkillDescription& bo);
+ void toAron(armarx::skills::arondto::SkillDescription& dto, const skills::SkillDescription& bo);
+
+ void fromAron(const armarx::skills::arondto::SkillExecutionRequest& dto,
+ skills::SkillExecutionRequest& bo);
+ void toAron(armarx::skills::arondto::SkillExecutionRequest& dto,
+ const skills::SkillExecutionRequest& bo);
+
+ void fromAron(const armarx::skills::arondto::SkillStatusUpdate& dto,
+ skills::SkillStatusUpdate& bo);
+ void toAron(armarx::skills::arondto::SkillStatusUpdate& dto,
+ const skills::SkillStatusUpdate& bo);
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
index 89e7e1c92af94e46c2493a5c18825c94b0551ac7..d2f99544ee1b2bde93a9c8609f0f100ac2baaf4f 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.cpp
@@ -2,8 +2,11 @@
#include <SimoxUtility/algorithm/string.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
+#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/datatype/DatatypeConverterVisitor.h>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
@@ -29,32 +32,16 @@ namespace armarx::skills::segment
}
void
- ExecutableSkillLibraryCoreSegment::addSkillProvider(
- const skills::manager::dto::ProviderInfo& info)
+ ExecutableSkillLibraryCoreSegment::addSkillProvider(const skills::ProviderInfo& info)
{
// add skills
auto skills = info.providedSkills;
+ auto provId = id().withProviderSegmentName(info.providerId.providerName);
- auto provId = id().withProviderSegmentName(info.providerName);
-
- for (const auto& [key, desc] : skills)
+ for (const auto& [s, d] : skills)
{
armarx::skills::arondto::SkillDescription skillDescription;
- skillDescription.skillName = desc.skillName;
- skillDescription.description = desc.description;
- skillDescription.iceInfo = info.provider->ice_toString();
- skillDescription.robots = desc.robots;
- skillDescription.timeoutMs = desc.timeoutMs;
-
- if (desc.acceptedType)
- {
- auto t = aron::type::Object::FromAronObjectDTO(desc.acceptedType);
-
- aron::type::converter::AronDatatypeConverterVisitor c;
- aron::type::visit(c, t);
-
- skillDescription.acceptedType = aron::data::Dict::DynamicCastAndCheck(c.latest);
- }
+ armem::toAron(skillDescription, d);
armem::Commit commit;
auto& entityUpdate = commit.add();
@@ -63,7 +50,7 @@ namespace armarx::skills::segment
entityUpdate.sentTime = armem::Time::Now();
entityUpdate.arrivedTime = armem::Time::Now();
entityUpdate.instancesData = {skillDescription.toAron()};
- entityUpdate.entityID = provId.withEntityName(skillDescription.skillName);
+ entityUpdate.entityID = provId.withEntityName(d.skillId.skillName);
// Commit data to memory and notify
iceMemory.commit(commit);
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h
index a2d4227bb2c0789c028b31ef47beb21b68891ee4..e71a8a74ab714be9c9c7cd1faafb084a105e4576 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/ExecutableSkillLibrarySegment.h
@@ -4,19 +4,18 @@
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
// ArmarX
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/interface/core/Profiler.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
+#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
namespace armarx::skills::segment
{
- class ExecutableSkillLibraryCoreSegment :
- public armem::server::segment::SpecializedCoreSegment
+ class ExecutableSkillLibraryCoreSegment : public armem::server::segment::SpecializedCoreSegment
{
using Base = armem::server::segment::SpecializedCoreSegment;
@@ -25,10 +24,10 @@ namespace armarx::skills::segment
ExecutableSkillLibraryCoreSegment(armem::server::MemoryToIceAdapter& iceMemory);
- void defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix);
+ void defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix);
void init();
- void addSkillProvider(const skills::manager::dto::ProviderInfo& info);
+ void addSkillProvider(const skills::ProviderInfo& info);
void removeSkillProvider(const std::string& providerName);
size_t size() const;
@@ -36,4 +35,4 @@ namespace armarx::skills::segment
private:
std::map<std::string, std::map<std::string, skills::manager::dto::ProviderInfo>> skills;
};
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
index 1fa0989e8938206462674f93d9646f48b065123f..11dc8d0dfb40953de6acc8f81921884808339376 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
@@ -1,53 +1,44 @@
#include "SkillEventSegment.h"
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <SimoxUtility/algorithm/string.h>
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
+#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
namespace armarx::skills::segment
{
- SkillEventCoreSegment::SkillEventCoreSegment(armem::server::MemoryToIceAdapter& iceMemory):
- Base(iceMemory, CoreSegmentName, armarx::skills::arondto::SkillExecutionRequest::ToAronType())
+ SkillEventCoreSegment::SkillEventCoreSegment(armem::server::MemoryToIceAdapter& iceMemory) :
+ Base(iceMemory,
+ CoreSegmentName,
+ armarx::skills::arondto::SkillExecutionRequest::ToAronType())
{
}
- void SkillEventCoreSegment::defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix)
+ void
+ SkillEventCoreSegment::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
{
// No properties! (meaning no name and no max size)
}
- void SkillEventCoreSegment::init()
+ void
+ SkillEventCoreSegment::init()
{
Base::init();
}
- void SkillEventCoreSegment::addSkillUpdateEvent(const skills::provider::dto::SkillStatusUpdate& update)
+ void
+ SkillEventCoreSegment::addSkillUpdateEvent(const skills::SkillStatusUpdate& update)
{
- // add update for skill to memory
- static std::map<armarx::skills::provider::dto::Execution::Status, std::string> ExecutionStatus2String = {
- {armarx::skills::provider::dto::Execution::Status::Idle, "Idle"},
- {armarx::skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
- {armarx::skills::provider::dto::Execution::Status::Running, "Running"},
- {armarx::skills::provider::dto::Execution::Status::Aborted, "Aborted"},
- {armarx::skills::provider::dto::Execution::Status::Failed, "Failed"},
- {armarx::skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}
- };
-
// create commit about new update
- armarx::skills::arondto::SkillExecutionEvent event;
- event.providerName = update.header.skillId.providerName;
- event.skillName = update.header.skillId.skillName;
- event.status = ExecutionStatus2String.at(update.header.status);
- event.params = aron::data::Dict::FromAronDictDTO(update.header.usedParams);
- event.data = aron::data::Dict::FromAronDictDTO(update.data);
+ armarx::skills::arondto::SkillStatusUpdate event;
+ armem::toAron(event, update);
armem::MemoryID commitId = id();
- commitId.providerSegmentName = event.providerName;
- commitId.entityName = event.skillName;
+ commitId.providerSegmentName = event.skillId.providerId.providerName;
+ commitId.entityName = event.skillId.skillName;
auto aron = event.toAron();
@@ -55,9 +46,65 @@ namespace armarx::skills::segment
auto& entityUpdate = comm.add();
entityUpdate.confidence = 1.0;
entityUpdate.referencedTime = armem::Time::Now();
- entityUpdate.instancesData = { aron };
+ entityUpdate.instancesData = {aron};
entityUpdate.entityID = commitId;
- iceMemory.commit(comm);
+ iceMemory.commitLocking(comm);
+ }
+
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
+ SkillEventCoreSegment::getSkillStatusUpdates()
+ {
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> ret;
+ 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 (auto it = ret.find(up.executionId); it != ret.end() && up < it->second)
+ {
+ return;
+ }
+
+ // set or replace
+ ret[up.executionId] = up;
+ });
+
+ // for (const auto& [k, v] : ret)
+ // {
+ // ARMARX_IMPORTANT << "Skill " << k.skillId << " has stati: " << int(v.status);
+ // }
+
+ return ret;
+ }
+
+ 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 93df4bf7a429a459ccd51603af035d96f3f9e3d6..0537cbdbdf97d80649e10baa30786d2d1fb5870c 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.h
@@ -8,13 +8,12 @@
// ArmarX
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
namespace armarx::skills::segment
{
- class SkillEventCoreSegment :
- public armem::server::segment::SpecializedCoreSegment
+ class SkillEventCoreSegment : public armem::server::segment::SpecializedCoreSegment
{
using Base = armem::server::segment::SpecializedCoreSegment;
@@ -23,11 +22,16 @@ namespace armarx::skills::segment
SkillEventCoreSegment(armem::server::MemoryToIceAdapter& iceMemory);
- void defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix);
+ void defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix);
void init();
- void addSkillUpdateEvent(const skills::provider::dto::SkillStatusUpdate& update);
+ void addSkillUpdateEvent(const skills::SkillStatusUpdate& update);
+
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> getSkillStatusUpdates();
+
+ std::optional<skills::SkillStatusUpdate>
+ getSkillStatusUpdate(const skills::SkillExecutionID& id);
private:
};
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
index 818f3d13bb4315ebf69909c744c042b1ac268a39..833deb3f68144a10a71812f9eb39dbec455d9c0c 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.cpp
@@ -1,33 +1,36 @@
#include "SkillExecutionRequestSegment.h"
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <SimoxUtility/algorithm/string.h>
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_skills/aron/Skill.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_skills/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
namespace armarx::skills::segment
{
- SkillExecutionRequestCoreSegment::SkillExecutionRequestCoreSegment(armem::server::MemoryToIceAdapter& iceMemory):
+ SkillExecutionRequestCoreSegment::SkillExecutionRequestCoreSegment(
+ armem::server::MemoryToIceAdapter& iceMemory) :
Base(iceMemory, CoreSegmentName, skills::arondto::SkillExecutionRequest::ToAronType())
{
}
- void SkillExecutionRequestCoreSegment::defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix)
+ void
+ SkillExecutionRequestCoreSegment::defineProperties(PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
// No properties! (meaning no name and no max size)
}
- void SkillExecutionRequestCoreSegment::init()
+ void
+ SkillExecutionRequestCoreSegment::init()
{
Base::init();
}
-
- skills::manager::dto::SkillExecutionRequest SkillExecutionRequestCoreSegment::convertCommit(const aron::data::dto::DictPtr& commitData)
+ skills::SkillExecutionRequest
+ SkillExecutionRequestCoreSegment::convertCommit(const aron::data::dto::DictPtr& commitData)
{
// convert ice commitData to aron
auto commitDataAron = std::make_shared<aron::data::Dict>(commitData);
@@ -36,23 +39,22 @@ namespace armarx::skills::segment
skills::arondto::SkillExecutionRequest request;
request.fromAron(commitDataAron);
- skills::manager::dto::SkillExecutionRequest info;
- info.skillId = {request.providerName, request.skillName};
- info.params = request.params->toAronDictDTO();
+ skills::SkillExecutionRequest info{
+ .skillId = SkillID{.providerId = ProviderID{.providerName = ""}, .skillName = ""},
+ .executorName = ""};
+ armem::fromAron(request, info);
return info;
}
-
- void SkillExecutionRequestCoreSegment::addSkillExecutionRequest(const skills::manager::dto::SkillExecutionRequest& info)
+ void
+ SkillExecutionRequestCoreSegment::addSkillExecutionRequest(
+ const skills::SkillExecutionRequest& info)
{
// override directly execution to add a request to the memory
armem::Commit comm;
skills::arondto::SkillExecutionRequest request;
- request.executorName = info.executorName;
- request.providerName = info.skillId.providerName;
- request.skillName = info.skillId.skillName;
- request.params = aron::data::Dict::FromAronDictDTO(info.params);
+ armem::toAron(request, info);
auto aron = request.toAron();
@@ -60,11 +62,11 @@ namespace armarx::skills::segment
auto& entityUpdate = comm.add();
armem::MemoryID skillExecutionMemID = id();
- skillExecutionMemID.providerSegmentName = request.providerName;
- skillExecutionMemID.entityName = request.skillName;
+ skillExecutionMemID.providerSegmentName = request.skillId.providerId.providerName;
+ skillExecutionMemID.entityName = request.skillId.skillName;
entityUpdate.entityID = skillExecutionMemID;
- entityUpdate.instancesData = { aron };
+ entityUpdate.instancesData = {aron};
entityUpdate.confidence = 1.0;
entityUpdate.referencedTime = armem::Time::Now();
}
@@ -77,7 +79,7 @@ namespace armarx::skills::segment
skillExecutionMemID.entityName = "All Skill Execution Requests";
entityUpdate.entityID = skillExecutionMemID;
- entityUpdate.instancesData = { aron };
+ entityUpdate.instancesData = {aron};
entityUpdate.confidence = 1.0;
entityUpdate.referencedTime = armem::Time::Now();
}
@@ -85,4 +87,4 @@ namespace armarx::skills::segment
iceMemory.commit(comm);
}
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h
index 262bc829b6906061e92e725f88a00b6bec6ed0c6..c5af9dd8a64cff67a2e2088f1c714718f3126203 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillExecutionRequestSegment.h
@@ -4,17 +4,17 @@
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
// ArmarX
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/interface/core/Profiler.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
namespace armarx::skills::segment
{
- class SkillExecutionRequestCoreSegment :
- public armem::server::segment::SpecializedCoreSegment
+ class SkillExecutionRequestCoreSegment : public armem::server::segment::SpecializedCoreSegment
{
using Base = armem::server::segment::SpecializedCoreSegment;
@@ -23,11 +23,11 @@ namespace armarx::skills::segment
SkillExecutionRequestCoreSegment(armem::server::MemoryToIceAdapter& iceMemory);
- void defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix);
+ void defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix);
void init();
- skills::manager::dto::SkillExecutionRequest convertCommit(const aron::data::dto::DictPtr& commitData);
+ skills::SkillExecutionRequest convertCommit(const aron::data::dto::DictPtr& commitData);
- void addSkillExecutionRequest(const skills::manager::dto::SkillExecutionRequest& info);
+ void addSkillExecutionRequest(const skills::SkillExecutionRequest& info);
};
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
index 66b3c77d68c4cdf598b4712819c618249e49cdb8..2c5a6d1d30bd297e35a4b5592a8bf7e32ef8abcb 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
@@ -115,8 +115,8 @@ namespace armarx::aron::codegenerator::cpp::generator
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
block_if_data->addLine(
- variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({" + cppAccessor +
- nextEl() + "width, " + cppAccessor + nextEl() + "height, " +
+ variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({ static_cast<int>(" + cppAccessor +
+ nextEl() + "width), static_cast<int>(" + cppAccessor + nextEl() + "height), " +
std::to_string(std::get<1>(VoxelType2Cpp.at(type.getVoxelType()))) + "}, " + "\"" +
std::get<0>(VoxelType2Cpp.at(type.getVoxelType())) + "\", " +
"reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() +
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
index be011068dffcabda87b34bc3da51d6e8b432a2e1..27ab8bff523e82ccd52f02bede86a33b51e79225 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
@@ -67,6 +67,7 @@ namespace armarx::aron::framed
{
dto.header.frame = bo.frame;
dto.header.agent = bo.agent;
+ dto.pose = Eigen::Matrix4f::Identity();
Eigen::Vector3f vec;
vec.x() = bo.position->x;
vec.y() = bo.position->y;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index 21235afefb794e8c78443e840a240f56c6720b14..1d85a7f800b038befc8dedd21561c133a1f13439 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -193,6 +193,36 @@ namespace armarx::aron::data
setElementCopy(key, data);
}
+ void
+ Dict::mergeAndReplace(const DictPtr& d)
+ {
+ if (d == nullptr)
+ {
+ return;
+ }
+
+ // merge and overwrite
+ for (const auto& [k, v] : getElements())
+ {
+ this->setElement(k, v);
+ }
+ }
+
+ void
+ Dict::mergeAndReplaceCopy(const DictPtr& d)
+ {
+ if (d == nullptr)
+ {
+ return;
+ }
+
+ // merge and overwrite
+ for (const auto& [k, v] : getElements())
+ {
+ this->setElementCopy(k, v);
+ }
+ }
+
bool
Dict::hasElement(const std::string& key) const
{
@@ -231,8 +261,9 @@ namespace armarx::aron::data
const auto& p = data->getPath();
if (not p.hasDirectPrefix(this->getPath()))
{
- ARMARX_WARNING << "An element added to a dict does not have a correct path set. This "
- "may cause errors. Please use setElemetCopy() instead.";
+ ARMARX_WARNING
+ << "An element added to a dict does not have a correct path set. This "
+ "may cause errors. Please use setElemetCopy() instead.";
}
}
@@ -316,65 +347,68 @@ namespace armarx::aron::data
{
ARMARX_TRACE;
auto objectTypeNav = type::Object::DynamicCastAndCheck(type);
- for (const auto& [key, nav] : childrenNavigators)
+
+ // here we need to iterate over the elements of the type. That must be fulfilled.
+ // If this dict has more members that its fine.
+ for (const auto& [key, childTypeNav] : objectTypeNav->getMemberTypes())
{
- if (!objectTypeNav->hasMemberType(key))
+ if (!this->hasElement(key))
{
- ARMARX_TRACE;
- return false;
+ return false; // key must exist
}
- auto childTypeNav = objectTypeNav->getMemberType(key);
- if (!nav)
+ if (!childTypeNav)
{
- ARMARX_TRACE;
- if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
+ continue; // no information whih is fine --> continue
+ }
+
+ auto childNav = this->getElement(key);
+ if (childNav)
+ {
+ if (not childNav->fullfillsType(childTypeNav))
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return true;
+ // else childnav fulfills type which is fine --> continue
+ continue;
}
- if (!nav->fullfillsType(childTypeNav))
+ else
{
- ARMARX_TRACE;
- return false;
+ if (childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ return false;
+ }
+ // else: childTypeNav == maybe && nav == null which is fine --> continue
+ continue;
}
}
- ARMARX_TRACE;
return true;
}
case type::Descriptor::DICT:
{
ARMARX_TRACE;
auto dictTypeNav = type::Dict::DynamicCastAndCheck(type);
- for (const auto& [key, nav] : childrenNavigators)
+ for (const auto& [key, childNav] : childrenNavigators)
{
(void)key;
auto childTypeNav = dictTypeNav->getAcceptedType();
- if (!nav)
+ if (!childNav)
{
- ARMARX_TRACE;
if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return true;
+ // else childTypeNav is null or maybe and childNav is null which is fine.
+ continue;
}
- if (!nav->fullfillsType(childTypeNav))
+ if (!childNav->fullfillsType(childTypeNav))
{
- ARMARX_TRACE;
return false;
}
}
- ARMARX_TRACE;
return true;
}
default:
- ARMARX_TRACE;
return false;
}
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
index f6c2d93955f53af91a04364ada5f3da724ae7033..a49add482dc4e7e6f1a1d5308d76e571785dbe73 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
@@ -73,6 +73,9 @@ namespace armarx::aron::data
VariantPtr getElement(const std::string&) const;
std::map<std::string, VariantPtr> getElements() const;
+ void mergeAndReplace(const DictPtr& d);
+ void mergeAndReplaceCopy(const DictPtr& d);
+
VariantPtr at(const std::string&) const;
void removeElement(const std::string& key);
void clear();
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
index 83e23f76f43ca4a7ebf8b057c51142256b5ba618..82955eabfa8414f5e984c6ad0de704d7cab09a00 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
@@ -271,59 +271,51 @@ namespace armarx::aron::data
{
if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return true;
+ continue;
}
if (!nav->fullfillsType(childTypeNav))
{
- ARMARX_TRACE;
return false;
}
}
- ARMARX_TRACE;
return true;
}
case type::Descriptor::TUPLE:
{
+ ARMARX_TRACE;
auto tupleTypeNav = type::Tuple::DynamicCastAndCheck(type);
unsigned int i = 0;
- for (const auto& nav : childrenNavigators)
+ for (const auto& childTypeNav : tupleTypeNav->getAcceptedTypes())
{
- if (!tupleTypeNav->hasAcceptedType(i))
+ if (!this->hasElement(i))
{
- ARMARX_TRACE;
return false;
}
- auto childTypeNav = tupleTypeNav->getAcceptedType(i);
- if (!nav)
+ auto childNav = this->getElement(i);
+ if (!childNav)
{
if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return false;
+ continue;
}
- if (!nav->fullfillsType(tupleTypeNav->getAcceptedType(i)))
+ if (!childNav->fullfillsType(tupleTypeNav->getAcceptedType(i)))
{
- ARMARX_TRACE;
return false;
}
}
- ARMARX_TRACE;
return true;
}
case type::Descriptor::PAIR:
{
+ ARMARX_TRACE;
auto pairTypeNav = type::Pair::DynamicCastAndCheck(type);
if (childrenSize() != 2)
{
- ARMARX_TRACE;
return false;
}
auto firstChildTypeNav = pairTypeNav->getFirstAcceptedType();
@@ -331,24 +323,17 @@ namespace armarx::aron::data
{
if (firstChildTypeNav && firstChildTypeNav->getMaybe() == type::Maybe::NONE)
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return false;
}
auto secondChildTypeNav = pairTypeNav->getSecondAcceptedType();
if (!childrenNavigators[1])
{
if (secondChildTypeNav && secondChildTypeNav->getMaybe() == type::Maybe::NONE)
{
- ARMARX_TRACE;
return false;
}
- ARMARX_TRACE;
- return false;
}
- ARMARX_TRACE;
return childrenNavigators[0]->fullfillsType(firstChildTypeNav) &&
childrenNavigators[1]->fullfillsType(secondChildTypeNav);
}
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
index 69cabb5ba160f3abef84d331b971c4146d866c29..b5444266d3a2bfbc09e2cc151755eb90402e511a 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
@@ -45,7 +45,7 @@ namespace armarx::aron::data::detail
using ValueType = ValueT;
public:
- using Base::SpecializedVariantBase;
+ using typename Base::SpecializedVariantBase;
PrimitiveVariant(const ValueT& v,
const data::Descriptor descriptor,
diff --git a/source/RobotAPI/libraries/robotapi/CMakeLists.txt b/source/RobotAPI/libraries/robotapi/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d62c2b5f8297012bfb4be7c6a90f74ac6d542c11
--- /dev/null
+++ b/source/RobotAPI/libraries/robotapi/CMakeLists.txt
@@ -0,0 +1,3 @@
+add_subdirectory(core)
+add_subdirectory(client)
+add_subdirectory(server)
diff --git a/source/RobotAPI/libraries/skills/CMakeLists.txt b/source/RobotAPI/libraries/skills/CMakeLists.txt
index 53f82b11a33901183e7eb321e7b3a18829f15527..372d6ddfe231c98a882aa5d3d4fa53523bfdf4be 100644
--- a/source/RobotAPI/libraries/skills/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/CMakeLists.txt
@@ -1,59 +1,23 @@
+add_subdirectory(core)
+add_subdirectory(provider)
+add_subdirectory(manager)
+
+
+
set(LIB_NAME RobotAPISkills)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
armarx_add_library(
- LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- ArmarXCoreObservers
-
- RobotAPI::Core
- aronjsonconverter
- arondatatypeconverter
-
- SOURCES
- ./error/Exception.cpp
- ./manager/SkillManagerComponentPlugin.cpp
- ./provider/SkillProviderComponentPlugin.cpp
- ./provider/Skill.cpp
- ./provider/SkillProxy.cpp
- ./provider/PeriodicSkill.cpp
- ./provider/SpecializedSkill.cpp
- ./provider/PeriodicSpecializedSkill.cpp
- ./provider/SkillDescription.cpp
- ./provider/SkillStatusUpdate.cpp
- ./provider/SkillParameterization.cpp
- ./provider/LambdaSkill.cpp
- ./provider/SkillContext.cpp
- ./provider/SkillID.cpp
- ./provider/detail/SkillImplementationWrapper.cpp
+ LIBS
+ RobotAPISkillsCore
+ RobotAPISkillsProvider
+ RobotAPISkillsManager
+ SOURCES
+ skills.cpp
HEADERS
- ./error/Exception.h
- ./manager/SkillManagerComponentPlugin.h
- ./provider/SkillProviderComponentPlugin.h
- ./provider/Skill.h
- ./provider/SkillProxy.h
- ./provider/PeriodicSkill.h
- ./provider/SpecializedSkill.h
- ./provider/PeriodicSpecializedSkill.h
- ./provider/SkillDescription.h
- ./provider/SkillStatusUpdate.h
- ./provider/SkillParameterization.h
- ./provider/LambdaSkill.h
- ./provider/SkillContext.h
- ./provider/SkillID.h
- ./provider/detail/SkillImplementationWrapper.h
-
- provider/mixins/All.h
- provider/mixins/ArvizSkillMixin.h
- provider/mixins/MNSSkillMixin.h
- provider/mixins/MemoryReadingSkillMixin.h
- provider/mixins/RobotReadingSkillMixin.h
- provider/mixins/ObjectReadingSkillMixin.h
- provider/mixins/ObjectWritingSkillMixin.h
- provider/mixins/GraspReadingSkillMixin.h
+ skills.h
)
add_library(RobotAPI::skills ALIAS RobotAPISkills)
diff --git a/source/RobotAPI/libraries/skills/core/CMakeLists.txt b/source/RobotAPI/libraries/skills/core/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..29145e60e305fa10decf06200685dcb99ec6a3d5
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/CMakeLists.txt
@@ -0,0 +1,44 @@
+set(LIB_NAME RobotAPISkillsCore)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ aronjsonconverter
+ arondatatypeconverter
+
+ SOURCES
+ error/Exception.cpp
+ SkillID.cpp
+ ProviderID.cpp
+ ProviderInfo.cpp
+ SkillExecutionRequest.cpp
+ SkillStatusUpdate.cpp
+ SkillExecutionID.cpp
+ SkillPreparationInput.cpp
+ SkillParameterization.cpp
+ Skill.cpp
+ SkillProxy.cpp
+ SkillDescription.cpp
+ HEADERS
+ error/Exception.h
+ SkillID.h
+ ProviderID.h
+ ProviderInfo.h
+ SkillExecutionRequest.h
+ SkillStatusUpdate.h
+ SkillExecutionID.h
+ SkillPreparationInput.h
+ SkillParameterization.h
+ Skill.h
+ SkillProxy.h
+ SkillDescription.h
+)
+
+add_library(RobotAPI::skills::core ALIAS RobotAPISkillsCore)
diff --git a/source/RobotAPI/libraries/skills/core/ProviderID.cpp b/source/RobotAPI/libraries/skills/core/ProviderID.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..53d6aad6807a7002a80a097d94b1e1fb5ec9f864
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/ProviderID.cpp
@@ -0,0 +1,69 @@
+#include "ProviderID.h"
+
+#include "SkillID.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ bool
+ ProviderID::operator==(const ProviderID& other) const
+ {
+ return toString() == other.toString();
+ }
+
+ bool
+ ProviderID::operator!=(const ProviderID& other) const
+ {
+ return not(*this == other);
+ }
+
+ bool
+ ProviderID::operator<(const ProviderID& other) const
+ {
+ 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{.providerName = s.providerName};
+ }
+
+ ProviderID
+ ProviderID::FromIce(const callback::dto::ProviderID& s)
+ {
+ return ProviderID{.providerName = s.providerName};
+ }
+
+ manager::dto::ProviderID
+ ProviderID::toManagerIce() const
+ {
+ return {providerName};
+ }
+
+ callback::dto::ProviderID
+ ProviderID::toCallbackIce() const
+ {
+ return {providerName};
+ }
+
+ std::string
+ ProviderID::toString() const
+ {
+ return providerName;
+ }
+ } // namespace skills
+
+ std::ostream&
+ skills::operator<<(std::ostream& os, const ProviderID& id)
+ {
+ return os << "'" << id.toString() << "'";
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/ProviderID.h b/source/RobotAPI/libraries/skills/core/ProviderID.h
new file mode 100644
index 0000000000000000000000000000000000000000..5687f649185fa472d0afe1b7a60e6eb2eba46d34
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/ProviderID.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SkillID;
+
+ class ProviderID
+ {
+ public:
+ 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;
+
+ static ProviderID FromIce(const manager::dto::ProviderID&);
+ static ProviderID FromIce(const callback::dto::ProviderID&);
+
+ std::string toString() const;
+
+ std::string providerName;
+ };
+
+ std::ostream& operator<<(std::ostream& os, const ProviderID& id);
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/ProviderInfo.cpp b/source/RobotAPI/libraries/skills/core/ProviderInfo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..135b81c594aaf004d65170c81f65a1a6b9529a07
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/ProviderInfo.cpp
@@ -0,0 +1,32 @@
+#include "ProviderInfo.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ skills::manager::dto::ProviderInfo
+ ProviderInfo::toIce() const
+ {
+ skills::manager::dto::ProviderInfo ret;
+ ret.providerId = providerId.toManagerIce();
+ ret.providerInterface = providerInterface;
+
+ for (const auto& [k, v] : providedSkills)
+ {
+ ret.providedSkills[k.toManagerIce()] = v.toManagerIce();
+ }
+ return ret;
+ }
+
+ ProviderInfo
+ ProviderInfo::FromIce(const manager::dto::ProviderInfo& i)
+ {
+ std::map<SkillID, SkillDescription> m;
+ for (const auto& [k, v] : i.providedSkills)
+ {
+ m.insert({skills::SkillID::FromIce(k), skills::SkillDescription::FromIce(v)});
+ }
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..438a2cf9d894f25f97150dc10ab6c5d9538cdc1e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/ProviderInfo.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+
+#include "ProviderID.h"
+#include "SkillDescription.h"
+#include "SkillID.h"
+#include "error/Exception.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ class ProviderInfo
+ {
+ public:
+ ProviderID providerId;
+ provider::dti::SkillProviderInterfacePrx providerInterface;
+ std::map<SkillID, SkillDescription> providedSkills;
+
+ skills::manager::dto::ProviderInfo toIce() const;
+
+ static ProviderInfo FromIce(const manager::dto::ProviderInfo&);
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/Skill.cpp b/source/RobotAPI/libraries/skills/core/Skill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7d94de3e90815a156289948fae26463ce5f28247
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/Skill.cpp
@@ -0,0 +1,384 @@
+#include "Skill.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ 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());
+ }
+
+ // install a local condition via a lambda
+ void
+ Skill::installConditionWithCallback(std::function<bool()>&& f, std::function<void()>&& cb)
+ {
+ 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 << "'";
+ this->initializing = true;
+ this->constructing = false;
+ this->preparing = false;
+ this->running = false;
+ this->exiting = false;
+ this->finished = false;
+
+ // install timeout condition
+ installConditionWithCallback(
+ [&]() {
+ return (armarx::core::time::DateTime::Now() >= (started + description.timeout));
+ },
+ [&]() { notifyTimeoutReached(); });
+
+ conditionCheckingThread = std::thread(
+ [&]()
+ {
+ armarx::core::time::Metronome metronome(conditionCheckingThreadFrequency);
+ while (initializing or preparing or
+ running) // when the skill ends/aborts this variable will be set to false
+ {
+ {
+ std::scoped_lock l(conditionCallbacksMutex);
+ for (auto& p : conditionCallbacks)
+ {
+ auto& f = p.first;
+ auto& cb = p.second;
+ if (f())
+ {
+ cb();
+ }
+ }
+ }
+
+ const auto sleepDuration = metronome.waitForNextTick();
+ if (not sleepDuration.isPositive())
+ {
+ ARMARX_WARNING << deactivateSpam()
+ << "ConditionCheckingThread: execution took too long ("
+ << -sleepDuration << " vs "
+ << conditionCheckingThreadFrequency.toCycleDuration()
+ << ")";
+ }
+ }
+ });
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::PrepareResult
+ Skill::_prepare()
+ {
+ 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
+ if (not description.parametersType)
+ {
+ return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
+ }
+ if (this->parameters && this->parameters->fullfillsType(description.parametersType))
+ {
+ // wait until parameters fulfill type
+ return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
+ }
+
+ // false if we have to wait for parameters
+ return {.status = ActiveOrTerminatedSkillStatus::Running};
+ }
+
+ Skill::MainResult
+ Skill::_main()
+ {
+ this->running = true;
+ this->initializing = false;
+ this->constructing = false;
+ this->preparing = false;
+ this->exiting = false;
+ this->finished = false;
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::ExitResult
+ Skill::_exit()
+ {
+ // ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
+ 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::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::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::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::mergeSkillStatuseses(_res.status, res.status)};
+ }
+
+ void
+ Skill::throwIfSkillShouldTerminate(const std::function<void()>& do_before,
+ const std::string& abortedMessage)
+ {
+ 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::MakeSucceededResult(aron::data::DictPtr data)
+ {
+ return MainResult{
+ .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::notifySkillToStop()
+ {
+ std::scoped_lock l(subskillsMutex);
+ stopped = true;
+ _onStopRequested();
+ onStopRequested();
+ }
+
+ void
+ Skill::notifyTimeoutReached()
+ {
+ std::scoped_lock l(subskillsMutex);
+ timeoutReached = true;
+ _onTimeoutReached();
+ onTimeoutReached();
+ }
+
+ bool
+ 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()
+ {
+ }
+
+ void
+ Skill::onStopRequested()
+ {
+ }
+
+ // always called before prepare (should not take longer than 100ms)
+ Skill::InitResult
+ Skill::init()
+ {
+ // Default nothing to init
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ // always called before main (should not take longer than 100ms)
+ Skill::PrepareResult
+ Skill::prepare()
+ {
+ // Default nothing to prepare
+ return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
+ }
+
+ // always called after main or if skill fails (should not take longer than 100ms)
+ Skill::ExitResult
+ Skill::exit()
+ {
+ // Default nothing to exit
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::MainResult
+ Skill::main()
+ {
+ // This is just a dummy implementation
+ ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillId
+ << "'. Please overwrite this method.";
+ return {.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
+ }
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/Skill.h b/source/RobotAPI/libraries/skills/core/Skill.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ec80e77d9956e8516ae84ffec16313bab0c9206
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/Skill.h
@@ -0,0 +1,247 @@
+#pragma once
+
+// std/stl
+#include <functional>
+#include <mutex>
+#include <queue>
+#include <thread>
+
+// base class
+#include <ArmarXCore/core/logging/Logging.h>
+
+// ArmarX
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+#include "SkillDescription.h"
+#include "SkillID.h"
+#include "SkillPreparationInput.h"
+#include "SkillProxy.h"
+#include "SkillStatusUpdate.h"
+#include "error/Exception.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ class Skill : public armarx::Logging
+ {
+ public:
+ 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 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();
+
+ /// Notify the skill from extern to stop
+ void notifySkillToStop();
+
+ /// Returns whether the skill should terminate as soon as possible
+ bool shouldSkillTerminate() const;
+
+ /// 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);
+
+ /// Get the parameters of a skill that have been set so far
+ aron::data::DictPtr getParameters() const;
+
+ protected:
+ 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();
+
+ /// Override this method with the actual implementation.
+ virtual PrepareResult prepare();
+
+ /// Override this method with the actual implementation. The callback is for status updates to the calling instance
+ virtual MainResult main();
+
+ /// Override this method with the actual implementation.
+ virtual ExitResult exit();
+
+ protected:
+ /// Override these methods if you want to do something special when notification comes
+ virtual void onTimeoutReached();
+ virtual void onStopRequested();
+
+ protected:
+ /// install a condition which is frequently checked from the conditionCheckingThread
+ 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::Now();
+ armarx::core::time::DateTime exited = armarx::core::time::DateTime::Invalid();
+
+ 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 = "";
+
+ protected:
+ // 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;
+
+ // 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:
+ // 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>
+ concept isSkill = std::is_base_of<Skill, T>::value;
+
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillDescription.cpp b/source/RobotAPI/libraries/skills/core/SkillDescription.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2fa37b33ceab97b944e55644c99c530ba3f371c3
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillDescription.cpp
@@ -0,0 +1,69 @@
+#include "SkillDescription.h"
+
+#include <ArmarXCore/core/ice_conversions.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ provider::dto::SkillDescription
+ SkillDescription::toProviderIce() const
+ {
+ provider::dto::SkillDescription ret;
+ 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(rootProfileDefaults);
+
+ armarx::core::time::toIce(ret.timeout, timeout);
+ return ret;
+ }
+
+ manager::dto::SkillDescription
+ SkillDescription::toManagerIce() const
+ {
+ manager::dto::SkillDescription ret;
+ 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(rootProfileDefaults);
+
+ armarx::core::time::toIce(ret.timeout, timeout);
+ return ret;
+ }
+
+ SkillDescription
+ SkillDescription::FromIce(const provider::dto::SkillDescription& i,
+ const std::optional<ProviderID>& pid)
+ {
+ armarx::core::time::Duration _d;
+ armarx::core::time::fromIce(i.timeout, _d);
+ 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
+ SkillDescription::FromIce(const manager::dto::SkillDescription& i)
+ {
+ armarx::core::time::Duration _d;
+ armarx::core::time::fromIce(i.timeout, _d);
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..4bc8a3cd8fe92f7ebdf13c32b1e812be8596fbbd
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillDescription.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <ArmarXCore/core/time/Duration.h>
+
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+
+#include "SkillID.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ struct SkillDescription
+ {
+ SkillID skillId;
+ 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;
+
+ static SkillDescription FromIce(const provider::dto::SkillDescription& i,
+ const std::optional<ProviderID>& = std::nullopt);
+ static SkillDescription FromIce(const manager::dto::SkillDescription& i);
+ };
+
+ template <class T>
+ concept isSkillDescription = std::is_base_of<SkillDescription, T>::value;
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp b/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d2f305c1a5e42b5dc28da3285ef46dcbe154b3e6
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionID.cpp
@@ -0,0 +1,44 @@
+#include "SkillExecutionID.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ skills::manager::dto::SkillExecutionID
+ SkillExecutionID::toManagerIce() const
+ {
+ skills::manager::dto::SkillExecutionID ret;
+ ret.skillId = skillId.toManagerIce();
+ ret.executorName = executorName;
+ armarx::core::time::toIce(ret.executionStartedTime, executionStartedTime);
+ return ret;
+ }
+
+ skills::provider::dto::SkillExecutionID
+ SkillExecutionID::toProviderIce() const
+ {
+ skills::provider::dto::SkillExecutionID ret;
+ ret.skillId = skillId.toProviderIce();
+ ret.executorName = executorName;
+ armarx::core::time::toIce(ret.executionStartedTime, executionStartedTime);
+ return ret;
+ }
+
+ SkillExecutionID
+ SkillExecutionID::FromIce(const skills::manager::dto::SkillExecutionID& i)
+ {
+ armarx::core::time::DateTime t;
+ armarx::core::time::fromIce(i.executionStartedTime, t);
+ return {skills::SkillID::FromIce(i.skillId), i.executorName, t};
+ }
+
+ SkillExecutionID
+ SkillExecutionID::FromIce(const skills::provider::dto::SkillExecutionID& i,
+ const std::optional<skills::ProviderID>& providerName)
+ {
+ armarx::core::time::DateTime t;
+ armarx::core::time::fromIce(i.executionStartedTime, t);
+ return {skills::SkillID::FromIce(i.skillId, providerName), i.executorName, t};
+ }
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionID.h b/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
new file mode 100644
index 0000000000000000000000000000000000000000..6c4975388e716c5b0491518a356add0b15cbbf5e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionID.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include "SkillID.h"
+#include "SkillParameterization.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ struct SkillExecutionID
+ {
+ bool
+ operator==(const SkillExecutionID& other) const
+ {
+ return this->toString() == other.toString();
+ }
+
+ bool
+ operator<(const SkillExecutionID& other) const
+ {
+ 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;
+
+ skills::provider::dto::SkillExecutionID toProviderIce() const;
+
+ static SkillExecutionID FromIce(const skills::manager::dto::SkillExecutionID&);
+
+ static SkillExecutionID FromIce(const skills::provider::dto::SkillExecutionID&,
+ const std::optional<skills::ProviderID>& providerName);
+
+
+ SkillID skillId;
+ std::string executorName;
+ armarx::core::time::DateTime executionStartedTime;
+ };
+
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.cpp b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d6a0181bbbb8990f44ada113c55a69a532ee99f
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.cpp
@@ -0,0 +1,48 @@
+#include "SkillExecutionRequest.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ manager::dto::SkillExecutionRequest
+ SkillExecutionRequest::toManagerIce() const
+ {
+ manager::dto::SkillExecutionRequest ret;
+ ret.skillId = skillId.toManagerIce();
+ ret.executorName = executorName;
+ ret.parameters = armarx::aron::data::Dict::ToAronDictDTO(parameters);
+ return ret;
+ }
+
+ provider::dto::SkillExecutionRequest
+ SkillExecutionRequest::toProviderIce() const
+ {
+ provider::dto::SkillExecutionRequest ret;
+ ret.skillId = skillId.toProviderIce();
+ ret.executorName = executorName;
+ ret.parameters = armarx::aron::data::Dict::ToAronDictDTO(parameters);
+ ret.callbackInterface = callbackInterface;
+ return ret;
+ }
+
+ SkillExecutionRequest
+ SkillExecutionRequest::FromIce(const manager::dto::SkillExecutionRequest& req)
+ {
+ return {skills::SkillID::FromIce(req.skillId),
+ req.executorName,
+ armarx::aron::data::Dict::FromAronDictDTO(req.parameters),
+ nullptr};
+ }
+
+ SkillExecutionRequest
+ SkillExecutionRequest::FromIce(const provider::dto::SkillExecutionRequest& req,
+ const std::optional<skills::ProviderID>& providerId)
+ {
+ return {skills::SkillID::FromIce(req.skillId, providerId),
+ req.executorName,
+ armarx::aron::data::Dict::FromAronDictDTO(req.parameters),
+ req.callbackInterface};
+ }
+
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h
new file mode 100644
index 0000000000000000000000000000000000000000..d1f5bbcdd175f9f3516d45856186d453af2f987e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillExecutionRequest.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include "SkillID.h"
+#include "error/Exception.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SkillExecutionRequest
+ {
+ public:
+ manager::dto::SkillExecutionRequest toManagerIce() const;
+ provider::dto::SkillExecutionRequest toProviderIce() const;
+
+ static SkillExecutionRequest FromIce(const manager::dto::SkillExecutionRequest&);
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..ece268aeeec9234a8dd4b2dcaa437d616988317e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillID.cpp
@@ -0,0 +1,79 @@
+#include "SkillID.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ bool
+ SkillID::operator==(const SkillID& other) const
+ {
+ return this->toString() == other.toString();
+ }
+
+ bool
+ SkillID::operator!=(const SkillID& other) const
+ {
+ return not(*this == other);
+ }
+
+ bool
+ SkillID::operator<(const SkillID& other) const
+ {
+ 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{.providerId =
+ skills::ProviderID{.providerName = s.providerId.providerName},
+ .skillName = s.skillName};
+ }
+
+ SkillID
+ SkillID::FromIce(const provider::dto::SkillID& s,
+ const std::optional<ProviderID>& providerId)
+ {
+ if (providerId.has_value())
+ {
+ return SkillID{.providerId = *providerId, .skillName = s.skillName};
+ }
+ return SkillID{.skillName = s.skillName};
+ }
+
+ manager::dto::SkillID
+ SkillID::toManagerIce() const
+ {
+ ARMARX_CHECK(isFullySpecified());
+ return {providerId->toManagerIce(), skillName};
+ }
+
+ provider::dto::SkillID
+ SkillID::toProviderIce() const
+ {
+ return {skillName};
+ }
+
+ std::string
+ SkillID::toString() const
+ {
+ if (providerId.has_value())
+ {
+ return providerId->providerName + NAME_SEPARATOR + skillName;
+ }
+ return NAME_SEPARATOR + skillName;
+ }
+ } // namespace skills
+
+ std::ostream&
+ skills::operator<<(std::ostream& os, const SkillID& id)
+ {
+ return os << "'" << id.toString() << "'";
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.h b/source/RobotAPI/libraries/skills/core/SkillID.h
new file mode 100644
index 0000000000000000000000000000000000000000..e611a82828442ad378dde77d4d90edf477b5e5f1
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillID.h
@@ -0,0 +1,64 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+
+#include "ProviderID.h"
+#include "error/Exception.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SkillID
+ {
+ public:
+ static const constexpr char* NAME_SEPARATOR = "/";
+
+ 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
+ isFullySpecified() const
+ {
+ return isSkillSpecified() and isProviderSpecified();
+ }
+
+ bool
+ isSkillSpecified() const
+ {
+ return not skillName.empty();
+ }
+
+ bool
+ isProviderSpecified() const
+ {
+ if (not providerId.has_value())
+ {
+ return false;
+ }
+ return not providerId->providerName.empty();
+ }
+
+ manager::dto::SkillID toManagerIce() const;
+ provider::dto::SkillID toProviderIce() const;
+
+ static SkillID FromIce(const manager::dto::SkillID&);
+ static SkillID FromIce(const provider::dto::SkillID&,
+ const std::optional<ProviderID>& providerId = std::nullopt);
+
+ std::string toString() const;
+
+ std::optional<ProviderID> providerId = std::nullopt;
+ std::string skillName;
+ };
+
+ std::ostream& operator<<(std::ostream& os, const SkillID& id);
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillParameterization.cpp b/source/RobotAPI/libraries/skills/core/SkillParameterization.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0ec681e2b772aa773a6cc890198e24c28714207
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillParameterization.cpp
@@ -0,0 +1,8 @@
+#include "SkillParameterization.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillParameterization.h b/source/RobotAPI/libraries/skills/core/SkillParameterization.h
new file mode 100644
index 0000000000000000000000000000000000000000..9b54b3fff3bfe0aaedcb79d9c275165aefe5a37d
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillParameterization.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillPreparationInput.cpp b/source/RobotAPI/libraries/skills/core/SkillPreparationInput.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0ec681e2b772aa773a6cc890198e24c28714207
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillPreparationInput.cpp
@@ -0,0 +1,8 @@
+#include "SkillParameterization.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillPreparationInput.h b/source/RobotAPI/libraries/skills/core/SkillPreparationInput.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5cc2bb20c392864ccb31607e1ef02e9174fb211
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillPreparationInput.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+
+ } // 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
new file mode 100644
index 0000000000000000000000000000000000000000..986aab1bc9be867aa918c14e4471ac9fe3e7cf3f
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
@@ -0,0 +1,388 @@
+#include "SkillStatusUpdate.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ SkillStatus
+ toSkillStatus(const ActiveOrTerminatedSkillStatus& d)
+ {
+ switch (d)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ return SkillStatus::Running;
+ case ActiveOrTerminatedSkillStatus::Failed:
+ return SkillStatus::Failed;
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ return SkillStatus::Succeeded;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ return SkillStatus::Aborted;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ SkillStatus
+ toSkillStatus(const TerminatedSkillStatus& d)
+ {
+ switch (d)
+ {
+ case TerminatedSkillStatus::Failed:
+ return SkillStatus::Failed;
+ case TerminatedSkillStatus::Succeeded:
+ return SkillStatus::Succeeded;
+ case TerminatedSkillStatus::Aborted:
+ return SkillStatus::Aborted;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ TerminatedSkillStatus
+ mergeSkillStatuseses(const TerminatedSkillStatus t1, const TerminatedSkillStatus t2)
+ {
+ // if both are equal
+ if (t1 == t2)
+ {
+ return t1;
+ }
+
+ // if one is failed (most unspecific result)
+ if (t1 == TerminatedSkillStatus::Failed or t2 == TerminatedSkillStatus::Failed)
+ {
+ return TerminatedSkillStatus::Failed;
+ }
+
+ // else one must be aborted and the other one must be succeeded
+ return TerminatedSkillStatus::Aborted;
+ }
+
+ ActiveOrTerminatedSkillStatus
+ mergeSkillStatuseses(const ActiveOrTerminatedSkillStatus t1,
+ const ActiveOrTerminatedSkillStatus t2)
+ {
+ // if both are equal
+ if (t1 == t2)
+ {
+ return t1;
+ }
+
+ // if one is failed (most unspecific result)
+ if (t1 == ActiveOrTerminatedSkillStatus::Failed or
+ t2 == ActiveOrTerminatedSkillStatus::Failed)
+ {
+ return ActiveOrTerminatedSkillStatus::Failed;
+ }
+
+ // if none is failed and one is aborted (second most unspecific result)
+ if (t1 == ActiveOrTerminatedSkillStatus::Aborted or
+ t2 == ActiveOrTerminatedSkillStatus::Aborted)
+ {
+ return ActiveOrTerminatedSkillStatus::Aborted;
+ }
+
+ // else one must be running and the other one must be succeeded
+ return ActiveOrTerminatedSkillStatus::Running;
+ }
+
+ void
+ toIce(core::dto::Execution::Status& ret, const SkillStatus& status)
+ {
+ switch (status)
+ {
+ case SkillStatus::Constructing:
+ ret = core::dto::Execution::Status::Constructing;
+ return;
+ case SkillStatus::Initializing:
+ ret = core::dto::Execution::Status::Initializing;
+ return;
+ case SkillStatus::Preparing:
+ ret = core::dto::Execution::Status::Preparing;
+ return;
+ case SkillStatus::Running:
+ ret = core::dto::Execution::Status::Running;
+ return;
+ case SkillStatus::Failed:
+ ret = core::dto::Execution::Status::Failed;
+ return;
+ case SkillStatus::Succeeded:
+ ret = core::dto::Execution::Status::Succeeded;
+ return;
+ case SkillStatus::Aborted:
+ ret = core::dto::Execution::Status::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ void
+ toIce(core::dto::Execution::Status& ret, const ActiveOrTerminatedSkillStatus& status)
+ {
+ switch (status)
+ {
+ case ActiveOrTerminatedSkillStatus::Running:
+ ret = core::dto::Execution::Status::Running;
+ return;
+ case ActiveOrTerminatedSkillStatus::Failed:
+ ret = core::dto::Execution::Status::Failed;
+ return;
+ case ActiveOrTerminatedSkillStatus::Succeeded:
+ ret = core::dto::Execution::Status::Succeeded;
+ return;
+ case ActiveOrTerminatedSkillStatus::Aborted:
+ ret = core::dto::Execution::Status::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ void
+ toIce(core::dto::Execution::Status& ret, const TerminatedSkillStatus& status)
+ {
+ switch (status)
+ {
+ case TerminatedSkillStatus::Failed:
+ ret = core::dto::Execution::Status::Failed;
+ return;
+ case TerminatedSkillStatus::Succeeded:
+ ret = core::dto::Execution::Status::Succeeded;
+ return;
+ case TerminatedSkillStatus::Aborted:
+ ret = core::dto::Execution::Status::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ void
+ fromIce(const core::dto::Execution::Status& status, TerminatedSkillStatus& ret)
+ {
+ switch (status)
+ {
+ case core::dto::Execution::Status::Constructing:
+ [[fallthrough]];
+ case core::dto::Execution::Status::Initializing:
+ [[fallthrough]];
+ case core::dto::Execution::Status::Preparing:
+ [[fallthrough]];
+ case core::dto::Execution::Status::Running:
+ break;
+ case core::dto::Execution::Status::Failed:
+ ret = TerminatedSkillStatus::Failed;
+ return;
+ case core::dto::Execution::Status::Succeeded:
+ ret = TerminatedSkillStatus::Succeeded;
+ return;
+ case core::dto::Execution::Status::Aborted:
+ ret = TerminatedSkillStatus::Aborted;
+ return;
+ }
+ throw error::SkillException(
+ __PRETTY_FUNCTION__,
+ "You entered an invalid execution status type to convert to a terminating status.");
+ }
+
+ void
+ fromIce(const core::dto::Execution::Status& status, ActiveOrTerminatedSkillStatus& ret)
+ {
+ switch (status)
+ {
+ case core::dto::Execution::Status::Constructing:
+ [[fallthrough]];
+ case core::dto::Execution::Status::Initializing:
+ [[fallthrough]];
+ case core::dto::Execution::Status::Preparing:
+ break;
+ case core::dto::Execution::Status::Running:
+ ret = ActiveOrTerminatedSkillStatus::Running;
+ return;
+ case core::dto::Execution::Status::Failed:
+ ret = ActiveOrTerminatedSkillStatus::Failed;
+ return;
+ case core::dto::Execution::Status::Succeeded:
+ ret = ActiveOrTerminatedSkillStatus::Succeeded;
+ return;
+ case core::dto::Execution::Status::Aborted:
+ ret = ActiveOrTerminatedSkillStatus::Aborted;
+ return;
+ }
+ throw error::SkillException(
+ __PRETTY_FUNCTION__,
+ "You entered an invalid execution status type to convert to a terminating status.");
+ }
+
+ void
+ fromIce(const core::dto::Execution::Status& status, SkillStatus& ret)
+ {
+ switch (status)
+ {
+ case core::dto::Execution::Status::Constructing:
+ ret = SkillStatus::Constructing;
+ return;
+ case core::dto::Execution::Status::Initializing:
+ ret = SkillStatus::Initializing;
+ return;
+ case core::dto::Execution::Status::Preparing:
+ ret = SkillStatus::Preparing;
+ return;
+ case core::dto::Execution::Status::Running:
+ ret = SkillStatus::Running;
+ return;
+ case core::dto::Execution::Status::Failed:
+ ret = SkillStatus::Failed;
+ return;
+ case core::dto::Execution::Status::Succeeded:
+ ret = SkillStatus::Succeeded;
+ return;
+ case core::dto::Execution::Status::Aborted:
+ ret = SkillStatus::Aborted;
+ return;
+ }
+ throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
+ }
+
+ manager::dto::SkillStatusUpdate
+ SkillStatusUpdateBase::toManagerIce() const
+ {
+ manager::dto::SkillStatusUpdate ret;
+ ret.executionId.skillId = executionId.skillId.toManagerIce();
+ ret.executionId.executorName = executionId.executorName;
+ armarx::core::time::toIce(ret.executionId.executionStartedTime,
+ executionId.executionStartedTime);
+ ret.parameters = aron::data::Dict::ToAronDictDTO(parameters);
+ ret.callbackInterface = callbackInterface;
+ ret.result = aron::data::Dict::ToAronDictDTO(result);
+ return ret;
+ }
+
+ provider::dto::SkillStatusUpdate
+ SkillStatusUpdateBase::toProviderIce() const
+ {
+ provider::dto::SkillStatusUpdate ret;
+ ret.executionId.skillId = executionId.skillId.toProviderIce();
+ ret.executionId.executorName = executionId.executorName;
+ armarx::core::time::toIce(ret.executionId.executionStartedTime,
+ executionId.executionStartedTime);
+ ret.parameters = aron::data::Dict::ToAronDictDTO(parameters);
+ ret.callbackInterface = callbackInterface;
+ ret.result = aron::data::Dict::ToAronDictDTO(result);
+ return ret;
+ }
+
+ manager::dto::SkillStatusUpdate
+ TerminatedSkillStatusUpdate::toManagerIce() const
+ {
+ manager::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toManagerIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ provider::dto::SkillStatusUpdate
+ TerminatedSkillStatusUpdate::toProviderIce() const
+ {
+ provider::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toProviderIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ manager::dto::SkillStatusUpdate
+ SkillStatusUpdate::toManagerIce() const
+ {
+ manager::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toManagerIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ provider::dto::SkillStatusUpdate
+ SkillStatusUpdate::toProviderIce() const
+ {
+ provider::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toProviderIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ manager::dto::SkillStatusUpdate
+ ActiveOrTerminatedSkillStatusUpdate::toManagerIce() const
+ {
+ manager::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toManagerIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ provider::dto::SkillStatusUpdate
+ ActiveOrTerminatedSkillStatusUpdate::toProviderIce() const
+ {
+ provider::dto::SkillStatusUpdate ret = SkillStatusUpdateBase::toProviderIce();
+ skills::toIce(ret.status, status);
+ return ret;
+ }
+
+ TerminatedSkillStatusUpdate
+ TerminatedSkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
+ {
+ 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;
+ }
+
+ TerminatedSkillStatusUpdate
+ TerminatedSkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId)
+ {
+ 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;
+ }
+
+ SkillStatusUpdate
+ SkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
+ {
+ 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;
+ }
+
+ SkillStatusUpdate
+ SkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId)
+ {
+ 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;
+ }
+
+ ActiveOrTerminatedSkillStatusUpdate
+ ActiveOrTerminatedSkillStatusUpdate::FromIce(const manager::dto::SkillStatusUpdate& update)
+ {
+ 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;
+ }
+
+ ActiveOrTerminatedSkillStatusUpdate
+ ActiveOrTerminatedSkillStatusUpdate::FromIce(
+ const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId)
+ {
+ 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;
+ }
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
new file mode 100644
index 0000000000000000000000000000000000000000..d517fc1ec8aa862d041a4d18fcbcc981f3c6f3f4
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.h
@@ -0,0 +1,209 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/interface/skills/SkillProviderInterface.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include "SkillExecutionID.h"
+#include "SkillID.h"
+#include "SkillParameterization.h"
+
+namespace armarx
+{
+ namespace skills
+ {
+ enum class SkillStatus
+ {
+ Constructing = 0,
+ Initializing = 1,
+ Preparing = 2,
+ Running = 4,
+ Failed = 8,
+ Succeeded = 16,
+ Aborted = 32
+ };
+
+ enum class ActiveOrTerminatedSkillStatus
+ {
+ Running = 2,
+ Failed = 4,
+ Succeeded = 8,
+ Aborted = 16
+ };
+
+ enum class TerminatedSkillStatus
+ {
+ Failed = 4,
+ Succeeded = 8,
+ Aborted = 16
+ };
+
+ SkillStatus toSkillStatus(const ActiveOrTerminatedSkillStatus&);
+ SkillStatus toSkillStatus(const TerminatedSkillStatus&);
+
+ void toIce(core::dto::Execution::Status& ret, const SkillStatus& status);
+ void toIce(core::dto::Execution::Status& ret, const ActiveOrTerminatedSkillStatus& status);
+ void toIce(core::dto::Execution::Status& ret, const TerminatedSkillStatus& status);
+
+ void fromIce(const core::dto::Execution::Status& status, TerminatedSkillStatus& ret);
+ void fromIce(const core::dto::Execution::Status& status,
+ ActiveOrTerminatedSkillStatus& ret);
+ void fromIce(const core::dto::Execution::Status& status, SkillStatus& ret);
+
+ TerminatedSkillStatus mergeSkillStatuseses(const TerminatedSkillStatus t1,
+ const TerminatedSkillStatus t2);
+ ActiveOrTerminatedSkillStatus mergeSkillStatuseses(const ActiveOrTerminatedSkillStatus t1,
+ const ActiveOrTerminatedSkillStatus t2);
+
+ struct SkillStatusUpdateBase
+ {
+ // header
+ SkillExecutionID executionId;
+ aron::data::DictPtr parameters;
+ callback::dti::SkillProviderCallbackInterfacePrx callbackInterface;
+
+ // data
+ aron::data::DictPtr result = nullptr;
+
+ manager::dto::SkillStatusUpdate toManagerIce() const;
+
+ provider::dto::SkillStatusUpdate toProviderIce() const;
+ };
+
+ // Will be returned after the execution of a skill
+ struct TerminatedSkillStatusUpdate : public SkillStatusUpdateBase
+ {
+ TerminatedSkillStatus status = TerminatedSkillStatus::Failed;
+
+ bool
+ hasBeenTerminated() const
+ {
+ return true;
+ }
+
+ manager::dto::SkillStatusUpdate toManagerIce() const;
+
+ provider::dto::SkillStatusUpdate toProviderIce() const;
+
+ static TerminatedSkillStatusUpdate
+ FromIce(const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId = std::nullopt);
+
+ static TerminatedSkillStatusUpdate
+ FromIce(const manager::dto::SkillStatusUpdate& update);
+ };
+
+ // Will be returned from periodic skills which can still run
+ struct ActiveOrTerminatedSkillStatusUpdate : public SkillStatusUpdateBase
+ {
+ ActiveOrTerminatedSkillStatus status = ActiveOrTerminatedSkillStatus::Failed;
+
+ bool
+ hasBeenTerminated() const
+ {
+ return status == ActiveOrTerminatedSkillStatus::Succeeded ||
+ status == ActiveOrTerminatedSkillStatus::Failed ||
+ status == ActiveOrTerminatedSkillStatus::Aborted;
+ }
+
+ manager::dto::SkillStatusUpdate toManagerIce() const;
+
+ provider::dto::SkillStatusUpdate toProviderIce() const;
+
+ static ActiveOrTerminatedSkillStatusUpdate
+ FromIce(const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId = std::nullopt);
+
+ static ActiveOrTerminatedSkillStatusUpdate
+ FromIce(const manager::dto::SkillStatusUpdate& update);
+ };
+
+ // Will be used as status updates from skills to the callback interface
+ struct SkillStatusUpdate : public SkillStatusUpdateBase
+ {
+ SkillStatus status = SkillStatus::Constructing;
+
+ bool
+ operator<(const SkillStatusUpdate& o) const
+ {
+ if (!hasBeenConstructed() and o.hasBeenConstructed())
+ {
+ return true;
+ }
+ if (!hasBeenInitialized() and o.hasBeenInitialized())
+ {
+ return true;
+ }
+ if (!hasBeenPrepared() and o.hasBeenPrepared())
+ {
+ return true;
+ }
+ if (!hasBeenRunning() and o.hasBeenRunning())
+ {
+ return true;
+ }
+ if (!hasBeenTerminated() and o.hasBeenTerminated())
+ {
+ return true;
+ }
+ return false;
+ }
+
+ bool
+ operator<=(const SkillStatusUpdate& o) const
+ {
+ if (status == o.status)
+ {
+ return true;
+ }
+ return *this < o;
+ }
+
+ bool
+ hasBeenConstructed() const
+ {
+ return status != SkillStatus::Constructing;
+ }
+
+ bool
+ hasBeenInitialized() const
+ {
+ return status != SkillStatus::Initializing && hasBeenConstructed();
+ }
+
+ bool
+ hasBeenPrepared() const
+ {
+ return status != SkillStatus::Preparing && hasBeenInitialized();
+ }
+
+ bool
+ hasBeenRunning() const
+ {
+ return status != SkillStatus::Running || hasBeenPrepared();
+ }
+
+ bool
+ hasBeenTerminated() const
+ {
+ return status == SkillStatus::Succeeded || status == SkillStatus::Failed ||
+ status == SkillStatus::Aborted;
+ }
+
+ manager::dto::SkillStatusUpdate toManagerIce() const;
+
+ provider::dto::SkillStatusUpdate toProviderIce() const;
+
+ static SkillStatusUpdate
+ FromIce(const provider::dto::SkillStatusUpdate& update,
+ const std::optional<skills::ProviderID>& providerId = std::nullopt);
+
+ static SkillStatusUpdate FromIce(const manager::dto::SkillStatusUpdate& update);
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/error/Exception.cpp b/source/RobotAPI/libraries/skills/core/error/Exception.cpp
similarity index 100%
rename from source/RobotAPI/libraries/skills/error/Exception.cpp
rename to source/RobotAPI/libraries/skills/core/error/Exception.cpp
diff --git a/source/RobotAPI/libraries/skills/error/Exception.h b/source/RobotAPI/libraries/skills/core/error/Exception.h
similarity index 61%
rename from source/RobotAPI/libraries/skills/error/Exception.h
rename to source/RobotAPI/libraries/skills/core/error/Exception.h
index b73ccbf1e0271f82cf95107bd6beaecdcb4440a8..5e10baed1e9af3131226320397c11919d7a826e0 100644
--- a/source/RobotAPI/libraries/skills/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/CMakeLists.txt b/source/RobotAPI/libraries/skills/manager/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b45f4a92926c16c92ba35fdce0cda59ab1dfdd74
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/manager/CMakeLists.txt
@@ -0,0 +1,24 @@
+set(LIB_NAME RobotAPISkillsManager)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ RobotAPI::skills::core
+
+ aronjsonconverter
+ arondatatypeconverter
+
+ SOURCES
+ SkillManagerComponentPlugin.cpp
+ HEADERS
+ SkillManagerComponentPlugin.h
+)
+
+add_library(RobotAPI::skills::manager ALIAS RobotAPISkillsManager)
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 2908d2e77a0a16627d65d2eb757d3ad8431c3271..3999c956b3ec3d85be444e7b27d56c27fbc2d98e 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -1,8 +1,12 @@
#include "SkillManagerComponentPlugin.h"
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
-#include "../error/Exception.h"
+#include <RobotAPI/libraries/skills/core/SkillID.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
namespace armarx::plugins
{
@@ -14,267 +18,684 @@ namespace armarx::plugins
void
SkillManagerComponentPlugin::preOnConnectComponent()
{
+ auto& p = parent<SkillManagerComponentPluginUser>();
+ p.getProxy(myPrx, -1);
}
void
SkillManagerComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
}
-} // namespace armarx::plugins
-namespace armarx
-{
- SkillManagerComponentPluginUser::SkillManagerComponentPluginUser()
+ skills::ProviderID
+ SkillManagerComponentPlugin::getFirstProviderNameThatHasSkill(const skills::SkillID& skillId)
{
- addPlugin(plugin);
+ // NON LOCKING! WE ASSERT THAT THE CALLER HOLDS LOCK
+ for (const auto& [providerName, providerPrx] : skillProviderMap)
+ {
+ auto allSkills = providerPrx->getSkillDescriptions();
+ for (const auto& [currentSkillID, skillDesc] : allSkills)
+ {
+ if (currentSkillID.skillName == skillId.skillName)
+ {
+ return {providerName};
+ }
+ }
+ }
+ return {"INVALID PROVIDER NAME"};
}
void
- SkillManagerComponentPluginUser::addProvider(const skills::manager::dto::ProviderInfo& info,
- const Ice::Current&)
+ SkillManagerComponentPlugin::addProvider(const skills::ProviderInfo& providerInfo)
{
- std::lock_guard l(skillProviderMapMutex);
- if (skillProviderMap.find(info.providerName) == skillProviderMap.end())
+ std::scoped_lock l(skillProviderMapMutex);
+ if (skillProviderMap.find(providerInfo.providerId) == skillProviderMap.end())
{
- ARMARX_INFO << "Adding a provider with name '" << info.providerName << "'.";
- skillProviderMap.insert({info.providerName, info.provider});
+ ARMARX_INFO << "Adding a provider with name '" << providerInfo.providerId.providerName
+ << "'.";
+ skillProviderMap.insert({providerInfo.providerId, providerInfo.providerInterface});
}
else
{
- ARMARX_INFO << "Trying to add a provider with name '" << info.providerName
+ ARMARX_INFO << "Trying to add a provider with name '"
+ << providerInfo.providerId.providerName
<< "' but the provider already exists. "
<< "Overwriting the old provider info.";
- skillProviderMap[info.providerName] = info.provider;
+ skillProviderMap[providerInfo.providerId] = providerInfo.providerInterface;
}
}
void
- SkillManagerComponentPluginUser::removeProvider(const std::string& providerName,
- const Ice::Current&)
+ SkillManagerComponentPlugin::removeProvider(const skills::ProviderID& providerId)
{
- std::lock_guard l(skillProviderMapMutex);
- if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+ std::scoped_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(providerId); it != skillProviderMap.end())
{
- ARMARX_INFO << "Removing a provider with name '" << providerName << "'.";
+ ARMARX_INFO << "Removing a provider with name '" << providerId.providerName << "'.";
skillProviderMap.erase(it);
}
else
{
- ARMARX_INFO << "Trying to remove a provider with name '" << providerName
+ ARMARX_INFO << "Trying to remove a provider with name '" << providerId.providerName
<< "' but it couldn't be found.";
}
}
- std::string
- SkillManagerComponentPluginUser::getFirstProviderNameThatHasSkill(const std::string& skillName)
+ skills::SkillStatusUpdate
+ SkillManagerComponentPlugin::executeSkill(const skills::SkillExecutionRequest& executionRequest)
{
- for (const auto& [providerName, providerPrx] : skillProviderMap)
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
+
+ std::unique_lock l(skillProviderMapMutex);
+
+ skills::ProviderID provderId(*executionRequest.skillId.providerId);
+
+ // TODO: Really support regexes!
+ if (executionRequest.skillId.providerId->providerName == "*")
{
- auto allSkills = providerPrx->getSkillDescriptions();
- for (const auto& [currentSkillName, skillDesc] : allSkills)
- {
- if (currentSkillName == skillName)
- {
- return providerName;
- }
- }
+ provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId);
}
- return "INVALID PROVIDER NAME";
- }
- using SkillProviderInterfacePrxMap =
- std::map<std::string, skills::provider::dti::SkillProviderInterfacePrx>;
- skills::provider::dto::SkillStatusUpdate
- SkillManagerComponentPluginUser::executeSkill(
- const skills::manager::dto::SkillExecutionRequest& info,
- const Ice::Current&)
- {
- std::string providerName = "INVALID PROVIDER NAME";
- if (info.skillId.providerName == "*")
+ if (auto it = skillProviderMap.find(provderId); it != skillProviderMap.end())
{
- providerName = getFirstProviderNameThatHasSkill(info.skillId.skillName);
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << provderId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
+ }
+
+ try
+ {
+ skills::SkillExecutionRequest provider_executionRequest{
+ .skillId = executionRequest.skillId,
+ .executorName = executionRequest.executorName,
+ .parameters = executionRequest.parameters,
+ .callbackInterface = myPrx};
+
+ auto async =
+ provider->begin_executeSkill(provider_executionRequest.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto provider_statusUpdate_ice = provider->end_executeSkill(async);
+
+ // convert to manager view
+ auto statusUpdate =
+ skills::SkillStatusUpdate::FromIce(provider_statusUpdate_ice, provderId);
+ return statusUpdate;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__
+ << ": Found disconnected or buggy skill provider '" << provderId
+ << "' during execution. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
+ }
}
- else if (not(info.skillId.providerName.empty()))
+ else
{
- providerName = info.skillId.providerName;
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
}
+ }
+
+ skills::SkillExecutionID
+ SkillManagerComponentPlugin::executeSkillAsync(
+ const skills::SkillExecutionRequest& executionRequest)
+ {
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
- SkillProviderInterfacePrxMap skillProviderMap;
+ std::unique_lock l(skillProviderMapMutex);
+
+ skills::ProviderID provderId(*executionRequest.skillId.providerId);
+
+ // TODO: Really support regexes!
+ if (executionRequest.skillId.providerId->providerName == "*")
{
- std::scoped_lock l(skillProviderMapMutex);
- skillProviderMap = this->skillProviderMap;
+ provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId);
}
- bool remove = false;
- if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+
+ if (auto it = skillProviderMap.find(provderId); it != skillProviderMap.end())
{
- const auto& n = it->first;
- const auto& s = it->second;
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << provderId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
+ }
try
{
- if (s)
- {
- skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
- getProxy(myPrx, -1);
+ skills::SkillExecutionRequest provider_executionRequest{
+ .skillId = executionRequest.skillId,
+ .executorName = executionRequest.executorName,
+ .parameters = executionRequest.parameters,
+ .callbackInterface = myPrx};
- skills::provider::dto::SkillExecutionRequest exInfo;
- exInfo.skillName = info.skillId.skillName;
- exInfo.executorName = info.executorName;
- exInfo.callbackInterface = myPrx;
- exInfo.params = info.params;
+ auto async =
+ provider->begin_executeSkillAsync(provider_executionRequest.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto provider_executionID_ice = provider->end_executeSkillAsync(async);
- return s->executeSkill(exInfo);
- }
- else
- {
- remove = true;
- }
+ // convert to manager view
+ auto executionId =
+ skills::SkillExecutionID::FromIce(provider_executionID_ice, provderId);
+ executionId.skillId.providerId = provderId;
+ return executionId;
}
catch (...)
{
- remove = true;
+ ARMARX_WARNING << __PRETTY_FUNCTION__
+ << ": Found disconnected or buggy skill provider '" << provderId
+ << "' during execution. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
}
+ }
+ else
+ {
+ throw skills::error::SkillException(
+ __PRETTY_FUNCTION__,
+ "Skill execution failed. Could not execute a skill of provider '" +
+ provderId.toString() + "' because the provider does not exist.");
+ }
+ }
+ bool
+ SkillManagerComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
+ const aron::data::DictPtr& data)
+ {
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
- if (remove)
+ 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)
{
- std::scoped_lock l(skillProviderMapMutex);
- // No copy!
- SkillProviderInterfacePrxMap& skillProviderMap = this->skillProviderMap;
- if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
- {
- ARMARX_WARNING << __PRETTY_FUNCTION__
- << ": Found disconnected or buggy skill provider '" << n
- << "' during execution. Removing it from skills.";
- it = skillProviderMap.erase(it);
- }
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+ return false;
+ }
+
+ try
+ {
+ auto async = provider->begin_updateSkillParameters(executionId.toProviderIce(),
+ data->toAronDictDTO());
+ l.unlock(); // allow parallel e.g. stopping
+ 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
{
- ARMARX_ERROR << "Could not execute a skill of provider '" + providerName +
- "' because the provider does not exist.";
- throw skills::error::SkillException(
- __PRETTY_FUNCTION__,
- "Skill execution failed. Could not execute a skill of provider '" + providerName +
- "' because the provider does not exist.");
+ return false;
}
- return {}; // Never happens
}
- void
- SkillManagerComponentPluginUser::abortSkill(const std::string& providerName,
- const std::string& skillName,
- const Ice::Current& current)
+ bool
+ SkillManagerComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
{
- SkillProviderInterfacePrxMap skillProviderMap;
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
+ it != skillProviderMap.end())
{
- std::scoped_lock l(skillProviderMapMutex);
- skillProviderMap = this->skillProviderMap;
+ 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());
+ l.unlock(); // allow parallel e.g. stopping
+ 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;
+ }
+ }
- if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+ 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& n = it->first;
- const auto& s = it->second;
+ 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
{
- if (s)
- {
- s->abortSkill(skillName);
- }
- else
- {
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
- << n << "'. Removing it from skills on next execute.";
- }
+ auto async = provider->begin_abortSkill(executionId.toProviderIce());
+ return true;
}
catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '" << n
- << "'. Removing it from skills on next execute.";
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills on next execute.";
+ return false;
}
}
+ else
+ {
+ return false;
+ }
}
- void
- SkillManagerComponentPluginUser::updateStatusForSkill(
- const skills::provider::dto::SkillStatusUpdate& statusUpdate,
- const Ice::Current&)
+ std::optional<skills::SkillDescription>
+ SkillManagerComponentPlugin::getSkillDescription(const skills::SkillID& skillId)
{
- (void)statusUpdate;
- // If you want to use the status, implement this method!
+ ARMARX_CHECK(skillId.isFullySpecified());
+
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*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);
+
+ throw skills::error::SkillException(__PRETTY_FUNCTION__,
+ "Skill execution failed. Could not query a "
+ "status update of a skill of provider '" +
+ providerId.toString() +
+ "' because the provider does not exist.");
+ }
+
+ try
+ {
+ auto async = provider->begin_getSkillDescription(skillId.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto provider_desc_ice = provider->end_getSkillDescription(async);
+
+ if (not provider_desc_ice)
+ {
+ return std::nullopt;
+ }
+
+ // convert to manager view
+ auto desc =
+ skills::SkillDescription::FromIce(provider_desc_ice.value(), providerId);
+ return desc;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__
+ << ": Found disconnected or buggy skill provider '" << providerId
+ << "' during execution. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(__PRETTY_FUNCTION__,
+ "Skill execution failed. Could not query a "
+ "status update of a skill of provider '" +
+ providerId.toString() +
+ "' because the provider does not exist.");
+ }
+ }
+ else
+ {
+ return std::nullopt;
+ }
}
- skills::manager::dto::SkillDescriptionMapMap
- SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current& current)
+ std::map<skills::SkillID, skills::SkillDescription>
+ SkillManagerComponentPlugin::getSkillDescriptions()
{
- skills::manager::dto::SkillDescriptionMapMap ret;
+ std::map<skills::SkillID, skills::SkillDescription> ret;
std::scoped_lock l(skillProviderMapMutex);
for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
{
- const auto& n = it->first;
- const auto& s = it->second;
+ 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.";
+ it = skillProviderMap.erase(it);
+ continue;
+ }
+
try
{
- if (s)
- {
- skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
- ret.insert({n, m});
- ++it;
- }
- else
+ skills::provider::dto::SkillDescriptionMap m = provider->getSkillDescriptions();
+
+ for (const auto& [provider_skillId_ice, skillDescription_ice] : m)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
- << n << "'. Removing it from skills.";
- it = skillProviderMap.erase(it);
+ ret.insert(
+ {skills::SkillID::FromIce(provider_skillId_ice, providerId),
+ skills::SkillDescription::FromIce(skillDescription_ice, providerId)});
}
+
+ ++it;
}
catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '" << n
- << "'. Removing it from skills.";
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills.";
it = skillProviderMap.erase(it);
}
}
return ret;
}
- skills::manager::dto::SkillStatusUpdateMapMap
- SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& current)
+ std::optional<skills::SkillStatusUpdate>
+ SkillManagerComponentPlugin::getSkillExecutionStatus(
+ const skills::SkillExecutionID& executionId)
{
- skills::manager::dto::SkillStatusUpdateMapMap ret;
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
- std::scoped_lock l(skillProviderMapMutex);
- for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
+ it != skillProviderMap.end())
{
- const auto& n = it->first;
- const auto& s = it->second;
+ 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);
+
+ throw skills::error::SkillException(__PRETTY_FUNCTION__,
+ "Skill execution failed. Could not query a "
+ "status update of a skill of provider '" +
+ providerId.toString() +
+ "' because the provider does not exist.");
+ }
+
try
{
- if (s)
+ auto async = provider->begin_getSkillExecutionStatus(executionId.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto provider_statusUpdate_ice = provider->end_getSkillExecutionStatus(async);
+
+ if (not provider_statusUpdate_ice)
{
- skills::provider::dto::SkillStatusUpdateMap m = s->getSkillExecutionStatuses();
- ret.insert({n, m});
- it++;
+ return std::nullopt;
}
- else
+
+ // convert to manager view
+ auto statusUpdate = skills::SkillStatusUpdate::FromIce(
+ provider_statusUpdate_ice.value(), providerId);
+ return statusUpdate;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__
+ << ": Found disconnected or buggy skill provider '" << providerId
+ << "' during execution. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ throw skills::error::SkillException(__PRETTY_FUNCTION__,
+ "Skill execution failed. Could not query a "
+ "status update of a skill of provider '" +
+ providerId.toString() +
+ "' because the provider does not exist.");
+ }
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
+ SkillManagerComponentPlugin::getSkillExecutionStatuses()
+ {
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> ret;
+
+ std::scoped_lock l(skillProviderMapMutex);
+ for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
+ {
+ 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.";
+ it = skillProviderMap.erase(it);
+ continue;
+ }
+
+ try
+ {
+ auto m = provider->getSkillExecutionStatuses();
+
+ for (const auto& [provider_executionId_ice, provider_statusUpdate_ice] : m)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
- << n << "'. Removing it from skills.";
- it = skillProviderMap.erase(it);
+ ret.insert(
+ {skills::SkillExecutionID::FromIce(provider_executionId_ice, providerId),
+ skills::SkillStatusUpdate::FromIce(provider_statusUpdate_ice,
+ providerId)});
}
+ it++;
}
catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '" << n
- << "'. Removing it from skills.";
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills.";
it = skillProviderMap.erase(it);
}
}
return ret;
}
+
+} // namespace armarx::plugins
+
+namespace armarx
+{
+ SkillManagerComponentPluginUser::SkillManagerComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ void
+ SkillManagerComponentPluginUser::addProvider(const skills::manager::dto::ProviderInfo& info,
+ const Ice::Current&)
+ {
+ auto i = skills::ProviderInfo::FromIce(info);
+ this->plugin->addProvider(i);
+ }
+
+ void
+ SkillManagerComponentPluginUser::removeProvider(
+ const skills::manager::dto::ProviderID& provider,
+ const Ice::Current&)
+ {
+ auto i = skills::ProviderID::FromIce(provider);
+ this->plugin->removeProvider(i);
+ }
+
+ skills::manager::dto::SkillStatusUpdate
+ SkillManagerComponentPluginUser::executeSkill(
+ const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current&)
+ {
+ auto e = skills::SkillExecutionRequest::FromIce(info);
+ return this->plugin->executeSkill(e).toManagerIce();
+ }
+
+ skills::manager::dto::SkillExecutionID
+ SkillManagerComponentPluginUser::executeSkillAsync(
+ const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current)
+ {
+ auto e = skills::SkillExecutionRequest::FromIce(info);
+ return this->plugin->executeSkillAsync(e).toManagerIce();
+ }
+
+ 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);
+ ret.success = this->plugin->updateSkillParameters(e, a);
+ return ret;
+ }
+
+ 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);
+ 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
+ SkillManagerComponentPluginUser::updateStatusForSkill(
+ const skills::provider::dto::SkillStatusUpdate& statusUpdate,
+ const skills::callback::dto::ProviderID& pid,
+ const Ice::Current&)
+ {
+ (void)statusUpdate;
+ (void)pid;
+ // If you want to use the status, implement this method!
+ }
+
+ skills::manager::dto::SkillDescriptionMap
+ SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current& current)
+ {
+ skills::manager::dto::SkillDescriptionMap ret;
+
+ auto m = this->plugin->getSkillDescriptions();
+
+ for (const auto& [k, v] : m)
+ {
+ ret.insert({k.toManagerIce(), v.toManagerIce()});
+ }
+
+ return ret;
+ }
+
+ IceUtil::Optional<skills::manager::dto::SkillDescription>
+ SkillManagerComponentPluginUser::getSkillDescription(const skills::manager::dto::SkillID& id,
+ const Ice::Current& current)
+ {
+ auto e = skills::SkillID::FromIce(id);
+ auto o = this->plugin->getSkillDescription(e);
+ if (o.has_value())
+ {
+ return o->toManagerIce();
+ }
+ return {};
+ }
+
+ IceUtil::Optional<skills::manager::dto::SkillStatusUpdate>
+ SkillManagerComponentPluginUser::getSkillExecutionStatus(
+ const skills::manager::dto::SkillExecutionID& executionId,
+ const Ice::Current& current)
+ {
+ auto e = skills::SkillExecutionID::FromIce(executionId);
+ auto o = this->plugin->getSkillExecutionStatus(e);
+ if (o.has_value())
+ {
+ return o->toManagerIce();
+ }
+ return {};
+ }
+
+ skills::manager::dto::SkillStatusUpdateMap
+ SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& current)
+ {
+ skills::manager::dto::SkillStatusUpdateMap ret;
+
+ auto m = this->plugin->getSkillExecutionStatuses();
+
+ for (const auto& [k, v] : m)
+ {
+ ret.insert({k.toManagerIce(), v.toManagerIce()});
+ }
+
+ return ret;
+ }
} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index 7b14adf04d141900166968c773f498b6060623cb..8aeea6f1ab7d387ce6c46e4212e1455c1c2a9435 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -6,6 +6,15 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/skills/core/ProviderID.h>
+#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+
+namespace armarx
+{
+ class SkillManagerComponentPluginUser; // forward declaration
+}
namespace armarx::plugins
{
@@ -19,36 +28,99 @@ namespace armarx::plugins
void preOnConnectComponent() override;
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void addProvider(const skills::ProviderInfo& info);
+
+ void removeProvider(const skills::ProviderID& id);
+
+ skills::SkillStatusUpdate executeSkill(const skills::SkillExecutionRequest& req);
+
+ skills::SkillExecutionID executeSkillAsync(const skills::SkillExecutionRequest& req);
+
+ bool updateSkillParameters(const skills::SkillExecutionID& id,
+ const aron::data::DictPtr& data);
+
+ bool abortSkill(const skills::SkillExecutionID& id);
+
+ bool abortSkillAsync(const skills::SkillExecutionID& id);
+
+ std::optional<skills::SkillDescription> getSkillDescription(const skills::SkillID& id);
+
+ std::map<skills::SkillID, skills::SkillDescription> getSkillDescriptions();
+
+ std::optional<skills::SkillStatusUpdate>
+ getSkillExecutionStatus(const skills::SkillExecutionID& id);
+
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> getSkillExecutionStatuses();
+
+ skills::ProviderID getFirstProviderNameThatHasSkill(const skills::SkillID& skillid);
+
+ private:
+ skills::manager::dti::SkillManagerInterfacePrx myPrx;
+
+ std::mutex skillProviderMapMutex;
+ std::map<skills::ProviderID, skills::provider::dti::SkillProviderInterfacePrx>
+ skillProviderMap;
+
+ friend class armarx::SkillManagerComponentPluginUser;
};
-}
+} // namespace armarx::plugins
namespace armarx
{
class SkillManagerComponentPluginUser :
- virtual public ManagedIceObject,
- virtual public skills::manager::dti::SkillManagerInterface
+ virtual public ManagedIceObject,
+ virtual public skills::manager::dti::SkillManagerInterface
{
public:
SkillManagerComponentPluginUser();
- void addProvider(const skills::manager::dto::ProviderInfo& providerInfo, const Ice::Current ¤t) override;
- void removeProvider(const std::string&, const Ice::Current ¤t) override;
+ void addProvider(const skills::manager::dto::ProviderInfo& providerInfo,
+ const Ice::Current& current) override;
+ void removeProvider(const skills::manager::dto::ProviderID& provider,
+ const Ice::Current& current) override;
+
+ skills::manager::dto::SkillStatusUpdate
+ executeSkill(const skills::manager::dto::SkillExecutionRequest& info,
+ const Ice::Current& current) override;
+
+ skills::manager::dto::SkillExecutionID
+ executeSkillAsync(const skills::manager::dto::SkillExecutionRequest& skillExecutionRequest,
+ 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;
- skills::provider::dto::SkillStatusUpdate executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current ¤t) override;
- void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update, const Ice::Current ¤t) override;
- void abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current ¤t) override;
+ void updateStatusForSkill(const skills::provider::dto::SkillStatusUpdate& update,
+ const skills::callback::dto::ProviderID& id,
+ const Ice::Current& current) override;
- skills::manager::dto::SkillDescriptionMapMap getSkillDescriptions(const Ice::Current ¤t) override;
- skills::manager::dto::SkillStatusUpdateMapMap getSkillExecutionStatuses(const Ice::Current ¤t) override;
+ skills::provider::dto::AbortSkillResult
+ abortSkill(const skills::manager::dto::SkillExecutionID& id,
+ const Ice::Current& current) override;
- protected:
- std::string getFirstProviderNameThatHasSkill(const std::string& skillName);
+ 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;
+
+ IceUtil::Optional<skills::manager::dto::SkillDescription>
+ getSkillDescription(const skills::manager::dto::SkillID& 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
+ getSkillExecutionStatuses(const Ice::Current& current) override;
private:
armarx::plugins::SkillManagerComponentPlugin* plugin = nullptr;
-
- protected:
- std::mutex skillProviderMapMutex;
- std::map<std::string, skills::provider::dti::SkillProviderInterfacePrx> skillProviderMap;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/CMakeLists.txt b/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9cb59c319fdf937ee9eb341610071f44114de3c1
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
@@ -0,0 +1,60 @@
+set(LIB_NAME RobotAPISkillsProvider)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ RobotAPI::skills::core
+
+ aronjsonconverter
+ arondatatypeconverter
+
+ SOURCES
+ SkillProviderComponentPlugin.cpp
+ SkillFactory.cpp
+ LambdaSkill.cpp
+ SimpleSkill.cpp
+ SimpleSpecializedSkill.cpp
+ SimplePeriodicSpecializedSkill.cpp
+ SimplePeriodicSkill.cpp
+ SpecializedSkillProxy.cpp
+ PeriodicSkill.cpp
+ SpecializedSkill.cpp
+ PeriodicSpecializedSkill.cpp
+ detail/SkillImplementationWrapper.cpp
+ SkillContext.cpp
+
+
+ HEADERS
+ SkillProviderComponentPlugin.h
+ SkillFactory.h
+ LambdaSkill.h
+ SimpleSkill.h
+ SimpleSpecializedSkill.h
+ SimplePeriodicSpecializedSkill.h
+ SimplePeriodicSkill.h
+ SpecializedSkillProxy.h
+ PeriodicSkill.h
+ SpecializedSkill.h
+ PeriodicSpecializedSkill.h
+ detail/SkillImplementationWrapper.h
+ SkillContext.h
+
+
+ mixins/All.h
+ mixins/ArvizSkillMixin.h
+ mixins/MNSSkillMixin.h
+ mixins/MemoryReadingSkillMixin.h
+ mixins/RobotReadingSkillMixin.h
+ mixins/ObjectReadingSkillMixin.h
+ mixins/ObjectWritingSkillMixin.h
+ mixins/GraspReadingSkillMixin.h
+)
+
+add_library(RobotAPI::skills::provider ALIAS RobotAPISkillsProvider)
diff --git a/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp b/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp
index 280cdb93e5eb62413a159f232b20c5da0b7e3bd7..804042b8ac47b2b1c0ec762c87c4e4ce186daf97 100644
--- a/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/LambdaSkill.cpp
@@ -5,11 +5,12 @@ namespace armarx
namespace skills
{
- Skill::MainResult LambdaSkill::main(const MainInput& in)
+ Skill::MainResult
+ LambdaSkill::main()
{
- TerminatedSkillStatus res = fun(in.executorName, in.params);
+ TerminatedSkillStatus res = fun();
return {.status = res, .data = nullptr};
}
- }
-}
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/LambdaSkill.h b/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
index 01e3a3897e78b444c222b352556bea118f5bc50f..16d39dd39578e25ecb901815e10311e7e57bab72 100644
--- a/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
@@ -1,6 +1,6 @@
#pragma once
-#include "Skill.h"
+#include <RobotAPI/libraries/skills/core/Skill.h>
namespace armarx
{
@@ -9,19 +9,16 @@ namespace armarx
class LambdaSkill : public Skill
{
public:
- using FunT = std::function<TerminatedSkillStatus(const std::string clientId, const aron::data::DictPtr&)>;
+ using FunctionType = std::function<TerminatedSkillStatus()>;
LambdaSkill() = delete;
- LambdaSkill(const FunT& f, const SkillDescription& desc) :
- Skill(desc),
- fun(f)
- {};
+ LambdaSkill(const SkillDescription& desc, const FunctionType& f) : Skill(desc), fun(f){};
private:
- MainResult main(const MainInput& in) override;
+ 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 70daa959f0256646125c8477768f304db5555312..fa354d03d242c9e19b95a4a846309227503ae182 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.cpp
@@ -26,65 +26,57 @@
#include "ArmarXCore/core/time/Frequency.h"
#include <ArmarXCore/core/time/Metronome.h>
-#include "RobotAPI/libraries/skills/provider/Skill.h"
+#include "RobotAPI/libraries/skills/core/Skill.h"
namespace armarx::skills
{
- PeriodicSkill::PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency) :
+ PeriodicSkill::PeriodicSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
Skill(skillDescription), frequency(frequency)
{
}
- PeriodicSkill::StepResult PeriodicSkill::stepOfSkill(const MainInput& in)
+ Skill::MainResult
+ PeriodicSkill::main()
{
- return this->step(in);
- }
-
- Skill::MainResult PeriodicSkill::main(const MainInput& in)
- {
- core::time::Metronome metronome(frequency);
+ armarx::core::time::Metronome metronome(frequency);
- while (not Skill::checkWhetherSkillShouldStopASAP())
+ while (true)
{
- const auto res = stepOfSkill(in);
+ 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!");
}
-
- PeriodicSkill::StepResult PeriodicSkill::step(const MainInput& in)
+ PeriodicSkill::StepResult
+ PeriodicSkill::step()
{
- ARMARX_IMPORTANT << "Dummy executing once skill '" << description.skillName << "'. Please overwrite this method!";
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << description.skillId
+ << "'. Please overwrite this method!";
return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
}
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
index 5df6e07e9b7304b8e7c4db10141d3980f31a041c..aff91f19677f0ec362fbd7ca34ab2f3976661b96 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSkill.h
@@ -23,7 +23,8 @@
#include <ArmarXCore/core/time/Frequency.h>
-#include "Skill.h"
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
#include "SpecializedSkill.h"
namespace armarx::skills
@@ -40,19 +41,16 @@ namespace armarx::skills
PeriodicSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency);
- StepResult stepOfSkill(const MainInput& in);
-
- private:
+ protected:
/// Do not use anymore
- Skill::MainResult main(const MainInput& in) final;
+ Skill::MainResult main() final;
/// Override this method with your own step function
- virtual StepResult step(const MainInput& in);
+ virtual StepResult step();
- private:
+ protected:
const armarx::Frequency frequency;
};
-
} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
index 4efac5d6d3ff33206063b928cd202e45476b0b63..914c6da0221973624309a57666064669e05de14a 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -24,7 +24,8 @@
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
-#include "Skill.h"
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
#include "PeriodicSkill.h"
#include "SpecializedSkill.h"
@@ -42,71 +43,60 @@ namespace armarx::skills
using StepResult = PeriodicSkill::StepResult;
PeriodicSpecializedSkill() = delete;
- PeriodicSpecializedSkill(const SkillDescription& skillDescription, const armarx::Frequency& frequency) :
- Base(skillDescription), frequency(frequency)
- {
- }
-
- StepResult stepOfSkill(const typename Base::SpecializedMainInput& in)
+ PeriodicSpecializedSkill(const SkillDescription& skillDescription,
+ const armarx::Frequency& frequency) :
+ Base(skillDescription), frequency(frequency)
{
- return this->step(in);
}
- private:
- using Skill::stopped;
- using Skill::timeoutReached;
-
+ protected:
/// Do not use anymore
- Skill::MainResult main(const typename Base::SpecializedMainInput& in) final
+ Skill::MainResult
+ main() final
{
- core::time::Metronome metronome(frequency);
+ armarx::core::time::Metronome metronome(frequency);
- while (not Skill::checkWhetherSkillShouldStopASAP())
+ while (true)
{
- const auto statusUpdate = stepOfSkill(in);
- 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!");
}
/// Override this method with your own step function
- virtual StepResult step(const typename Base::SpecializedMainInput& in)
+ virtual StepResult
+ step()
{
- ARMARX_IMPORTANT << "Dummy executing once skill '" << this->description.skillName << "'. Please overwrite this method!";
+ ARMARX_IMPORTANT << "Dummy executing once skill '" << this->description.skillName
+ << "'. Please overwrite this method!";
return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
}
- private:
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..b6e834b4cf3114bd0938328482d00a2d243eb60e
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSkill.cpp
@@ -0,0 +1,55 @@
+#include "SimpleSkill.h"
+
+namespace armarx::skills
+{
+ Skill::InitResult
+ SimpleSkill::init()
+ {
+ InitInput i;
+ i.executorName = this->executorName;
+ i.parameters = this->parameters;
+ return this->init(i);
+ }
+
+ Skill::MainResult
+ SimpleSkill::main()
+ {
+ MainInput i;
+ i.executorName = this->executorName;
+ i.parameters = this->parameters;
+ i.callback = this->callback;
+ return this->main(i);
+ }
+
+ Skill::ExitResult
+ SimpleSkill::exit()
+ {
+ ExitInput i;
+ i.executorName = this->executorName;
+ i.parameters = this->parameters;
+ return this->exit(i);
+ }
+
+ Skill::InitResult
+ SimpleSkill::init(const InitInput& in)
+ {
+ // Default nothing to init
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::MainResult
+ SimpleSkill::main(const MainInput& in)
+ {
+ // This is just a dummy implementation
+ ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillId
+ << "'. Please overwrite this method.";
+ return {.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
+ }
+
+ Skill::ExitResult
+ SimpleSkill::exit(const ExitInput& in)
+ {
+ // Default nothing to exit
+ return {.status = TerminatedSkillStatus::Succeeded};
+ }
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSkill.h b/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..45e4715d8bfaf4cc06c98199c67679396c071674
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSkill.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SimpleSkill : public Skill
+ {
+ public:
+ using Base = Skill;
+
+ using Base::Base;
+
+ struct InitInput
+ {
+ std::string executorName;
+ aron::data::DictPtr parameters;
+ };
+
+ struct MainInput
+ {
+ std::string executorName;
+ aron::data::DictPtr parameters;
+ CallbackT callback;
+ };
+
+ struct ExitInput
+ {
+ std::string executorName;
+ aron::data::DictPtr parameters;
+ };
+
+
+ protected:
+ // legacy methods
+ virtual InitResult init(const InitInput& in);
+ virtual MainResult main(const MainInput& in);
+ virtual ExitResult exit(const ExitInput& in);
+
+ InitResult init() final;
+ MainResult main() final;
+ ExitResult exit() final;
+
+ private:
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..49a5c2e4956b3c419c0e5cb495f0de5ea11732de
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.cpp
@@ -0,0 +1,6 @@
+#include "SimpleSpecializedSkill.h"
+
+namespace armarx::skills
+{
+
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
new file mode 100644
index 0000000000000000000000000000000000000000..c134754888f6749a834ffdd1e256b0aed9660562
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h
@@ -0,0 +1,90 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ template <class AronT>
+ class SimpleSpecializedSkill : public SpecializedSkill<AronT>
+ {
+ public:
+ using Base = SpecializedSkill<AronT>;
+ using ParamType = AronT;
+
+ using Base::Base;
+
+ struct SpecializedInitInput
+ {
+ std::string executorName;
+ AronT parameters;
+ };
+
+ struct SpecializedMainInput
+ {
+ std::string executorName;
+ AronT parameters;
+ Skill::CallbackT callback;
+ };
+
+ struct SpecializedExitInput
+ {
+ std::string executorName;
+ AronT parameters;
+ };
+
+
+ protected:
+ // legacy methods
+ virtual Skill::InitResult
+ init(const SpecializedInitInput& in)
+ {
+ return Skill::InitResult{.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ virtual Skill::MainResult
+ main(const SpecializedMainInput& in)
+ {
+ ARMARX_IMPORTANT << "Dummy executing skill '" << this->description.skillId
+ << "'. Please overwrite this method.";
+ return Skill::MainResult{.status = TerminatedSkillStatus::Succeeded,
+ .data = nullptr};
+ }
+
+ virtual Skill::ExitResult
+ exit(const SpecializedExitInput& in)
+ {
+ return Skill::ExitResult{.status = TerminatedSkillStatus::Succeeded};
+ }
+
+ Skill::InitResult
+ init() final
+ {
+ SpecializedInitInput i;
+ i.executorName = this->executorName;
+ i.parameters = this->getParameters();
+ return this->init(i);
+ }
+
+ Skill::MainResult
+ main() final
+ {
+ SpecializedMainInput i;
+ i.executorName = this->executorName;
+ i.callback = this->callback;
+ i.parameters = this->getParameters();
+ return this->main(i);
+ }
+
+ Skill::ExitResult
+ exit() final
+ {
+ SpecializedExitInput i;
+ i.executorName = this->executorName;
+ i.parameters = this->getParameters();
+ return this->exit(i);
+ }
+ };
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.cpp b/source/RobotAPI/libraries/skills/provider/Skill.cpp
deleted file mode 100644
index ba48923e7beab1876c6477d04845d05ce64883b9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/Skill.cpp
+++ /dev/null
@@ -1,203 +0,0 @@
-#include "Skill.h"
-
-namespace armarx
-{
- namespace skills
- {
- Skill::Skill(const SkillDescription& desc):
- description(desc)
- {
- // replace constructor if you want to have a specific logging tag
- Logging::setTag("armarx::skills::" + description.skillName);
- }
-
- // install a local condition via a lambda
- void Skill::installConditionWithCallback(std::function<bool()>&& f, std::function<void()>&& cb)
- {
- std::lock_guard l(callbacksMutex);
- callbacks.push_back({f, cb});
- }
-
- bool Skill::isSkillAvailable(const InitInput &in) const
- {
- return this->isAvailable(in);
- }
-
- bool Skill::isAvailable(const InitInput& in) const
- {
- (void) in;
- return true;
- }
-
- void Skill::resetSkill()
- {
- //ARMARX_IMPORTANT << "Resetting skill '" << description.skillName << "'";
-
- // resetting log times
- started = armarx::core::time::DateTime::Invalid();
- exited = armarx::core::time::DateTime::Invalid();
-
- // resetting master running variable
- running = false;
-
- // resetting conditional variables
- stopped = false;
- timeoutReached = false;
-
- this->reset();
- }
-
- void Skill::waitForDependenciesOfSkill()
- {
- //ARMARX_IMPORTANT << "Waiting for dependencies of skill '" << description.skillName << "'";
- this->waitForDependencies();
- }
-
- void Skill::_init()
- {
- //ARMARX_IMPORTANT << "Initializing skill '" << description.skillName << "'";
- callbacks.clear();
- running = true;
- started = armarx::core::time::DateTime::Now();
-
- // install timeout condition
- installConditionWithCallback(
- [&](){ return (armarx::core::time::DateTime::Now() >= (started + description.timeout)); },
- [&](){ notifyTimeoutReached(); }
- );
-
- conditionCheckingThread = std::thread([&]()
- {
- armarx::core::time::Metronome metronome(conditionCheckingThreadFrequency);
- while (running) // when the skill ends/aborts this variable will be set to false
- {
- {
- std::scoped_lock l(callbacksMutex);
- for (auto& p : callbacks)
- {
- auto& f = p.first;
- auto& cb = p.second;
- if (f())
- {
- cb();
- }
- }
- }
- const auto sleepDuration = metronome.waitForNextTick();
- if (not sleepDuration.isPositive())
- {
- ARMARX_WARNING << deactivateSpam() << "PeriodicSkill: execution took too long (" << -sleepDuration << " vs " << conditionCheckingThreadFrequency.toCycleDuration() << ")";
- }
- }
- });
- }
- Skill::InitResult Skill::initSkill(const InitInput& in)
- {
- this->_init();
- return this->init(in);
- }
-
- void Skill::_main()
- {
- // Nothing here yet...
- }
- Skill::MainResult Skill::mainOfSkill(const MainInput& in)
- {
- this->_main();
- return this->main(in);
- }
-
- void Skill::_exit()
- {
- // ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
- running = false; // stop checking conditions
-
- if (conditionCheckingThread.joinable())
- {
- conditionCheckingThread.join();
- }
- exited = armarx::core::time::DateTime::Now();
- }
- Skill::ExitResult Skill::exitSkill(const ExitInput& in)
- {
- auto ret = this->exit(in);
- this->_exit();
- return ret;
- }
-
- void Skill::notifyTimeoutReached()
- {
- timeoutReached = true;
- onTimeoutReached();
- }
-
- Skill::MainResult Skill::MakeAbortedResult(aron::data::DictPtr data)
- {
- return MainResult{
- .status = TerminatedSkillStatus::Aborted,
- .data = data,
- };
- }
-
- void Skill::notifySkillToStopASAP()
- {
- stopped = true;
- onStopRequested();
- }
-
- bool Skill::checkWhetherSkillShouldStopASAP() const
- {
- return stopped || timeoutReached;
- }
-
- // condition effects
- void Skill::onTimeoutReached()
- {
- }
- void Skill::onStopRequested()
- {
- }
-
- // reset all local variables
- void Skill::reset()
- {
- // Default nothing to reset
- }
-
- // Wait if needed
- void Skill::waitForDependencies()
- {
- // Default wait for nothing
- }
-
- // always called before execute (should not take longer than 100ms)
- Skill::InitResult Skill::init(const InitInput&)
- {
- // Default nothing to init
- return {.status = TerminatedSkillStatus::Succeeded};
- }
-
- // always called after execute or if skill fails (should not take longer than 100ms)
- Skill::ExitResult Skill::exit(const ExitInput&)
- {
- // Default nothing to exit
- return {.status = TerminatedSkillStatus::Succeeded};
- }
-
- Skill::MainResult Skill::main(const MainInput& in)
- {
- // This is just a dummy implementation
- ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillName << "'. Please overwrite this method.";
- return {.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
- }
-
- Skill::MainResult Skill::executeFullSkill(const MainInput& in)
- {
- this->resetSkill();
- this->initSkill(InitInput{.executorName = in.executorName, .params = in.params});
- auto ret = this->mainOfSkill(in);
- this->exitSkill(ExitInput{.executorName = in.executorName, .params = in.params});
- return ret;
- }
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/Skill.h b/source/RobotAPI/libraries/skills/provider/Skill.h
deleted file mode 100644
index bfba4d4cce7c37a880ea93b919373bdd3e126402..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/Skill.h
+++ /dev/null
@@ -1,176 +0,0 @@
-#pragma once
-
-// std/stl
-#include <mutex>
-#include <queue>
-#include <thread>
-#include <functional>
-
-// base class
-#include <ArmarXCore/core/logging/Logging.h>
-
-// ArmarX
-#include <ArmarXCore/core/time/DateTime.h>
-#include <ArmarXCore/core/time/Metronome.h>
-
-#include <RobotAPI/interface/skills/SkillManagerInterface.h>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-
-#include "../error/Exception.h"
-
-#include "SkillID.h"
-#include "SkillStatusUpdate.h"
-#include "SkillDescription.h"
-
-namespace armarx
-{
- namespace skills
- {
- class Skill : public armarx::Logging
- {
- public:
- using CallbackT = std::function<void(const aron::data::DictPtr&)>;
-
- struct InitResult
- {
- TerminatedSkillStatus status;
- };
-
- struct InitInput
- {
- std::string executorName;
- aron::data::DictPtr params;
- };
-
- struct MainInput
- {
- std::string executorName;
- aron::data::DictPtr params;
- CallbackT callback;
- };
-
- struct MainResult
- {
- TerminatedSkillStatus status;
- aron::data::DictPtr data = nullptr;
- };
-
- struct ExitResult
- {
- TerminatedSkillStatus status;
- };
-
- struct ExitInput
- {
- std::string executorName;
- aron::data::DictPtr params;
- };
-
- Skill() = delete;
- Skill(const SkillDescription&);
- virtual ~Skill() = default;
-
- /// The id of the skill (combination of provider and name must be unique).
- SkillID getSkillId() const
- {
- return {providerName, description.skillName};
- }
-
- // Non virtual base methods. They internally call the virtual methods
- // Lifecycle of a skill:
- // 1. check if it is available
- bool isSkillAvailable(const InitInput& in) const;
-
- // 2. reset skill
- void resetSkill();
-
- // 3. Set skill scheduled. Wait for dependencies
- void waitForDependenciesOfSkill();
-
- // 4. All dependencies resolved. Init skill
- InitResult initSkill(const InitInput& in);
-
- // 5. Execute main function of skill
- MainResult mainOfSkill(const MainInput& in);
-
- // 6. Exit skill. This method is called in any case once the skill is scheduled.
- ExitResult exitSkill(const ExitInput& in);
-
- // Condition listeners
- // used to notify the skill from extern to stop
- void notifySkillToStopASAP();
-
- // returns whether the skill should terminate as soon as possible
- bool checkWhetherSkillShouldStopASAP() const;
-
- /// Do init, main, exit together
- MainResult executeFullSkill(const MainInput& in);
-
- protected:
- static MainResult MakeAbortedResult(aron::data::DictPtr data = nullptr);
-
- // fires if the skill reaches timeout
- void notifyTimeoutReached();
-
- // helper methods to do all the static initialization stuff
- void _init();
- void _main();
- void _exit();
-
- private:
- /// Override if your skill can be unavailable. It receives the same input as the init method
- virtual bool isAvailable(const InitInput& in) const;
-
- /// Override if you have special members that needs to be resetted. It is called before the skill ititializes
- virtual void reset();
-
- /// Override if you have special dependencies you have to wait for
- virtual void waitForDependencies();
-
- /// Override this method with the actual implementation.
- virtual InitResult init(const InitInput& in);
-
- /// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual MainResult main(const MainInput& in);
-
- /// Override this method with the actual implementation.
- virtual ExitResult exit(const ExitInput& in);
-
- /// Override these methods if you want to do something special when notification comes
- virtual void onTimeoutReached();
- virtual void onStopRequested();
-
- protected:
- /// install a condition which is frequently checked from the conditionCheckingThread
- void installConditionWithCallback(std::function<bool()>&& f, std::function<void()>&& cb);
-
- public:
- /// The descripion of the skill, which will be available via the provider/manager
- const SkillDescription description;
-
- /// running params
- armarx::core::time::DateTime started = armarx::core::time::DateTime::Invalid();
- armarx::core::time::DateTime exited = armarx::core::time::DateTime::Invalid();
-
- /// proxy that called the skills. Will be set from provider and is const afterwards
- manager::dti::SkillManagerInterfacePrx manager = nullptr;
-
- /// the provider that owns this skill. Will be set from provider and is const afterwards
- std::string providerName = "INVALID PROVIDER NAME";
-
- protected:
- /// active conditions. First is condition (bool return func)
- std::vector<std::pair<std::function<bool()>, std::function<void()>>> callbacks;
- mutable std::mutex callbacksMutex;
-
- /// Use conditions this way
- std::atomic_bool running = false;
- std::atomic_bool stopped = false;
- std::atomic_bool timeoutReached = false;
-
- private:
- std::thread conditionCheckingThread; // A therad that checks the conditions frequently
- armarx::Frequency conditionCheckingThreadFrequency = armarx::Frequency::Hertz(20);
- };
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillContext.h b/source/RobotAPI/libraries/skills/provider/SkillContext.h
index 9bdbc0f0260729fb213941ef64c589f1b7595a05..a5efb050b3e3e2fa005f20b5f6690026ddcb8573 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillContext.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillContext.h
@@ -7,20 +7,23 @@ namespace armarx
{
namespace skills
{
- /* A 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:
SkillContext() = default;
- virtual void defineProperties(const armarx::PropertyDefinitionsPtr& defs, const std::string& prefix) {};
+ virtual void defineProperties(const armarx::PropertyDefinitionsPtr& defs,
+ const std::string& prefix){};
- virtual void onInit(armarx::Component& parent) {};
- virtual void onConnected(armarx::Component& parent) {};
- virtual void onDisconnected(armarx::Component& parent) {};
- virtual void onStopped(armarx::Component& parent) {};
+ virtual void onInit(armarx::Component& parent){};
+ virtual void onConnected(armarx::Component& parent){};
+ virtual void onDisconnected(armarx::Component& parent){};
+ virtual void onStopped(armarx::Component& parent){};
private:
};
- }
-}
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp b/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp
deleted file mode 100644
index 4e195f39e3953d47c98371023ecb90d4c95f9fab..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillDescription.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-#include "SkillDescription.h"
-
-namespace armarx
-{
- namespace skills
- {
- provider::dto::SkillDescription SkillDescription::toIce() const
- {
- provider::dto::SkillDescription ret;
- ret.acceptedType = aron::type::Object::ToAronObjectDTO(acceptedType);
- ret.description = description;
- ret.skillName = skillName;
- ret.robots = robots;
- ret.timeoutMs = timeout.toMilliSeconds();
- ret.defaultParams = aron::data::Dict::ToAronDictDTO(defaultParams);
- return ret;
- }
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillDescription.h b/source/RobotAPI/libraries/skills/provider/SkillDescription.h
deleted file mode 100644
index 57c3c93b18230c81b4152d9d915e4d2337beb294..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillDescription.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <ArmarXCore/core/time/Duration.h>
-
-#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
-
-namespace armarx
-{
- namespace skills
- {
- struct SkillDescription
- {
- std::string skillName = "NOT INITIALIZED YET";
- std::string description = "NOT INITIALIZED YET";
- std::vector<std::string> robots = {};
- armarx::core::time::Duration timeout = armarx::core::time::Duration::MilliSeconds(-1);
- aron::type::ObjectPtr acceptedType = nullptr;
- aron::data::DictPtr defaultParams = nullptr;
-
- provider::dto::SkillDescription toIce() const;
- };
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillFactory.cpp b/source/RobotAPI/libraries/skills/provider/SkillFactory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..78ec9ef6d60dabdf1dab84964aa3722cec9256e5
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SkillFactory.cpp
@@ -0,0 +1,6 @@
+#include "SkillFactory.h"
+
+namespace armarx
+{
+
+}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillFactory.h b/source/RobotAPI/libraries/skills/provider/SkillFactory.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e3b40e3daf092e9214fc65846ec1c58b57e2b52
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/SkillFactory.h
@@ -0,0 +1,68 @@
+#pragma once
+
+#include <tuple>
+
+#include <RobotAPI/libraries/skills/core/Skill.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+ class SkillBlueprint
+ {
+ public:
+ using FunctionTypeToCreateSkill = std::function<std::unique_ptr<Skill>()>;
+
+ 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<SkillBlueprint>
+ ForSkill(Args&&... args)
+ {
+ auto _createSkill = [=]()
+ {
+ auto ptr = std::make_unique<_Skill>(args...);
+ return ptr;
+ };
+
+ auto ret = std::make_unique<SkillBlueprint>(std::move(_createSkill));
+ return ret;
+ }
+
+ virtual std::unique_ptr<Skill>
+ createSkill(const skills::ProviderID& pid) const
+ {
+ auto s = _createSkill();
+ s->setProviderId(pid);
+ return s;
+ }
+
+ virtual SkillDescription
+ createSkillDescription(const skills::ProviderID& pid) const
+ {
+ auto s = createSkill(pid);
+ return s->getSkillDescription();
+ }
+
+
+ protected:
+ FunctionTypeToCreateSkill _createSkill;
+ };
+
+ template <class T>
+ concept isSkillBlueprint = std::is_base_of<SkillBlueprint, T>::value;
+
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/SkillID.cpp b/source/RobotAPI/libraries/skills/provider/SkillID.cpp
deleted file mode 100644
index 07c6248b2f8c366ceeb7782b78315a6d16f74c70..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillID.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#include "SkillID.h"
-
-namespace armarx
-{
- namespace skills
- {
- SkillID::SkillID(const std::string& providerName, const std::string& skillName) : providerName(providerName), skillName(skillName)
- {
- if (simox::alg::contains(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(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 toString() == other.toString();
- }
-
- bool SkillID::operator<(const SkillID& other) const
- {
- return toString() < other.toString();
- }
-
- provider::dto::SkillID SkillID::toIce() const
- {
- return {providerName, skillName};
- }
-
- std::string SkillID::toString(const std::string& prefix) const
- {
- return (prefix.empty() ? std::string("") : (prefix + PREFIX_SEPARATOR)) + providerName + NAME_SEPARATOR + skillName;
- }
- }
-
- std::ostream& skills::operator<<(std::ostream& os, const SkillID& id)
- {
- return os << "'" << id.toString() << "'";
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillID.h b/source/RobotAPI/libraries/skills/provider/SkillID.h
deleted file mode 100644
index 320a6e946e04694c6d703d0f78b18552d756cf7d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillID.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <SimoxUtility/algorithm/string.h>
-
-#include "../error/Exception.h"
-
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-
-namespace armarx
-{
- namespace skills
- {
- class SkillID
- {
- public:
- static const constexpr char* PREFIX_SEPARATOR = "->";
- static const constexpr char* NAME_SEPARATOR = "/";
-
- std::string providerName;
- std::string skillName;
-
- SkillID() = delete;
- SkillID(const std::string& providerName, const std::string& skillName);
-
- bool operator==(const SkillID& other) const;
- bool operator<(const SkillID& other) const;
-
- provider::dto::SkillID toIce() const;
- std::string toString(const std::string& prefix = "") const;
- };
-
- std::ostream& operator<<(std::ostream& os, const SkillID& id);
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp b/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
deleted file mode 100644
index f977b2ac595fd3506856d2ba42468cdef72d2e54..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-#include "SkillParameterization.h"
-
-namespace armarx
-{
- namespace skills
- {
- aron::data::dto::DictPtr SkillParameterization::toIce() const
- {
- return aron::data::Dict::ToAronDictDTO(usedInputParams);
- }
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h b/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
deleted file mode 100644
index 72ec6d2eb11eec1954e753021d6e634a641027fd..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillParameterization.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
-namespace armarx
-{
- namespace skills
- {
- struct SkillParameterization
- {
- aron::data::DictPtr usedInputParams = nullptr;
- callback::dti::SkillProviderCallbackInterfacePrx usedCallbackInterface = nullptr;
-
- aron::data::dto::DictPtr toIce() const;
- };
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index c76da14105facaf03ff55742b22ac968fe1fd196..a42d7808284d958d965cfba0dd70aa866a47cc17 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -3,6 +3,7 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/All.h>
+#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
namespace armarx::plugins
{
@@ -24,23 +25,13 @@ namespace armarx::plugins
auto& p = parent<SkillProviderComponentPluginUser>();
const std::string providerName = p.getName();
- // update skill ownership
- for (auto& [skillName, impl] : skillImplementations)
- {
- impl.skill->manager = manager;
- impl.skill->providerName = providerName;
-
- impl.statusUpdate.skillId = {providerName, skillName};
- }
-
// register self to manager
- skills::manager::dto::ProviderInfo i;
- i.provider = myPrx;
- i.providerName = providerName;
- i.providedSkills = p.getSkillDescriptions();
- manager->addProvider(i);
+ skills::ProviderInfo i{.providerId = skills::ProviderID{.providerName = providerName},
+ .providerInterface = myPrx,
+ .providedSkills = getSkillDescriptions()};
- connected = true;
+ ARMARX_INFO << "Adding provider to manager: " << i.providerId;
+ manager->addProvider(i.toIce()); // add provider info to manager
}
void
@@ -49,7 +40,8 @@ namespace armarx::plugins
auto& p = parent<SkillProviderComponentPluginUser>();
std::string providerName = p.getName();
- manager->removeProvider(providerName);
+ auto id = skills::manager::dto::ProviderID{providerName};
+ manager->removeProvider(id);
}
void
@@ -64,130 +56,384 @@ namespace armarx::plugins
}
void
- SkillProviderComponentPlugin::addSkill(const skills::LambdaSkill::FunT& f,
- const skills::SkillDescription& desc)
+ SkillProviderComponentPlugin::addSkillFactory(std::unique_ptr<skills::SkillBlueprint>&& fac)
{
- auto lambda = std::make_unique<skills::LambdaSkill>(f, desc);
- addSkill(std::move(lambda));
- }
-
- void
- SkillProviderComponentPlugin::addSkill(std::unique_ptr<skills::Skill>&& skill)
- {
- if (!skill)
+ if (!fac)
{
return;
}
- std::string skillName = skill->description.skillName;
- if (connected) // TODO: fix so that skills can be added anytime!!!
- {
- ARMARX_WARNING << "The SkillProvider already registered to a manager. The skill '" +
- skillName +
- "' therefore cannot be added anymore. Please only add skills in "
- "the onInit method.";
- return;
- }
+ auto& p = parent<SkillProviderComponentPluginUser>();
+ const std::string componentName = p.getName();
+
+ const skills::ProviderID providerId({componentName});
// lock skills map
- const std::unique_lock l(skillsMutex);
- if (skillImplementations.find(skillName) != skillImplementations.end())
+ const std::unique_lock l(skillFactoriesMutex);
+ auto skillId = fac->createSkillDescription(providerId).skillId;
+
+ if (skillFactories.find(skillId) != skillFactories.end())
{
- ARMARX_WARNING << "Try to add a skill '" + skillName +
+ ARMARX_WARNING << "Try to add a skill factory for skill '" + skillId.toString() +
"' which already exists in list. Ignoring this skill.";
return;
}
- ARMARX_INFO << "Adding skill and set owning provider name" << skillName;
- auto s = skillImplementations.emplace(skillName, std::move(skill));
- s.first->second.statusUpdate.skillId = skills::SkillID(parent().getName(), skillName);
+ ARMARX_INFO << "Adding skill `" << skillId << "` to component `" << componentName << "` .";
+
+ skillFactories.emplace(skillId, std::move(fac));
+
+ // if (connected)
+ // {
+ // // if skill is added after onConnect we have to set the proxies manually.
+ // std::string providerName = parent().getName();
+ // s.first->second.skill->manager = manager;
+ // s.first->second.skill->providerName = providerName;
+ // }
}
- skills::detail::SkillImplementationWrapper&
- SkillProviderComponentPlugin::getSkill(const std::string& name)
+ skills::SkillBlueprint*
+ SkillProviderComponentPlugin::addSkillFactory(const skills::SkillDescription& desc,
+ const skills::LambdaSkill::FunctionType& f)
{
- ARMARX_CHECK_GREATER(skillImplementations.count(name), 0)
- << "Skill '" + name + "' not found.";
- const std::unique_lock l(skillsMutex);
- return skillImplementations.at(name);
+ return addSkillFactory<skills::LambdaSkill>(desc, f);
}
- skills::provider::dto::SkillStatusUpdateMap
+ skills::SkillBlueprint*
+ SkillProviderComponentPlugin::getSkillFactory(const armarx::skills::SkillID& skillId)
+ {
+ // NON BLOCKING: WE ASSERT THAT THE LOCK IS ALREADY TAKEN
+ ARMARX_CHECK(skillId.isFullySpecified());
+
+ if (skillFactories.count(skillId) == 0)
+ {
+ ARMARX_INFO << "Could not find a skill factory for id: " << skillId;
+ return nullptr;
+ }
+
+ auto* facPtr = skillFactories.at(skillId).get();
+ return static_cast<skills::SkillBlueprint*>(facPtr);
+ }
+
+ std::optional<skills::SkillStatusUpdate>
+ SkillProviderComponentPlugin::getSkillExecutionStatus(
+ const skills::SkillExecutionID& execId) const
+ {
+ ARMARX_CHECK(execId.skillId.isSkillSpecified());
+
+ const std::unique_lock l(skillExecutionsMutex);
+ if (skillExecutions.find(execId) == skillExecutions.end())
+ {
+ ARMARX_WARNING << "Skill execution for skill '" + execId.skillId.toString() +
+ "' not found!";
+ return std::nullopt;
+ }
+
+ return skillExecutions.at(execId).statusUpdate;
+ }
+
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
SkillProviderComponentPlugin::getSkillExecutionStatuses() const
{
- skills::provider::dto::SkillStatusUpdateMap skillUpdates;
- const std::unique_lock l(skillsMutex);
- for (const auto& [key, impl] : skillImplementations)
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> skillUpdates;
+
+ const std::scoped_lock l(skillExecutionsMutex);
+ for (const auto& [key, impl] : skillExecutions)
{
- const std::shared_lock l2(impl.skillStatusMutex);
- skillUpdates.insert({key, impl.statusUpdate.toIce()});
+ const std::scoped_lock l2(impl.skillStatusesMutex);
+ skillUpdates.insert({key, impl.statusUpdate});
}
return skillUpdates;
}
- skills::provider::dto::SkillDescriptionMap
+ std::optional<skills::SkillDescription>
+ SkillProviderComponentPlugin::getSkillDescription(const skills::SkillID& skillId) const
+ {
+ ARMARX_CHECK(skillId.isFullySpecified());
+
+ const std::unique_lock l(skillFactoriesMutex);
+ if (skillFactories.find(skillId) == skillFactories.end())
+ {
+ 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;
+ }
+
+ return skillFactories.at(skillId)->createSkillDescription(*skillId.providerId);
+ }
+
+ std::map<skills::SkillID, skills::SkillDescription>
SkillProviderComponentPlugin::getSkillDescriptions() const
{
- skills::provider::dto::SkillDescriptionMap skillDesciptions;
- const std::unique_lock l(skillsMutex);
- for (const auto& [key, impl] : skillImplementations)
+ std::map<skills::SkillID, skills::SkillDescription> skillDesciptions;
+ const std::unique_lock l(skillFactoriesMutex);
+ for (const auto& [key, fac] : skillFactories)
{
- skillDesciptions.insert({key, impl.skill->description.toIce()});
+ ARMARX_CHECK(key.isFullySpecified());
+ skillDesciptions.insert({key, fac->createSkillDescription(*key.providerId)});
}
return skillDesciptions;
}
-} // namespace armarx::plugins
+ skills::SkillStatusUpdate
+ SkillProviderComponentPlugin::executeSkill(
+ const skills::SkillExecutionRequest& executionRequest)
+ {
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
-namespace armarx
-{
- SkillProviderComponentPluginUser::SkillProviderComponentPluginUser()
+ skills::SkillExecutionID executionId{.skillId = executionRequest.skillId,
+ .executorName = executionRequest.executorName,
+ .executionStartedTime =
+ armarx::core::time::DateTime::Now()};
+
+ skills::SkillStatusUpdate ret{
+ {executionId, executionRequest.parameters, executionRequest.callbackInterface}};
+
+ skills::detail::SkillRuntime* wrapper;
+ {
+ auto l1 = std::unique_lock{skillFactoriesMutex};
+
+ 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
+ wrapper->execution = std::thread(
+ [&]()
+ {
+ // execute waits until the previous execution finishes.
+ auto x = wrapper->executeSkill();
+ 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 (wrapper && wrapper->execution.joinable())
+ {
+ 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);
+ // }
+ }
+ return ret;
+ }
+
+ skills::SkillExecutionID
+ SkillProviderComponentPlugin::executeSkillAsync(
+ const skills::SkillExecutionRequest& executionRequest)
{
- addPlugin(plugin);
+ ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
+
+ skills::SkillExecutionID executionId{executionRequest.skillId,
+ executionRequest.executorName,
+ armarx::core::time::DateTime::Now()};
+
+ skills::detail::SkillRuntime* wrapper;
+ {
+ auto l1 = std::unique_lock{skillFactoriesMutex};
+
+ 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;
+ }
+
+ wrapper->execution = std::thread(
+ [&]()
+ {
+ // execute waits until the previous execution finishes.
+ auto x = wrapper->executeSkill();
+ });
+ }
+
+ // 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));
+ }
+
+ return executionId;
}
- void
- SkillProviderComponentPluginUser::addSkill(std::unique_ptr<skills::Skill>&& skill)
+ bool
+ SkillProviderComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
+ const armarx::aron::data::DictPtr& input)
{
- plugin->addSkill(std::move(skill));
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ const std::scoped_lock l{skillExecutionsMutex};
+ auto it = skillExecutions.find(executionId);
+ if (it == skillExecutions.end())
+ {
+ ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
+ "' found! Ignoring prepareSkill request.";
+ return false;
+ }
+
+ std::scoped_lock l2{it->second.skillStatusesMutex};
+ if (it->second.statusUpdate.status != skills::SkillStatus::Preparing)
+ {
+ ARMARX_INFO << "Could not prepare the skill '" + executionId.skillId.toString() +
+ "' because its not in preparing phase.";
+ return false;
+ }
+
+ it->second.updateSkillParameters(input);
+ return true;
}
- void
- SkillProviderComponentPluginUser::addSkill(const skills::LambdaSkill::FunT& f,
- const skills::SkillDescription& desc)
+ bool
+ SkillProviderComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
{
- plugin->addSkill(f, desc);
+ 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;
+ }
+
+ 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;
}
- skills::provider::dto::SkillDescription
- SkillProviderComponentPluginUser::getSkillDescription(const std::string& name,
- const Ice::Current& /*unused*/)
+ 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
+
+namespace armarx
+{
+ SkillProviderComponentPluginUser::SkillProviderComponentPluginUser()
{
+ addPlugin(plugin);
+ }
- const auto& skillWrapper = plugin->getSkill(name);
- return skillWrapper.skill->description.toIce();
+ IceUtil::Optional<skills::provider::dto::SkillDescription>
+ SkillProviderComponentPluginUser::getSkillDescription(
+ const skills::provider::dto::SkillID& skillId,
+ const Ice::Current& /*unused*/)
+ {
+ auto id = skills::SkillID::FromIce(skillId, skills::ProviderID{.providerName = getName()});
+ auto o = plugin->getSkillDescription(id);
+ if (o.has_value())
+ {
+ return o->toProviderIce();
+ }
+ return {};
}
skills::provider::dto::SkillDescriptionMap
SkillProviderComponentPluginUser::getSkillDescriptions(const Ice::Current& /*unused*/)
{
- return plugin->getSkillDescriptions();
+ skills::provider::dto::SkillDescriptionMap ret;
+ for (const auto& [k, v] : plugin->getSkillDescriptions())
+ {
+ ret.insert({k.toProviderIce(), v.toProviderIce()});
+ }
+ return ret;
}
- skills::provider::dto::SkillStatusUpdate
- SkillProviderComponentPluginUser::getSkillExecutionStatus(const std::string& skill,
- const Ice::Current& /*unused*/)
+ IceUtil::Optional<skills::provider::dto::SkillStatusUpdate>
+ SkillProviderComponentPluginUser::getSkillExecutionStatus(
+ const skills::provider::dto::SkillExecutionID& executionId,
+ const Ice::Current& /*unused*/)
{
- auto& skillWrapper = plugin->getSkill(skill);
-
- const std::shared_lock l(skillWrapper.skillStatusMutex);
- return skillWrapper.statusUpdate.toIce();
+ auto execId = skills::SkillExecutionID::FromIce(
+ executionId, skills::ProviderID{.providerName = getName()});
+ auto o = plugin->getSkillExecutionStatus(execId);
+ if (o.has_value())
+ {
+ return o->toProviderIce();
+ }
+ return {};
}
skills::provider::dto::SkillStatusUpdateMap
SkillProviderComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& /*unused*/)
{
- return plugin->getSkillExecutionStatuses();
+ skills::provider::dto::SkillStatusUpdateMap ret;
+ for (const auto& [k, v] : plugin->getSkillExecutionStatuses())
+ {
+ ret.insert({k.toProviderIce(), v.toProviderIce()});
+ }
+ return ret;
}
// Please not that this method waits until the skill can be scheduled!
@@ -196,41 +442,59 @@ namespace armarx
const skills::provider::dto::SkillExecutionRequest& info,
const Ice::Current& /*unused*/)
{
- // The skill will be executed in a different thread
- std::thread execution;
+ auto exec = skills::SkillExecutionRequest::FromIce(
+ info, skills::ProviderID{.providerName = getName()});
+ auto up = this->plugin->executeSkill(exec);
+ return up.toProviderIce();
+ }
- // setup input args for skill execution
- skills::SkillParameterization usedParameterization;
- usedParameterization.usedCallbackInterface = info.callbackInterface;
- usedParameterization.usedInputParams = aron::data::Dict::FromAronDictDTO(info.params);
+ skills::provider::dto::SkillExecutionID
+ SkillProviderComponentPluginUser::executeSkillAsync(
+ const skills::provider::dto::SkillExecutionRequest& info,
+ const Ice::Current& current /*unused*/)
+ {
+ auto exec = skills::SkillExecutionRequest::FromIce(
+ info, skills::ProviderID{.providerName = getName()});
+ auto id = this->plugin->executeSkillAsync(exec);
+ return id.toProviderIce();
+ }
- skills::provider::dto::SkillStatusUpdate ret;
- {
- const std::string skillName = info.skillName;
- auto& wrapper = plugin->getSkill(skillName);
+ skills::provider::dto::ParameterUpdateResult
+ SkillProviderComponentPluginUser::updateSkillParameters(
+ const skills::provider::dto::SkillExecutionID& id,
+ const aron::data::dto::DictPtr& input,
+ const Ice::Current& current /*unused*/)
+ {
+ skills::provider::dto::ParameterUpdateResult res;
- // async start execution. But we wait for the execution to finish at the end of this method
- execution = std::thread(
- [&ret, &wrapper, &info, &usedParameterization]()
- {
- // execute waits until the previous execution finishes.
- auto x = wrapper.setupAndExecuteSkill(info.executorName, usedParameterization);
- ret = x.toIce();
- });
- } // 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
+ auto exec =
+ skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
+ auto prep = armarx::aron::data::Dict::FromAronDictDTO(input);
+ res.success = this->plugin->updateSkillParameters(exec, prep);
+ return res;
+ }
- if (execution.joinable())
- {
- execution.join();
- }
- return ret;
+ skills::provider::dto::AbortSkillResult
+ SkillProviderComponentPluginUser::abortSkill(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->abortSkill(exec);
+ return res;
}
- void
- SkillProviderComponentPluginUser::abortSkill(const std::string& skillName, const Ice::Current& /*unused*/)
+ skills::provider::dto::AbortSkillResult
+ SkillProviderComponentPluginUser::abortSkillAsync(
+ const skills::provider::dto::SkillExecutionID& id,
+ const Ice::Current& /*unused*/)
{
- auto& wrapper = plugin->getSkill(skillName);
- wrapper.skill->notifySkillToStopASAP();
+ 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 ab6c8c4662ce303f532e94c6ebddc2d39dba11e3..812cc0a8814db843793038ebdd16a3a927c912cf 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -1,11 +1,11 @@
#pragma once
+#include <experimental/memory>
#include <functional>
#include <queue>
#include <shared_mutex>
#include <thread>
#include <type_traits>
-#include <experimental/memory>
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -15,18 +15,24 @@
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
// Include all types of skills
+#include <RobotAPI/libraries/skills/core/Skill.h>
+#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
+#include <RobotAPI/libraries/skills/core/SkillPreparationInput.h>
+
#include "LambdaSkill.h"
#include "PeriodicSkill.h"
#include "PeriodicSpecializedSkill.h"
-#include "Skill.h"
+#include "SkillFactory.h"
#include "SpecializedSkill.h"
// Helper wrapper for execution
#include "detail/SkillImplementationWrapper.h"
+
namespace armarx
{
class SkillProviderComponentPluginUser; // forward declaration
}
+
namespace armarx::plugins
{
class SkillProviderComponentPlugin : public ComponentPlugin
@@ -43,40 +49,73 @@ namespace armarx::plugins
void preOnDisconnectComponent() override;
- void addSkill(const skills::LambdaSkill::FunT&, const skills::SkillDescription&);
- void addSkill(std::unique_ptr<skills::Skill>&&);
+ void addSkillFactory(std::unique_ptr<skills::SkillBlueprint>&&);
- template <typename T, typename... Args>
- T*
- addSkill(Args&&... args)
+ skills::SkillBlueprint* addSkillFactory(const skills::SkillDescription&,
+ const skills::LambdaSkill::FunctionType&);
+
+ template <class SkillT, typename... Args>
+
+ requires skills::isSkill<SkillT> skills::SkillBlueprint*
+ addSkillFactory(Args&&... args)
{
- static_assert(std::is_base_of<skills::Skill, T>::value,
- "T must be derived from skills::Skill!");
+ auto fac = skills::SkillBlueprint::ForSkill<SkillT>(std::forward<Args>(args)...);
+ auto* facPtr = fac.get();
+ addSkillFactory(std::move(fac));
+ return static_cast<skills::SkillBlueprint*>(facPtr);
+ }
+
+ template <typename FactoryT, typename... Args>
- auto skill = std::make_unique<T>(std::forward<Args>(args)...);
- auto* skillPtr = skill.get();
- addSkill(std::move(skill));
- return static_cast<T*>(skillPtr);
+ requires skills::isSkillBlueprint<FactoryT> FactoryT*
+ addCustomSkillFactory(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);
}
+ // Ice forwards
+ std::optional<skills::SkillStatusUpdate>
+ getSkillExecutionStatus(const skills::SkillExecutionID&) const;
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
+ getSkillExecutionStatuses() const;
+
+ std::optional<skills::SkillDescription> getSkillDescription(const skills::SkillID&) const;
+ std::map<skills::SkillID, skills::SkillDescription> getSkillDescriptions() const;
+
+ skills::SkillStatusUpdate executeSkill(const skills::SkillExecutionRequest& executionInfo);
+
+ skills::SkillExecutionID
+ executeSkillAsync(const skills::SkillExecutionRequest& executionInfo);
+
+ bool updateSkillParameters(const skills::SkillExecutionID& id,
+ const armarx::aron::data::DictPtr& params);
+
+ bool abortSkill(const skills::SkillExecutionID& execId);
+
+ bool abortSkillAsync(const skills::SkillExecutionID& execId);
+
private:
- skills::detail::SkillImplementationWrapper& getSkill(const std::string& name);
- skills::provider::dto::SkillStatusUpdateMap getSkillExecutionStatuses() const;
- skills::provider::dto::SkillDescriptionMap getSkillDescriptions() const;
+ skills::SkillBlueprint* getSkillFactory(const armarx::skills::SkillID& name);
+
skills::manager::dti::SkillManagerInterfacePrx manager;
skills::provider::dti::SkillProviderInterfacePrx myPrx;
- mutable std::shared_mutex skillsMutex;
+ mutable std::mutex skillFactoriesMutex;
+ std::map<skills::SkillID, std::unique_ptr<skills::SkillBlueprint>> skillFactories;
- bool connected = false;
- std::map<std::string, skills::detail::SkillImplementationWrapper> skillImplementations;
+ mutable std::mutex skillExecutionsMutex;
+ std::map<skills::SkillExecutionID, skills::detail::SkillRuntime> skillExecutions;
friend class armarx::SkillProviderComponentPluginUser;
};
} // namespace armarx::plugins
namespace armarx
+
{
class SkillProviderComponentPluginUser :
virtual public ManagedIceObject,
@@ -85,35 +124,72 @@ namespace armarx
public:
SkillProviderComponentPluginUser();
- skills::provider::dto::SkillDescription
- getSkillDescription(const std::string&,
+ // Ice Implementations
+ IceUtil::Optional<skills::provider::dto::SkillDescription>
+ getSkillDescription(const skills::provider::dto::SkillID& skill,
const Ice::Current& current = Ice::Current()) override;
+
skills::provider::dto::SkillDescriptionMap
getSkillDescriptions(const Ice::Current& current = Ice::Current()) override;
- skills::provider::dto::SkillStatusUpdate
- getSkillExecutionStatus(const std::string& skill,
+
+ IceUtil::Optional<skills::provider::dto::SkillStatusUpdate>
+ getSkillExecutionStatus(const skills::provider::dto::SkillExecutionID& executionId,
const Ice::Current& current = Ice::Current()) override;
+
skills::provider::dto::SkillStatusUpdateMap
getSkillExecutionStatuses(const Ice::Current& current = Ice::Current()) override;
skills::provider::dto::SkillStatusUpdate
executeSkill(const skills::provider::dto::SkillExecutionRequest& executionInfo,
const Ice::Current& current = Ice::Current()) override;
- void abortSkill(const std::string& name,
+
+ skills::provider::dto::SkillExecutionID
+ executeSkillAsync(const skills::provider::dto::SkillExecutionRequest& executionInfo,
+ 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;
+
+ skills::provider::dto::AbortSkillResult
+ abortSkillAsync(const skills::provider::dto::SkillExecutionID& skill,
const Ice::Current& current = Ice::Current()) override;
+ // Plugin
const std::experimental::observer_ptr<plugins::SkillProviderComponentPlugin>&
getSkillProviderPlugin() const;
protected:
- void addSkill(const skills::LambdaSkill::FunT&, const skills::SkillDescription&);
- void addSkill(std::unique_ptr<skills::Skill>&&);
+ // -----------------------------------------------------------------------------------------
+ // New
+ // -----------------------------------------------------------------------------------------
+ skills::SkillBlueprint*
+ addSkillFactory(const skills::SkillDescription& desc,
+ const skills::LambdaSkill::FunctionType& f)
+ {
+ auto ret = plugin->addSkillFactory(desc, f);
+ return ret;
+ }
+
+ template <class SkillT, typename... Args>
+
+ requires skills::isSkill<SkillT> skills::SkillBlueprint*
+ addSkillFactory(Args&&... args)
+ {
+ return plugin->addSkillFactory<SkillT>(std::forward<Args>(args)...);
+ }
+
+ template <typename FacT, typename... Args>
- template <typename T, typename... Args>
- T*
- addSkill(Args&&... args)
+ requires skills::isSkillBlueprint<FacT> FacT*
+ addCustomSkillFactory(Args&&... args)
{
- return plugin->addSkill<T>(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 93e18a137f3f927017426aab55ae6b2c05507830..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include "SkillProxy.h"
-
-namespace armarx
-{
- namespace skills
- {
- SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager, const SkillID& skillId) :
- manager(manager),
- skillId(skillId)
- {
- }
-
- SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager, const std::string& skillProviderName, const std::string& skillName) :
- manager(manager),
- skillId(skillProviderName, skillName)
- {
- }
-
- SkillProxy::SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager, const std::string& skillProviderName, const SkillDescription& skillDesc) :
- manager(manager),
- skillId(skillProviderName, skillDesc.skillName)
- {
- }
-
- TerminatedSkillStatusUpdate SkillProxy::executeFullSkill(const std::string& executorName, const aron::data::DictPtr& params)
- {
- skills::manager::dto::SkillExecutionRequest req;
- req.executorName = executorName;
- req.params = params->toAronDictDTO();
- req.skillId = skillId.toIce();
-
- auto terminatingUpdate = manager->executeSkill(req);
- return TerminatedSkillStatusUpdate::FromIce(terminatingUpdate);
- }
-
- IceInternal::Handle<Ice::AsyncResult> SkillProxy::begin_executeFullSkill(const std::string& executorName, const aron::data::DictPtr& params)
- {
- skills::manager::dto::SkillExecutionRequest req;
- req.executorName = executorName;
- req.params = params->toAronDictDTO();
- req.skillId = skillId.toIce();
-
- auto future = manager->begin_executeSkill(req);
- return future;
- }
-
- void SkillProxy::abortSkill(const std::string& executorName)
- {
- // TODO: This will be used in the future, do not remove it!
- (void) executorName;
- manager->abortSkill(skillId.providerName, skillId.skillName);
- }
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.h b/source/RobotAPI/libraries/skills/provider/SkillProxy.h
deleted file mode 100644
index 6fb15914686e6d9f01d0387719b6d5729b98c495..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#pragma once
-
-#include "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 std::string& skillProviderName, const std::string& skillName);
- SkillProxy(const manager::dti::SkillManagerInterfacePrx& manager, const std::string& skillProviderName, const SkillDescription& skillDesc);
-
- TerminatedSkillStatusUpdate executeFullSkill(const std::string& executorName, const aron::data::DictPtr& params = nullptr);
- IceInternal::Handle<Ice::AsyncResult> begin_executeFullSkill(const std::string& executorName, const aron::data::DictPtr& params = nullptr);
-
- void abortSkill(const std::string& executorName);
-
- private:
- const manager::dti::SkillManagerInterfacePrx& manager;
-
- const SkillID skillId;
- };
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
deleted file mode 100644
index 18c4b8647bf3390a64343546b278155ab6a9ce30..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.cpp
+++ /dev/null
@@ -1,245 +0,0 @@
-#include "SkillStatusUpdate.h"
-
-namespace armarx
-{
- namespace skills
- {
- SkillStatus toSkillStatus(const ActiveOrTerminatedSkillStatus& d)
- {
- switch (d)
- {
- case ActiveOrTerminatedSkillStatus::Running:
- return SkillStatus::Running;
- case ActiveOrTerminatedSkillStatus::Failed:
- return SkillStatus::Failed;
- case ActiveOrTerminatedSkillStatus::Succeeded:
- return SkillStatus::Succeeded;
- case ActiveOrTerminatedSkillStatus::Aborted:
- return SkillStatus::Aborted;
- }
- throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
- }
-
- SkillStatus toSkillStatus(const TerminatedSkillStatus& d)
- {
- switch (d)
- {
- case TerminatedSkillStatus::Failed:
- return SkillStatus::Failed;
- case TerminatedSkillStatus::Succeeded:
- return SkillStatus::Succeeded;
- case TerminatedSkillStatus::Aborted:
- return SkillStatus::Aborted;
- }
- throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
- }
-
- void toIce(provider::dto::Execution::Status& ret, const SkillStatus& status)
- {
- switch(status)
- {
- case SkillStatus::Idle:
- ret = provider::dto::Execution::Status::Idle;
- return;
- case SkillStatus::Scheduled:
- ret = provider::dto::Execution::Status::Scheduled;
- return;
- case SkillStatus::Running:
- ret = provider::dto::Execution::Status::Running;
- return;
- case SkillStatus::Failed:
- ret = provider::dto::Execution::Status::Failed;
- return;
- case SkillStatus::Succeeded:
- ret = provider::dto::Execution::Status::Succeeded;
- return;
- case SkillStatus::Aborted:
- ret = provider::dto::Execution::Status::Aborted;
- return;
- }
- throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
- }
-
- void toIce(provider::dto::Execution::Status& ret, const ActiveOrTerminatedSkillStatus& status)
- {
- switch(status)
- {
- case ActiveOrTerminatedSkillStatus::Running:
- ret = provider::dto::Execution::Status::Running;
- return;
- case ActiveOrTerminatedSkillStatus::Failed:
- ret = provider::dto::Execution::Status::Failed;
- return;
- case ActiveOrTerminatedSkillStatus::Succeeded:
- ret = provider::dto::Execution::Status::Succeeded;
- return;
- case ActiveOrTerminatedSkillStatus::Aborted:
- ret = provider::dto::Execution::Status::Aborted;
- return;
- }
- throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
- }
-
- void toIce(provider::dto::Execution::Status& ret, const TerminatedSkillStatus& status)
- {
- switch(status)
- {
- case TerminatedSkillStatus::Failed:
- ret = provider::dto::Execution::Status::Failed;
- return;
- case TerminatedSkillStatus::Succeeded:
- ret = provider::dto::Execution::Status::Succeeded;
- return;
- case TerminatedSkillStatus::Aborted:
- ret = provider::dto::Execution::Status::Aborted;
- return;
- }
- throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
- }
-
- void fromIce(const provider::dto::Execution::Status& status, TerminatedSkillStatus& ret)
- {
- switch(status)
- {
- case provider::dto::Execution::Status::Idle:
- [[fallthrough]];
- case provider::dto::Execution::Status::Scheduled:
- [[fallthrough]];
- case provider::dto::Execution::Status::Running:
- break;
- case provider::dto::Execution::Status::Failed:
- ret = TerminatedSkillStatus::Failed;
- return;
- case provider::dto::Execution::Status::Succeeded:
- ret = TerminatedSkillStatus::Succeeded;
- return;
- case provider::dto::Execution::Status::Aborted:
- ret = TerminatedSkillStatus::Aborted;
- return;
- }
- throw error::SkillException(__PRETTY_FUNCTION__, "You entered an invalid execution status type to convert to a terminating status.");
- }
-
- void fromIce(const provider::dto::Execution::Status& status, ActiveOrTerminatedSkillStatus& ret)
- {
- switch(status)
- {
- case provider::dto::Execution::Status::Idle:
- [[fallthrough]];
- case provider::dto::Execution::Status::Scheduled:
- break;
- case provider::dto::Execution::Status::Running:
- ret = ActiveOrTerminatedSkillStatus::Running;
- return;
- case provider::dto::Execution::Status::Failed:
- ret = ActiveOrTerminatedSkillStatus::Failed;
- return;
- case provider::dto::Execution::Status::Succeeded:
- ret = ActiveOrTerminatedSkillStatus::Succeeded;
- return;
- case provider::dto::Execution::Status::Aborted:
- ret = ActiveOrTerminatedSkillStatus::Aborted;
- return;
- }
- throw error::SkillException(__PRETTY_FUNCTION__, "You entered an invalid execution status type to convert to a terminating status.");
- }
-
- void fromIce(const provider::dto::Execution::Status& status, SkillStatus& ret)
- {
- switch(status)
- {
- case provider::dto::Execution::Status::Idle:
- ret = SkillStatus::Idle;
- return;
- case provider::dto::Execution::Status::Scheduled:
- ret = SkillStatus::Scheduled;
- return;
- case provider::dto::Execution::Status::Running:
- ret = SkillStatus::Running;
- return;
- case provider::dto::Execution::Status::Failed:
- ret = SkillStatus::Failed;
- return;
- case provider::dto::Execution::Status::Succeeded:
- ret = SkillStatus::Succeeded;
- return;
- case provider::dto::Execution::Status::Aborted:
- ret = SkillStatus::Aborted;
- return;
- }
- throw error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
- }
-
-
-
-
- provider::dto::SkillStatusUpdate TerminatedSkillStatusUpdate::toIce() const
- {
- provider::dto::SkillStatusUpdate ret;
- ret.header.skillId = skillId.toIce();
- ret.header.executorName = executorName;
- ret.data = aron::data::Dict::ToAronDictDTO(data);
- skills::toIce(ret.header.status, status);
- ret.header.usedCallbackInterface = usedParameterization.usedCallbackInterface;
- ret.header.usedParams = usedParameterization.toIce();
- return ret;
- }
-
- provider::dto::SkillStatusUpdate SkillStatusUpdate::toIce() const
- {
- provider::dto::SkillStatusUpdate ret;
- ret.header.skillId = skillId.toIce();
- ret.header.executorName = executorName;
- ret.data = aron::data::Dict::ToAronDictDTO(data);
- skills::toIce(ret.header.status, status);
- ret.header.usedCallbackInterface = usedParameterization.usedCallbackInterface;
- ret.header.usedParams = usedParameterization.toIce();
- return ret;
- }
-
- provider::dto::SkillStatusUpdate ActiveOrTerminatedSkillStatusUpdate::toIce() const
- {
- provider::dto::SkillStatusUpdate ret;
- ret.header.skillId = skillId.toIce();
- ret.header.executorName = executorName;
- ret.data = aron::data::Dict::ToAronDictDTO(data);
- skills::toIce(ret.header.status, status);
- ret.header.usedCallbackInterface = usedParameterization.usedCallbackInterface;
- ret.header.usedParams = usedParameterization.toIce();
- return ret;
- }
-
- TerminatedSkillStatusUpdate TerminatedSkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update)
- {
- TerminatedSkillStatusUpdate ret;
- ret.skillId = {update.header.skillId.providerName, update.header.skillId.skillName};
- ret.executorName = update.header.executorName;
- skills::fromIce(update.header.status, ret.status);
- ret.usedParameterization = {aron::data::Dict::FromAronDictDTO(update.header.usedParams), update.header.usedCallbackInterface};
- ret.data = aron::data::Dict::FromAronDictDTO(update.data);
- return ret;
- }
-
- SkillStatusUpdate SkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update)
- {
- SkillStatusUpdate ret;
- ret.skillId = {update.header.skillId.providerName, update.header.skillId.skillName};
- ret.executorName = update.header.executorName;
- skills::fromIce(update.header.status, ret.status);
- ret.usedParameterization = {aron::data::Dict::FromAronDictDTO(update.header.usedParams), update.header.usedCallbackInterface};
- ret.data = aron::data::Dict::FromAronDictDTO(update.data);
- return ret;
- }
-
- ActiveOrTerminatedSkillStatusUpdate ActiveOrTerminatedSkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update)
- {
- ActiveOrTerminatedSkillStatusUpdate ret;
- ret.skillId = {update.header.skillId.providerName, update.header.skillId.skillName};
- ret.executorName = update.header.executorName;
- skills::fromIce(update.header.status, ret.status);
- ret.usedParameterization = {aron::data::Dict::FromAronDictDTO(update.header.usedParams), update.header.usedCallbackInterface};
- ret.data = aron::data::Dict::FromAronDictDTO(update.data);
- return ret;
- }
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h b/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
deleted file mode 100644
index 6e1ec3c169c01d337ddb94f337414f2443d11ff8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillStatusUpdate.h
+++ /dev/null
@@ -1,90 +0,0 @@
-#pragma once
-
-#include <string>
-#include <vector>
-
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <RobotAPI/interface/skills/SkillProviderInterface.h>
-
-#include "SkillID.h"
-#include "SkillParameterization.h"
-
-namespace armarx
-{
- namespace skills
- {
- enum class SkillStatus
- {
- Idle = 0,
- Scheduled = 1,
- Running = 2,
- Failed = 4,
- Succeeded = 8,
- Aborted = 16
- };
-
- enum class ActiveOrTerminatedSkillStatus
- {
- Running = 2,
- Failed = 4,
- Succeeded = 8,
- Aborted = 16
- };
-
- enum class TerminatedSkillStatus
- {
- Failed = 4,
- Succeeded = 8,
- Aborted = 16
- };
-
- SkillStatus toSkillStatus(const ActiveOrTerminatedSkillStatus&);
- SkillStatus toSkillStatus(const TerminatedSkillStatus&);
-
- void toIce(provider::dto::Execution::Status& ret, const SkillStatus& status);
- void toIce(provider::dto::Execution::Status& ret, const ActiveOrTerminatedSkillStatus& status);
- void toIce(provider::dto::Execution::Status& ret, const TerminatedSkillStatus& status);
-
- void fromIce(const provider::dto::Execution::Status& status, TerminatedSkillStatus& ret);
- void fromIce(const provider::dto::Execution::Status& status, ActiveOrTerminatedSkillStatus& ret);
- void fromIce(const provider::dto::Execution::Status& status, SkillStatus& ret);
-
- struct SkillStatusUpdateBase
- {
- // header
- SkillID skillId = {"NOT INITIALIZED YET", "NOT INITIALIZED YET"};
- std::string executorName = "";
- SkillParameterization usedParameterization;
-
- // data
- aron::data::DictPtr data = nullptr;
- };
-
- // Will be returned after the execution of a skill
- struct TerminatedSkillStatusUpdate : public SkillStatusUpdateBase
- {
- TerminatedSkillStatus status = TerminatedSkillStatus::Failed;
-
- provider::dto::SkillStatusUpdate toIce() const;
- static TerminatedSkillStatusUpdate FromIce(const provider::dto::SkillStatusUpdate& update);
- };
-
- // Will be returned from periodic skills which can still run
- struct ActiveOrTerminatedSkillStatusUpdate : public SkillStatusUpdateBase
- {
- ActiveOrTerminatedSkillStatus status = ActiveOrTerminatedSkillStatus::Failed;
-
- provider::dto::SkillStatusUpdate toIce() const;
- static ActiveOrTerminatedSkillStatusUpdate FromIce(const provider::dto::SkillStatusUpdate& update);
- };
-
- // Will be used as status updates from skills to the callback interface
- struct SkillStatusUpdate : public SkillStatusUpdateBase
- {
- SkillStatus status = SkillStatus::Idle;
-
- provider::dto::SkillStatusUpdate toIce() const;
- static SkillStatusUpdate FromIce(const provider::dto::SkillStatusUpdate& update);
- };
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index 3f2b9371e2b087b4a5da77fb06e898bca525c320..2fded9a0a50ede4cdc3e8d76861217fbc1127c58 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -1,8 +1,7 @@
#pragma once
-#include "Skill.h"
-
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include <RobotAPI/libraries/skills/core/Skill.h>
// Debug
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
@@ -15,127 +14,36 @@ namespace armarx
class SpecializedSkill : public Skill
{
public:
+ using Base = Skill;
using ParamType = AronT;
- struct SpecializedInitInput
- {
- std::string executorName;
- AronT params;
- };
-
- struct SpecializedMainInput
- {
- std::string executorName;
- AronT params;
- CallbackT callback;
- };
-
- struct SpecializedExitInput
- {
- std::string executorName;
- AronT params;
- };
-
using Skill::Skill;
virtual ~SpecializedSkill() = default;
- /// returns the accepted type of the skill
- static armarx::aron::type::ObjectPtr GetAcceptedType()
- {
- return AronT::ToAronType();
- }
-
- bool isSkillAvailable(const SpecializedInitInput& in) const
- {
- return this->isAvailable();
- }
-
- Skill::InitResult initSkill(const SpecializedInitInput& in)
- {
- Skill::_init();
- return this->init(in);
- }
- Skill::MainResult mainOfSkill(const SpecializedMainInput& in)
- {
- Skill::_main();
- return this->main(in);
- }
- Skill::ExitResult exitSkill(const SpecializedExitInput& in)
- {
- Skill::_exit();
- return this->exit(in);
- }
-
- Skill::MainResult executeFullSkill(const SpecializedMainInput& in)
+ void
+ setParameters(const AronT& d)
{
- this->resetSkill();
- this->initSkill(SpecializedInitInput({.executorName = in.executorName, .params = in.params}));
- auto ret = this->mainOfSkill(in);
- this->exit(SpecializedExitInput({.executorName = in.executorName, .params = in.params}));
- return ret;
+ Base::setParameters(d.toAron());
}
- private:
- /// Override this method if you want to disable a skill based on certain conditions
- virtual bool isAvailable(const SpecializedInitInput&) const
+ /// Overwrite getter for parameters. Shadow Skill::getParameters()
+ AronT
+ getParameters() const
{
- return true;
+ AronT d;
+ d.fromAron(this->parameters);
+ return d;
}
- /// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Skill::InitResult init(const SpecializedInitInput&)
- {
- return InitResult{.status = TerminatedSkillStatus::Succeeded};
- }
-
- /// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Skill::MainResult main(const SpecializedMainInput& in)
- {
- ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillName << "'. Please overwrite this method.";
- return Skill::MainResult{.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
- }
-
- /// Override this method with the actual implementation. The callback is for status updates to the calling instance
- virtual Skill::ExitResult exit(const SpecializedExitInput&)
- {
- return ExitResult{.status = TerminatedSkillStatus::Succeeded};
- }
-
- /// Do not use anymore
- bool isAvailable(const InitInput& in) const final
- {
- AronT p;
- p.fromAron(in.params);
-
- return isAvailable(SpecializedInitInput({.executorName = in.executorName, .params = p}));
- }
-
- /// Do not use anymore
- Skill::InitResult init(const InitInput& in) final
- {
- AronT p;
- p.fromAron(in.params);
-
- return init(SpecializedInitInput({.executorName = in.executorName, .params = p}));
- }
-
- /// Do not use anymore
- Skill::MainResult main(const MainInput& in) final
+ /// returns the accepted type of the skill
+ static armarx::aron::type::ObjectPtr
+ GetAcceptedType()
{
- AronT p;
- p.fromAron(in.params);
-
- return main(SpecializedMainInput({.executorName = in.executorName, .params = p, .callback = in.callback}));
+ return AronT::ToAronType();
}
- /// Do not use anymore
- Skill::ExitResult exit(const ExitInput& in) final
- {
- AronT p;
- p.fromAron(in.params);
- return exit(SpecializedExitInput({.executorName = in.executorName, .params = p}));
- }
+ protected:
};
- }
-}
+ } // 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 81e91b131294d542c286659e0691056ea0bc0894..60db0fa913274594078d59c3f7a930b86e1423be 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -4,219 +4,346 @@ namespace armarx
{
namespace skills::detail
{
- SkillImplementationWrapper::SkillImplementationWrapper(std::unique_ptr<skills::Skill>&& skill) :
- skill(std::move(skill))
+ SkillRuntime::SkillRuntime(
+ const skills::SkillBlueprint& fac,
+ const skills::SkillExecutionID& execId,
+ const aron::data::DictPtr& initial_parameters,
+ const skills::callback::dti::SkillProviderCallbackInterfacePrx& callbackInterface) :
+ factory(fac), statusUpdate{{execId, initial_parameters, callbackInterface}}
{
- ARMARX_CHECK_NOT_NULL(this->skill);
}
- TerminatedSkillStatusUpdate SkillImplementationWrapper::setupAndExecuteSkill(const std::string& executorName, const skills::SkillParameterization parameterization)
+ // ask a skill to stop
+ void
+ SkillRuntime::stopSkill()
{
- std::unique_lock l(executingMutex);
+ while (this->skill == nullptr)
+ {
+ ARMARX_INFO << deactivateSpam()
+ << "Trying to stop a skill that has not been constructed yet... "
+ "Waiting until construction...";
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
- const std::string skillName = skill->description.skillName;
- ARMARX_INFO_S << "Executing skill: " << skillName;
+ this->skill->notifySkillToStop();
+ }
- // reset execution params. This func is also used to clean up once this method returns
- auto resetExecParam = [&](){
- std::unique_lock l2(skillStatusMutex); // skill is not updating
- //statusUpdate.status = skills::provider::dto::Execution::Status::Idle; I decided to not update the status to idle every time the skill stops.
- statusUpdate.data = nullptr;
- statusUpdate.executorName = "";
- };
+ void
+ SkillRuntime::updateSkillParameters(const aron::data::DictPtr& i)
+ {
+ while (this->skill == nullptr)
+ {
+ ARMARX_INFO << deactivateSpam()
+ << "Trying to add params to a skill that has not been constructed "
+ "yet... Waiting until construction...";
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
- // set params and setup variables
- auto setExecParams = [&](){
- std::lock_guard l(skillStatusMutex);
- statusUpdate.usedParameterization = parameterization;
- statusUpdate.executorName = executorName;
- };
+ this->skill->updateParameters(i);
+ }
- resetExecParam();
- setExecParams();
+ TerminatedSkillStatusUpdate
+ SkillRuntime::executeSkill()
+ {
+ std::scoped_lock l(this->executionMutex);
+ // -------------------------------------------------------------------------------------
+ // sanity check
+ // -------------------------------------------------------------------------------------
+ ARMARX_CHECK(this->skill == nullptr)
+ << "A SKILL HAS ALREADY BEEN CONSTRUCTED. THIS SHOULD NOT HAPPEN!!";
+
+
+ // -------------------------------------------------------------------------------------
+ // 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.isFullySpecified());
+
+ 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);
- auto& aron_params = parameterization.usedInputParams;
- auto updateStatus = [&](const SkillStatus status, const aron::data::DictPtr& data = nullptr){
- std::lock_guard l(skillStatusMutex);
- statusUpdate.status = status;
- statusUpdate.data = data;
+ ARMARX_INFO_S << "Executing skill: " << skillName;
- auto& callbackInterface = statusUpdate.usedParameterization.usedCallbackInterface;
+ auto updateStatus =
+ [&](const SkillStatus status, const aron::data::DictPtr& data = nullptr)
+ {
+ std::unique_lock l(skillStatusesMutex);
+ statusUpdate.status = status;
+ statusUpdate.result = data;
+ if (skill) // if skill has been constructed
+ {
+ // update parameterization
+ statusUpdate.parameters = skill->getParameters();
+ }
- if (callbackInterface)
+ if (callback_interface) // if callback interface is used
{
- callbackInterface->updateStatusForSkill(statusUpdate.toIce());
+ auto pid = providerId.toCallbackIce();
+ callback_interface->updateStatusForSkill(statusUpdate.toProviderIce(), pid);
}
};
- auto createErrorMessage = [](const std::string& message){
+ auto createErrorMessageData = [](const std::string& message)
+ {
auto obj = aron::make_dict();
auto m = aron::make_string(message, aron::Path({"errormessage"}));
obj->addElement("errormessage", m);
return obj;
};
- // Check params
- if (skill->description.acceptedType && not(aron_params))
{
- std::string message = "SkillError 001: The Skill '" + skillName + "' requires a type but params are NULL.";
- ARMARX_ERROR_S << message;
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ std::unique_lock l(skillStatusesMutex);
+
+ // ---------------------------------------------------------------------------------
+ // check times
+ // ---------------------------------------------------------------------------------
+ if ((armarx::core::time::DateTime::Now() -
+ statusUpdate.executionId.executionStartedTime) >
+ armarx::core::time::Duration::Seconds(1))
+ {
+ ARMARX_WARNING_S << "SkillError 001: For some reason the skill '"
+ << skillId.toString()
+ << "' has been scheduled > 1s ago. The execution should start "
+ "much faster! Continue, but this is bad behavior..";
+ }
}
- if (skill->description.acceptedType && aron_params && not(aron_params->fullfillsType(skill->description.acceptedType)))
+ // -------------------------------------------------------------------------------------
+ // construct skill
+ // -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Construct skill: " << skillName;
+
+ updateStatus(SkillStatus::Constructing);
+ this->skill = this->factory.createSkill(providerId);
+ 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)
+ this->updateSkillParameters(initial_aron_params);
+
+ auto makeAbortedResult = [&](const std::string& message)
{
- std::string message = "SkillError 002: The Skill '" + skillName + "' has a type and got parameters but the input does not match the type.";
- ARMARX_ERROR_S << message;
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
- }
+ 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;
+ };
- // Check if skill is available with the given parameterization
- try
+
+ auto makeFailedResult = [&](const std::string& message)
{
- if (not skill->isSkillAvailable(Skill::InitInput{.executorName = executorName, .params = aron_params}))
- {
- std::string message = "SkillError 101: The Skill '" + skillName + "' is not available.";
- ARMARX_WARNING << message;
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
- }
- }
- catch (const std::exception& ex)
+ updateStatus(SkillStatus::Failed, createErrorMessageData(message));
+
+ std::unique_lock l(skillStatusesMutex);
+ auto terminated = TerminatedSkillStatusUpdate{
+ {.executionId = statusUpdate.executionId,
+ .parameters = statusUpdate.parameters,
+ .callbackInterface = statusUpdate.callbackInterface}};
+ terminated.status = TerminatedSkillStatus::Failed;
+ return terminated;
+ };
+
+ // -------------------------------------------------------------------------------------
+ // Check params
+ // -------------------------------------------------------------------------------------
+ // NOT RELEVANT ANYMORE!!
+ // if (skill->getSkillDescription().acceptedType && initial_aron_params &&
+ // not(initial_aron_params->fullfillsType(skill->getSkillDescription().acceptedType)))
+ // {
+ // std::string message =
+ // "SkillError 002: The Skill '" + skillName +
+ // "' has a type and got parameters but the input does not match the type.";
+ // ARMARX_ERROR_S << message;
+ // return makeTerminationResult(message);
+ // }
+
+
+ auto exitAndMakeFailedResult = [&](const std::string& message)
{
- std::string message = "SkillError 101e: An error occured during the check whether skill '" + skillName + "' is available. The error was: " + GetHandledExceptionString();
- ARMARX_ERROR_S << message;
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
- }
+ 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 makeAbortedResult(message);
+ };
- // set scheduled
- updateStatus(SkillStatus::Scheduled);
+ // Construction succeeded!
+ // -------------------------------------------------------------------------------------
+ // Init skill
+ // -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Init skill: " << skillName;
+ updateStatus(SkillStatus::Initializing);
- // reset skill and perhaps wait for dependencies
try
{
- skill->resetSkill();
+ Skill::InitResult initRet = skill->initSkill();
+ if (initRet.status != TerminatedSkillStatus::Succeeded)
+ {
+ std::string message = "SkillError 101: The initialization of skill '" +
+ skillName + "' did not succeed.";
+ return exitAndMakeFailedResult(message);
+ }
}
- catch (const std::exception& ex)
+ catch (const error::SkillAbortedException& ex)
{
- std::string message = "SkillError 201e: An error occured during the reset of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
- ARMARX_ERROR_S << message;
-
- updateStatus(SkillStatus::Failed);
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ return exitAndMakeAbortedResult(GetHandledExceptionString());
}
-
- try
+ catch (const error::SkillFailedException& ex)
{
- skill->waitForDependenciesOfSkill();
+ return exitAndMakeFailedResult(GetHandledExceptionString());
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 301e: An error occured during waiting for skill dependencies of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
+ std::string message =
+ "SkillError 101e: An error occured during the initialization of skill '" +
+ skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
-
- updateStatus(SkillStatus::Failed);
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ return exitAndMakeFailedResult(message);
}
- // execute. If the skill fails for some reason, from this point it will always execute its exit function.
- updateStatus(SkillStatus::Running);
-
+ // Init succeeded!
+ // -------------------------------------------------------------------------------------
+ // Prepare skill
+ // -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Prepare skill: " << skillName;
+ updateStatus(SkillStatus::Preparing);
try
{
- Skill::InitResult initRet = skill->initSkill({executorName, aron_params});
- if (initRet.status != TerminatedSkillStatus::Succeeded)
+ auto prepareRet = skill->prepareSkill();
+ while (prepareRet.status == ActiveOrTerminatedSkillStatus::Running)
{
- std::string message = "SkillError 401: The initialization of skill '" + skillName + "' did not succeed.";
- skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
+ ARMARX_INFO << deactivateSpam() << "Not all requirements for skill "
+ << skillName << " fulfilled. Waiting...";
- updateStatus(skills::toSkillStatus(initRet.status));
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, initRet.status});
+ // wait...
+ 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 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 401e: An error occured during the initialization of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
+ std::string message = "SkillError 201e: An error occured during waiting for skill "
+ "dependencies of skill '" +
+ skillName +
+ "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
- skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
-
- updateStatus(SkillStatus::Failed);
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ return exitAndMakeFailedResult(message);
}
- // Init succeeded!
+ // 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);
Skill::MainResult mainRet;
try
{
- mainRet = skill->mainOfSkill({executorName, aron_params, [&updateStatus](const aron::data::DictPtr& update)
- {
- // during execution the statusUpdate.status is always RUNNING
- updateStatus(SkillStatus::Running, update);
- }});
+ mainRet = skill->mainOfSkill();
if (mainRet.status != TerminatedSkillStatus::Succeeded)
{
- std::string message = "SkillError 501: The main method of skill '" + skillName + "' did not succeed.";
- skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
-
- updateStatus(skills::toSkillStatus(mainRet.status));
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, mainRet.status});
+ std::string message = "SkillError 501: The main method of skill '" + skillName +
+ "' did not succeed.";
+ 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();
+ std::string message =
+ "SkillError 501e: An error occured during the main method of skill '" +
+ skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
- skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
-
- updateStatus(SkillStatus::Failed);
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ return exitAndMakeFailedResult(message);
}
- // Main succeeded!
+ // Main succeeded!
+ // -------------------------------------------------------------------------------------
+ // Exit of skill
+ // -------------------------------------------------------------------------------------
+ ARMARX_INFO_S << "Exit skill: " << skillName;
try
{
- Skill::ExitResult exitRet = skill->exitSkill({executorName, aron_params});
+ Skill::ExitResult exitRet = skill->exitSkill();
if (exitRet.status != TerminatedSkillStatus::Succeeded)
{
- std::string message = "SkillError 601: The exit method of skill '" + skillName + "' did not succeed.";
- skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
-
- updateStatus(skills::toSkillStatus(exitRet.status));
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, exitRet.status});
+ std::string message = "SkillError 601: The exit method of skill '" + skillName +
+ "' did not succeed.";
+ 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();
+ std::string message =
+ "SkillError 601e: An error occured during the exit method of skill '" +
+ skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
-
- updateStatus(SkillStatus::Failed);
- resetExecParam();
- return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
+ return makeFailedResult(message);
}
- // Exit succeeded!
-
+ // Exit succeeded!
// All succeeded!
- updateStatus(SkillStatus::Succeeded);
-
- // Tidy up
- resetExecParam();
-
- // return result of main method
- return {{skill->getSkillId(), executorName, parameterization, mainRet.data}, TerminatedSkillStatus::Succeeded};
+ {
+ updateStatus(SkillStatus::Succeeded);
+
+ // return result of main method
+ std::unique_lock l(skillStatusesMutex);
+ TerminatedSkillStatusUpdate ret{
+ {.executionId = statusUpdate.executionId,
+ .parameters = statusUpdate.parameters,
+ .callbackInterface = statusUpdate.callbackInterface}};
+ ret.result = mainRet.data;
+ ret.status = TerminatedSkillStatus::Succeeded;
+ return ret;
+ }
}
- }
-}
+ } // namespace skills::detail
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index 982bcc366dd415ceb85ce055a35992ae99a8165f..e5a5b1f82c3db4e5939fd4c880a738373967402b 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -2,11 +2,12 @@
#include <shared_mutex>
-#include "../SkillDescription.h"
-#include "../SkillStatusUpdate.h"
-#include "../Skill.h"
-
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
+#include <RobotAPI/libraries/skills/core/Skill.h>
+#include <RobotAPI/libraries/skills/core/SkillDescription.h>
+#include <RobotAPI/libraries/skills/core/SkillPreparationInput.h>
+#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+#include <RobotAPI/libraries/skills/provider/SkillFactory.h>
namespace armarx
{
@@ -14,28 +15,39 @@ namespace armarx
{
namespace detail
{
- class SkillImplementationWrapper
+ class SkillRuntime
{
- public:
- // fixed values. Do not change after skill instantiation
- const std::unique_ptr<Skill> skill;
+ private:
+ const skills::SkillBlueprint factory;
+
+ std::unique_ptr<Skill> skill;
+ mutable std::mutex executionMutex;
- // Current execution status. Changes during execution
+ public:
+ // Current execution statuses. Changes during execution
// The status also holds the used parameterization
- // skillName and providerName are const after registering the skill in a provider
- mutable std::shared_mutex skillStatusMutex;
+ mutable std::mutex skillStatusesMutex;
SkillStatusUpdate statusUpdate;
- // Task information
- mutable std::shared_mutex executingMutex;
+ // Set exteranally to store the execution somewhere
+ std::thread execution;
// ctor
- SkillImplementationWrapper(std::unique_ptr<skills::Skill>&& skill);
+ 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)
- TerminatedSkillStatusUpdate setupAndExecuteSkill(const std::string& executorName, const skills::SkillParameterization);
+ TerminatedSkillStatusUpdate executeSkill();
+
+ // ask a skill to stop. Must be concurrent to execute skill
+ void stopSkill();
+
+ // add parameters to a skill. Must be concurrent to execute skill
+ void updateSkillParameters(const aron::data::DictPtr& i);
};
- }
- }
-}
+ } // namespace detail
+ } // namespace skills
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/skills.cpp b/source/RobotAPI/libraries/skills/skills.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/libraries/skills/skills.h b/source/RobotAPI/libraries/skills/skills.h
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
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