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/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml b/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
index 12a22e5a6776cc9bf98e540065b18a469c2f46f8..1fa5e30d4c0bc0f881b60a7c9a79a105739f1b93 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
@@ -114,6 +114,15 @@
<CollisionModel>
<File type="Inventor">convexModel/neck_yaw_link.wrl</File>
</CollisionModel>
+ <PrimitiveModel>
+ <Primitives>
+ <Sphere radius="140">
+ <Transform>
+ <Translation x="40" y="0" z="180" />
+ </Transform>
+ </Sphere>
+ </Primitives>
+ </PrimitiveModel>
<Child name="Head_Tilt"/>
</RobotNode>
diff --git a/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml b/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml
index ecf4546927d69f887db438d0a00d3a483ffa247d..2b1f340f2e9edf79fcd7faf9f0faecf808a04804 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII-LeftArm.xml
@@ -33,6 +33,16 @@
<CollisionModel>
<File type="Inventor">convexModel/shoulder_l.wrl</File>
</CollisionModel>
+ <PrimitiveModel>
+ <Primitives>
+ <Capsule height="80" radius="120">
+ <Transform>
+ <Translation x="0" y="0" z="-90" />
+ <rpy roll="90" pitch="0" yaw="00" unit="degree"/>
+ </Transform>
+ </Capsule>
+ </Primitives>
+ </PrimitiveModel>
<Child name="Shoulder 2 L"/>
</RobotNode>
@@ -60,6 +70,15 @@
<CollisionModel>
<File type="Inventor">convexModel/shoulder2_l_rot.wrl</File>
</CollisionModel>
+ <PrimitiveModel>
+ <Primitives>
+ <Capsule height="310" radius="50">
+ <Transform>
+ <Translation x="0" y="150" z="0" />
+ </Transform>
+ </Capsule>
+ </Primitives>
+ </PrimitiveModel>
<Child name="Upperarm L"/>
</RobotNode>
@@ -138,6 +157,16 @@
<CollisionModel>
<File type="Inventor">convexModel/underarm_l.wrl</File>
</CollisionModel>
+ <PrimitiveModel>
+ <Primitives>
+ <Capsule height="240" radius="50">
+ <Transform>
+ <Translation x="0" y="0" z="-110" />
+ <rpy roll="90" pitch="0" yaw="0" unit="degree"/>
+ </Transform>
+ </Capsule>
+ </Primitives>
+ </PrimitiveModel>
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="2.26566087" units="kg" />
diff --git a/data/RobotAPI/robots/Armar3/ArmarIII-RightArm.xml b/data/RobotAPI/robots/Armar3/ArmarIII-RightArm.xml
index d311a2e63fde6cf2f82eed5c12e904e131ffe08a..8f4c401573a3e238383431a1143f319b7f286c68 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII-RightArm.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII-RightArm.xml
@@ -34,6 +34,16 @@
<CollisionModel>
<File type="Inventor">convexModel/shoulder_r.wrl</File>
</CollisionModel>
+ <PrimitiveModel>
+ <Primitives>
+ <Capsule height="80" radius="120">
+ <Transform>
+ <Translation x="0" y="0" z="90" />
+ <rpy roll="90" pitch="0" yaw="00" unit="degree"/>
+ </Transform>
+ </Capsule>
+ </Primitives>
+ </PrimitiveModel>
<Child name="Shoulder 2 R"/>
</RobotNode>
@@ -59,6 +69,15 @@
<CollisionModel>
<File type="Inventor">convexModel/shoulder2_r_rot.wrl</File>
</CollisionModel>
+ <PrimitiveModel>
+ <Primitives>
+ <Capsule height="310" radius="50">
+ <Transform>
+ <Translation x="0" y="150" z="0" />
+ </Transform>
+ </Capsule>
+ </Primitives>
+ </PrimitiveModel>
<Child name="Upperarm R"/>
</RobotNode>
@@ -133,6 +152,16 @@
<CollisionModel>
<File type="Inventor">convexModel/underarm_r.wrl</File>
</CollisionModel>
+ <PrimitiveModel>
+ <Primitives>
+ <Capsule height="240" radius="50">
+ <Transform>
+ <Translation x="0" y="0" z="-110" />
+ <rpy roll="90" pitch="0" yaw="0" unit="degree"/>
+ </Transform>
+ </Capsule>
+ </Primitives>
+ </PrimitiveModel>
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="2.26566087" units="kg" />
diff --git a/data/RobotAPI/robots/Armar3/ArmarIII.xml b/data/RobotAPI/robots/Armar3/ArmarIII.xml
index 0d089db9f076105390f9d5d9ae198189acd23d5b..3668928088271ddfc5ba8bcd2bbaa201512d930d 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII.xml
@@ -70,6 +70,16 @@
<CollisionModel>
<File type="Inventor">convexModel/platform.wrl</File>
</CollisionModel>
+ <PrimitiveModel>
+ <Primitives>
+ <Capsule height="400" radius="390">
+ <Transform>
+ <Translation x="0" y="0" z="380" />
+ <rpy roll="90" pitch="0" yaw="0" unit="degree"/>
+ </Transform>
+ </Capsule>
+ </Primitives>
+ </PrimitiveModel>
<Child name="Laser Scanner Front"/>
<Child name="Laser Scanner Back Left"/>
<Child name="Laser Scanner Back Right"/>
@@ -175,6 +185,16 @@
<CollisionModel>
<File type="Inventor">convexModel/torso.wrl</File>
</CollisionModel>
+ <PrimitiveModel>
+ <Primitives>
+ <Capsule height="400" radius="210">
+ <Transform>
+ <Translation x="0" y="-40" z="225" />
+ <rpy roll="90" pitch="0" yaw="0" unit="degree"/>
+ </Transform>
+ </Capsule>
+ </Primitives>
+ </PrimitiveModel>
<Child name="Center of Arms"/>
</RobotNode>
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/ArMemExample/config/ExampleMemory.cfg b/scenarios/ArMemExample/config/ExampleMemory.cfg
index 251382f2af086afccf73924421ea31a2aad01017..274619e2d4d59a0921c674950c7dd6413b7535d1 100644
--- a/scenarios/ArMemExample/config/ExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemory.cfg
@@ -145,19 +145,27 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemory.mem.ltm.configuration:
# Attributes:
-# - Default: {}
+# - Default: {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.configuration = {}
+# ArmarX.ExampleMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
-# ArmarX.ExampleMemory.mem.ltm.enabled:
+# ArmarX.ExampleMemory.mem.ltm.enable_querying:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.enabled = false
+# ArmarX.ExampleMemory.mem.ltm.enable_querying = false
+
+
+# ArmarX.ExampleMemory.mem.ltm.mode:
+# Attributes:
+# - Default: DISABLED
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemory.mem.ltm.mode = DISABLED
# ArmarX.ExampleMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/ArticulatedObjectWriterExample/ArticulatedObjectWriterExample.scx b/scenarios/ArticulatedObjectWriterExample/ArticulatedObjectWriterExample.scx
index 9e4ef2810eae6db0c1a475e9b220fae7e18418b6..af532a553a7b0b7a2d140c40bbd0637deec91cc3 100644
--- a/scenarios/ArticulatedObjectWriterExample/ArticulatedObjectWriterExample.scx
+++ b/scenarios/ArticulatedObjectWriterExample/ArticulatedObjectWriterExample.scx
@@ -6,5 +6,6 @@
<application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotStateMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArticulatedObjectWriterExample/config/ArticulatedObjectLocalizerExample.cfg b/scenarios/ArticulatedObjectWriterExample/config/ArticulatedObjectLocalizerExample.cfg
index e6c221120c7a8f33693caf9dc5fa229875dabb4a..3754d03657a08e8d6105dddaf36338864203681c 100644
--- a/scenarios/ArticulatedObjectWriterExample/config/ArticulatedObjectLocalizerExample.cfg
+++ b/scenarios/ArticulatedObjectWriterExample/config/ArticulatedObjectLocalizerExample.cfg
@@ -60,14 +60,6 @@
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName = Object
-# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.read.ProviderName:
-# Attributes:
-# - Default: PriorKnowledgeData
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.read.ProviderName = PriorKnowledgeData
-
-
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.write.ProviderName: Name of this provider
# Attributes:
# - Case sensitivity: yes
@@ -98,7 +90,7 @@ ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.write.ProviderName
# - Default: mobile-dishwasher
# - Case sensitivity: yes
# - Required: no
-ArmarX.ArticulatedObjectLocalizerExample.p.obj.class = cabinet
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.class = mobile-dishwasher
# ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset:
@@ -106,7 +98,7 @@ ArmarX.ArticulatedObjectLocalizerExample.p.obj.class = cabinet
# - Default: Kitchen
# - Case sensitivity: yes
# - Required: no
-ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset = RBO-articulated-object-dataset
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset = Kitchen
# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
diff --git a/scenarios/ArticulatedObjectWriterExample/config/DebugObserver.cfg b/scenarios/ArticulatedObjectWriterExample/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..8dc7ead26b3bd2f7678b3b3e7a1b00c01213225d
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/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/ArticulatedObjectWriterExample/config/ObjectMemory.cfg b/scenarios/ArticulatedObjectWriterExample/config/ObjectMemory.cfg
index 95961b39febd5772f9f3ee485552631f407b73cc..0297d31a40c207ebb82d244091d7be2e6c53efea 100644
--- a/scenarios/ArticulatedObjectWriterExample/config/ObjectMemory.cfg
+++ b/scenarios/ArticulatedObjectWriterExample/config/ObjectMemory.cfg
@@ -150,6 +150,22 @@
# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
+# ArmarX.ObjectMemory.mem..marker.Name: Marker Memory Name
+# Attributes:
+# - Default: Marker
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem..marker.Name = Marker
+
+
+# ArmarX.ObjectMemory.mem..marker.maxHistorySize: Maximum marker memory history size
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem..marker.maxHistorySize = -1
+
+
# ArmarX.ObjectMemory.mem.MemoryName: Name of this memory server.
# Attributes:
# - Default: Object
@@ -375,14 +391,17 @@
# ArmarX.ObjectMemory.mem.inst.scene.11_Directory = scenes
-# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
-# You can also specify paths relative to 'Package/scenes/'.
-# You can also specify a ; separated list of scenes.
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene(s) to load on startup.
+# Specify multiple scenes in a ; separated list.
+# Each entry must be one of the following:
+# (1) A scene file in 'Package/scenes/' (with or without '.json' extension), e.g. 'MyScene', 'MyScene.json'
+# (2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' extension), e.g. 'path/to/MyScene', 'path/to/MyScene.json'
+# (3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = ""
+ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003
# ArmarX.ObjectMemory.mem.inst.seg.CoreMaxHistorySize: Maximal size of the Instance entity histories (-1 for infinite).
@@ -558,10 +577,10 @@
# ArmarX.ObjectMemory.mem.ltm.configuration:
# Attributes:
-# - Default: {}
+# - Default: {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.ltm.configuration = {}
+# ArmarX.ObjectMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# ArmarX.ObjectMemory.mem.ltm.enabled:
diff --git a/scenarios/ArticulatedObjectWriterExample/config/RobotStateMemory.cfg b/scenarios/ArticulatedObjectWriterExample/config/RobotStateMemory.cfg
index 5839620e5e3c5df7e080f39405a021bdbcd4a0ea..fd3b8a1217bb4f5732e7a1dbf78a25db133a7223 100644
--- a/scenarios/ArticulatedObjectWriterExample/config/RobotStateMemory.cfg
+++ b/scenarios/ArticulatedObjectWriterExample/config/RobotStateMemory.cfg
@@ -216,6 +216,22 @@
# ArmarX.RobotStateMemory.mem.desc.seg.CoreSegmentName = Description
+# ArmarX.RobotStateMemory.mem.ext.seg.CoreMaxHistorySize: Maximal size of the Exteroception entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.ext.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.RobotStateMemory.mem.ext.seg.CoreSegmentName: Name of the Exteroception core segment.
+# Attributes:
+# - Default: Exteroception
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.ext.seg.CoreSegmentName = Exteroception
+
+
# ArmarX.RobotStateMemory.mem.loc.seg.CoreMaxHistorySize: Maximal size of the Localization entity histories (-1 for infinite).
# Attributes:
# - Default: 1024
@@ -234,10 +250,10 @@
# ArmarX.RobotStateMemory.mem.ltm.configuration:
# Attributes:
-# - Default: {}
+# - Default: {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotStateMemory.mem.ltm.configuration = {}
+# ArmarX.RobotStateMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# ArmarX.RobotStateMemory.mem.ltm.enabled:
@@ -274,6 +290,15 @@
# ArmarX.RobotStateMemory.mem.visu.enabled = true
+# ArmarX.RobotStateMemory.mem.visu.famesEnabled: Enable or disable visualization of frames.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.visu.famesEnabled = false
+
+
# ArmarX.RobotStateMemory.mem.visu.frequenzyHz: Frequency of visualization.
# Attributes:
# - Default: 25
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/RobotSkillsMemory/RobotSkillsMemory.scx b/scenarios/RobotSkillsMemory/RobotSkillsMemory.scx
index 9622c34e544899a5f2e81e01f5f6f7ad6744b013..84c9820b1df4a8304787c971df18f978d8096cd4 100644
--- a/scenarios/RobotSkillsMemory/RobotSkillsMemory.scx
+++ b/scenarios/RobotSkillsMemory/RobotSkillsMemory.scx
@@ -3,5 +3,7 @@
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" 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="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/RobotSkillsMemory/config/ArVizStorage.cfg b/scenarios/RobotSkillsMemory/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..302ac28c37dd28de3e68fb4fe4c2174faa4ec3bf
--- /dev/null
+++ b/scenarios/RobotSkillsMemory/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/RobotSkillsMemory/config/DebugObserver.cfg b/scenarios/RobotSkillsMemory/config/DebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..8dc7ead26b3bd2f7678b3b3e7a1b00c01213225d
--- /dev/null
+++ b/scenarios/RobotSkillsMemory/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/RobotSkillsMemory/config/MemoryNameSystem.cfg b/scenarios/RobotSkillsMemory/config/MemoryNameSystem.cfg
index 7dd22218243ca4f9e67e843da8b42916f3b8568a..b8bc70a66ca7f32a628886ad1bf13e373f9750d3 100644
--- a/scenarios/RobotSkillsMemory/config/MemoryNameSystem.cfg
+++ b/scenarios/RobotSkillsMemory/config/MemoryNameSystem.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/RobotSkillsMemory/config/RemoteGuiProviderApp.cfg b/scenarios/RobotSkillsMemory/config/RemoteGuiProviderApp.cfg
index 4fd690cefd94559b207493cf40e346a3e47f3b12..4b6abea40d72afd7d313ee47a9b191f3b26de30d 100644
--- a/scenarios/RobotSkillsMemory/config/RemoteGuiProviderApp.cfg
+++ b/scenarios/RobotSkillsMemory/config/RemoteGuiProviderApp.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/RobotSkillsMemory/config/SkillsMemory.cfg b/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
index 33ff12f5046dad45169c1661741ff7216055c26d..57d1fbc563156ae096bdaad0324321323a1a0724 100644
--- a/scenarios/RobotSkillsMemory/config/SkillsMemory.cfg
+++ b/scenarios/RobotSkillsMemory/config/SkillsMemory.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
@@ -117,88 +117,105 @@
# ArmarX.SecondsStartupDelay = 0
-# ArmarX.SkillsMemory.:
+# ArmarX.SkillMemory.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.SkillMemory.EnableProfiling = false
+
+
+# ArmarX.SkillMemory.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.SkillMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.SkillMemory.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory. = ""
+# ArmarX.SkillMemory.ObjectName = ""
-# ArmarX.SkillsMemory.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.SkillMemory.StatechartCoreSegmentName: Name of the core segment for statecharts.
# Attributes:
-# - Default: false
+# - Default: Statechart
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillsMemory.EnableProfiling = false
+# ArmarX.SkillMemory.StatechartCoreSegmentName = Statechart
-# ArmarX.SkillsMemory.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.SkillMemory.TransitionsProviderSegmentName: Name of the provider segment for statechart transitions.
# Attributes:
-# - Default: Undefined
+# - Default: Transitions
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.SkillsMemory.MinimumLoggingLevel = Undefined
+# ArmarX.SkillMemory.TransitionsProviderSegmentName = Transitions
-# ArmarX.SkillsMemory.ObjectName: Name of IceGrid well-known object
+# ArmarX.SkillMemory.mem.MemoryName: Name of this memory server.
# Attributes:
-# - Default: ""
+# - Default: Skill
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.ObjectName = ""
+# ArmarX.SkillMemory.mem.MemoryName = Skill
-# ArmarX.SkillsMemory.mem.MemoryName: Name of this memory server.
+# ArmarX.SkillMemory.mem.ltm.configuration:
# Attributes:
-# - Default: Skills
+# - Default: {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.mem.MemoryName = Skills
+# ArmarX.SkillMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
-# ArmarX.SkillsMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.SkillMemory.mem.ltm.enable_querying:
# Attributes:
-# - Default: true
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillsMemory.mns.MemoryNameSystemEnabled = true
+# ArmarX.SkillMemory.mem.ltm.enable_querying = false
-# ArmarX.SkillsMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.SkillMemory.mem.ltm.mode:
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: DISABLED
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.SkillMemory.mem.ltm.mode = DISABLED
-# ArmarX.SkillsMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# ArmarX.SkillMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
# Attributes:
-# - Default: DebugObserver
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.tpc.pub.DebugObserver = DebugObserver
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.SkillMemory.mns.MemoryNameSystemEnabled = true
-# ArmarX.SkillsMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# ArmarX.SkillMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: MemoryUpdates
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.tpc.pub.MemoryListener = MemoryUpdates
+# ArmarX.SkillMemory.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.SkillsMemory.tpc.sub.ProfilerListener: Name of the ProfilerListenerInterface topics to subscribe.
+# ArmarX.SkillMemory.tpc.sub.ProfilerListener: Name of the ProfilerListenerInterface topics to subscribe.
# Attributes:
# - Default: StateReportingTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillsMemory.tpc.sub.ProfilerListener = StateReportingTopic
+# ArmarX.SkillMemory.tpc.sub.ProfilerListener = StateReportingTopic
# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
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 8b2bf04855a21e5a8ca955da89ba3a309460241e..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
@@ -169,10 +178,10 @@
# ArmarX.SkillMemory.mem.ltm.configuration:
# Attributes:
-# - Default: {}
+# - Default: {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillMemory.mem.ltm.configuration = {}
+# ArmarX.SkillMemory.mem.ltm.configuration = {"SnapshotFrequencyFilter": { "WaitingTimeInMs": 1000}, "PngConverter": {}}
# ArmarX.SkillMemory.mem.ltm.enabled:
diff --git a/source/RobotAPI/applications/AronCodeGenerator/CMakeLists.txt b/source/RobotAPI/applications/AronCodeGenerator/CMakeLists.txt
index c25c4215e20f19107a7ff6de7b46b2e396236962..e5b6b01d37403febbacd1088e35125c51a275ea1 100644
--- a/source/RobotAPI/applications/AronCodeGenerator/CMakeLists.txt
+++ b/source/RobotAPI/applications/AronCodeGenerator/CMakeLists.txt
@@ -1,5 +1,5 @@
armarx_component_set_name("AronCodeGeneratorApp")
-set(COMPONENT_LIBS ArmarXCore aroncodegenerator)
+set(COMPONENT_LIBS ArmarXCore aroncodegeneration)
set(SOURCES
main.cpp
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/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index f2d406fece5753b91d89cc56f26c824711efde62..84dcad9ce36dc40c34553afb085aa3cdd1925460 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -80,8 +80,8 @@ namespace viz
InteractionFeedbackRange interactions() const
{
- InteractionFeedback* begin = (InteractionFeedback*) data_.interactions.data();
- InteractionFeedback* end = begin + data_.interactions.size();
+ const InteractionFeedback* begin = reinterpret_cast<const InteractionFeedback*>(data_.interactions.data());
+ const InteractionFeedback* end = begin + data_.interactions.size();
return InteractionFeedbackRange{begin, end};
}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h b/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h
index f00061ea4c9114c06981863032cca209a4240806..b4a45c3a361cbd0cc9bb6a9889f102828fdfe18a 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h
@@ -82,7 +82,7 @@ namespace armarx::viz
vertices.reserve(p3.size_of_vertices());
faces.reserve(p3.size_of_facets());
auto vbeg = p3.vertices_begin();
- for (const auto& v : IteratorRange{vbeg, p3.vertices_end()})
+ for (const auto& v : CGAL::Iterator_range{vbeg, p3.vertices_end()})
{
const auto& p = v.point();
armarx::Vector3f visp;
@@ -91,7 +91,7 @@ namespace armarx::viz
visp.e2 = p.z();
vertices.emplace_back(visp);
}
- for (const auto& fidx : IteratorRange{p3.facets_begin(), p3.facets_end()})
+ for (const auto& fidx : CGAL::Iterator_range{p3.facets_begin(), p3.facets_end()})
{
auto circ = fidx.facet_begin();
ARMARX_CHECK_EQUAL(3, CGAL::circulator_size(circ))
diff --git a/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h b/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
index a77f79632d3f29955f46773d0aea5c5602dfd0f9..78591128d2daddab633ef3fd04139a4c46ba2340 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
@@ -71,8 +71,8 @@ namespace armarx::viz
PointCloud& points(std::vector<ColoredPoint> const& ps)
{
std::size_t memorySize = ps.size() * sizeof(ps[0]);
- Ice::Byte* begin = (Ice::Byte*)ps.data();
- Ice::Byte* end = begin + memorySize;
+ const Ice::Byte* const begin = reinterpret_cast<const Ice::Byte*>(ps.data());
+ const Ice::Byte* const end = begin + memorySize;
data_->points.assign(begin, end);
return *this;
}
@@ -92,8 +92,8 @@ namespace armarx::viz
PointCloud& addPointUnchecked(ColoredPoint const& p)
{
- Ice::Byte* begin = (Ice::Byte*)&p;
- Ice::Byte* end = begin + sizeof(p);
+ const Ice::Byte* const begin = reinterpret_cast<const Ice::Byte*>(&p);
+ const Ice::Byte* const end = begin + sizeof(p);
data_->points.insert(data_->points.end(), begin, end);
return *this;
}
@@ -391,5 +391,3 @@ namespace armarx::viz
};
}
-
-
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/AronComponentConfigExample/aron/ComponentConfig.xml b/source/RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.xml
index aabd27602d4f4ef4f0313dd00af36ccfa9c2a9dd..d674de128dc90f3711fcc1120f34f8a8f812fbcd 100644
--- a/source/RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.xml
+++ b/source/RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.xml
@@ -15,13 +15,13 @@
<string/>
</ObjectChild>
<ObjectChild key='intMember'>
- <int/>
+ <int32 />
</ObjectChild>
<ObjectChild key='floatMember'>
- <float/>
+ <float32 />
</ObjectChild>
<ObjectChild key='longMember'>
- <long/>
+ <int64 />
</ObjectChild>
<ObjectChild key='boolMember'>
<bool/>
@@ -36,7 +36,7 @@
</ObjectChild>
<ObjectChild key='intListMember'>
<List>
- <int/>
+ <int32 />
</List>
</ObjectChild>
<ObjectChild key='stringDictMember'>
@@ -46,7 +46,7 @@
</ObjectChild>
<ObjectChild key='intDictMember'>
<Dict>
- <int/>
+ <int32 />
</Dict>
</ObjectChild>
</Object>
@@ -56,13 +56,13 @@
<string/>
</ObjectChild>
<ObjectChild key='intMember'>
- <int/>
+ <int32 />
</ObjectChild>
<ObjectChild key='floatMember'>
- <float/>
+ <float32 />
</ObjectChild>
<ObjectChild key='doubleMember'>
- <double/>
+ <float64 />
</ObjectChild>
<ObjectChild key='boolMember'>
<bool/>
@@ -77,13 +77,13 @@
<string/>
</ObjectChild>
<ObjectChild key='intMember'>
- <int/>
+ <int32 />
</ObjectChild>
<ObjectChild key='floatMember'>
- <float/>
+ <float32 />
</ObjectChild>
<ObjectChild key='longMember'>
- <long/>
+ <int64 />
</ObjectChild>
<ObjectChild key='boolMember'>
<bool/>
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 77943d887c6833b87a730b1ce45b926273795ad2..e5762482e69b710fa361bf26dd5705849b4b0efb 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -32,6 +32,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -75,11 +76,10 @@ namespace armarx::articulated_object
void
ArticulatedObjectLocalizerExample::onInitComponent()
{
- auto& articulatedObjectReader = articulatedObjectReaderPlugin->get();
+ // auto& articulatedObjectReader = articulatedObjectReaderPlugin->get();
auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
-
- // Reader overwrote property registered property of articulatedObjectWriter.
- articulatedObjectWriter.setProviderName(articulatedObjectReader.getProviderName());
+
+ articulatedObjectWriter.setProviderName(getName());
}
void
@@ -110,14 +110,15 @@ namespace armarx::articulated_object
ArticulatedObjectLocalizerExample::createArticulatedObject()
{
auto& articulatedObjectReader = articulatedObjectReaderPlugin->get();
-
+
const std::string dishwasherName = p.obj.dataset + "/" + p.obj.className;
- const auto descriptions = articulatedObjectReader.queryDescriptions(armem::Time::Now());
+ const auto descriptions =
+ articulatedObjectReader.queryDescriptions(armem::Time::Now(), std::nullopt);
ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
- for(const auto& description: descriptions)
+ for (const auto& description : descriptions)
{
ARMARX_INFO << "- " << description.name;
}
@@ -134,9 +135,8 @@ namespace armarx::articulated_object
return nullptr;
}
- auto obj =
- VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
- VirtualRobot::RobotIO::eStructure);
+ auto obj = VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(),
+ VirtualRobot::RobotIO::eStructure);
if (not obj)
{
@@ -149,7 +149,6 @@ namespace armarx::articulated_object
return obj;
}
-
void
ArticulatedObjectLocalizerExample::run()
{
@@ -163,6 +162,14 @@ namespace armarx::articulated_object
}
}
+ const auto state = articulatedObjectReaderPlugin->get().queryState(
+ articulatedObject->getType() + "/" + articulatedObject->getName(),
+ Clock::Now(),
+ p.obj.readProviderName);
+
+ ARMARX_CHECK(state.has_value());
+ articulatedObject->setGlobalPose(state->globalPose.matrix());
+
ARMARX_DEBUG << "Reporting articulated objects";
const armem::Time now = armem::Time::Now();
@@ -179,7 +186,7 @@ namespace armarx::articulated_object
jointValue = node->unscaleJointValue(k, 0, 1);
}
- articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
+ // articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
articulatedObject->setJointValues(jointValues);
auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index 384068048dd1ab8d1457065d15873de08ff7b82b..56c025a1260c2398b0e25d3e9f990146d5285fdc 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -81,6 +81,8 @@ namespace armarx::articulated_object
{
std::string dataset = "Kitchen";
std::string className = "mobile-dishwasher";
+
+ std::string readProviderName = "R003";
} obj;
} p;
};
diff --git a/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt b/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
index c3b764a153b15400568f67eb1dc9c2813874b921..8154ff69af2b057c18189c6ad07262e2a6f33142 100644
--- a/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
@@ -1,6 +1,6 @@
armarx_component_set_name("GamepadControlUnit")
-set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers RobotAPIInterfaces )
+set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers RobotAPIComponentPlugins RobotAPIInterfaces )
set(SOURCES GamepadControlUnit.cpp)
set(HEADERS GamepadControlUnit.h)
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
index 5227bee38e728999d4a0f7fed6809913144573b6..5113961eeebb95c1af593714fbb1eca93ea84909 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
@@ -22,15 +22,15 @@
#include "GamepadControlUnit.h"
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
-
-
-
namespace armarx
{
- void GamepadControlUnit::onInitComponent()
+ void
+ GamepadControlUnit::onInitComponent()
{
ARMARX_INFO << "oninit GamepadControlUnit";
usingProxy(getProperty<std::string>("PlatformUnitName").getValue());
@@ -41,67 +41,80 @@ namespace armarx
scaleY = getProperty<float>("ScaleY").getValue();
scaleRotation = getProperty<float>("ScaleAngle").getValue();
ARMARX_INFO << "oninit GamepadControlUnit end";
-
-
-
}
-
- void GamepadControlUnit::onConnectComponent()
+ void
+ GamepadControlUnit::onConnectComponent()
{
ARMARX_INFO << "onConnect GamepadControlUnit";
- platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(getProperty<std::string>("PlatformUnitName").getValue());
+ platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(
+ getProperty<std::string>("PlatformUnitName").getValue());
emergencyStop = getProxy<EmergencyStopMasterInterfacePrx>("EmergencyStopMaster");
- }
- void GamepadControlUnit::onDisconnectComponent()
- {
-
+ if (enableHeartBeat)
+ {
+ this->heartbeatPlugin->signUp(armarx::core::time::Duration::MilliSeconds(1000),
+ armarx::core::time::Duration::MilliSeconds(1500),
+ {"Gamepad"},
+ "The GamepadControlUnit");
+ }
}
+ void
+ GamepadControlUnit::onDisconnectComponent()
+ {
+ }
- void GamepadControlUnit::onExitComponent()
+ void
+ GamepadControlUnit::onExitComponent()
{
ARMARX_INFO << "exit GamepadControlUnit";
}
- armarx::PropertyDefinitionsPtr GamepadControlUnit::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ GamepadControlUnit::createPropertyDefinitions()
{
- auto defs = armarx::PropertyDefinitionsPtr(new GamepadControlUnitPropertyDefinitions(
- getConfigIdentifier()));
- defs->topic(robotHealthTopic);
- defs->optional(enableHeartBeat, "EnableHeartBeat", "Flag to enable send a heart beat to the robot healh topic");
- return defs;
+ auto defs = armarx::PropertyDefinitionsPtr(
+ new GamepadControlUnitPropertyDefinitions(getConfigIdentifier()));
+ defs->optional(enableHeartBeat,
+ "EnableHeartBeat",
+ "Flag to enable send a heart beat to the robot healh topic");
+ return defs;
}
- void GamepadControlUnit::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
+ void
+ GamepadControlUnit::reportGamepadState(const std::string& device,
+ const std::string& name,
+ const GamepadData& data,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c)
{
- // struct GamepadData {
- // float leftStickX;
- // float leftStickY;
- // float rightStickX;
- // float rightStickY;
- // float dPadX;
- // float dPadY;
- // float leftTrigger;
- // float rightTrigger;
- //
- // bool leftButton;
- // bool rightButton;
- // bool backButton;
- // bool startButton;
- // bool xButton;
- // bool yButton;
- // bool aButton;
- // bool bButton;
- // bool theMiddleButton;
- // bool leftStickButton;
- // bool rightStickButton;
- //
- // };
+ // struct GamepadData {
+ // float leftStickX;
+ // float leftStickY;
+ // float rightStickX;
+ // float rightStickY;
+ // float dPadX;
+ // float dPadY;
+ // float leftTrigger;
+ // float rightTrigger;
+ //
+ // bool leftButton;
+ // bool rightButton;
+ // bool backButton;
+ // bool startButton;
+ // bool xButton;
+ // bool yButton;
+ // bool aButton;
+ // bool bButton;
+ // bool theMiddleButton;
+ // bool leftStickButton;
+ // bool rightStickButton;
+ //
+ // };
if (data.leftTrigger > 0)
@@ -116,7 +129,9 @@ namespace armarx
//scales are for the robot axis
if (data.rightTrigger > 0)
{
- platformUnitPrx->move(data.leftStickY * scaleX, data. leftStickX * scaleY, data.rightStickX * scaleRotation);
+ platformUnitPrx->move(data.leftStickY * scaleX,
+ data.leftStickX * scaleY,
+ data.rightStickX * scaleRotation);
}
else
{
@@ -124,64 +139,68 @@ namespace armarx
}
- if (data.leftButton)
- {
-
- if(leftHandTime <= 0.0)
- {
- leftHandTime = IceUtil::Time::now().toMicroSeconds();
- }
- else if ((IceUtil::Time::now().toMicroSeconds() - leftHandTime) > 1000* 1000)
- {
-
- HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("LeftHandUnit");
- if (handUnit)
- {
- std::string shapeName = (leftHandOpen) ? "Close" : "Open";
- handUnit->setShape(shapeName);
- leftHandOpen = !leftHandOpen;
- leftHandTime = 0.0;
- }
- }
- }
- else
- {
- leftHandTime = 0.0;
- }
-
- if(data.rightButton)
- {
-
- if(rightHandTime <= 0.0)
- {
- rightHandTime = IceUtil::Time::now().toMicroSeconds();
- }
- else if ((IceUtil::Time::now().toMicroSeconds() - rightHandTime) > 1000* 1000)
- {
- HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("RightHandUnit");
- if (handUnit)
- {
- std::string shapeName = (rightHandOpen) ? "Close" : "Open";
- handUnit->setShape(shapeName);
- rightHandOpen = !rightHandOpen;
- rightHandTime = 0.0;
- }
- }
- }
- else
- {
- rightHandTime = 0.0;
- }
-
-
- if(enableHeartBeat)
- {
- RobotHealthHeartbeatArgs args;
- args.maximumCycleTimeErrorMS = 1000;
- robotHealthTopic->heartbeat(getDefaultName(), args);
-
- }
-
+ if (data.leftButton)
+ {
+
+ if (leftHandTime <= 0.0)
+ {
+ leftHandTime = IceUtil::Time::now().toMicroSeconds();
+ }
+ else if ((IceUtil::Time::now().toMicroSeconds() - leftHandTime) > 1000 * 1000)
+ {
+
+ HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("LeftHandUnit");
+ if (handUnit)
+ {
+ std::string shapeName = (leftHandOpen) ? "Close" : "Open";
+ handUnit->setShape(shapeName);
+ leftHandOpen = !leftHandOpen;
+ leftHandTime = 0.0;
+ }
+ }
+ }
+ else
+ {
+ leftHandTime = 0.0;
+ }
+
+ if (data.rightButton)
+ {
+
+ if (rightHandTime <= 0.0)
+ {
+ rightHandTime = IceUtil::Time::now().toMicroSeconds();
+ }
+ else if ((IceUtil::Time::now().toMicroSeconds() - rightHandTime) > 1000 * 1000)
+ {
+ HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("RightHandUnit");
+ if (handUnit)
+ {
+ std::string shapeName = (rightHandOpen) ? "Close" : "Open";
+ handUnit->setShape(shapeName);
+ rightHandOpen = !rightHandOpen;
+ rightHandTime = 0.0;
+ }
+ }
+ }
+ else
+ {
+ rightHandTime = 0.0;
+ }
+
+
+ if (enableHeartBeat)
+ {
+ auto now = armarx::core::time::dto::DateTime();
+ armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+ heartbeatPlugin->heartbeat();
+ }
+
//ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation;
}
-}
+
+ GamepadControlUnit::GamepadControlUnit()
+ {
+ addPlugin(heartbeatPlugin);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
index da5945c44c7e289081df35385ea7e75184d69c90..9438f305fe6a96648eb4d6011112ea637a245369 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
@@ -30,6 +30,7 @@
#include <RobotAPI/interface/units/HandUnitInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
@@ -72,6 +73,8 @@ namespace armarx
virtual public GamepadUnitListener
{
public:
+ GamepadControlUnit();
+
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
@@ -109,10 +112,11 @@ namespace armarx
private:
PlatformUnitInterfacePrx platformUnitPrx;
+ plugins::HeartbeatComponentPlugin* heartbeatPlugin = nullptr;
- bool enableHeartBeat = false;
- RobotHealthInterfacePrx robotHealthTopic;
+
+ bool enableHeartBeat = false;
float scaleX;
float scaleY;
float scaleRotation;
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index 0e4b7f72ac1cc1f5f9b69b678c2d2f790c479184..8c14b9de7a9d80bcf2b09d45ae3253b94a2c8b5b 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -20,296 +20,535 @@
* GNU General Public License
*/
-#include <ArmarXCore/util/CPPUtility/trace.h>
-
#include "RobotHealth.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/DateTime.h"
+#include "ArmarXCore/core/time/Duration.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <algorithm>
#include <boost/regex.hpp>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <mutex>
+#include <optional>
namespace armarx
{
- void RobotHealth::onInitComponent()
- {
- ARMARX_TRACE;
- monitorHealthTask = new PeriodicTask<RobotHealth>(this, &RobotHealth::monitorHealthTaskClb, 10);
- defaultMaximumCycleTimeWarn = getProperty<int>("MaximumCycleTimeWarnMS").getValue();
- defaultMaximumCycleTimeErr = getProperty<int>("MaximumCycleTimeErrMS").getValue();
- usingTopic(getProperty<std::string>("RobotHealthTopicName").getValue());
- reportErrorsWithSpeech = getProperty<bool>("ReportErrorsWithSpeech").getValue();
- speechMinimumReportInterval = getProperty<int>("SpeechMinimumReportInterval").getValue();
+ inline const std::string PROPERTY_REQUESTER_ID = "required_default";
+ void
+ RobotHealth::onInitComponent()
+ {
ARMARX_TRACE;
- //robotUnitRequired = getProperty<bool>("RobotUnitRequired").getValue();
- /*if(robotUnitRequired)
- {
- ARMARX_IMPORTANT << "RobotUnit is required";
- usingProxy(getProperty<std::string>("RobotUnitName").getValue());
- }*/
+ monitorUpdateHealthTask =
+ new PeriodicTask<RobotHealth>(this, &RobotHealth::monitorHealthUpdateTaskClb, 10);
+ defaultMaximumCycleTimeWarn =
+ armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeWarnMS);
+ defaultMaximumCycleTimeErr =
+ armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeErrMS);
- //usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
+ const std::vector<std::string> requiredTags = simox::alg::split(p.requiredTags, ",");
+ // a special requester: cannot be changed during runtime.
+ tagsPerRequester[PROPERTY_REQUESTER_ID] = std::set<std::string>(requiredTags.begin(), requiredTags.end());
setDebugObserverBatchModeEnabled(true);
}
-
- void RobotHealth::onConnectComponent()
+ void
+ RobotHealth::onConnectComponent()
{
ARMARX_TRACE;
- emergencyStopTopicPrx = getTopic<EmergencyStopListenerPrx>(getProperty<std::string>("EmergencyStopTopicName").getValue());
- //remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
- aggregatedRobotHealthTopicPrx = getTopic<AggregatedRobotHealthInterfacePrx>(getProperty<std::string>("AggregatedRobotHealthTopicName").getValue());
- textToSpeechTopic = getTopic<TextListenerInterfacePrx>(getProperty<std::string>("TextToSpeechTopicName").getValue());
- lastSpeechOutput = TimeUtil::GetTime();
-
- /*if(robotUnitRequired)
- {
- robotUnitPrx = getTopic<RobotUnitInterfacePrx>(getProperty<std::string>("RobotUnitName").getValue());
- robotUnitPrx->enableHeartbeat();
- }*/
- monitorHealthTask->start();
+ monitorUpdateHealthTask->start();
}
- void RobotHealth::monitorHealthTaskClb()
+ void
+ RobotHealth::monitorHealthUpdateTaskClb()
{
- ARMARX_TRACE;
- long now = TimeUtil::GetTime().toMicroSeconds();
- bool hasNewErr = false;
+ const std::scoped_lock lock(updateMutex);
- RobotHealthState healthState = RobotHealthState::HealthOK;
+ ARMARX_TRACE;
+ auto now = armarx::core::time::DateTime::Now();
- for (size_t i = 0; i < validEntries; i++)
+ for (auto & e : updateEntries)
{
ARMARX_TRACE;
- Entry& e = entries.at(i);
- long delta = std::max(now - e.lastUpdate, e.lastDelta);
- if (e.required && delta > e.maximumCycleTimeErr)
+ const std::scoped_lock elock(e.mutex);
+
+ if (!e.enabled)
{
- healthState = RobotHealthState::HealthError;
+ continue;
}
- if (!e.enabled)
+ if(e.history.empty())
{
continue;
}
- ARMARX_TRACE;
+ auto deltaToArrival = now - e.history.back().arrivalTime;
+
+ ARMARX_TRACE;
- if (delta > e.maximumCycleTimeErr)
+ if (deltaToArrival > e.maximumCycleTimeErr)
{
ARMARX_TRACE;
- healthState = RobotHealthState::HealthError;
- if (e.isRunning)
+
+ if (e.state != HealthError) // not already in error state
{
ARMARX_TRACE;
- ARMARX_ERROR << deactivateSpam(0.1, e.name) << "Component " << e.name << " has died.";
- if (reportErrorsWithSpeech && (TimeUtil::GetTime() - lastSpeechOutput).toSecondsDouble() > speechMinimumReportInterval)
- {
- ARMARX_TRACE;
- lastSpeechOutput = TimeUtil::GetTime();
-
- std::string name;
-
- boost::smatch m;
- boost::regex regex("^[a-zA-Z]+");
- if (boost::regex_search(e.name, m, regex))
- {
- if (m.empty())
- {
- name = "Whatever";
- }
- else
- {
- name = m[0].str();
- }
- }
-
- textToSpeechTopic->reportText("Oh no! Component " + name + " is no longer running.");
- }
- hasNewErr = true;
- e.isRunning = false;
+ ARMARX_ERROR << deactivateSpam(0.1, e.name) << "Component " << e.name
+ << " has died.";
+ e.state = HealthError;
}
}
- else if (e.isRunning && delta > e.maximumCycleTimeWarn)
+ else if (deltaToArrival > e.maximumCycleTimeWarn)
{
ARMARX_TRACE;
- ARMARX_WARNING << deactivateSpam(0.1, e.name) << "Component " << e.name << " is running too slow.";
- if (healthState == RobotHealthState::HealthOK)
+ ARMARX_WARNING << deactivateSpam(0.1, e.name) << "Component " << e.name
+ << " is running too slow.";
+
+ e.state = HealthWarning;
+ }
+ else
+ {
+ // If everything is OK (again), update health
+ e.state = HealthOK;
+ }
+ }
+
+ // update aggregated health
+ RobotHealthState overallHealthState = RobotHealthState::HealthOK;
+
+
+ // get aggregated status
+ for (auto & e : updateEntries)
+ {
+ ARMARX_TRACE;
+ std::scoped_lock elock(e.mutex);
+
+ // trim history
+ while (e.history.size() > 20)
+ {
+ e.history.pop_front();
+ }
+
+ if (e.required)
+ {
+ if (e.state == HealthWarning && overallHealthState != HealthError)
{
- healthState = RobotHealthState::HealthWarning;
+ overallHealthState = RobotHealthState::HealthWarning;
+ }
+ else if (e.state == HealthError)
+ {
+ overallHealthState = RobotHealthState::HealthError;
+ }
+ }
+ else
+ {
+ if (e.state != HealthOK && overallHealthState != HealthError)
+ {
+ overallHealthState = RobotHealthState::HealthWarning;
}
}
-
}
- if (hasNewErr)
+
+ if (overallHealthState == HealthError)
{
ARMARX_TRACE;
- emergencyStopTopicPrx->reportEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ ARMARX_INFO << deactivateSpam(3) << "Requesting emergency stop";
+ ARMARX_CHECK_NOT_NULL(p.emergencyStopTopicPrx);
+ p.emergencyStopTopicPrx->reportEmergencyStopState(
+ EmergencyStopState::eEmergencyStopActive);
}
ARMARX_TRACE;
- aggregatedRobotHealthTopicPrx->aggregatedHeartbeat(healthState);
- /*if(allRequiredRunning && robotUnitPrx)
- {
- try
- {
- robotUnitPrx->sendHeartbeat();
- }
- catch(...)
- {}
- }*/
+ ARMARX_CHECK_NOT_NULL(p.aggregatedRobotHealthTopicPrx);
+ p.aggregatedRobotHealthTopicPrx->aggregatedHeartbeat(overallHealthState);
reportDebugObserver();
}
- void RobotHealth::onDisconnectComponent()
+ void
+ RobotHealth::onDisconnectComponent()
{
//robotUnitPrx = nullptr;
- monitorHealthTask->stop();
+ monitorUpdateHealthTask->stop();
}
+ void
+ RobotHealth::onExitComponent()
+ {
+ }
- void RobotHealth::onExitComponent()
+ armarx::PropertyDefinitionsPtr
+ RobotHealth::createPropertyDefinitions()
{
+ auto defs = armarx::PropertyDefinitionsPtr(
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier()));
+
+
+ defs->topic(p.emergencyStopTopicPrx, "EmergencyStop",
+ "The name of the topic over which changes of the emergencyStopState are sent.");
+ defs->topic<RobotHealthInterfacePrx>(p.robotHealthTopicName, "RobotHealthTopic",
+ "Name of the RobotHealth topic");
+ defs->topic(p.aggregatedRobotHealthTopicPrx, "AggregatedRobotHealthTopic", "Name of the AggregatedRobotHealthTopic");
+
+ defs->optional(p.maximumCycleTimeWarnMS, "MaximumCycleTimeWarnMS",
+ "Default value of the maximum cycle time for warnings");
+ defs->optional(p.maximumCycleTimeErrMS, "MaximumCycleTimeErrMS",
+ "Default value of the maximum cycle time for error");
+
+ defs->optional(p.requiredTags, "RequiredTags", "Tags that should be requested.");
+
+ return defs;
}
- armarx::PropertyDefinitionsPtr RobotHealth::createPropertyDefinitions()
+ RobotHealth::UpdateEntry*
+ RobotHealth::findUpdateEntry(const std::string& name)
{
- return armarx::PropertyDefinitionsPtr(new RobotHealthPropertyDefinitions(
- getConfigIdentifier()));
+ ARMARX_TRACE;
+
+ for (auto & updateEntrie : updateEntries)
+ {
+ if (updateEntrie.name == name)
+ {
+ return &updateEntrie;
+ }
+ }
+
+ return nullptr;
}
- RobotHealth::Entry& RobotHealth::findOrCreateEntry(const std::string& name)
+ std::pair<bool, RobotHealth::UpdateEntry&>
+ RobotHealth::findOrCreateUpdateEntry(const std::string& name)
{
ARMARX_TRACE;
- for (size_t i = 0; i < entries.size(); i++)
{
- if (entries.at(i).name == name)
+ for (auto & updateEntrie : updateEntries)
{
- return entries.at(i);
+ if (updateEntrie.name == name)
+ {
+ return {false, updateEntrie};
+ }
}
}
- std::unique_lock lock(mutex);
- ARMARX_INFO << "registering '" << name << "'";
- entries.emplace_back(name, defaultMaximumCycleTimeWarn * 1000, defaultMaximumCycleTimeErr * 1000);
- validEntries++;
- return entries.back();
- }
+ ARMARX_INFO << "registering for heartbeat: '" << name << "'";
+
+ updateEntries.emplace_back(name, defaultMaximumCycleTimeWarn, defaultMaximumCycleTimeErr);
+ return {true, updateEntries.back()};
+ }
- void RobotHealth::heartbeat(const std::string& componentName, const RobotHealthHeartbeatArgs& args, const Ice::Current&)
+ void
+ RobotHealth::signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current)
{
ARMARX_TRACE;
- Entry& e = findOrCreateEntry(componentName);
- long now = TimeUtil::GetTime().toMicroSeconds();
- e.maximumCycleTimeWarn = args.maximumCycleTimeWarningMS * 1000;
- e.maximumCycleTimeErr = args.maximumCycleTimeErrorMS * 1000;
- if (!e.isRunning)
+ std::scoped_lock lockU(updateMutex);
+
+ auto e = findOrCreateUpdateEntry(args.identifier);
+
{
- ARMARX_INFO << "'" << componentName << "' is now alive and running";
- e.lastDelta = 0;
+ std::scoped_lock lock(e.second.mutex);
+
+ if (e.first)
+ {
+ ARMARX_INFO << "Newly registering component " << args.identifier << " for updates.";
+ }
+ // else no messsage as components may spam sign up for legacy reasons (when there was no sign up)
+
+ armarx::core::time::fromIce(args.maximumCycleTimeWarning, e.second.maximumCycleTimeWarn);
+ armarx::core::time::fromIce(args.maximumCycleTimeError, e.second.maximumCycleTimeErr);
+
+ e.second.tags = args.tags;
+ // e.second.required = args.requiredByDefault; // FIXME
+ e.second.description = args.description;
+ e.second.enabled = true;
+ e.second.state = HealthOK;
+ e.second.history.clear();
}
- else
+
+ updateRequiredElements();
+ }
+
+ void
+ RobotHealth::heartbeat(const std::string& identifier,
+ const core::time::dto::DateTime& referenceTime,
+ const Ice::Current& current)
+ {
+ ARMARX_TRACE;
+ ARMARX_VERBOSE << "Finding update entry";
+
+ const DateTime arrivalTimestamp = Clock::Now();
+
+
+ // We hold a reference to 'o' which is an element in a vector.
+ // If we don't lock until the end of this scope, the vector size might change and 'o' will be invalidated.
+ std::scoped_lock lockU(updateMutex);
+ auto* const entry = findUpdateEntry(identifier);
+
+ if (entry == nullptr)
{
- e.lastDelta = now - e.lastUpdate;
- e.history.at(e.historyIndex) = e.lastDelta;
- e.historyIndex = (e.historyIndex + 1) % e.history.size();
+ ARMARX_WARNING << deactivateSpam() << "Attention. Component `" << identifier
+ << "` was not signed up for heartbeat. Ignoring heartbeat for now...";
+ return;
}
- ARMARX_TRACE;
- e.lastUpdate = now;
- e.isRunning = true;
- e.enabled = true;
+
+ ARMARX_CHECK_NOT_NULL(entry);
+
+ std::scoped_lock lock(entry->mutex);
+
+ if (not entry->enabled)
{
- std::unique_lock lock(e.messageMutex);
- e.message = args.message;
+ ARMARX_WARNING << deactivateSpam() << "Attention. Component `" << identifier
+ << "` was not enabled. The heartbeat is ignored... ";
+ return;
}
- }
- void armarx::RobotHealth::unregister(const std::string& componentName, const Ice::Current&)
- {
- ARMARX_TRACE;
- Entry& e = findOrCreateEntry(componentName);
- e.isRunning = false;
- e.enabled = false;
- ARMARX_INFO << "unregistering " << componentName;
+ ARMARX_VERBOSE << "Extending history";
+ // auto now = armarx::core::time::DateTime::Now();
+
+
+ armarx::core::time::DateTime refTime;
+ fromIce(referenceTime, refTime);
+ entry->history.push_back(UpdateEntry::TimeInfo{.referenceTime = refTime, .arrivalTime = arrivalTimestamp});
}
- std::string RobotHealth::getSummary(const Ice::Current&)
+ void
+ armarx::RobotHealth::unregister(const std::string& identifier, const Ice::Current&)
{
ARMARX_TRACE;
- long now = TimeUtil::GetTime().toMicroSeconds();
- std::stringstream ss;
- for (size_t i = 0; i < validEntries; i++)
- {
- ARMARX_TRACE;
- Entry& e = entries.at(i);
- long delta = std::max(now - e.lastUpdate, e.lastDelta);
+ std::scoped_lock lock(updateMutex);
- long minDelta = delta;
- long maxDelta = delta;
- for (size_t i = 1; i < e.history.size(); i++)
+ bool found = false;
+ {
+ for (auto & e : updateEntries)
{
- long historicalDelta = e.history.at(i);
- if (historicalDelta < 0)
+ if (e.name == identifier)
{
+ std::scoped_lock elock(e.mutex);
+ e.required = false;
+ e.enabled = false;
+ found = true;
break;
}
- minDelta = std::min(minDelta, historicalDelta);
- maxDelta = std::max(maxDelta, historicalDelta);
}
+ // Todo upgrade lock?
+ }
- ss << e.name;
- if (e.required)
+ if (found)
+ {
+ ARMARX_INFO << "Component " << identifier << " disabled from heartbeat";
+ }
+ else
+ {
+ ARMARX_INFO << "Could not unregister component " << identifier << ". Not found.";
+ }
+
+ updateRequiredElements();
+ }
+
+ void
+ armarx::RobotHealth::addRequiredTags(const std::string& requesterIdentifier,
+ const std::vector<std::string>& tags,
+ const Ice::Current& current)
+ {
+ std::scoped_lock lock(updateMutex);
+
+ // add newly requested tags
+ for(const auto& tag : tags)
+ {
+ tagsPerRequester[requesterIdentifier].insert(tag);
+ }
+
+ updateRequiredElements();
+ }
+
+ std::set<std::string>
+ armarx::RobotHealth::requestedTags() const
+ {
+ // obtain a list of all tags that are relevant / requested at the moment
+ std::set<std::string> allRequestedTags;
+ for(const auto& tgs: simox::alg::get_values(tagsPerRequester))
+ {
+ for(const auto& tag : tgs)
{
- ss << ", required";
+ allRequestedTags.insert(tag);
}
+ }
+ return allRequestedTags;
+ }
- if (!e.required && !e.enabled)
- {
- ss << ": disabled";
+ // checks whether any element in 'search' is in 'values'
+ bool containsAnyOf(const std::set<std::string>& values, const std::vector<std::string>& searchKeys)
+ {
+ return std::any_of(searchKeys.begin(), searchKeys.end(), [&values](const auto& key){
+ return values.count(key) > 0;
+ });
+ }
+
+ void armarx::RobotHealth::updateRequiredElements()
+ {
+ ARMARX_IMPORTANT << "updateRequiredElements";
+ const std::set<std::string> allRequestedTags = requestedTags();
+
+ for (auto &e : updateEntries) {
+ ARMARX_INFO << e.name;
+
+ e.required = false; // if required, will be set in the following
+
+ std::unique_lock lock(e.mutex);
+
+ if (containsAnyOf(allRequestedTags, e.tags)) {
+ ARMARX_INFO << e.name << " is required.";
+ e.required = true;
+ if (not e.enabled) {
+ ARMARX_WARNING << "You are requiring the disabled component "
+ << e.name << ". Enabling it...";
+ e.enabled = true;
+ }
}
- else
+ }
+ }
+
+ void
+ armarx::RobotHealth::removeRequiredTags(const std::string& requesterIdentifier,
+ const std::vector<std::string>& tags,
+ const Ice::Current& current)
+ {
+ std::scoped_lock lock(updateMutex);
+
+ // update the required tags list / map
+ for(const auto& tag : tags)
+ {
+ tagsPerRequester[requesterIdentifier].erase(tag);
+ }
+
+ updateRequiredElements();
+ }
+
+ void
+ armarx::RobotHealth::resetRequiredTags(const Ice::Current& current)
+ {
+ std::scoped_lock lock(updateMutex);
+
+ // treat the special provider (see onInitComponent()) appropriately
+ ARMARX_CHECK(tagsPerRequester.count(PROPERTY_REQUESTER_ID) > 0);
+ const auto propertyProviderTags = tagsPerRequester.at(PROPERTY_REQUESTER_ID);
+
+ // clear everything except the special provider
+ tagsPerRequester.clear();
+ tagsPerRequester.emplace(PROPERTY_REQUESTER_ID, propertyProviderTags);
+ }
+
+ RobotHealthInfo
+ RobotHealth::getSummary(const Ice::Current&)
+ {
+ std::scoped_lock lock(updateMutex);
+
+ ARMARX_TRACE;
+ RobotHealthInfo ret;
+
+ auto now = armarx::core::time::DateTime::Now();
+
+ armarx::core::time::Duration minDelta;
+ armarx::core::time::Duration maxDelta;
+
+ for (const auto& e : updateEntries)
+ {
+ RobotHealthInfoEntry healthEntry;
+
+ for (size_t i = 1; i < e.history.size(); i++)
{
- if (delta > e.maximumCycleTimeErr)
- {
- ss << ": ERROR";
- }
- else if (delta > e.maximumCycleTimeWarn)
+ auto later = e.history[i];
+ auto pre = e.history[i - 1];
+
+ auto delta = later.arrivalTime - pre.arrivalTime;
+
+ if (minDelta > delta)
{
- ss << ": warning";
+ minDelta = delta;
}
- else
+
+ if (maxDelta < delta)
{
- ss << ": ok";
+ maxDelta = delta;
}
- ss << " (" << (delta / 1000) << "ms, min: " << (minDelta / 1000) << "ms, max: " << (maxDelta / 1000) << "ms)";
}
- ARMARX_TRACE;
- std::unique_lock lock(e.messageMutex);
- if (e.message.size())
- {
- ss << " - " << e.message;
- }
+ const Duration timeSinceLastUpdateArrival = e.history.empty() ? Duration() : Clock::Now() - e.history.back().arrivalTime;
+ const Duration timeSinceLastUpdateReference = e.history.empty() ? Duration() : Clock::Now() - e.history.back().referenceTime;
+
+ const DateTime lastReferenceTime = e.history.empty() ? armarx::core::time::DateTime::Invalid() : e.history.back().referenceTime;
+
+ const Duration timeSyncDelayAndIce = e.history.empty() ? armarx::core::time::Duration() : e.history.back().arrivalTime - e.history.back().referenceTime;
- ss << "\n";
+ healthEntry.identifier = e.name;
+ healthEntry.state = e.state;
+ healthEntry.enabled = e.enabled;
+ healthEntry.required = e.required;
+ toIce(healthEntry.minDelta, minDelta);
+ toIce(healthEntry.maxDelta,maxDelta);
+ toIce(healthEntry.lastReferenceTimestamp, lastReferenceTime);
+ toIce(healthEntry.timeSinceLastArrival, timeSinceLastUpdateArrival);
+ toIce(healthEntry.timeSyncDelayAndIce, timeSyncDelayAndIce);
+ toIce(healthEntry.timeSinceLastUpdateReference, timeSinceLastUpdateReference);
+ toIce(healthEntry.maximumCycleTimeWarning, e.maximumCycleTimeWarn);
+ toIce(healthEntry.maximumCycleTimeError, e.maximumCycleTimeErr);
+ healthEntry.tags = e.tags;
+ ret.entries[e.name] = healthEntry;
}
- return ss.str();
+
+ const auto tags = requestedTags();
+ ret.activeTags = std::vector<std::string>(tags.begin(), tags.end());
+
+ return ret;
}
- void RobotHealth::reportDebugObserver()
+ std::string
+ RobotHealth::getTopicName(const Ice::Current& current)
{
- for(const auto& entry: entries)
+ return p.robotHealthTopicName;
+ }
+
+ void
+ RobotHealth::reportDebugObserver()
+ {
+ for (const auto& entry : updateEntries)
{
- setDebugObserverDatafield("RobotHealth_" + entry.name + "_lastDelta", entry.lastDelta);
- setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeWarn", entry.maximumCycleTimeWarn);
- setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeErr", entry.maximumCycleTimeErr);
+ if (entry.history.size() > 1)
+ {
+ auto later = entry.history[entry.history.size() - 1];
+ auto pre = entry.history[entry.history.size() - 2];
+ const Duration delta = later.arrivalTime - pre.arrivalTime;
+
+ const Duration timeSinceLastArrival = Clock::Now() - entry.history.back().arrivalTime;
+ const Duration timeToLastReference = Clock::Now() - entry.history.back().referenceTime;
+ const Duration timeSyncDelay = entry.history.back().arrivalTime - entry.history.back().referenceTime;
+
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_lastDelta",
+ delta.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_timeSinceLastArrival",
+ timeSinceLastArrival.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_timeToLastReference",
+ timeToLastReference.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_timeSyncDelayAndIce",
+ timeSyncDelay.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeWarn",
+ entry.maximumCycleTimeWarn.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeErr",
+ entry.maximumCycleTimeErr.toMilliSecondsDouble());
+ }
}
sendDebugObserverBatch();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index 3da12c1797ffa0afbc9af7310ec40953ecc51309..1e45f31116382a9761b879eaffb80375541fdaf7 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -22,54 +22,28 @@
#pragma once
-#include <RobotAPI/interface/components/RobotHealthInterface.h>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include <RobotAPI/interface/speech/SpeechInterface.h>
-
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <array>
+#include <atomic>
+#include <deque>
+#include <mutex>
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <atomic>
-#include <mutex>
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <RobotAPI/interface/speech/SpeechInterface.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
namespace armarx
{
- /**
- * @class RobotHealthPropertyDefinitions
- * @brief
- */
- class RobotHealthPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
- {
- public:
- RobotHealthPropertyDefinitions(std::string prefix):
- armarx::ComponentPropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::string>("EmergencyStopTopicName", "EmergencyStop", "The name of the topic over which changes of the emergencyStopState are sent.");
- defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
- defineOptionalProperty<bool>("ReportErrorsWithSpeech", true, "");
- defineOptionalProperty<std::string>("TextToSpeechTopicName", "TextToSpeech", "Name of the TextToSpeech topic");
- defineOptionalProperty<int>("MaximumCycleTimeWarnMS", 50, "Default value of the maximum cycle time for warnings");
- defineOptionalProperty<int>("MaximumCycleTimeErrMS", 100, "Default value of the maximum cycle time for error");
- defineOptionalProperty<std::string>("AggregatedRobotHealthTopicName", "AggregatedRobotHealthTopic", "Name of the AggregatedRobotHealthTopic");
- defineOptionalProperty<std::string>("RequiredComponents", "", "Comma separated list of required components");
-
- defineOptionalProperty<int>("SpeechMinimumReportInterval", 60, "Time that has to pass between reported messages in seconds.");
-
- //defineOptionalProperty<std::string>("RemoteGuiName", "RemoteGuiProvider", "Name of the remote GUI provider");
- //defineOptionalProperty<std::string>("RobotUnitName", "Armar6Unit", "Name of the RobotUnit");
- //defineOptionalProperty<bool>("RobotUnitRequired", true, "Wait for RobotUnit");
-
- //defineRequiredProperty<std::string>("PropertyName", "Description");
- //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
- }
- };
-
/**
* @defgroup Component-RobotHealth RobotHealth
* @ingroup RobotAPI-Components
@@ -90,33 +64,34 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotHealth";
}
- struct Entry
- {
- Entry(std::string name, long maximumCycleTimeWarn, long maximumCycleTimeErr)
- : name(name), maximumCycleTimeWarn(maximumCycleTimeWarn), maximumCycleTimeErr(maximumCycleTimeErr), history(100, -1)
- {}
- Entry(Entry&&) = default;
- Entry& operator=(Entry&&) = default;
+ // RobotHealthInterface interface
+ void signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current) override;
+ void unregister(const std::string& identifier, const Ice::Current&) override;
+
+
+ void addRequiredTags(const std::string& requesterIdentifier,
+ const std::vector<std::string>& tags,
+ const Ice::Current& current) override;
+ void removeRequiredTags(const std::string& requesterIdentifier,
+ const std::vector<std::string>& tags,
+ const Ice::Current& current) override;
+ void resetRequiredTags(const Ice::Current& current) override;
+
+
+ void heartbeat(const std::string& identifier,
+ const core::time::dto::DateTime& referenceTime,
+ const Ice::Current& current) override;
+
+ std::string getTopicName(const Ice::Current& current) override;
+
+ RobotHealthInfo getSummary(const Ice::Current& current) override;
- std::string name;
- long maximumCycleTimeWarn;
- long maximumCycleTimeErr;
- long lastUpdate = 0;
- long lastDelta = 0;
- RobotHealthState state = HealthOK;
- std::string message = "";
- bool isRunning = false;
- bool required = false;
- bool enabled = true;
- std::mutex messageMutex;
- std::vector<long> history;
- size_t historyIndex = 0;
- };
protected:
/**
@@ -144,32 +119,77 @@ namespace armarx
*/
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- void monitorHealthTaskClb();
- Entry& findOrCreateEntry(const std::string& name);
+ private:
+ struct UpdateEntry
+ {
+ UpdateEntry(const std::string& name,
+ const armarx::core::time::Duration& maximumCycleTimeWarn,
+ const armarx::core::time::Duration& maximumCycleTimeErr) :
+ name(name),
+ maximumCycleTimeWarn(maximumCycleTimeWarn),
+ maximumCycleTimeErr(maximumCycleTimeErr)
+ {
+ }
+
+ std::string name;
+ std::vector<std::string> tags;
+ RobotHealthState state = HealthOK;
+ std::string description = "";
+
+ bool required = false;
+ bool enabled = false;
+
+ mutable std::mutex mutex;
+
+ struct TimeInfo
+ {
+ //< Timestamp sent by component
+ armarx::core::time::DateTime referenceTime;
+
+ //< Timestamp on this PC, set by this component
+ armarx::core::time::DateTime arrivalTime;
+ };
+
+ std::deque<TimeInfo> history;
+
+ armarx::core::time::Duration maximumCycleTimeWarn;
+ armarx::core::time::Duration maximumCycleTimeErr;
+ };
+
+ void monitorHealthUpdateTaskClb();
+
+ UpdateEntry* findUpdateEntry(const std::string& name);
+ std::pair<bool, UpdateEntry&> findOrCreateUpdateEntry(const std::string& name);
void reportDebugObserver();
- std::mutex mutex;
- std::deque<Entry> entries;
- std::atomic_size_t validEntries {0};
- PeriodicTask<RobotHealth>::pointer_type monitorHealthTask;
- int defaultMaximumCycleTimeWarn;
- int defaultMaximumCycleTimeErr;
- EmergencyStopListenerPrx emergencyStopTopicPrx;
- //RobotUnitInterfacePrx robotUnitPrx;
- //bool robotUnitRequired;
- RemoteGuiInterfacePrx remoteGuiPrx;
- AggregatedRobotHealthInterfacePrx aggregatedRobotHealthTopicPrx;
- TextListenerInterfacePrx textToSpeechTopic;
- bool reportErrorsWithSpeech;
- int speechMinimumReportInterval;
- IceUtil::Time lastSpeechOutput;
+
+ void updateRequiredElements();
+ std::set<std::string> requestedTags() const;
- // RobotHealthInterface interface
- public:
- void heartbeat(const std::string& componentName, const RobotHealthHeartbeatArgs& args, const Ice::Current&) override;
- void unregister(const std::string& componentName, const Ice::Current&) override;
- std::string getSummary(const Ice::Current&) override;
+ // Mutex to restrict access to all interface / public methods. This ensures that all requests are
+ // handled sequentially.
+ mutable std::mutex updateMutex;
+
+ std::deque<UpdateEntry> updateEntries;
+
+ std::map<std::string, std::set<std::string>> tagsPerRequester;
+ PeriodicTask<RobotHealth>::pointer_type monitorUpdateHealthTask;
+
+ armarx::core::time::Duration defaultMaximumCycleTimeWarn;
+ armarx::core::time::Duration defaultMaximumCycleTimeErr;
+
+ struct Properties
+ {
+ EmergencyStopListenerPrx emergencyStopTopicPrx;
+ std::string robotHealthTopicName = "RobotHealthTopic";
+ AggregatedRobotHealthInterfacePrx aggregatedRobotHealthTopicPrx;
+ RemoteGuiInterfacePrx remoteGuiPrx;
+ long maximumCycleTimeWarnMS = 50;
+ long maximumCycleTimeErrMS = 100;
+
+ std::string requiredTags;
+ } p;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
index b239fdd0719716c1e579a803a6a89265168240c3..34894b6ea9c449d13f47af00d2c012a47f764f1d 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
@@ -21,26 +21,35 @@
*/
#include "RobotHealthDummy.h"
+
#include <time.h>
+
#include <thread>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
namespace armarx
{
- void RobotHealthDummy::onInitComponent()
+ void
+ RobotHealthDummy::onInitComponent()
{
dummyTask = new RunningTask<RobotHealthDummy>(this, &RobotHealthDummy::runTask);
getProperty(sleepmode, "SleepMode");
}
-
- void RobotHealthDummy::onConnectComponent()
+ void
+ RobotHealthDummy::onConnectComponent()
{
- robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue());
+ robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(
+ getProperty<std::string>("RobotHealthTopicName").getValue());
dummyTask->start();
}
-#define NANOSECS_PER_SEC 1000000000
- int RobotHealthDummy::NanoSleep(long nanosec)
+#define NANOSECS_PER_SEC 1000000000
+
+ int
+ RobotHealthDummy::NanoSleep(long nanosec)
{
struct timespec ts;
ts.tv_sec = nanosec / NANOSECS_PER_SEC;
@@ -48,101 +57,118 @@ namespace armarx
return nanosleep(&ts, nullptr); // jitter up to +100ms!
}
- void RobotHealthDummy::sleepwait(long microseconds)
+ void
+ RobotHealthDummy::sleepwait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
do
{
NanoSleep(1000);
- }
- while (TimeUtil::GetTime().toMicroSeconds() < end);
+ } while (TimeUtil::GetTime().toMicroSeconds() < end);
}
- void RobotHealthDummy::yieldwait(long microseconds)
+
+ void
+ RobotHealthDummy::yieldwait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
do
{
std::this_thread::yield();
- }
- while (TimeUtil::GetTime().toMicroSeconds() < end); // jitter up to +25ms...
+ } while (TimeUtil::GetTime().toMicroSeconds() < end); // jitter up to +25ms...
}
- void RobotHealthDummy::busydwait(long microseconds)
+
+ void
+ RobotHealthDummy::busywait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
do
{
;
- }
- while (TimeUtil::GetTime().toMicroSeconds() < end);
+ } while (TimeUtil::GetTime().toMicroSeconds() < end);
}
- void RobotHealthDummy::runTask()
+ void
+ RobotHealthDummy::runTask()
{
-
-
+ 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";
- robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
- long afterTopicCall = TimeUtil::GetTime().toMicroSeconds();
+
+ armarx::core::time::dto::DateTime now;
+ armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+ robotHealthTopicPrx->heartbeat(getName(), now);
+ 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 RobotHealthDummy::onDisconnectComponent()
+ void
+ RobotHealthDummy::onDisconnectComponent()
{
//ARMARX_IMPORTANT << "onDisconnectComponent";
dummyTask->stop();
}
-
- void RobotHealthDummy::onExitComponent()
+ void
+ RobotHealthDummy::onExitComponent()
{
//ARMARX_IMPORTANT << "onExitComponent";
dummyTask->stop();
}
- armarx::PropertyDefinitionsPtr RobotHealthDummy::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ RobotHealthDummy::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new RobotHealthDummyPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new RobotHealthDummyPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
index cb9d46377437764fac06cb4a9535d1adabb5534c..a841720145b96f925c427443f3998475620509bb 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
@@ -24,10 +24,10 @@
#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
namespace armarx
{
@@ -35,15 +35,18 @@ namespace armarx
* @class RobotHealthDummyPropertyDefinitions
* @brief
*/
- class RobotHealthDummyPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class RobotHealthDummyPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- RobotHealthDummyPropertyDefinitions(std::string prefix):
+ RobotHealthDummyPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
- defineOptionalProperty<std::string>("SleepMode", "nanosleep", "Which sleep function to use: nanosleep, sleepwait, yieldwait, busywait");
+ defineOptionalProperty<std::string>(
+ "RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
+ defineOptionalProperty<std::string>(
+ "SleepMode",
+ "nanosleep",
+ "Which sleep function to use: nanosleep, sleepwait, yieldwait, busywait");
//defineRequiredProperty<std::string>("PropertyName", "Description");
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
@@ -61,22 +64,23 @@ namespace armarx
*
* Detailed description of class RobotHealthDummy.
*/
- class RobotHealthDummy :
- virtual public armarx::Component
+ class RobotHealthDummy : virtual public armarx::Component
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotHealthDummy";
}
int NanoSleep(long nanosec);
void yieldwait(long microseconds);
- void busydwait(long microseconds);
+ void busywait(long microseconds);
void sleepwait(long microseconds);
+
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
@@ -109,6 +113,5 @@ namespace armarx
RunningTask<RobotHealthDummy>::pointer_type dummyTask;
std::string sleepmode;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt b/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
index b9434aeb6575293b01482473f3467403e969ab86..d177d23e37b51f1de6a96ceee909e5f4a4b18819 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/CMakeLists.txt
@@ -10,7 +10,8 @@ set(COMPONENT_LIBS
RobotAPICore RobotAPIInterfaces
armem
aronopencvconverter aronjsonconverter
-
+ # ExampleMemory_aron
+ ExampleMemory # actually, the aron version is sufficient
${OpenCV_LIBRARIES}
)
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/ExampleMemory/aron/ExampleData.xml b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
index 4efce411b680ce9a91afceff7da5523b8f67d6cc..6ba767713bb6481f5c406b4dccf37f3a375e4c44 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
@@ -20,20 +20,20 @@ An example data containing different native ARON types.
<GenerateTypes>
<Object name='armarx::armem::example::InnerClass'>
<ObjectChild key='element_int'>
- <Int />
+ <int32 default="42" />
</ObjectChild>
<ObjectChild key='element_float'>
- <Float />
+ <float32 default="666.66" />
</ObjectChild>
<ObjectChild key='element_string'>
- <String />
+ <String default="foobar" />
</ObjectChild>
</Object>
<Object name='armarx::armem::example::LinkedData'>
<ObjectChild key='yet_another_int'>
- <Int />
+ <int32 />
</ObjectChild>
<ObjectChild key='yet_another_string'>
<String />
@@ -48,16 +48,16 @@ An example data containing different native ARON types.
<Object name='armarx::armem::example::ExampleData'>
<ObjectChild key='the_int'>
- <Int />
+ <int32 />
</ObjectChild>
<ObjectChild key='the_long'>
- <Long />
+ <int64 />
</ObjectChild>
<ObjectChild key='the_float'>
- <Float />
+ <float32 />
</ObjectChild>
<ObjectChild key='the_double'>
- <Double />
+ <float64 />
</ObjectChild>
<ObjectChild key='the_string'>
<String />
@@ -91,12 +91,12 @@ An example data containing different native ARON types.
<ObjectChild key='the_float_list'>
<List>
- <Float />
+ <float32 />
</List>
</ObjectChild>
<ObjectChild key='the_int_list'>
<List>
- <Int />
+ <int32 />
</List>
</ObjectChild>
<ObjectChild key='the_string_list'>
@@ -113,12 +113,12 @@ An example data containing different native ARON types.
<ObjectChild key='the_float_dict'>
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key='the_int_dict'>
<Dict>
- <Int />
+ <int32 />
</Dict>
</ObjectChild>
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 6fe11f3196b211677110b31dd697a3eb4f055d5d..e00518697c1b1c0dcc8d2d7c767df17d593e1e9e 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -76,7 +76,7 @@ namespace armarx::armem::server::obj
ObjectMemory::ObjectMemory() :
instance::SegmentAdapter(iceAdapter()),
classSegment(iceAdapter()),
- attachmentSegment(iceAdapter())
+ attachmentSegment(iceAdapter()), virtualRobotReaderPlugin(nullptr)
{
addPlugin(virtualRobotReaderPlugin);
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index f77e01607b0036a2f9525b88dc919f0f1882608b..2249bdf242a197d1bff3be840d846f58a2487568 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -21,25 +21,21 @@
*/
#include "RobotStateMemory.h"
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-#include <RobotAPI/interface/core/PoseBase.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/armem/core/Prediction.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
-#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+#include <SimoxUtility/algorithm/contains.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
-#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/algorithm/contains.h>
-#include <SimoxUtility/algorithm/string.h>
-
+#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx::armem::server::robot_state
{
@@ -57,29 +53,35 @@ namespace armarx::armem::server::robot_state
addPlugin(robotUnit.plugin);
ARMARX_CHECK_NOT_NULL(robotUnit.plugin);
robotUnit.plugin->setRobotUnitAsOptionalDependency(true);
+ robotUnit.dataBuffer = std::make_unique<RobotUnit::Queue>();
}
+ RobotStateMemory::~RobotStateMemory() = default;
- RobotStateMemory::~RobotStateMemory()
- {
- }
-
-
- armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ RobotStateMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr const defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
- const std::string robotUnitPrefix = robotUnit.sensorValuePrefix;
+ const std::string robotUnitPrefix = RobotUnit::sensorValuePrefix;
- defs->optional(robotUnit.waitForRobotUnit, "WaitForRobotUnit", "Add the robot unit as dependency to the component. This memory requires a running RobotUnit, therefore we should add it as explicit dependency.");
+ defs->optional(robotUnit.waitForRobotUnit,
+ "WaitForRobotUnit",
+ "Add the robot unit as dependency to the component. This memory requires a "
+ "running RobotUnit, therefore we should add it as explicit dependency.");
- defs->optional(robotUnit.reader.properties.sensorPrefix, robotUnitPrefix + "SensorValuePrefix",
+ defs->optional(robotUnit.reader.properties.sensorPrefix,
+ robotUnitPrefix + "SensorValuePrefix",
"Prefix of all sensor values.");
- defs->optional(robotUnit.pollFrequency, robotUnitPrefix + "UpdateFrequency",
+ defs->optional(robotUnit.pollFrequency,
+ robotUnitPrefix + "UpdateFrequency",
"The frequency to store values in Hz. All other values get discarded. "
- "Minimum is 1, max is " + std::to_string(robotUnit.ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
- .setMin(1).setMax(robotUnit.ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ "Minimum is 1, max is " +
+ std::to_string(RobotUnit::ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
+ .setMin(1)
+ .setMax(RobotUnit::ROBOT_UNIT_MAXIMUM_FREQUENCY);
const std::string prefix = "mem.";
@@ -95,14 +97,14 @@ namespace armarx::armem::server::robot_state
return defs;
}
-
- std::string RobotStateMemory::getDefaultName() const
+ std::string
+ RobotStateMemory::getDefaultName() const
{
return "RobotStateMemory";
}
-
- void RobotStateMemory::onInitComponent()
+ void
+ RobotStateMemory::onInitComponent()
{
descriptionSegment.init();
proprioceptionSegment.init();
@@ -110,7 +112,8 @@ namespace armarx::armem::server::robot_state
localizationSegment.init();
commonVisu.init();
- robotUnit.pollFrequency = std::clamp(robotUnit.pollFrequency, 0.f, robotUnit.ROBOT_UNIT_MAXIMUM_FREQUENCY);
+ robotUnit.pollFrequency =
+ std::clamp(robotUnit.pollFrequency, 0.F, RobotUnit::ROBOT_UNIT_MAXIMUM_FREQUENCY);
std::vector<std::string> includePaths;
std::vector<std::string> packages = armarx::Application::GetProjectDependencies();
@@ -123,10 +126,11 @@ namespace armarx::armem::server::robot_state
continue;
}
- CMakePackageFinder project(projectName);
- std::string pathsString = project.getIncludePaths();
+ const CMakePackageFinder project(projectName);
+ const std::string pathsString = project.getIncludePaths();
std::vector<std::string> projectIncludePaths = simox::alg::split(pathsString, ";,");
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ includePaths.insert(
+ includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
if (robotUnit.waitForRobotUnit)
@@ -135,20 +139,25 @@ namespace armarx::armem::server::robot_state
}
}
-
- void RobotStateMemory::onConnectComponent()
+ void
+ RobotStateMemory::onConnectComponent()
{
+ {
+ ARMARX_CHECK(not robotUnit.reader.task or not robotUnit.reader.task->isRunning());
+ ARMARX_CHECK(not robotUnit.writer.task or not robotUnit.writer.task->isRunning());
+ robotUnit.dataBuffer = std::make_unique<RobotUnit::Queue>();
+ }
ARMARX_CHECK_NOT_NULL(debugObserver->getDebugObserver());
// 1. General setup
localizationSegment.onConnect();
commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
- // 2. Check for RobotUnit. If RobotUnit is enabled. Otherwise we do not wait for a streaming service and do not setup the segments
+ // 2. Check for RobotUnit. If RobotUnit is enabled. Otherwise, we do not wait for a streaming service and do not set up the segments
if (robotUnit.plugin->hasRobotUnitName())
{
robotUnit.plugin->waitUntilRobotUnitIsRunning();
- RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
+ const RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
robotUnit.reader.setTag(getName());
robotUnit.writer.setTag(getName());
@@ -156,52 +165,61 @@ namespace armarx::armem::server::robot_state
descriptionSegment.onConnect(robotUnitPrx);
proprioceptionSegment.onConnect(robotUnitPrx);
- robotUnit.reader.connect(*robotUnit.plugin, *debugObserver,
- proprioceptionSegment.getRobotUnitProviderID().providerSegmentName);
+ robotUnit.reader.connect(
+ *robotUnit.plugin,
+ *debugObserver,
+ proprioceptionSegment.getRobotUnitProviderID().providerSegmentName);
robotUnit.writer.connect(*debugObserver);
- robotUnit.writer.properties.robotUnitProviderID = proprioceptionSegment.getRobotUnitProviderID();
-
- robotUnit.reader.task = new SimpleRunningTask<>([this]()
- {
- robotUnit.reader.run(robotUnit.pollFrequency, robotUnit.dataBuffer);
- }, "Robot Unit Reader");
- robotUnit.writer.task = new SimpleRunningTask<>([this]()
- {
- robotUnit.writer.run(
- robotUnit.pollFrequency, robotUnit.dataBuffer, iceAdapter(), localizationSegment
- );
- }, "Robot State Writer");
+ robotUnit.writer.properties.robotUnitProviderID =
+ proprioceptionSegment.getRobotUnitProviderID();
+
+ robotUnit.reader.task = new SimpleRunningTask<>(
+ [this]() { robotUnit.reader.run(robotUnit.pollFrequency, *robotUnit.dataBuffer); },
+ "Robot Unit Reader");
+ robotUnit.writer.task = new SimpleRunningTask<>(
+ [this]()
+ {
+ robotUnit.writer.run(robotUnit.pollFrequency,
+ *robotUnit.dataBuffer,
+ iceAdapter(),
+ localizationSegment);
+ },
+ "Robot State Writer");
startRobotUnitStream();
}
}
-
- void RobotStateMemory::onDisconnectComponent()
+ void
+ RobotStateMemory::onDisconnectComponent()
{
stopRobotUnitStream();
}
-
- void RobotStateMemory::onExitComponent()
+ void
+ RobotStateMemory::onExitComponent()
{
stopRobotUnitStream();
}
-
- armarx::PoseBasePtr RobotStateMemory::getGlobalRobotPose(Ice::Long timestamp_us, const std::string& robotName, const ::Ice::Current& /*unused*/)
+ armarx::PoseBasePtr
+ RobotStateMemory::getGlobalRobotPose(Ice::Long timestampUs,
+ const std::string& robotName,
+ const ::Ice::Current& /*unused*/)
{
- armem::Time timestamp { armem::Duration::MicroSeconds(timestamp_us) };
- ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << timestamp << ".";
+ const armem::Time timestamp{armem::Duration::MicroSeconds(timestampUs)};
+ ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << timestamp
+ << ".";
auto poseMap = localizationSegment.getRobotGlobalPoses(timestamp);
-
- bool robotNameFound = simox::alg::contains_key(poseMap, robotName);
+
+ const bool robotNameFound = simox::alg::contains_key(poseMap, robotName);
ARMARX_CHECK(robotNameFound)
- << "Robot with name " << robotName << " does not exist at or before timestamp " << timestamp << ".\n"
+ << "Robot with name " << robotName << " does not exist at or before timestamp "
+ << timestamp << ".\n"
<< "Available robots are: " << simox::alg::get_keys(poseMap);
- return { new Pose(poseMap[robotName].matrix()) };
+ return {new Pose(poseMap[robotName].matrix())};
}
/*************************************************************/
@@ -209,8 +227,10 @@ namespace armarx::armem::server::robot_state
/*************************************************************/
- void RobotStateMemory::startRobotUnitStream()
+ void
+ RobotStateMemory::startRobotUnitStream()
{
+ ARMARX_INFO << "Starting RobotUnit stream";
ARMARX_CHECK(robotUnit.plugin->robotUnitIsRunning());
if (robotUnit.reader.task->isRunning() || robotUnit.writer.task->isRunning())
@@ -227,7 +247,8 @@ namespace armarx::armem::server::robot_state
ss << "Getting sensor values for:" << std::endl;
for (const auto& [name, dataEntry] : robotUnit.reader.description.entries)
{
- const std::string type = RobotUnitDataStreaming::DataEntryNames.to_name(dataEntry.type);
+ const std::string type =
+ RobotUnitDataStreaming::DataEntryNames.to_name(dataEntry.type);
ss << "\t" << name << " (type: '" << type << "')" << std::endl;
}
ARMARX_INFO << ss.str();
@@ -237,11 +258,15 @@ namespace armarx::armem::server::robot_state
robotUnit.writer.task->start();
}
-
- void RobotStateMemory::stopRobotUnitStream()
+ void
+ RobotStateMemory::stopRobotUnitStream()
{
+ ARMARX_INFO << "Stopping RobotUnit stream";
+ robotUnit.dataBuffer->close();
robotUnit.reader.task->stop();
+ ARMARX_VERBOSE << "RobotUnit reader stopped";
robotUnit.writer.task->stop();
+ ARMARX_VERBOSE << "RobotUnit writer stopped";
}
-}
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 70513a0cf56ff0cd6d82603cc3266fb405e5af3d..25f19586c3a7202a272f4571f4e9c279a05454bc 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -33,19 +33,19 @@
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
-#include <RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h>
-
namespace armarx::plugins
{
class DebugObserverComponentPlugin;
class RobotUnitComponentPlugin;
} // namespace armarx::plugins
+
namespace armarx::armem::server::robot_state
{
@@ -68,7 +68,7 @@ namespace armarx::armem::server::robot_state
{
public:
RobotStateMemory();
- virtual ~RobotStateMemory() override;
+ ~RobotStateMemory() override;
std::string getDefaultName() const override;
@@ -77,7 +77,7 @@ namespace armarx::armem::server::robot_state
// GlobalRobotPoseProvider interface
armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp,
const std::string& robotName,
- const ::Ice::Current&) override;
+ const ::Ice::Current& /*unused*/) override;
protected:
@@ -94,7 +94,6 @@ namespace armarx::armem::server::robot_state
void stopRobotUnitStream();
- private:
armarx::plugins::DebugObserverComponentPlugin* debugObserver = nullptr;
@@ -115,7 +114,6 @@ namespace armarx::armem::server::robot_state
// Joint visu for all segments => robot pose and configuration
Visu commonVisu;
-
struct RobotUnit
{
// params
@@ -130,10 +128,11 @@ namespace armarx::armem::server::robot_state
// queue
using Queue = armarx::armem::server::robot_state::proprioception::Queue;
- Queue dataBuffer;
+ std::unique_ptr<Queue> dataBuffer;
bool waitForRobotUnit = false;
};
+
RobotUnit robotUnit;
};
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 9b16109a96f01e951974f84c70f7e7b159558de5..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,33 +25,38 @@ namespace armarx::skills::provider
SkillProviderExample::onInitComponent()
{
// Add example skill
- addSkill(std::make_unique<HelloWorldSkill>());
+ ARMARX_INFO << "Adding skill HelloWorldSkill";
+ addSkillFactory<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);
+ 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 cbd795396941e1d004fd5f7fdf0fa946d70cb71e..b8391b0881f625af03b00b601983251c4d90558b 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -27,58 +27,16 @@
#include <ArmarXCore/core/Component.h>
// RobotAPI
-#include <RobotAPI/libraries/skills/provider/SkillProxy.h>
#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.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
@@ -91,8 +49,8 @@ namespace armarx::skills::provider
* Detailed description of class ExampleClient.
*/
class SkillProviderExample :
- virtual public armarx::Component,
- virtual public SkillProviderComponentPluginUser
+ virtual public armarx::Component,
+ virtual public SkillProviderComponentPluginUser
{
public:
SkillProviderExample();
@@ -101,7 +59,6 @@ namespace armarx::skills::provider
std::string getDefaultName() const override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -111,4 +68,4 @@ namespace armarx::skills::provider
private:
};
-}
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Timeout.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..31dc03ae05caf0e2be256487e3a6a4294243e9d0
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.cpp
@@ -0,0 +1,28 @@
+
+#include "Timeout.h"
+
+namespace armarx::skills::provider
+{
+
+ TimeoutSkill::TimeoutSkill() :
+ PeriodicSkill(GetSkillDescription(), armarx::core::time::Frequency::Hertz(5))
+ {
+ }
+
+ SkillDescription
+ TimeoutSkill::GetSkillDescription()
+ {
+ return SkillDescription{.skillId = SkillID{.skillName = "Timeout"},
+ .description = "This fails with timeout reached",
+ .timeout = armarx::core::time::Duration::MilliSeconds(2000)};
+ }
+
+ PeriodicSkill::StepResult
+ TimeoutSkill::step()
+ {
+ // do heavy work
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+ return {ActiveOrTerminatedSkillStatus::Running, nullptr};
+ }
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Timeout.h b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.h
new file mode 100644
index 0000000000000000000000000000000000000000..347e216a956c22d8d38b3b0b434ec0b5f4cf0225
--- /dev/null
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Timeout.h
@@ -0,0 +1,42 @@
+
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+// RobotAPI
+#include <RobotAPI/libraries/skills/provider/PeriodicSkill.h>
+
+namespace armarx::skills::provider
+{
+
+ class TimeoutSkill : public PeriodicSkill
+ {
+ public:
+ TimeoutSkill();
+
+ static SkillDescription GetSkillDescription();
+
+ private:
+ PeriodicSkill::StepResult step() final;
+ };
+} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
index a2423c560b8548c265af0ae14602b8771fe46218..e90781ca62d7ef67751ed79b7556ab2c61f160a5 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
+++ b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
@@ -14,10 +14,10 @@
</IntEnum>
<Object name='armarx::skills::Example::HelloWorldAcceptedType'>
<ObjectChild key='some_int'>
- <Int />
+ <int32 />
</ObjectChild>
<ObjectChild key='some_float'>
- <Float />
+ <float32 />
</ObjectChild>
<ObjectChild key='some_text'>
<String />
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/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index 94c2946f758d1a387f49000c15d38082ab633877..5a7c01e37ef158ad16cf2350cb7ab03ecd8fd4be 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -22,20 +22,24 @@
#include "BasicControllers.h"
-#include <ArmarXCore/core/util/algorithm.h>
+#include <algorithm>
+
#include <ArmarXCore/core/logging/Logging.h>
-#include "util/CtrlUtil.h"
+#include <ArmarXCore/core/util/algorithm.h>
-#include <algorithm>
+#include "util/CtrlUtil.h"
namespace armarx
{
- float velocityControlWithAccelerationBounds(
- float dt, float maxDt,
- const float currentV, float targetV, float maxV,
- float acceleration, float deceleration,
- float directSetVLimit
- )
+ float
+ velocityControlWithAccelerationBounds(float dt,
+ float maxDt,
+ const float currentV,
+ float targetV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float directSetVLimit)
{
dt = std::min(std::abs(dt), std::abs(maxDt));
maxV = std::abs(maxV);
@@ -56,7 +60,7 @@ namespace armarx
}
//handle case 2 + 3
- const bool accelerate = sign(targetV) == sign(currentV) && // v in same direction
+ const bool accelerate = sign(targetV) == sign(currentV) && // v in same direction
std::abs(targetV) > std::abs(currentV); // currently to slow
const float usedacc = accelerate ? acceleration : -deceleration;
@@ -75,13 +79,20 @@ namespace armarx
////////////////////////////
//wip?
- float velocityControlWithAccelerationAndPositionBounds(float dt, float maxDt,
- float currentV, float targetV, float maxV,
- float acceleration, float deceleration,
- float directSetVLimit,
- float currentPosition,
- float positionLimitLoSoft, float positionLimitHiSoft,
- float positionLimitLoHard, float positionLimitHiHard)
+ float
+ velocityControlWithAccelerationAndPositionBounds(float dt,
+ float maxDt,
+ float currentV,
+ float targetV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float directSetVLimit,
+ float currentPosition,
+ float positionLimitLoSoft,
+ float positionLimitHiSoft,
+ float positionLimitLoHard,
+ float positionLimitHiHard)
{
if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
{
@@ -110,14 +121,17 @@ namespace armarx
float nextv;
//handle case 1
const float vsquared = currentV * currentV;
- const float brakingDist = sign(currentV) * vsquared / 2.f / std::abs(deceleration); //the braking distance points in the direction of the velocity
+ const float brakingDist =
+ sign(currentV) * vsquared / 2.f /
+ std::abs(deceleration); //the braking distance points in the direction of the velocity
const float posIfBrakingNow = currentPosition + brakingDist;
if (posIfBrakingNow <= positionLimitLoSoft || posIfBrakingNow >= positionLimitHiSoft)
{
//case 1. -> brake now! (we try to have v=0 at the limit)
- const auto limit = posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
+ const auto limit =
+ posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
const float wayToGo = limit - currentPosition;
//decelerate!
@@ -134,7 +148,8 @@ namespace armarx
else
{
//handle 2-3
- nextv = velocityControlWithAccelerationBounds(dt, maxDt, currentV, targetV, maxV, acceleration, deceleration, directSetVLimit);
+ nextv = velocityControlWithAccelerationBounds(
+ dt, maxDt, currentV, targetV, maxV, acceleration, deceleration, directSetVLimit);
}
if (softLimitViolation == sign(nextv))
{
@@ -147,14 +162,16 @@ namespace armarx
return nextv;
}
-
- float positionThroughVelocityControlWithAccelerationBounds(
- float dt, float maxDt,
- float currentV, float maxV,
- float acceleration, float deceleration,
- float currentPosition, float targetPosition,
- float p
- )
+ float
+ positionThroughVelocityControlWithAccelerationBounds(float dt,
+ float maxDt,
+ float currentV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float currentPosition,
+ float targetPosition,
+ float p)
{
dt = std::min(std::abs(dt), std::abs(maxDt));
maxV = std::abs(maxV);
@@ -173,15 +190,17 @@ namespace armarx
float newTargetVelPController = positionError * p;
//handle case 2-3
- const float brakingDistance = signV * currentV * currentV / 2.f / deceleration; //the braking distance points in the direction of the velocity
+ const float brakingDistance =
+ signV * currentV * currentV / 2.f /
+ deceleration; //the braking distance points in the direction of the velocity
const float posIfBrakingNow = currentPosition + brakingDistance;
const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
const bool decelerate =
std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
// std::abs(posIfBrakingNow - targetPosition) <= pControlPosErrorLimit || // we want to hit the target
- sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
- const float usedacc = decelerate ? -deceleration : acceleration;
+ sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
+ const float usedacc = decelerate ? -deceleration : acceleration;
const float maxDeltaV = std::abs(usedacc * dt);
const float deltaVel = std::clamp(signV * usedacc * dt, -maxDeltaV, maxDeltaV);
@@ -199,13 +218,18 @@ namespace armarx
}
}
- float positionThroughVelocityControlWithAccelerationAndPositionBounds(
- float dt, float maxDt,
- float currentV, float maxV,
- float acceleration, float deceleration,
- float currentPosition, float targetPosition,
- float p,
- float positionLimitLo, float positionLimitHi
+ float
+ positionThroughVelocityControlWithAccelerationAndPositionBounds(float dt,
+ float maxDt,
+ float currentV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float currentPosition,
+ float targetPosition,
+ float p,
+ float positionLimitLo,
+ float positionLimitHi
)
{
@@ -224,7 +248,9 @@ namespace armarx
//handle case 1
const float vsquared = currentV * currentV;
- const float brakingDistance = signV * vsquared / 2.f / deceleration; //the braking distance points in the direction of the velocity
+ const float brakingDistance =
+ signV * vsquared / 2.f /
+ deceleration; //the braking distance points in the direction of the velocity
const float posIfBrakingNow = currentPosition + brakingDistance;
if (posIfBrakingNow <= positionLimitLo || posIfBrakingNow >= positionLimitHi)
{
@@ -239,86 +265,102 @@ namespace armarx
//handle case 2-3
return positionThroughVelocityControlWithAccelerationBounds(
- dt, maxDt,
- currentV, maxV,
- acceleration, deceleration,
- currentPosition, std::clamp(targetPosition, positionLimitLo, positionLimitHi),
- p
- );
+ dt,
+ maxDt,
+ currentV,
+ maxV,
+ acceleration,
+ deceleration,
+ currentPosition,
+ std::clamp(targetPosition, positionLimitLo, positionLimitHi),
+ p);
}
////////////////////////////
//wip
- float positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(
- float dt, float maxDt,
- float currentV, float maxV,
- float acceleration, float deceleration,
- float currentPosition, float targetPosition,
- float pControlPosErrorLimit, float p,
+ float
+ positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(
+ float dt,
+ float maxDt,
+ float currentV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float currentPosition,
+ float targetPosition,
+ float pControlPosErrorLimit,
+ float p,
float& direction,
- float positionPeriodLo, float positionPeriodHi)
+ float positionPeriodLo,
+ float positionPeriodHi)
{
currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
- targetPosition = periodicClamp(targetPosition, positionPeriodLo, positionPeriodHi);
+ targetPosition = periodicClamp(targetPosition, positionPeriodLo, positionPeriodHi);
const float brakingDist = brakingDistance(currentV, deceleration);
const float posIfBrakingNow = currentPosition + brakingDist;
const float posIfBrakingNowError = targetPosition - posIfBrakingNow;
const float posError = targetPosition - currentPosition;
- if (
- std::abs(posIfBrakingNowError) <= pControlPosErrorLimit ||
- std::abs(posError) <= pControlPosErrorLimit
- )
+ if (std::abs(posIfBrakingNowError) <= pControlPosErrorLimit ||
+ std::abs(posError) <= pControlPosErrorLimit)
{
//this allows slight overshooting (in the limits of the p controller)
- return positionThroughVelocityControlWithAccelerationBounds(
- dt, maxDt,
- currentV, maxV,
- acceleration, deceleration,
- currentPosition, targetPosition,
- p
- );
+ return positionThroughVelocityControlWithAccelerationBounds(dt,
+ maxDt,
+ currentV,
+ maxV,
+ acceleration,
+ deceleration,
+ currentPosition,
+ targetPosition,
+ p);
}
//we transform the problem with periodic bounds into
//a problem with position bounds
- const float positionPeriodLength = std::abs(positionPeriodHi - positionPeriodLo);
+ const float positionPeriodLength = std::abs(positionPeriodHi - positionPeriodLo);
//we shift the positions such that 0 == target
- currentPosition = periodicClamp(currentPosition - targetPosition, 0.f, positionPeriodLength);
+ currentPosition =
+ periodicClamp(currentPosition - targetPosition, 0.f, positionPeriodLength);
//how many times will we go ovet the target if we simply bake now?
const float overshoot = std::trunc((currentPosition + brakingDist) / positionPeriodLength);
if (true || direction == 0)
{
//determine the direction to go (1 == pos vel, -1 == neg vel)
- direction = (periodicClamp(currentPosition + brakingDist, 0.f, positionPeriodLength) >= positionPeriodLength / 2) ? 1 : -1;
+ direction = (periodicClamp(currentPosition + brakingDist, 0.f, positionPeriodLength) >=
+ positionPeriodLength / 2)
+ ? 1
+ : -1;
}
//shift the target away from 0
- targetPosition = (overshoot - std::min(0.f, -direction)) * positionPeriodLength; // - direction * pControlPosErrorLimit;
+ targetPosition = (overshoot - std::min(0.f, -direction)) *
+ positionPeriodLength; // - direction * pControlPosErrorLimit;
//move
- return positionThroughVelocityControlWithAccelerationBounds(
- dt, maxDt,
- currentV, maxV,
- acceleration, deceleration,
- currentPosition, targetPosition,
- p
- );
+ return positionThroughVelocityControlWithAccelerationBounds(dt,
+ maxDt,
+ currentV,
+ maxV,
+ acceleration,
+ deceleration,
+ currentPosition,
+ targetPosition,
+ p);
}
- bool VelocityControllerWithAccelerationBounds::validParameters() const
+ bool
+ VelocityControllerWithAccelerationBounds::validParameters() const
{
- return maxV > 0 &&
- acceleration > 0 &&
- deceleration > 0 &&
- targetV <= maxV &&
- targetV >= -maxV;
+ return maxV > 0 && acceleration > 0 && deceleration > 0 && targetV <= maxV &&
+ targetV >= -maxV;
}
- float VelocityControllerWithAccelerationBounds::run() const
+ float
+ VelocityControllerWithAccelerationBounds::run() const
{
const float useddt = std::min(std::abs(dt), std::abs(maxDt));
@@ -334,7 +376,7 @@ namespace armarx
}
//handle case 2 + 3
- const bool accelerate = sign(targetV) == sign(currentV) && // v in same direction
+ const bool accelerate = sign(targetV) == sign(currentV) && // v in same direction
std::abs(targetV) > std::abs(currentV); // currently to slow
const float usedacc = accelerate ? acceleration : -deceleration;
@@ -351,15 +393,14 @@ namespace armarx
return nextV;
}
- bool VelocityControllerWithRampedAcceleration::validParameters() const
+ bool
+ VelocityControllerWithRampedAcceleration::validParameters() const
{
- return maxV > 0 &&
- jerk > 0 &&
- targetV <= maxV &&
- targetV >= -maxV;
+ return maxV > 0 && jerk > 0 && targetV <= maxV && targetV >= -maxV;
}
- VelocityControllerWithRampedAcceleration::Output VelocityControllerWithRampedAcceleration::run() const
+ VelocityControllerWithRampedAcceleration::Output
+ VelocityControllerWithRampedAcceleration::run() const
{
const double useddt = std::min(std::abs(dt), std::abs(maxDt));
@@ -374,7 +415,7 @@ namespace armarx
{
// double nextAcc = (clampedTargetV - currentV) / useddt; // calculated acc is unstable!!!
// double nextJerk = (nextAcc - currentAcc) / useddt;
- Output result {clampedTargetV, 0, 0};
+ Output result{clampedTargetV, 0, 0};
return result;
}
@@ -393,14 +434,17 @@ namespace armarx
double t_to_zero_acc = std::abs(currentAcc) / jerk;
double v_at_t = ctrlutil::v(t_to_zero_acc, currentV, currentAcc, -goalDir * jerk);
- increaseAcc = math::MathUtils::Sign(v_at_t - clampedTargetV) != goalDir ;
+ increaseAcc = math::MathUtils::Sign(v_at_t - clampedTargetV) != goalDir;
// ARMARX_INFO << VAROUT(t_to_zero_acc) << VAROUT(v_at_t) << VAROUT(increaseAcc);
}
// v = a*a/(2j)
- const double adjustedJerk = std::abs(currentAcc * currentAcc / (2 * (clampedTargetV - currentV)));
- double usedJerk = increaseAcc ? goalDir * jerk :
- -goalDir *
- ((std::abs(currentV) > 0.01 && !std::isnan(adjustedJerk)) ? adjustedJerk : jerk);
+ const double adjustedJerk =
+ std::abs(currentAcc * currentAcc / (2 * (clampedTargetV - currentV)));
+ double usedJerk = increaseAcc
+ ? goalDir * jerk
+ : -goalDir * ((std::abs(currentV) > 0.01 && !std::isnan(adjustedJerk))
+ ? adjustedJerk
+ : jerk);
// double t_to_target_v = ctrlutil::t_to_v(clampedTargetV - currentV, currentAcc, usedJerk);
// double v_at_t2 = ctrlutil::v(t_to_target_v, currentV, currentAcc, usedJerk);
// ctrlutil::t_to_v(currentV, currentAcc, jerk);
@@ -415,18 +459,19 @@ namespace armarx
double nextAcc = (clampedTargetV - currentV) / useddt;
double nextJerk = (nextAcc - currentAcc) / useddt;
// ARMARX_INFO << "overshooting! " << VAROUT(clampedTargetV) << VAROUT(nextAcc) << VAROUT(nextJerk);
- Output result {clampedTargetV, nextAcc, nextJerk};
+ Output result{clampedTargetV, nextAcc, nextJerk};
return result;
}
// const double deltaVel = boost::algorithm::clamp(sign(currentV) * usedacc * useddt, -maxDeltaV, maxDeltaV);
const double nextV = ctrlutil::v(useddt, currentV, currentAcc, usedJerk);
const double nextAcc = ctrlutil::a(useddt, currentAcc, usedJerk);
// ARMARX_INFO << VAROUT(clampedTargetV) << VAROUT(nextV) << VAROUT(nextAcc) << VAROUT(usedJerk);
- Output result {nextV, nextAcc, usedJerk};
+ Output result{nextV, nextAcc, usedJerk};
return result;
}
- float VelocityControllerWithAccelerationBounds::estimateTime() const
+ float
+ VelocityControllerWithAccelerationBounds::estimateTime() const
{
const float curverror = std::abs(targetV - currentV);
if (curverror < directSetVLimit)
@@ -436,17 +481,17 @@ namespace armarx
return curverror / ((targetV > currentV) ? acceleration : deceleration);
}
-
-
- bool VelocityControllerWithAccelerationAndPositionBounds::validParameters() const
+ bool
+ VelocityControllerWithAccelerationAndPositionBounds::validParameters() const
{
- return VelocityControllerWithAccelerationBounds::validParameters() &&
- // positionLimitLoHard < positionLimitLoSoft &&
- positionLimitLoSoft < positionLimitHiSoft;
+ return VelocityControllerWithAccelerationBounds::validParameters() &&
+ // positionLimitLoHard < positionLimitLoSoft &&
+ positionLimitLoSoft < positionLimitHiSoft;
// positionLimitHiSoft < positionLimitHiHard;
}
- float VelocityControllerWithAccelerationAndPositionBounds::run() const
+ float
+ VelocityControllerWithAccelerationAndPositionBounds::run() const
{
// if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
// {
@@ -474,14 +519,18 @@ namespace armarx
float nextv;
//handle case 1
const float vsquared = currentV * currentV;
- const float brakingDist = sign(currentV) * vsquared / 2.f / std::abs(deceleration); //the braking distance points in the direction of the velocity
+ const float brakingDist =
+ sign(currentV) * vsquared / 2.f /
+ std::abs(deceleration); //the braking distance points in the direction of the velocity
const float posIfBrakingNow = currentPosition + brakingDist;
// ARMARX_INFO << deactivateSpam(1) << VAROUT(currentPosition) << VAROUT(positionLimitLoSoft) << VAROUT(positionLimitHiSoft) << VAROUT(posIfBrakingNow);
- if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) || (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
+ if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) ||
+ (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
{
//case 1. -> brake now! (we try to have v=0 at the limit)
- const auto limit = posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
+ const auto limit =
+ posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
const float wayToGo = limit - currentPosition;
//decelerate!
@@ -501,7 +550,6 @@ namespace armarx
//handle 2-3
nextv = VelocityControllerWithAccelerationBounds::run();
// ARMARX_INFO << deactivateSpam(1) << "Desired target Vel: " << targetV << " " << VAROUT(nextv);
-
}
if (softLimitViolation == sign(nextv) && nextv != 0)
{
@@ -515,7 +563,8 @@ namespace armarx
return nextv;
}
- PositionThroughVelocityControllerWithAccelerationBounds::PositionThroughVelocityControllerWithAccelerationBounds()
+ PositionThroughVelocityControllerWithAccelerationBounds::
+ PositionThroughVelocityControllerWithAccelerationBounds()
{
#ifdef DEBUG_POS_CTRL
buffer = boost::circular_buffer<HelpStruct>(20);
@@ -524,7 +573,8 @@ namespace armarx
pid->threadSafe = false;
}
- float PositionThroughVelocityControllerWithAccelerationBounds::calculateProportionalGain() const
+ float
+ PositionThroughVelocityControllerWithAccelerationBounds::calculateProportionalGain() const
{
/* s = v_0*v_0/(2*a) -> sqrt(s*2*a) = v_0;
@@ -534,20 +584,17 @@ namespace armarx
return v_0 / pControlPosErrorLimit;
}
-
-
-
- bool PositionThroughVelocityControllerWithAccelerationBounds::validParameters() const
+ bool
+ PositionThroughVelocityControllerWithAccelerationBounds::validParameters() const
{
- return maxV > 0 &&
- acceleration > 0 &&
- deceleration > 0 &&
- // pControlPosErrorLimit > 0 &&
- // pControlVelLimit > 0 &&
- pid->Kp > 0;
+ return maxV > 0 && acceleration > 0 && deceleration > 0 &&
+ // pControlPosErrorLimit > 0 &&
+ // pControlVelLimit > 0 &&
+ pid->Kp > 0;
}
- float PositionThroughVelocityControllerWithAccelerationBounds::run() const
+ float
+ PositionThroughVelocityControllerWithAccelerationBounds::run() const
{
const float useddt = std::min(std::abs(dt), std::abs(maxDt));
const float signV = sign(currentV);
@@ -564,21 +611,24 @@ namespace armarx
float newTargetVelPController = pid->getControlValue();
//handle case 2-3
- const float brakingDistance = signV * currentV * currentV / 2.f / deceleration; //the braking distance points in the direction of the velocity
+ const float brakingDistance =
+ signV * currentV * currentV / 2.f /
+ deceleration; //the braking distance points in the direction of the velocity
const float posIfBrakingNow = currentPosition + brakingDistance;
const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
const bool hardBrakingNeeded = std::abs(brakingDistance) > std::abs(positionError);
- const float safePositionError = (std::abs(positionError) < 0.0001) ? (sign(positionError) * 0.0001) : positionError;
- const float usedDeceleration = hardBrakingNeeded ?
- std::abs(currentV * currentV / 2.f / safePositionError) :
- deceleration;
+ const float safePositionError =
+ (std::abs(positionError) < 0.0001) ? (sign(positionError) * 0.0001) : positionError;
+ const float usedDeceleration = hardBrakingNeeded
+ ? std::abs(currentV * currentV / 2.f / safePositionError)
+ : deceleration;
const bool decelerate =
std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
hardBrakingNeeded ||
- sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
+ sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
- const float usedacc = decelerate ? -usedDeceleration : acceleration;
+ const float usedacc = decelerate ? -usedDeceleration : acceleration;
const float deltaVel = signV * usedacc * useddt;
float newTargetVelRampCtrl = std::clamp(currentV + deltaVel, -maxV, maxV);
bool PIDActive = /*std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl)
@@ -589,10 +639,11 @@ namespace armarx
// ARMARX_INFO << "Switching to PID mode: " << VAROUT(positionError) << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl);
// }
this->currentlyPIDActive = PIDActive;
- float finalTargetVel = (currentlyPIDActive) ? newTargetVelPController : newTargetVelRampCtrl;
+ float finalTargetVel =
+ (currentlyPIDActive) ? newTargetVelPController : newTargetVelRampCtrl;
if (std::abs(positionError) < accuracy)
{
- return 0.0;// if close to target set zero velocity to avoid oscillating around target
+ return 0.0; // if close to target set zero velocity to avoid oscillating around target
}
// if (hardBrakingNeeded)
// {
@@ -604,10 +655,17 @@ namespace armarx
// VAROUT(usedacc) << VAROUT(deltaVel) << VAROUT(useddt);
// }
#ifdef DEBUG_POS_CTRL
- buffer.push_back({currentPosition, newTargetVelPController, newTargetVelRampCtrl, currentV, positionError, IceUtil::Time::now().toMicroSeconds()});
+ buffer.push_back({currentPosition,
+ newTargetVelPController,
+ newTargetVelRampCtrl,
+ currentV,
+ positionError,
+ IceUtil::Time::now().toMicroSeconds()});
// if (PIDModeActive != usePID)
- if (buffer.size() > 0 && sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 && eventHappeningCounter < 0)
+ if (buffer.size() > 0 &&
+ sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 &&
+ eventHappeningCounter < 0)
{
eventHappeningCounter = 10;
ARMARX_IMPORTANT << "HIGH VELOCITY DETECTED";
@@ -617,24 +675,30 @@ namespace armarx
ARMARX_IMPORTANT << "BUFFER CONTENT";
for (auto& elem : buffer)
{
- ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID) << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV) << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
+ ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID)
+ << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV)
+ << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
}
- ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
-
+ ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl)
+ << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
}
if (eventHappeningCounter >= 0)
{
eventHappeningCounter--;
}
- ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV) << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
+ ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController)
+ << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID)
+ << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV)
+ << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
PIDModeActive = usePID;
#endif
return finalTargetVel;
}
- float PositionThroughVelocityControllerWithAccelerationBounds::estimateTime() const
+ float
+ PositionThroughVelocityControllerWithAccelerationBounds::estimateTime() const
{
// const float brakingDistance = sign(currentV) * currentV * currentV / 2.f / deceleration;
const float posError = targetPosition - currentPosition;
@@ -649,7 +713,7 @@ namespace armarx
float t = 0;
float curVel = currentV;
float curPos = currentPosition;
- auto curPosEr = [&] {return targetPosition - curPos;};
+ auto curPosEr = [&] { return targetPosition - curPos; };
// auto curBrake = [&] {return std::abs(brakingDistance(curVel, deceleration));};
//check for overshooting of target
@@ -671,24 +735,27 @@ namespace armarx
// }
//curBrake()<=curPosEr() && curVel <= maxV
// auto tr = trapeze(curVel, acceleration, maxV, deceleration, 0, curPosEr());
- auto tr = trapeze(curVel, std::abs(curVel) > maxV ? deceleration : acceleration, maxV, deceleration, 0, curPosEr());
+ auto tr = trapeze(curVel,
+ std::abs(curVel) > maxV ? deceleration : acceleration,
+ maxV,
+ deceleration,
+ 0,
+ curPosEr());
return t + tr.at(0).dt + tr.at(1).dt + tr.at(2).dt;
-
-
}
- bool PositionThroughVelocityControllerWithAccelerationAndPositionBounds::validParameters() const
+ bool
+ PositionThroughVelocityControllerWithAccelerationAndPositionBounds::validParameters() const
{
- return PositionThroughVelocityControllerWithAccelerationBounds::validParameters() &&
- ! std::isnan(positionLimitLo) &&
- ! std::isnan(positionLimitHi) &&
- positionLimitLo < positionLimitHi &&
- targetPosition <= positionLimitHi &&
- positionLimitLo <= targetPosition;
+ return PositionThroughVelocityControllerWithAccelerationBounds::validParameters() &&
+ !std::isnan(positionLimitLo) && !std::isnan(positionLimitHi) &&
+ positionLimitLo < positionLimitHi && targetPosition <= positionLimitHi &&
+ positionLimitLo <= targetPosition;
}
- float PositionThroughVelocityControllerWithAccelerationAndPositionBounds::run() const
+ float
+ PositionThroughVelocityControllerWithAccelerationAndPositionBounds::run() const
{
const float useddt = std::min(std::abs(dt), std::abs(maxDt));
const float signV = sign(currentV);
@@ -701,7 +768,9 @@ namespace armarx
// 5. we need to decelerate (other cases)
//handle case 1
const float vsquared = currentV * currentV;
- const float brakingDistance = signV * vsquared / 2.f / deceleration; //the braking distance points in the direction of the velocity
+ const float brakingDistance =
+ signV * vsquared / 2.f /
+ deceleration; //the braking distance points in the direction of the velocity
const float posIfBrakingNow = currentPosition + brakingDistance;
float direction = targetPosition < currentPosition ? -1 : 1;
// ARMARX_INFO << deactivateSpam(0.5) << VAROUT(direction) << VAROUT(brakingDistance) << VAROUT(currentPosition) << VAROUT(targetPosition);
@@ -722,11 +791,12 @@ namespace armarx
return PositionThroughVelocityControllerWithAccelerationBounds::run();
}
- std::array<deltas, 3> trapeze(float v0, float acc, float vMax, float dec, float vt, float dx)
+ std::array<deltas, 3>
+ trapeze(float v0, float acc, float vMax, float dec, float vt, float dx)
{
- acc = std::abs(acc);
+ acc = std::abs(acc);
vMax = std::abs(vMax);
- dec = std::abs(dec);
+ dec = std::abs(dec);
auto calc = [&](float vmax)
{
const deltas dacc = accelerateToVelocity(v0, acc, vmax);
@@ -740,7 +810,7 @@ namespace armarx
mid.dx = dxMax - dx;
mid.dv = vMax;
mid.dt = std::abs(mid.dx / mid.dv);
- return std::make_pair(true, std::array<deltas, 3> {dacc, mid, ddec});
+ return std::make_pair(true, std::array<deltas, 3>{dacc, mid, ddec});
}
//we need a lower vMax (vx)
//in all following formulas # elem {0,t}
@@ -758,9 +828,11 @@ namespace armarx
// -> vx1/2 = pq(p,q)
const float a0 = acc;
const float at = dec;
- const float div = (1.f / 2.f / a0 + 1.f / 2.f / at);
- const float p = (v0 / 2.f - v0 / a0 + vt / 2.f - vt / at) / div;
- const float q = (v0 * v0 / 2.f * (1.f / a0 - 1.f) + vt * vt / 2.f * (1.f / at - 1.f) - dx) / div;
+ const float div = (1.f / 2.f / a0 + 1.f / 2.f / at);
+ const float p = (v0 / 2.f - v0 / a0 + vt / 2.f - vt / at) / div;
+ const float q =
+ (v0 * v0 / 2.f * (1.f / a0 - 1.f) + vt * vt / 2.f * (1.f / at - 1.f) - dx) /
+ div;
const auto vxs = pq(p, q);
//one or both of the results may be invalid
bool vx1Valid = std::isfinite(vxs.first) && std::abs(vxs.first) <= std::abs(vmax);
@@ -778,7 +850,7 @@ namespace armarx
mid.dx = 0;
mid.dv = vx;
mid.dt = 0;
- return std::make_pair(true, std::array<deltas, 3> {daccvx, mid, ddecvx});
+ return std::make_pair(true, std::array<deltas, 3>{daccvx, mid, ddecvx});
}
case 2:
{
@@ -792,20 +864,21 @@ namespace armarx
if (daccvx1.dt + ddecvx1.dt < daccvx2.dt + ddecvx2.dt)
{
mid.dv = vxs.first;
- return std::make_pair(true, std::array<deltas, 3> {daccvx1, mid, ddecvx1});
+ return std::make_pair(true,
+ std::array<deltas, 3>{daccvx1, mid, ddecvx1});
}
mid.dv = vxs.second;
- return std::make_pair(true, std::array<deltas, 3> {daccvx2, mid, ddecvx2});
+ return std::make_pair(true, std::array<deltas, 3>{daccvx2, mid, ddecvx2});
}
default:
- throw std::logic_error {"unreachable code (bool + bool neither 0,1,2)"};
+ throw std::logic_error{"unreachable code (bool + bool neither 0,1,2)"};
}
}
return std::make_pair(false, std::array<deltas, 3>());
};
const auto plusVMax = calc(vMax);
- const auto negVMax = calc(-vMax);
+ const auto negVMax = calc(-vMax);
switch (plusVMax.first + negVMax.first)
{
case 0:
@@ -814,26 +887,26 @@ namespace armarx
return plusVMax.first ? plusVMax.second : negVMax.second;
case 2:
{
- const float dt1 = plusVMax.second.at(0).dt + plusVMax.second.at(1).dt + plusVMax.second.at(2).dt;
- const float dt2 = negVMax .second.at(0).dt + negVMax .second.at(1).dt + negVMax .second.at(2).dt;
+ const float dt1 =
+ plusVMax.second.at(0).dt + plusVMax.second.at(1).dt + plusVMax.second.at(2).dt;
+ const float dt2 =
+ negVMax.second.at(0).dt + negVMax.second.at(1).dt + negVMax.second.at(2).dt;
return dt1 < dt2 ? plusVMax.second : negVMax.second;
}
default:
- throw std::logic_error {"unreachable code (bool + bool neither 0,1,2)"};
+ throw std::logic_error{"unreachable code (bool + bool neither 0,1,2)"};
}
}
-
-
-
-
- float PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition::run() const
+ float
+ PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition::run() const
{
//run should be static, but we need to adapt the parabs for the subcontroler -> have the subcontroler as value;
- PositionThroughVelocityControllerWithAccelerationBounds sub = *this; // slice parent class from this
+ PositionThroughVelocityControllerWithAccelerationBounds sub =
+ *this; // slice parent class from this
sub.currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
// ARMARX_INFO << deactivateSpam(1) << VAROUT(currentPosition) << VAROUT(sub.currentPosition) << VAROUT(positionPeriodLo) << VAROUT(positionPeriodHi);
- sub.targetPosition = periodicClamp(targetPosition, positionPeriodLo, positionPeriodHi);
+ sub.targetPosition = periodicClamp(targetPosition, positionPeriodLo, positionPeriodHi);
const float brakingDist = brakingDistance(currentV, deceleration);
// const float posIfBrakingNow = currentPosition + brakingDist;
@@ -850,10 +923,12 @@ namespace armarx
//we transform the problem with periodic bounds into
//a problem with position bounds
- const float positionPeriodLength = std::abs(positionPeriodHi - positionPeriodLo);
+ const float positionPeriodLength = std::abs(positionPeriodHi - positionPeriodLo);
//we shift the positions such that 0 == target
- sub.currentPosition = periodicClamp(currentPosition - targetPosition, -positionPeriodLength * 0.5f, positionPeriodLength * 0.5f);
+ sub.currentPosition = periodicClamp(currentPosition - targetPosition,
+ -positionPeriodLength * 0.5f,
+ positionPeriodLength * 0.5f);
//how many times will we go over the target if we simply brake now?
const float overshoot = std::trunc((currentPosition + brakingDist) / positionPeriodLength);
@@ -862,27 +937,34 @@ namespace armarx
if (direction == 0)
{
//determine the direction to go (1 == pos vel, -1 == neg vel)
- direction = (periodicClamp(currentPosition + brakingDist, -positionPeriodLength * 0.5f, positionPeriodLength * 0.5f) <= 0) ? 1 : -1;
+ direction = (periodicClamp(currentPosition + brakingDist,
+ -positionPeriodLength * 0.5f,
+ positionPeriodLength * 0.5f) <= 0)
+ ? 1
+ : -1;
}
//shift the target away from 0
- sub.targetPosition = (overshoot * -direction) * positionPeriodLength; // - direction * pControlPosErrorLimit;
+ sub.targetPosition =
+ (overshoot * -direction) * positionPeriodLength; // - direction * pControlPosErrorLimit;
//move
return sub.run();
}
- double PositionThroughVelocityControllerWithAccelerationRamps::getTargetPosition() const
+ double
+ PositionThroughVelocityControllerWithAccelerationRamps::getTargetPosition() const
{
return targetPosition;
}
- bool PositionThroughVelocityControllerWithAccelerationBounds::getCurrentlyPIDActive() const
+ bool
+ PositionThroughVelocityControllerWithAccelerationBounds::getCurrentlyPIDActive() const
{
return currentlyPIDActive;
}
-
- void PositionThroughVelocityControllerWithAccelerationRamps::setTargetPosition(double value)
+ void
+ PositionThroughVelocityControllerWithAccelerationRamps::setTargetPosition(double value)
{
if (std::abs(value - targetPosition) > 0.0001)
{
@@ -891,23 +973,22 @@ namespace armarx
targetPosition = value;
}
- PositionThroughVelocityControllerWithAccelerationRamps::PositionThroughVelocityControllerWithAccelerationRamps()
+ PositionThroughVelocityControllerWithAccelerationRamps::
+ PositionThroughVelocityControllerWithAccelerationRamps()
{
-
}
- bool PositionThroughVelocityControllerWithAccelerationRamps::validParameters() const
+ bool
+ PositionThroughVelocityControllerWithAccelerationRamps::validParameters() const
{
- return maxV > 0 &&
- acceleration > 0 &&
- deceleration > 0 &&
- jerk > 0 &&
- // pControlPosErrorLimit > 0 &&
- // pControlVelLimit > 0 &&
- p > 0;
+ return maxV > 0 && acceleration > 0 && deceleration > 0 && jerk > 0 &&
+ // pControlPosErrorLimit > 0 &&
+ // pControlVelLimit > 0 &&
+ p > 0;
}
- PositionThroughVelocityControllerWithAccelerationRamps::Output PositionThroughVelocityControllerWithAccelerationRamps::run()
+ PositionThroughVelocityControllerWithAccelerationRamps::Output
+ PositionThroughVelocityControllerWithAccelerationRamps::run()
{
PositionThroughVelocityControllerWithAccelerationRamps::Output result;
const double useddt = std::min(std::abs(dt), std::abs(maxDt));
@@ -940,34 +1021,48 @@ namespace armarx
// double newAcceleration = output.acceleration;
// ARMARX_INFO << "state: " << (int)state << " new state: " << (int)newState;
state = newState;
- const double brakingDistance = brData.dt2 >= 0 ? brData.s_total : signV * currentV * currentV / 2.f / deceleration; //the braking distance points in the direction of the velocity
+ const double brakingDistance =
+ brData.dt2 >= 0
+ ? brData.s_total
+ : signV * currentV * currentV / 2.f /
+ deceleration; //the braking distance points in the direction of the velocity
const double posIfBrakingNow = currentPosition + brakingDistance;
const double posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
const bool hardBrakingNeeded = std::abs(brakingDistance) > std::abs(positionError);
// const double usedJerk = jerk;//currentState==State::DecAccDecJerk?std::abs(ctrlutil::jerk(ctrlutil::t_to_v(currentV, -signV*currentAcc, signV*jerk), positionError, currentV, currentAcc))
// :jerk;
- const double jerkDir = (newState == State::DecAccIncJerk || newState == State::IncAccDecJerk) ? -1.0f : 1.0f;
+ const double jerkDir =
+ (newState == State::DecAccIncJerk || newState == State::IncAccDecJerk) ? -1.0f : 1.0f;
const double usedJerk = goalDir * jerkDir * usedAbsJerk;
- const double deltaAcc = usedJerk * useddt;
- const double newAcceleration = (newState == State::Keep) ? currentAcc : (newState == State::DecAccDecJerk ? output.acceleration : currentAcc + deltaAcc);
+ const double deltaAcc = usedJerk * useddt;
+ const double newAcceleration =
+ (newState == State::Keep)
+ ? currentAcc
+ : (newState == State::DecAccDecJerk ? output.acceleration : currentAcc + deltaAcc);
result.acceleration = newAcceleration;
result.jerk = usedJerk;
- const double usedDeceleration = hardBrakingNeeded ?
- -std::abs(currentV * currentV / 2.f / math::MathUtils::LimitTo(positionError, 0.0001)) :
- newAcceleration;
- (void) usedDeceleration;
+ const double usedDeceleration =
+ hardBrakingNeeded ? -std::abs(currentV * currentV / 2.f /
+ math::MathUtils::LimitTo(positionError, 0.0001))
+ : newAcceleration;
+ (void)usedDeceleration;
const bool decelerate =
std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
hardBrakingNeeded ||
- sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
- (void) decelerate;
-
- const double deltaVel = ctrlutil::v(useddt, 0, newAcceleration, usedJerk);//signV * newAcceleration * useddt;
- (void) deltaVel;
-
- double newTargetVelRampCtrl = ctrlutil::v(useddt, currentV, currentAcc, usedJerk);//boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
+ sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
+ (void)decelerate;
+
+ const double deltaVel =
+ ctrlutil::v(useddt, 0, newAcceleration, usedJerk); //signV * newAcceleration * useddt;
+ (void)deltaVel;
+
+ double newTargetVelRampCtrl =
+ ctrlutil::v(useddt,
+ currentV,
+ currentAcc,
+ usedJerk); //boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
newTargetVelRampCtrl = std::clamp(newTargetVelRampCtrl, -maxV, maxV);
bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl); //||
// std::abs(newTargetVelPController) < pControlVelLimit &&
@@ -979,18 +1074,27 @@ namespace armarx
state = State::PCtrl;
}
double finalTargetVel = usePID ? newTargetVelPController : newTargetVelRampCtrl;
- ARMARX_INFO << VAROUT(usePID) << VAROUT(result.jerk) << VAROUT(newTargetVelPController) << VAROUT(currentV);
+ ARMARX_INFO << VAROUT(usePID) << VAROUT(result.jerk) << VAROUT(newTargetVelPController)
+ << VAROUT(currentV);
// ARMARX_INFO << VAROUT(currentV) << VAROUT(currentAcc) << VAROUT(usedJerk) << VAROUT(newAcceleration) << VAROUT(deltaVel) << VAROUT(finalTargetVel) << " new distance: " << (targetPosition - ctrlutil::s(dt, currentPosition, currentV, currentAcc, usedJerk));
if (std::abs(positionError) < accuracy)
{
- finalTargetVel = 0.0f; // if close to target set zero velocity to avoid oscillating around target
+ finalTargetVel =
+ 0.0f; // if close to target set zero velocity to avoid oscillating around target
}
#ifdef DEBUG_POS_CTRL
- buffer.push_back({currentPosition, newTargetVelPController, newTargetVelRampCtrl, currentV, positionError, IceUtil::Time::now().toMicroSeconds()});
+ buffer.push_back({currentPosition,
+ newTargetVelPController,
+ newTargetVelRampCtrl,
+ currentV,
+ positionError,
+ IceUtil::Time::now().toMicroSeconds()});
// if (PIDModeActive != usePID)
- if (buffer.size() > 0 && sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 && eventHappeningCounter < 0)
+ if (buffer.size() > 0 &&
+ sign(positionError) * sign(buffer[buffer.size() - 2].currentError) < 0 &&
+ eventHappeningCounter < 0)
{
eventHappeningCounter = 10;
ARMARX_IMPORTANT << "HIGH VELOCITY DETECTED";
@@ -1000,30 +1104,37 @@ namespace armarx
ARMARX_IMPORTANT << "BUFFER CONTENT";
for (auto& elem : buffer)
{
- ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID) << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV) << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
+ ARMARX_INFO << VAROUT(elem.currentPosition) << VAROUT(elem.targetVelocityPID)
+ << VAROUT(elem.targetVelocityRAMP) << VAROUT(elem.currentV)
+ << VAROUT(elem.currentError) << VAROUT(elem.timestamp);
}
- ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
-
+ ARMARX_IMPORTANT << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl)
+ << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError);
}
if (eventHappeningCounter >= 0)
{
eventHappeningCounter--;
}
- ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController) << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID) << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV) << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
+ ARMARX_INFO << deactivateSpam(5) << VAROUT(newTargetVelPController)
+ << VAROUT(newTargetVelRampCtrl) << VAROUT(currentPosition) << VAROUT(usePID)
+ << VAROUT(positionError) << VAROUT(targetPosition) << VAROUT(currentV)
+ << VAROUT(deltaVel) << VAROUT(useddt) << VAROUT(usedacc);
PIDModeActive = usePID;
#endif
result.velocity = finalTargetVel;
return result;
}
- double PositionThroughVelocityControllerWithAccelerationRamps::estimateTime() const
+ double
+ PositionThroughVelocityControllerWithAccelerationRamps::estimateTime() const
{
throw LocalException("NYI");
return 0;
}
- double PositionThroughVelocityControllerWithAccelerationRamps::calculateProportionalGain() const
+ double
+ PositionThroughVelocityControllerWithAccelerationRamps::calculateProportionalGain() const
{
/* s = v_0*v_0/(2*a) -> sqrt(s*2*a) = v_0;
@@ -1034,7 +1145,8 @@ namespace armarx
}
std::pair<PositionThroughVelocityControllerWithAccelerationRamps::State,
- PositionThroughVelocityControllerWithAccelerationRamps::Output> PositionThroughVelocityControllerWithAccelerationRamps::calcState() const
+ PositionThroughVelocityControllerWithAccelerationRamps::Output>
+ PositionThroughVelocityControllerWithAccelerationRamps::calcState() const
{
// auto integratedChange = [](v0)
@@ -1048,20 +1160,29 @@ namespace armarx
const auto brData = ctrlutil::braking_distance_with_wedge(currentV, currentAcc, newJerk);
const auto goalDir = math::MathUtils::Sign(targetPosition - currentPosition);
const auto curVDir = math::MathUtils::Sign(currentV);
- const auto straightBrakingDistance = ctrlutil::brakingDistance(currentV, currentAcc, newJerk);
+ const auto straightBrakingDistance =
+ ctrlutil::brakingDistance(currentV, currentAcc, newJerk);
const double brakingDistance = brData.dt2 < 0 ? straightBrakingDistance : brData.s_total;
const double distance = std::abs(targetPosition - currentPosition);
- const double t_to_stop = ctrlutil::t_to_v(currentV, -curVDir * currentAcc, curVDir * newJerk);
- const auto calculatedJerk = std::abs(ctrlutil::jerk(t_to_stop, distance, currentV, currentAcc));
+ const double t_to_stop =
+ ctrlutil::t_to_v(currentV, -curVDir * currentAcc, curVDir * newJerk);
+ const auto calculatedJerk =
+ std::abs(ctrlutil::jerk(t_to_stop, distance, currentV, currentAcc));
double tmp_t, tmp_acc, tmp_jerk;
// std::tie( tmp_t, tmp_acc, tmp_jerk) = ctrlutil::calcAccAndJerk(targetPosition-(currentPosition+brData.s1), brData.v1);
- std::tie(tmp_t, tmp_acc, tmp_jerk) = ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
- const double posWhenBrakingWithCustomJerk = ctrlutil::s(tmp_t, currentPosition + brData.s1, brData.v1, tmp_acc, curVDir * tmp_jerk);
+ std::tie(tmp_t, tmp_acc, tmp_jerk) =
+ ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
+ const double posWhenBrakingWithCustomJerk =
+ ctrlutil::s(tmp_t, currentPosition + brData.s1, brData.v1, tmp_acc, curVDir * tmp_jerk);
// const double brakingDistanceWithCustomJerk = ctrlutil::s(tmp_t, 0, currentV, tmp_acc, curVDir * tmp_jerk);
- const double posWhenBrakingWithFixedJerk = ctrlutil::s(brData.dt2, currentPosition + brData.s1, brData.v1, brData.a1, curVDir * jerk);
- (void) calculatedJerk, (void) posWhenBrakingWithCustomJerk, (void) posWhenBrakingWithFixedJerk;
+ const double posWhenBrakingWithFixedJerk = ctrlutil::s(
+ brData.dt2, currentPosition + brData.s1, brData.v1, brData.a1, curVDir * jerk);
+ (void)calculatedJerk, (void)posWhenBrakingWithCustomJerk, (void)posWhenBrakingWithFixedJerk;
- ARMARX_INFO << VAROUT((int)state) << VAROUT(distance) << VAROUT(brakingDistance);//VAROUT(straightBrakingDistance) << VAROUT(brData.dt1) << VAROUT(brData.dt2) << VAROUT(brData.s_total) << VAROUT(calculatedJerk);
+ ARMARX_INFO
+ << VAROUT((int)state) << VAROUT(distance)
+ << VAROUT(
+ brakingDistance); //VAROUT(straightBrakingDistance) << VAROUT(brData.dt1) << VAROUT(brData.dt2) << VAROUT(brData.s_total) << VAROUT(calculatedJerk);
// if(brData.dt2 < 0)
// {
@@ -1074,8 +1195,10 @@ namespace armarx
// return;
// }
if (state < State::DecAccIncJerk &&
- ((distance > brakingDistance && (math::MathUtils::Sign(currentAcc) == curVDir || std::abs(currentAcc) < 0.1)) // we are accelerating
- || distance > brakingDistance * 2 // we are decelerating -> dont switch to accelerating to easily
+ ((distance > brakingDistance && (math::MathUtils::Sign(currentAcc) == curVDir ||
+ std::abs(currentAcc) < 0.1)) // we are accelerating
+ || distance > brakingDistance *
+ 2 // we are decelerating -> dont switch to accelerating to easily
|| curVDir != goalDir)) // we are moving away from goal
{
if (std::abs(maxV - currentV) < 0.1 && std::abs(currentAcc) < 0.1)
@@ -1085,19 +1208,19 @@ namespace armarx
return std::make_pair(newState, result);
}
// calculate if we need to increase or decrease acc
- double t_to_max_v = ctrlutil::t_to_v(goalDir * maxV - currentV, currentAcc, goalDir * newJerk);
+ double t_to_max_v =
+ ctrlutil::t_to_v(goalDir * maxV - currentV, currentAcc, goalDir * newJerk);
double acc_at_t = ctrlutil::a(t_to_max_v, std::abs(currentAcc), -newJerk);
if (acc_at_t < 0.1)
{
- newState = State::IncAccIncJerk;
+ newState = State::IncAccIncJerk;
return std::make_pair(newState, result);
}
else
{
- newState = State::IncAccDecJerk;
+ newState = State::IncAccDecJerk;
return std::make_pair(newState, result);
}
-
}
else
{
@@ -1106,41 +1229,44 @@ namespace armarx
double tmpV = ctrlutil::v(t_to_0, 0, currentAcc, -curVDir * newJerk);
double vAfterBraking = ctrlutil::v(t_to_0, currentV, currentAcc, -curVDir * newJerk);
double accAfterBraking = ctrlutil::a(t_to_0, currentAcc, curVDir * newJerk);
- const double posWhenBrakingNow = ctrlutil::s(t_to_0, currentPosition, currentV, currentAcc, -curVDir * newJerk);
- const double simpleBrakingDistance = ctrlutil::s(t_to_0, 0, currentV, currentAcc, -curVDir * newJerk);
- (void) tmpV, (void) vAfterBraking, (void) accAfterBraking, (void) posWhenBrakingNow, (void) simpleBrakingDistance;
+ const double posWhenBrakingNow =
+ ctrlutil::s(t_to_0, currentPosition, currentV, currentAcc, -curVDir * newJerk);
+ const double simpleBrakingDistance =
+ ctrlutil::s(t_to_0, 0, currentV, currentAcc, -curVDir * newJerk);
+ (void)tmpV, (void)vAfterBraking, (void)accAfterBraking, (void)posWhenBrakingNow,
+ (void)simpleBrakingDistance;
double acc_at_t = ctrlutil::a(t_to_0, std::abs(currentAcc), -newJerk);
- std::tie(t, a0, newJerk) = ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
+ std::tie(t, a0, newJerk) =
+ ctrlutil::calcAccAndJerk(targetPosition - currentPosition, currentV);
double at = ctrlutil::a(t, a0, newJerk);
double vt = ctrlutil::v(t, currentV, a0, newJerk);
double st = ctrlutil::s(t, targetPosition - currentPosition, currentV, a0, newJerk);
- (void) at, (void) vt, (void) st;
+ (void)at, (void)vt, (void)st;
if (this->state <= State::Keep)
{
- newState = State::DecAccIncJerk;
+ newState = State::DecAccIncJerk;
return std::make_pair(newState, result);
}
- else if ((state == State::DecAccIncJerk && acc_at_t > 1) || state == State::DecAccDecJerk)
+ else if ((state == State::DecAccIncJerk && acc_at_t > 1) ||
+ state == State::DecAccDecJerk)
{
result.jerk = newJerk;
result.acceleration = currentAcc + (a0 - currentAcc) * 0.5 + dt * newJerk;
- newState = State::DecAccDecJerk;
+ newState = State::DecAccDecJerk;
return std::make_pair(newState, result);
}
else
{
return std::make_pair(newState, result);
}
-
}
throw LocalException();
}
-
-
- void MinJerkPositionController::reset()
+ void
+ MinJerkPositionController::reset()
{
currentTime = 0;
@@ -1150,7 +1276,8 @@ namespace armarx
currentP_Phase3 = -1;
}
- double MinJerkPositionController::getTargetPosition() const
+ double
+ MinJerkPositionController::getTargetPosition() const
{
return targetPosition;
}
@@ -1245,8 +1372,6 @@ namespace armarx
// }
-
-
// double jerk = min_jerk_calc_jerk(remainingTime, currentPosition, currentV, currentAcc, targetPosition);
// double newAcc = currentAcc + jerk * dt;
// double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk);
@@ -1296,17 +1421,18 @@ namespace armarx
// }
- bool VelocityControllerWithRampedAccelerationAndPositionBounds::validParameters() const
+ bool
+ VelocityControllerWithRampedAccelerationAndPositionBounds::validParameters() const
{
return VelocityControllerWithRampedAcceleration::validParameters() &&
- positionLimitHiSoft > positionLimitLoSoft &&
- deceleration > 0;
+ positionLimitHiSoft > positionLimitLoSoft && deceleration > 0;
}
- VelocityControllerWithRampedAcceleration::Output VelocityControllerWithRampedAccelerationAndPositionBounds::run() const
+ VelocityControllerWithRampedAcceleration::Output
+ VelocityControllerWithRampedAccelerationAndPositionBounds::run() const
{
const double useddt = std::min(std::abs(dt), std::abs(maxDt));
- (void) useddt;
+ (void)useddt;
// if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
// {
@@ -1334,14 +1460,18 @@ namespace armarx
double nextv;
//handle case 1
const double vsquared = currentV * currentV;
- const double brakingDist = sign(currentV) * vsquared / 2.f / std::abs(deceleration); //the braking distance points in the direction of the velocity
+ const double brakingDist =
+ sign(currentV) * vsquared / 2.f /
+ std::abs(deceleration); //the braking distance points in the direction of the velocity
const double posIfBrakingNow = currentPosition + brakingDist;
// ARMARX_INFO << deactivateSpam(0.1) << VAROUT(currentPosition) << VAROUT(positionLimitLoSoft) << VAROUT(positionLimitHiSoft) << VAROUT(posIfBrakingNow) << VAROUT(currentV);
- if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) || (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
+ if ((posIfBrakingNow <= positionLimitLoSoft && currentV < 0) ||
+ (posIfBrakingNow >= positionLimitHiSoft && currentV > 0))
{
//case 1. -> brake now! (we try to have v=0 at the limit)
- const auto limit = posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
+ const auto limit =
+ posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
const double wayToGo = limit - currentPosition;
//decelerate!
@@ -1361,7 +1491,6 @@ namespace armarx
//handle 2-3
return VelocityControllerWithRampedAcceleration::run();
// ARMARX_INFO << deactivateSpam(1) << "Desired target Vel: " << targetV << " " << VAROUT(nextv);
-
}
if (softLimitViolation == sign(nextv) && nextv != 0)
{
@@ -1373,12 +1502,11 @@ namespace armarx
// double nextAcc = (nextv - currentV) / useddt; // doesnt work with calculated acc!!
// double nextJerk = (nextAcc - currentAcc) / useddt;
//the next velocity will not violate the pos bounds harder than they are already
- return Output {nextv, 0, 0};
+ return Output{nextv, 0, 0};
}
-
-
- void MinJerkPositionController::setTargetPosition(double newTargetPosition)
+ void
+ MinJerkPositionController::setTargetPosition(double newTargetPosition)
{
if (std::abs(targetPosition - newTargetPosition) > 0.0001)
{
@@ -1387,10 +1515,11 @@ namespace armarx
double rawFinishTime = 0.0;
double decelerationTime = std::abs(currentV / desiredDeceleration);
// calculate the time it should take to reach the goal (ToDo: find exact time)
- if (math::MathUtils::Sign(currentV) == math::MathUtils::Sign(distance)
- && std::abs(currentV) > 0.0
- && ctrlutil::s(decelerationTime, 0, std::abs(currentV), -desiredDeceleration, 0) > std::abs(distance))
- // && ctrlutil::brakingDistance(currentV, currentAcc, desiredJerk) < distance)
+ if (math::MathUtils::Sign(currentV) == math::MathUtils::Sign(distance) &&
+ std::abs(currentV) > 0.0 &&
+ ctrlutil::s(decelerationTime, 0, std::abs(currentV), -desiredDeceleration, 0) >
+ std::abs(distance))
+ // && ctrlutil::brakingDistance(currentV, currentAcc, desiredJerk) < distance)
{
// ARMARX_INFO << "branch 1: " << VAROUT(currentV) << VAROUT(desiredDeceleration) << VAROUT(distance);
rawFinishTime = decelerationTime;
@@ -1398,11 +1527,14 @@ namespace armarx
else
{
// ARMARX_INFO << "branch 2: " << VAROUT(currentV) << VAROUT(desiredDeceleration) << VAROUT(distance);
- double accelerationTime = std::abs((math::MathUtils::Sign(distance) * maxV - currentV) / desiredDeceleration);
+ double accelerationTime = std::abs(
+ (math::MathUtils::Sign(distance) * maxV - currentV) / desiredDeceleration);
double decelerationTime = std::abs(maxV / desiredDeceleration);
- rawFinishTime = std::max(decelerationTime + accelerationTime, std::abs(distance / (maxV * 0.75)));
+ rawFinishTime = std::max(decelerationTime + accelerationTime,
+ std::abs(distance / (maxV * 0.75)));
}
- double estimatedTime = ctrlutil::t_to_v_with_wedge_acc(std::abs(distance), std::abs(currentV), std::abs(desiredDeceleration));
+ double estimatedTime = ctrlutil::t_to_v_with_wedge_acc(
+ std::abs(distance), std::abs(currentV), std::abs(desiredDeceleration));
double minTime = 0.8;
if (!std::isnan(estimatedTime))
{
@@ -1414,22 +1546,27 @@ namespace armarx
}
targetPosition = newTargetPosition;
// ARMARX_INFO << "New finish time: " << finishTime << " " << VAROUT(rawFinishTime) << VAROUT(estimatedTime);
-
}
-
-
}
- MinJerkPositionController::Output MinJerkPositionController::run()
+ MinJerkPositionController::Output
+ MinJerkPositionController::run()
{
- auto min_jerk_calc_jerk = [&](double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
+ auto min_jerk_calc_jerk =
+ [&](double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
{
double D = tf - t0;
return 60.0 / (D * D * D) * (xf - s0) - 36.0 / (D * D) * v0 - 9.0 / D * a0;
};
- auto min_jerk = [&](double t, double tf, double s0, double v0, double a0, double xf = 0, double t0 = 0.0)
+ auto min_jerk = [&](double t,
+ double tf,
+ double s0,
+ double v0,
+ double a0,
+ double xf = 0,
+ double t0 = 0.0)
{
double D = tf - t0;
ARMARX_CHECK(D != 0.0);
@@ -1448,9 +1585,10 @@ namespace armarx
double tau4 = tau3 * tau;
double tau5 = tau4 * tau;
double st = b0 + b1 * tau + b2 * tau2 + b3 * tau3 + b4 * tau4 + b5 * tau5;
- double vt = b1 / D + 2 * b2 / D * tau + 3 * b3 / D * tau2 + 4 * b4 / D * tau3 + 5 * b5 / D * tau4;
+ double vt = b1 / D + 2 * b2 / D * tau + 3 * b3 / D * tau2 + 4 * b4 / D * tau3 +
+ 5 * b5 / D * tau4;
double at = 2 * b2 / D2 + 6 * b3 / D2 * tau + 12 * b4 / D2 * tau2 + 20 * b5 / D2 * tau3;
- return State {t, st, vt, at};
+ return State{t, st, vt, at};
};
double remainingTime = finishTime - currentTime;
@@ -1484,18 +1622,20 @@ namespace armarx
double newJerk = (newAcc - currentAcc) / dt;
double newPos = currentPosition + newVel * dt;
- Output result {newPos, newVel, newAcc, newJerk};
+ Output result{newPos, newVel, newAcc, newJerk};
currentTime += dt;
// State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
// ARMARX_INFO << deactivateSpam(0.1) << VAROUT(remainingTime) << VAROUT(signedDistance) << VAROUT(signedDistance * currentP_Phase3) << VAROUT(currentP_Phase3);// << VAROUT(s.s) << VAROUT(s.v) << VAROUT(s.a);
return result;
}
- if (std::abs(signedDistance) < phase2SwitchDistance && remainingTime < phase2SwitchMaxRemainingTime)
+ if (std::abs(signedDistance) < phase2SwitchDistance &&
+ remainingTime < phase2SwitchMaxRemainingTime)
{
if (!fixedMinJerkState)
{
- fixedMinJerkState.reset(new FixedMinJerkState {currentTime, currentPosition, currentV, currentAcc});
+ fixedMinJerkState.reset(
+ new FixedMinJerkState{currentTime, currentPosition, currentV, currentAcc});
currentP_Phase2 = 0;
}
// std::vector<State> states;
@@ -1507,30 +1647,42 @@ namespace armarx
// states.push_back(s);
// }
currentTime += dt;
- State s = min_jerk(currentTime, finishTime, fixedMinJerkState->s0, fixedMinJerkState->v0, fixedMinJerkState->a0, targetPosition, fixedMinJerkState->t0);
+ State s = min_jerk(currentTime,
+ finishTime,
+ fixedMinJerkState->s0,
+ fixedMinJerkState->v0,
+ fixedMinJerkState->a0,
+ targetPosition,
+ fixedMinJerkState->t0);
double newVelocity = (s.s - (currentPosition + s.v * dt)) * currentP_Phase2 + s.v;
double newAcc = (newVelocity - currentV) / dt;
double newJerk = (newAcc - currentAcc) / dt;
- double newPos = ctrlutil::s(dt, currentPosition, currentV, currentAcc, newJerk);//currentPosition + newVelocity * dt;
+ double newPos = ctrlutil::s(dt,
+ currentPosition,
+ currentV,
+ currentAcc,
+ newJerk); //currentPosition + newVelocity * dt;
// ARMARX_INFO /*<< deactivateSpam(0.1) */ << VAROUT(currentP_Phase2) << VAROUT(currentPosition) << VAROUT(signedDistance) << VAROUT(s.s) << VAROUT(s.v) << VAROUT(newVelocity) << VAROUT(newAcc) << VAROUT(newJerk) << VAROUT(s.a);
currentP_Phase2 = currentP_Phase2 * p_adjust_ratio + p * (1.0 - p_adjust_ratio);
- Output result {newPos, newVelocity, newAcc, newJerk};
+ Output result{newPos, newVelocity, newAcc, newJerk};
return result;
}
-
-
- double jerk = min_jerk_calc_jerk(remainingTime, currentPosition, currentV, currentAcc, targetPosition);
+ double jerk = min_jerk_calc_jerk(
+ remainingTime, currentPosition, currentV, currentAcc, targetPosition);
double newAcc = currentAcc + jerk * dt;
double newVelocity = ctrlutil::v(dt, currentV, currentAcc, jerk);
- double accAtEnd = ctrlutil::a(remainingTime, currentAcc, jerk); // assumes that fixed jerk would be used, which is not the case here
- (void) accAtEnd;
+ double accAtEnd =
+ ctrlutil::a(remainingTime,
+ currentAcc,
+ jerk); // assumes that fixed jerk would be used, which is not the case here
+ (void)accAtEnd;
std::vector<State> states;
std::vector<State> states2;
//#define debug
-#ifdef debug
+#ifdef debug
{
double dt = 0.001;
double t = 1;
@@ -1542,7 +1694,7 @@ namespace armarx
simV = ctrlutil::v(dt, simV, simAcc, 0);
simS += dt * simV;
t -= dt;
- states.push_back(State {t, simS, simV, simAcc, jerk});
+ states.push_back(State{t, simS, simV, simAcc, jerk});
}
}
{
@@ -1557,7 +1709,7 @@ namespace armarx
simV = ctrlutil::v(dt, simV, simAcc, jerk);
simAcc = simAcc + jerk * dt;
t -= dt;
- states2.push_back(State {t, simS, simV, simAcc, jerk});
+ states2.push_back(State{t, simS, simV, simAcc, jerk});
}
}
#endif
@@ -1566,8 +1718,7 @@ namespace armarx
double newPos = ctrlutil::s(dt, currentPosition, currentV, currentAcc, jerk);
- Output result {newPos, newVelocity, newAcc, jerk};
+ Output result{newPos, newVelocity, newAcc, jerk};
return result;
-
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
index c544ea8dd4b54eb15cebd9fb10e36a00e0623234..0c35892237fa15de457dca9b2582832c0a40df3e 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -24,29 +24,32 @@
#include <cmath>
#include <type_traits>
+
#include <ArmarXCore/core/util/algorithm.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
+
#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
// #define DEBUG_POS_CTRL
#ifdef DEBUG_POS_CTRL
#include <boost/circular_buffer.hpp>
#endif
-
namespace armarx
{
- template<class T, class = typename std::enable_if<std:: is_floating_point<T>::value>::type >
- T periodicClamp(T value, T periodLo, T periodHi)
+ template <class T, class = typename std::enable_if<std::is_floating_point<T>::value>::type>
+ T
+ periodicClamp(T value, T periodLo, T periodHi)
{
float dist = periodHi - periodLo;
return std::fmod(value - periodLo + dist, dist) + periodLo;
}
- template<class T, class = typename std::enable_if<std:: is_floating_point<T>::value>::type >
- std::pair<T, T> pq(T p, T q)
+ template <class T, class = typename std::enable_if<std::is_floating_point<T>::value>::type>
+ std::pair<T, T>
+ pq(T p, T q)
{
- T a = - p / 2;
+ T a = -p / 2;
T b = std::sqrt(std::pow(p / 2, 2) - q);
return {a + b, a - b};
}
@@ -58,7 +61,8 @@ namespace armarx
float dt;
};
- inline deltas accelerateToVelocity(float v0, float acc, float vt)
+ inline deltas
+ accelerateToVelocity(float v0, float acc, float vt)
{
acc = std::abs(acc);
deltas d;
@@ -70,21 +74,29 @@ namespace armarx
std::array<deltas, 3> trapeze(float v0, float acc, float vMax, float dec, float vt, float dx);
-
- inline float trapezeArea(float v0, float vmax, float dt1, float dt2, float dt3)
+ inline float
+ trapezeArea(float v0, float vmax, float dt1, float dt2, float dt3)
{
return (v0 + vmax) / 2 * dt1 + dt2 * vmax + dt3 * vmax * 0.5;
}
-
- inline void findVelocityAndAccelerationForTimeAndDistance(float distance, float v0, float vmax, float dec, std::array<deltas, 3> trapeze, float newDt, float& newV, float& newAcc, float& newDec)
+ inline void
+ findVelocityAndAccelerationForTimeAndDistance(float distance,
+ float v0,
+ float vmax,
+ float dec,
+ std::array<deltas, 3> trapeze,
+ float newDt,
+ float& newV,
+ float& newAcc,
+ float& newDec)
{
float oldDt = trapeze.at(0).dt + trapeze.at(1).dt + trapeze.at(2).dt;
- float dt0 = trapeze.at(0).dt;
- float dt1 = trapeze.at(1).dt;
- float dt2 = trapeze.at(2).dt;
+ float dt0 = trapeze.at(0).dt;
+ float dt1 = trapeze.at(1).dt;
+ float dt2 = trapeze.at(2).dt;
float area = trapezeArea(v0, vmax, dt0, dt1, dt2);
dt1 += newDt - oldDt;
newV = (-dt0 * v0 + 2 * area) / (dt0 + 2 * dt1 + dt2);
@@ -130,25 +142,25 @@ namespace armarx
// newAcc = dec;
// newDec = dec;
-
}
-
-
}
- std::array<deltas, 3> trapezeWithDt(float v0, float acc, float vMax, float dec, float vt, float dx, float dt);
+ std::array<deltas, 3>
+ trapezeWithDt(float v0, float acc, float vMax, float dec, float vt, float dx, float dt);
/**
* @param v0 The initial velocity.
* @param deceleration The deceleration.
* @return The braking distance given the parameters.
*/
- inline float brakingDistance(float v0, float deceleration)
+ inline float
+ brakingDistance(float v0, float deceleration)
{
return accelerateToVelocity(std::abs(v0), std::abs(deceleration), 0).dx;
}
- inline float angleDistance(float angle1, float angle2)
+ inline float
+ angleDistance(float angle1, float angle2)
{
return M_PI - std::fabs(std::fmod(std::fabs(angle1 - angle2), M_PI * 2) - M_PI);
}
@@ -178,6 +190,7 @@ namespace armarx
double acceleration;
double jerk;
};
+
// config
float maxDt;
float maxV;
@@ -195,8 +208,10 @@ namespace armarx
Output run() const;
};
- typedef std::shared_ptr<class RampedAccelerationVelocityControllerConfiguration> RampedAccelerationVelocityControllerConfigurationPtr;
- typedef std::shared_ptr<const RampedAccelerationVelocityControllerConfiguration> RampedAccelerationVelocityControllerConfigurationCPtr;
+ typedef std::shared_ptr<class RampedAccelerationVelocityControllerConfiguration>
+ RampedAccelerationVelocityControllerConfigurationPtr;
+ typedef std::shared_ptr<const RampedAccelerationVelocityControllerConfiguration>
+ RampedAccelerationVelocityControllerConfigurationCPtr;
class RampedAccelerationVelocityControllerConfiguration
{
@@ -210,7 +225,8 @@ namespace armarx
float directSetVLimit;
};
- struct VelocityControllerWithRampedAccelerationAndPositionBounds: VelocityControllerWithRampedAcceleration
+ struct VelocityControllerWithRampedAccelerationAndPositionBounds :
+ VelocityControllerWithRampedAcceleration
{
double currentPosition;
@@ -223,11 +239,14 @@ namespace armarx
Output run() const;
};
- float velocityControlWithAccelerationBounds(
- float dt, float maxDt,
- const float currentV, float targetV, float maxV,
- float acceleration, float deceleration,
- float directSetVLimit);
+ float velocityControlWithAccelerationBounds(float dt,
+ float maxDt,
+ const float currentV,
+ float targetV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float directSetVLimit);
/**
* @brief Performs velocity control while staying in positional bounds, obeying acceleration / deceleration and staying below a velocity bound.
@@ -250,7 +269,8 @@ namespace armarx
* @param positionLimitHi
* @return The next velocity.
*/
- struct VelocityControllerWithAccelerationAndPositionBounds: VelocityControllerWithAccelerationBounds
+ struct VelocityControllerWithAccelerationAndPositionBounds :
+ VelocityControllerWithAccelerationBounds
{
float currentPosition;
@@ -263,16 +283,19 @@ namespace armarx
float run() const;
};
-
-
- float velocityControlWithAccelerationAndPositionBounds(float dt, float maxDt,
- float currentV, float targetV, float maxV,
- float acceleration, float deceleration,
- float directSetVLimit,
- float currentPosition,
- float positionLimitLoSoft, float positionLimitHiSoft,
- float positionLimitLoHard, float positionLimitHiHard);
-
+ float velocityControlWithAccelerationAndPositionBounds(float dt,
+ float maxDt,
+ float currentV,
+ float targetV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float directSetVLimit,
+ float currentPosition,
+ float positionLimitLoSoft,
+ float positionLimitHiSoft,
+ float positionLimitLoHard,
+ float positionLimitHiHard);
struct PositionThroughVelocityControllerWithAccelerationBounds
{
@@ -285,7 +308,8 @@ namespace armarx
float currentPosition;
float targetPosition;
float pControlPosErrorLimit = 0.01;
- float pControlVelLimit = 0.002; // if target is below this threshold, PID controller will be used
+ float pControlVelLimit =
+ 0.002; // if target is below this threshold, PID controller will be used
float accuracy = 0.001;
std::shared_ptr<PIDController> pid;
// float p;
@@ -296,6 +320,7 @@ namespace armarx
float estimateTime() const;
#ifdef DEBUG_POS_CTRL
mutable bool PIDModeActive = false;
+
struct HelpStruct
{
float currentPosition;
@@ -315,7 +340,6 @@ namespace armarx
private:
mutable bool currentlyPIDActive = false;
-
};
struct PositionThroughVelocityControllerWithAccelerationRamps
@@ -329,8 +353,8 @@ namespace armarx
DecAccIncJerk,
DecAccDecJerk,
PCtrl
-
};
+
struct Output
{
double velocity;
@@ -347,14 +371,17 @@ namespace armarx
double deceleration;
double jerk;
double currentPosition;
+
protected:
double targetPosition;
+
public:
double getTargetPosition() const;
void setTargetPosition(double value);
double pControlPosErrorLimit = 0.01;
- double pControlVelLimit = 0.002; // if target is below this threshold, PID controller will be used
+ double pControlVelLimit =
+ 0.002; // if target is below this threshold, PID controller will be used
double accuracy = 0.001;
double p;
bool usePIDAtEnd = true;
@@ -369,6 +396,7 @@ namespace armarx
std::pair<State, Output> calcState() const;
#ifdef DEBUG_POS_CTRL
mutable bool PIDModeActive = false;
+
struct HelpStruct
{
double currentPosition;
@@ -382,16 +410,20 @@ namespace armarx
mutable boost::circular_buffer<HelpStruct> buffer;
mutable int eventHappeningCounter = -1;
#endif
-
};
- float positionThroughVelocityControlWithAccelerationBounds(float dt, float maxDt,
- float currentV, float maxV,
- float acceleration, float deceleration,
- float currentPosition, float targetPosition,
- float p);
-
- struct PositionThroughVelocityControllerWithAccelerationAndPositionBounds: PositionThroughVelocityControllerWithAccelerationBounds
+ float positionThroughVelocityControlWithAccelerationBounds(float dt,
+ float maxDt,
+ float currentV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float currentPosition,
+ float targetPosition,
+ float p);
+
+ struct PositionThroughVelocityControllerWithAccelerationAndPositionBounds :
+ PositionThroughVelocityControllerWithAccelerationBounds
{
float positionLimitLo;
float positionLimitHi;
@@ -399,26 +431,42 @@ namespace armarx
bool validParameters() const;
float run() const;
};
- float positionThroughVelocityControlWithAccelerationAndPositionBounds(float dt, float maxDt,
- float currentV, float maxV,
- float acceleration, float deceleration,
- float currentPosition, float targetPosition, float p,
- float positionLimitLo, float positionLimitHi);
- struct PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition: PositionThroughVelocityControllerWithAccelerationBounds
+ float positionThroughVelocityControlWithAccelerationAndPositionBounds(float dt,
+ float maxDt,
+ float currentV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float currentPosition,
+ float targetPosition,
+ float p,
+ float positionLimitLo,
+ float positionLimitHi);
+
+ struct PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition :
+ PositionThroughVelocityControllerWithAccelerationBounds
{
// float direction;
float positionPeriodLo;
float positionPeriodHi;
float run() const;
};
- float positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(float dt, float maxDt,
- float currentV, float maxV,
- float acceleration, float deceleration,
- float currentPosition, float targetPosition,
- float pControlPosErrorLimit, float p,
- float& direction,
- float positionPeriodLo, float positionPeriodHi);
+
+ float positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(
+ float dt,
+ float maxDt,
+ float currentV,
+ float maxV,
+ float acceleration,
+ float deceleration,
+ float currentPosition,
+ float targetPosition,
+ float pControlPosErrorLimit,
+ float p,
+ float& direction,
+ float positionPeriodLo,
+ float positionPeriodHi);
class MinJerkPositionController
{
@@ -427,10 +475,12 @@ namespace armarx
{
double t, s, v, a;
};
+
struct FixedMinJerkState
{
double t0, s0, v0, a0;
};
+
struct Output
{
double position;
@@ -459,6 +509,7 @@ namespace armarx
double finishTime = 0;
double currentTime = 0;
std::unique_ptr<FixedMinJerkState> fixedMinJerkState;
+
public:
void reset();
double getTargetPosition() const;
@@ -466,5 +517,4 @@ namespace armarx
Output run();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Constants.h b/source/RobotAPI/components/units/RobotUnit/Constants.h
index b8452dcd42700f5313a96366844c019a55781c38..30680e31423c8365d64a8ab1544ba7f55e4ca3d9 100644
--- a/source/RobotAPI/components/units/RobotUnit/Constants.h
+++ b/source/RobotAPI/components/units/RobotUnit/Constants.h
@@ -31,5 +31,4 @@ namespace armarx::ControllerConstants
#pragma GCC diagnostic ignored "-Wunused-variable"
static const float ValueNotSetNaN = std::nanf(std::to_string((1 << 16) - 1).c_str());
#pragma GCC diagnostic pop
-}
-
+} // namespace armarx::ControllerConstants
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlModes.h b/source/RobotAPI/components/units/RobotUnit/ControlModes.h
index b8b5d4e7cdec1eb2fcfbea1902bf25ccf4d2ea6a..9309cbc1155b8ad7f3080d73a3a67952f0713019 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlModes.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlModes.h
@@ -28,8 +28,8 @@ namespace armarx::ControlModes
{
//'normal' actor modes
static const std::string Position1DoF = "ControlMode_Position1DoF";
- static const std::string Torque1DoF = "ControlMode_Torque1DoF";
- static const std::string ZeroTorque1DoF = "ControlMode_ZeroTorque1DoF";
+ static const std::string Torque1DoF = "ControlMode_Torque1DoF";
+ static const std::string ZeroTorque1DoF = "ControlMode_ZeroTorque1DoF";
static const std::string Velocity1DoF = "ControlMode_Velocity1DoF";
static const std::string Current1DoF = "ControlMode_Current1DoF";
static const std::string PWM1DoF = "ControlMode_PWM1DoF";
@@ -43,12 +43,13 @@ namespace armarx::ControlModes
//modes for holonomic platforms
static const std::string HolonomicPlatformVelocity = "ControlMode_HolonomicPlatformVelocity";
- static const std::string HolonomicPlatformRelativePosition = "ControlMode_HolonomicPlatformRelativePosition";
+ static const std::string HolonomicPlatformRelativePosition =
+ "ControlMode_HolonomicPlatformRelativePosition";
//special sentinel modes
static const std::string EmergencyStop = "ControlMode_EmergencyStop";
static const std::string StopMovement = "ControlMode_StopMovement";
-}
+} // namespace armarx::ControlModes
namespace armarx::HardwareControlModes
{
@@ -59,4 +60,4 @@ namespace armarx::HardwareControlModes
//modes for holonomic platforms
static const std::string HolonomicPlatformVelocity = "HolonomicPlatformVelocity";
-}
+} // namespace armarx::HardwareControlModes
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
index 68512914578343ac958a7ed4377b427a04357603..460aed21bc55edda36a0c3f132ff74eb1621beff 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
@@ -22,102 +22,147 @@
#pragma once
-#include "ControlTargetBase.h"
-
#include <RobotAPI/components/units/RobotUnit/Constants.h> // for ControllerConstants::ValueNotSetNaN
+#include "ControlTargetBase.h"
+
namespace armarx
{
-#define make_ControlTarget1DoFActuator(type, invalid, name, varname, cmode) \
- class name: public ControlTargetBase \
- { \
- public: \
- type varname = ControllerConstants::ValueNotSetNaN; \
- name() = default; \
- name(const name&) = default; \
- name(name&&) = default; \
- name(type val) : varname{val} {} \
- name& operator=(const name&) = default; \
- name& operator=(name&&) = default; \
- name& operator=(type val) {varname = val; return *this;} \
- virtual const std::string& getControlMode() const override \
- { \
- return cmode; \
- } \
- virtual void reset() override \
- { \
- varname = invalid; \
- } \
- virtual bool isValid() const override \
- { \
- return std::isfinite(varname); \
- } \
- DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
- static ControlTargetInfo<name> GetClassMemberInfo() \
- { \
- ControlTargetInfo<name> cti; \
- cti.addMemberVariable(&name::varname, #varname); \
- return cti; \
- } \
+#define make_ControlTarget1DoFActuator(type, invalid, name, varname, cmode) \
+ class name : public ControlTargetBase \
+ { \
+ public: \
+ type varname = ControllerConstants::ValueNotSetNaN; \
+ name() = default; \
+ name(const name&) = default; \
+ name(name&&) = default; \
+ name(type val) : varname{val} \
+ { \
+ } \
+ name& operator=(const name&) = default; \
+ name& operator=(name&&) = default; \
+ name& \
+ operator=(type val) \
+ { \
+ varname = val; \
+ return *this; \
+ } \
+ virtual const std::string& \
+ getControlMode() const override \
+ { \
+ return cmode; \
+ } \
+ virtual void \
+ reset() override \
+ { \
+ varname = invalid; \
+ } \
+ virtual bool \
+ isValid() const override \
+ { \
+ return std::isfinite(varname); \
+ } \
+ DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION static ControlTargetInfo<name> \
+ GetClassMemberInfo() \
+ { \
+ ControlTargetInfo<name> cti; \
+ cti.addMemberVariable(&name::varname, #varname); \
+ return cti; \
+ } \
}
- make_ControlTarget1DoFActuator(float, ControllerConstants::ValueNotSetNaN, ControlTarget1DoFActuatorPosition, position, ControlModes::Position1DoF);
- make_ControlTarget1DoFActuator(float, ControllerConstants::ValueNotSetNaN, ControlTarget1DoFActuatorVelocity, velocity, ControlModes::Velocity1DoF);
- make_ControlTarget1DoFActuator(float, ControllerConstants::ValueNotSetNaN, ControlTarget1DoFActuatorTorque, torque, ControlModes::Torque1DoF);
- make_ControlTarget1DoFActuator(float, ControllerConstants::ValueNotSetNaN, ControlTarget1DoFActuatorZeroTorque, torque, ControlModes::ZeroTorque1DoF);
- make_ControlTarget1DoFActuator(float, ControllerConstants::ValueNotSetNaN, ControlTarget1DoFActuatorCurrent, current, ControlModes::Current1DoF);
+ make_ControlTarget1DoFActuator(float,
+ ControllerConstants::ValueNotSetNaN,
+ ControlTarget1DoFActuatorPosition,
+ position,
+ ControlModes::Position1DoF);
+ make_ControlTarget1DoFActuator(float,
+ ControllerConstants::ValueNotSetNaN,
+ ControlTarget1DoFActuatorVelocity,
+ velocity,
+ ControlModes::Velocity1DoF);
+ make_ControlTarget1DoFActuator(float,
+ ControllerConstants::ValueNotSetNaN,
+ ControlTarget1DoFActuatorTorque,
+ torque,
+ ControlModes::Torque1DoF);
+ make_ControlTarget1DoFActuator(float,
+ ControllerConstants::ValueNotSetNaN,
+ ControlTarget1DoFActuatorZeroTorque,
+ torque,
+ ControlModes::ZeroTorque1DoF);
+ make_ControlTarget1DoFActuator(float,
+ ControllerConstants::ValueNotSetNaN,
+ ControlTarget1DoFActuatorCurrent,
+ current,
+ ControlModes::Current1DoF);
#undef make_ControlTarget1DoFActuator
- class ControlTarget1DoFActuatorTorqueVelocity: public ControlTarget1DoFActuatorVelocity
+ class ControlTarget1DoFActuatorTorqueVelocity : public ControlTarget1DoFActuatorVelocity
{
public:
float maxTorque;
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ControlModes::VelocityTorque;
}
- void reset() override
+
+ void
+ reset() override
{
ControlTarget1DoFActuatorVelocity::reset();
maxTorque = -1.0f; // if < 0, default value for joint is to be used
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
return ControlTarget1DoFActuatorVelocity::isValid() && std::isfinite(maxTorque);
}
- static ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity> GetClassMemberInfo()
+
+ static ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTarget1DoFActuatorTorqueVelocity> cti;
cti.addBaseClass<ControlTarget1DoFActuatorVelocity>();
cti.addMemberVariable(&ControlTarget1DoFActuatorTorqueVelocity::maxTorque, "maxTorque");
return cti;
}
+
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
- class ActiveImpedanceControlTarget: public ControlTargetBase
+ class ActiveImpedanceControlTarget : public ControlTargetBase
{
public:
float position;
float kp;
float kd;
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ControlModes::ActiveImpedance;
}
- void reset() override
+
+ void
+ reset() override
{
position = 0;
kp = 0;
kd = 0;
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
return std::isfinite(position) && kp >= 0;
}
- static ControlTargetInfo<ActiveImpedanceControlTarget> GetClassMemberInfo()
+
+ static ControlTargetInfo<ActiveImpedanceControlTarget>
+ GetClassMemberInfo()
{
ControlTargetInfo<ActiveImpedanceControlTarget> cti;
cti.addMemberVariable(&ActiveImpedanceControlTarget::position, "position");
@@ -125,6 +170,7 @@ namespace armarx
cti.addMemberVariable(&ActiveImpedanceControlTarget::kd, "kd");
return cti;
}
+
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
@@ -132,101 +178,134 @@ namespace armarx
{
public:
std::int16_t pwm;
+
public:
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ControlModes::PWM1DoF;
}
- void reset() override
+
+ void
+ reset() override
{
pwm = std::numeric_limits<std::int16_t>::max();
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
- return std::abs(pwm) < std::numeric_limits<std::int16_t>::max() ;
+ return std::abs(pwm) < std::numeric_limits<std::int16_t>::max();
}
- static ControlTargetInfo<ControlTarget1DoFActuatorPWM> GetClassMemberInfo()
+
+ static ControlTargetInfo<ControlTarget1DoFActuatorPWM>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTarget1DoFActuatorPWM> cti;
cti.addMemberVariable(&ControlTarget1DoFActuatorPWM::pwm, "pwm_motor");
return cti;
}
+
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
- class ControlTarget1DoFActuatorPositionWithPWMLimit: public ControlTarget1DoFActuatorPosition
+ class ControlTarget1DoFActuatorPositionWithPWMLimit : public ControlTarget1DoFActuatorPosition
{
public:
int16_t maxPWM;
+
protected:
std::int16_t pwmDefaultLimit = 256;
std::int16_t pwmHardLimit = 256;
+
public:
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ControlModes::PositionWithPwmLimit1DoF;
}
- void reset() override
+
+ void
+ reset() override
{
maxPWM = pwmDefaultLimit;
ControlTarget1DoFActuatorPosition::reset();
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
- return std::abs(maxPWM) <= pwmHardLimit &&
- ControlTarget1DoFActuatorPosition::isValid();
+ return std::abs(maxPWM) <= pwmHardLimit && ControlTarget1DoFActuatorPosition::isValid();
}
- static ControlTargetInfo<ControlTarget1DoFActuatorPositionWithPWMLimit> GetClassMemberInfo()
+
+ static ControlTargetInfo<ControlTarget1DoFActuatorPositionWithPWMLimit>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTarget1DoFActuatorPositionWithPWMLimit> cti;
cti.addBaseClass<ControlTarget1DoFActuatorPosition>();
cti.addMemberVariable(&ControlTarget1DoFActuatorPositionWithPWMLimit::maxPWM, "maxPWM");
- cti.addMemberVariable(&ControlTarget1DoFActuatorPositionWithPWMLimit::pwmHardLimit, "pwmHardLimit");
+ cti.addMemberVariable(&ControlTarget1DoFActuatorPositionWithPWMLimit::pwmHardLimit,
+ "pwmHardLimit");
return cti;
}
- void setPWMLimits(std::int16_t hard, std::int16_t def, ControlDeviceAccessToken)
+
+ void
+ setPWMLimits(std::int16_t hard, std::int16_t def, ControlDeviceAccessToken)
{
pwmHardLimit = hard;
pwmDefaultLimit = def;
}
+
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
- class ControlTarget1DoFActuatorVelocityWithPWMLimit: public ControlTarget1DoFActuatorVelocity
+
+ class ControlTarget1DoFActuatorVelocityWithPWMLimit : public ControlTarget1DoFActuatorVelocity
{
public:
int16_t maxPWM;
+
protected:
std::int16_t pwmDefaultLimit = 256;
std::int16_t pwmHardLimit = 256;
+
public:
- const std::string& getControlMode() const override
+ const std::string&
+ getControlMode() const override
{
return ControlModes::VelocityWithPwmLimit1DoF;
}
- void reset() override
+
+ void
+ reset() override
{
maxPWM = pwmDefaultLimit;
ControlTarget1DoFActuatorVelocity::reset();
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
- return std::abs(maxPWM) <= pwmHardLimit &&
- ControlTarget1DoFActuatorVelocity::isValid();
+ return std::abs(maxPWM) <= pwmHardLimit && ControlTarget1DoFActuatorVelocity::isValid();
}
- static ControlTargetInfo<ControlTarget1DoFActuatorVelocityWithPWMLimit> GetClassMemberInfo()
+
+ static ControlTargetInfo<ControlTarget1DoFActuatorVelocityWithPWMLimit>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTarget1DoFActuatorVelocityWithPWMLimit> cti;
cti.addBaseClass<ControlTarget1DoFActuatorVelocity>();
cti.addMemberVariable(&ControlTarget1DoFActuatorVelocityWithPWMLimit::maxPWM, "maxPWM");
- cti.addMemberVariable(&ControlTarget1DoFActuatorVelocityWithPWMLimit::pwmHardLimit, "pwmHardLimit");
+ cti.addMemberVariable(&ControlTarget1DoFActuatorVelocityWithPWMLimit::pwmHardLimit,
+ "pwmHardLimit");
return cti;
}
- void setPWMLimits(std::int16_t hard, std::int16_t def, ControlDeviceAccessToken)
+
+ void
+ setPWMLimits(std::int16_t hard, std::int16_t def, ControlDeviceAccessToken)
{
pwmHardLimit = hard;
pwmDefaultLimit = def;
}
+
DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
index cf19d2b9f4cf9583bbd5e75e82b71f7cca6e8d0d..ca3b048750edfaa65e0737f9d7d2ec4bda740821 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
@@ -22,17 +22,16 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
-#include <RobotAPI/components/units/RobotUnit//util/HeterogenousContinuousContainerMacros.h>
-#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
-
-#include <IceUtil/Time.h>
-#include <Ice/Handle.h>
-
+#include <map>
#include <memory>
#include <string>
-#include <map>
+#include <Ice/Handle.h>
+#include <IceUtil/Time.h>
+
+#include <RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h>
+#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
+#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
namespace armarx
{
@@ -55,7 +54,7 @@ namespace armarx
ControlDeviceAccessToken() = default;
};
- template<class DerivedClass>
+ template <class DerivedClass>
using ControlTargetInfo = introspection::ClassMemberInfo<ControlTargetBase, DerivedClass>;
virtual ~ControlTargetBase() = default;
@@ -65,138 +64,152 @@ namespace armarx
virtual void reset() = 0;
virtual bool isValid() const = 0;
- template<class T>
- bool isA() const
+ template <class T>
+ bool
+ isA() const
{
return asA<T>();
}
- template<class T>
- const T* asA() const
+ template <class T>
+ const T*
+ asA() const
{
return dynamic_cast<const T*>(this);
}
- template<class T>
- T* asA()
+ template <class T>
+ T*
+ asA()
{
return dynamic_cast<T*>(this);
}
//logging functions
/// @brief used to send the data to the DebugObserverTopic and to other Components (e.g. GUI widgets)
- virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const = 0;
+ virtual std::map<std::string, VariantBasePtr>
+ toVariants(const IceUtil::Time& timestamp) const = 0;
virtual std::size_t getNumberOfDataFields() const = 0;
virtual std::vector<std::string> getDataFieldNames() const = 0;
- virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
virtual void getDataFieldAs(std::size_t i, Ice::Double& out) const = 0;
virtual void getDataFieldAs(std::size_t i, std::string& out) const = 0;
virtual const std::type_info& getDataFieldType(std::size_t i) const = 0;
//management functions
- template<class T, class = typename std::enable_if<std::is_base_of<ControlTargetBase, T>::value>::type>
- void _copyTo(std::unique_ptr<T>& target) const
+ template <
+ class T,
+ class = typename std::enable_if<std::is_base_of<ControlTargetBase, T>::value>::type>
+ void
+ _copyTo(std::unique_ptr<T>& target) const
{
_copyTo(target.get());
}
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
- ControlTargetHasGetClassMemberInfo,
- GetClassMemberInfo, ControlTargetInfo<T>(*)(void));
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(ControlTargetHasGetClassMemberInfo,
+ GetClassMemberInfo,
+ ControlTargetInfo<T> (*)(void));
ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(ControlTargetBase)
};
-#define DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
- ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
- using ControlTargetBase = ::armarx::ControlTargetBase; \
- using VariantBasePtr = ::armarx::VariantBasePtr; \
- std::string getControlTargetType(bool withoutNamespaceSpecifier = false) const override \
- { \
- return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
- } \
- void _check_for_static_GetClassMemberInfo_overload() \
- { \
- static_assert(ControlTargetHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
- "This class has to implement GetClassMemberInfo() returning " \
- "an instance of ControlTargetInfo<THIS_CLASS_TYPE>"); \
- } \
- std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp,this); \
- } \
- std::size_t getNumberOfDataFields() const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
- } \
- void getDataFieldAs (std::size_t i, bool& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Byte& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Short& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Int& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Long& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Float& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Double& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, std::string& out) const override \
- { \
- ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- const std::type_info& getDataFieldType(std::size_t i) const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
- } \
- std::vector<std::string> getDataFieldNames() const override \
- { \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
+#define DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
+ ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
+ using ControlTargetBase = ::armarx::ControlTargetBase; \
+ using VariantBasePtr = ::armarx::VariantBasePtr; \
+ std::string getControlTargetType(bool withoutNamespaceSpecifier = false) const override \
+ { \
+ return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
+ } \
+ void _check_for_static_GetClassMemberInfo_overload() \
+ { \
+ static_assert( \
+ ControlTargetHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
+ "This class has to implement GetClassMemberInfo() returning " \
+ "an instance of ControlTargetInfo<THIS_CLASS_TYPE>"); \
+ } \
+ std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) \
+ const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp, this); \
+ } \
+ std::size_t getNumberOfDataFields() const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
+ } \
+ void getDataFieldAs(std::size_t i, bool& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Byte& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Short& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Int& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Long& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Float& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Double& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, std::string& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ const std::type_info& getDataFieldType(std::size_t i) const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
+ } \
+ std::vector<std::string> getDataFieldNames() const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
}
-#define make_DummyControlTarget(Suffix,ControlMode) \
- class DummyControlTarget##Suffix : public ControlTargetBase \
- { \
- public: \
- virtual const std::string& getControlMode() const override \
- { \
- return ControlMode; \
- } \
- virtual void reset() override {} \
- virtual bool isValid() const override \
- { \
- return true; \
- } \
- DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
- static ControlTargetInfo<DummyControlTarget##Suffix> \
- GetClassMemberInfo() \
- { \
- return ControlTargetInfo<DummyControlTarget##Suffix> {}; \
- } \
+#define make_DummyControlTarget(Suffix, ControlMode) \
+ class DummyControlTarget##Suffix : public ControlTargetBase \
+ { \
+ public: \
+ virtual const std::string& \
+ getControlMode() const override \
+ { \
+ return ControlMode; \
+ } \
+ virtual void \
+ reset() override \
+ { \
+ } \
+ virtual bool \
+ isValid() const override \
+ { \
+ return true; \
+ } \
+ DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION static ControlTargetInfo< \
+ DummyControlTarget##Suffix> \
+ GetClassMemberInfo() \
+ { \
+ return ControlTargetInfo<DummyControlTarget##Suffix>{}; \
+ } \
}
make_DummyControlTarget(EmergencyStop, ControlModes::EmergencyStop);
make_DummyControlTarget(StopMovement, ControlModes::StopMovement);
#undef make_DummyControlTarget
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
index cad312b44637034c4c254a8b300e00c42cbb09fb..dc2fc32863abf6e9384cebd9188968dfd419c9e2 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
@@ -22,10 +22,10 @@
#pragma once
-#include "ControlTargetBase.h"
-
#include <RobotAPI/components/units/RobotUnit/Constants.h> // for ControllerConstants::ValueNotSetNaN
+#include "ControlTargetBase.h"
+
namespace armarx
{
/**
@@ -35,36 +35,44 @@ namespace armarx
*
* Detailed description of class ControlTargetHolonomicPlatformVelocity.
*/
- class ControlTargetHolonomicPlatformVelocity: public ControlTargetBase
+ class ControlTargetHolonomicPlatformVelocity : public ControlTargetBase
{
public:
float velocityX = ControllerConstants::ValueNotSetNaN;
float velocityY = ControllerConstants::ValueNotSetNaN;
float velocityRotation = ControllerConstants::ValueNotSetNaN;
- const std::string& getControlMode() const override
+
+ const std::string&
+ getControlMode() const override
{
return ControlModes::HolonomicPlatformVelocity;
}
- void reset() override
+
+ void
+ reset() override
{
velocityX = ControllerConstants::ValueNotSetNaN;
velocityY = ControllerConstants::ValueNotSetNaN;
velocityRotation = ControllerConstants::ValueNotSetNaN;
}
- bool isValid() const override
+
+ bool
+ isValid() const override
{
- return std::isfinite(velocityX) && std::isfinite(velocityY) && std::isfinite(velocityRotation);
+ return std::isfinite(velocityX) && std::isfinite(velocityY) &&
+ std::isfinite(velocityRotation);
}
- DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
- static ControlTargetInfo<ControlTargetHolonomicPlatformVelocity> GetClassMemberInfo()
+ DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION static ControlTargetInfo<
+ ControlTargetHolonomicPlatformVelocity>
+ GetClassMemberInfo()
{
ControlTargetInfo<ControlTargetHolonomicPlatformVelocity> cti;
cti.addMemberVariable(&ControlTargetHolonomicPlatformVelocity::velocityX, "velocityX");
cti.addMemberVariable(&ControlTargetHolonomicPlatformVelocity::velocityY, "velocityY");
- cti.addMemberVariable(&ControlTargetHolonomicPlatformVelocity::velocityRotation, "velocityRotation");
+ cti.addMemberVariable(&ControlTargetHolonomicPlatformVelocity::velocityRotation,
+ "velocityRotation");
return cti;
}
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
index a2cce575f38d3bb3f015fd3decdbcb30d3d816bb..5ea5de5a0decff5792d66ea890a1e39729c5c4a0 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
@@ -1,30 +1,30 @@
#include "CartesianImpedanceController.h"
+#include <algorithm>
+
+#include <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
-#include <SimoxUtility/math/compare/is_equal.h>
-
#include <VirtualRobot/MathTools.h>
-#include <algorithm>
-
using namespace armarx;
-
-int CartesianImpedanceController::bufferSize() const
+int
+CartesianImpedanceController::bufferSize() const
{
return targetJointTorques.size();
}
-void CartesianImpedanceController::initialize(const VirtualRobot::RobotNodeSetPtr& rns)
+
+void
+CartesianImpedanceController::initialize(const VirtualRobot::RobotNodeSetPtr& rns)
{
ARMARX_CHECK_NOT_NULL(rns);
tcp = rns->getTCP();
ARMARX_CHECK_NOT_NULL(tcp);
ik.reset(new VirtualRobot::DifferentialIK(
- rns, rns->getRobot()->getRootNode(),
- VirtualRobot::JacobiProvider::eSVDDamped));
+ rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
const auto size = rns->getSize();
qpos.resize(size);
@@ -36,14 +36,16 @@ void CartesianImpedanceController::initialize(const VirtualRobot::RobotNodeSetPt
I = Eigen::MatrixXf::Identity(size, size);
}
-const Eigen::VectorXf& CartesianImpedanceController::run(
+
+const Eigen::VectorXf&
+CartesianImpedanceController::run(
const Config& cfg,
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors,
std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors,
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors
-)
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors)
{
- ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.Knull.size())); ///TODO move this to core
+ ARMARX_CHECK_EXPRESSION(
+ simox::math::is_equal(bufferSize(), cfg.Knull.size())); ///TODO move this to core
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.Dnull.size()));
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.desiredJointPosition.size()));
ARMARX_CHECK_GREATER_EQUAL(cfg.torqueLimit, 0);
@@ -52,10 +54,12 @@ const Eigen::VectorXf& CartesianImpedanceController::run(
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), velocitySensors.size()));
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), positionSensors.size()));
- const Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All); ///TODO output param version
+ const Eigen::MatrixXf jacobi = ik->getJacobianMatrix(
+ tcp, VirtualRobot::IKSolver::CartesianSelection::All); ///TODO output param version
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), jacobi.cols()));
ARMARX_CHECK_EXPRESSION(simox::math::is_equal(6, jacobi.rows()));
- const Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);///TODO output param version
+ const Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(
+ jacobi.transpose(), lambda); ///TODO output param version
for (size_t i = 0; i < velocitySensors.size(); ++i)
{
@@ -65,22 +69,15 @@ const Eigen::VectorXf& CartesianImpedanceController::run(
const Eigen::Vector6f tcptwist = jacobi * qvel;
- const Eigen::Vector3f currentTCPLinearVelocity
- {
- 0.001f * tcptwist(0),
- 0.001f * tcptwist(1),
- 0.001f * tcptwist(2)
- };
- const Eigen::Vector3f currentTCPAngularVelocity
- {
- tcptwist(3),
- tcptwist(4),
- tcptwist(5)
- };
+ const Eigen::Vector3f currentTCPLinearVelocity{
+ 0.001f * tcptwist(0), 0.001f * tcptwist(1), 0.001f * tcptwist(2)};
+ const Eigen::Vector3f currentTCPAngularVelocity{tcptwist(3), tcptwist(4), tcptwist(5)};
const Eigen::Vector3f currentTCPPosition = tcp->getPositionInRootFrame();
- const Eigen::Vector3f tcpForces = 0.001 * cfg.Kpos.cwiseProduct(cfg.desiredPosition - currentTCPPosition);
- const Eigen::Vector3f tcpDesiredForce = tcpForces - cfg.Dpos.cwiseProduct(currentTCPLinearVelocity);
+ const Eigen::Vector3f tcpForces =
+ 0.001 * cfg.Kpos.cwiseProduct(cfg.desiredPosition - currentTCPPosition);
+ const Eigen::Vector3f tcpDesiredForce =
+ tcpForces - cfg.Dpos.cwiseProduct(currentTCPLinearVelocity);
const Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
const Eigen::Matrix3f currentRotMat = currentTCPPose.block<3, 3>(0, 0);
@@ -92,10 +89,9 @@ const Eigen::VectorXf& CartesianImpedanceController::run(
const Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
const Eigen::Vector3f tcpDesiredTorque =
- cfg.Kori.cwiseProduct(rpy) -
- cfg.Dori.cwiseProduct(currentTCPAngularVelocity);
+ cfg.Kori.cwiseProduct(rpy) - cfg.Dori.cwiseProduct(currentTCPAngularVelocity);
Eigen::Vector6f tcpDesiredWrench;
- tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
+ tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
// calculate desired joint torque
nullqerror = cfg.desiredJointPosition - qpos;
@@ -110,11 +106,11 @@ const Eigen::VectorXf& CartesianImpedanceController::run(
nullspaceTorque = cfg.Knull.cwiseProduct(nullqerror) - cfg.Dnull.cwiseProduct(qvel);
targetJointTorques =
- jacobi.transpose() * tcpDesiredWrench +
- (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
+ jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
for (int i = 0; i < targetJointTorques.size(); ++i)
{
- targetJointTorques(i) = std::clamp(targetJointTorques(i), -cfg.torqueLimit, cfg.torqueLimit);
+ targetJointTorques(i) =
+ std::clamp(targetJointTorques(i), -cfg.torqueLimit, cfg.torqueLimit);
}
//write debug data
{
diff --git a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
index 5e108d287d49f1f2115ade20428fdbe8ce016b86..0c0ed889cebc1671c436eee9226dc79457850a25 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
@@ -1,7 +1,7 @@
#pragma once
-#include <VirtualRobot/Robot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Robot.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
@@ -24,6 +24,7 @@ namespace armarx
Eigen::VectorXf desiredJointPosition;
float torqueLimit;
};
+
//state for debugging
struct Debug
{
@@ -32,6 +33,7 @@ namespace armarx
Eigen::Vector3f tcpDesiredForce;
Eigen::Vector3f tcpDesiredTorque;
};
+
private:
Eigen::VectorXf qpos;
Eigen::VectorXf qvel;
@@ -46,16 +48,16 @@ namespace armarx
VirtualRobot::RobotNodePtr tcp;
const float lambda = 2;
+
public:
Debug dbg;
int bufferSize() const;
void initialize(const VirtualRobot::RobotNodeSetPtr& rns);
- const Eigen::VectorXf& run(
- const Config& cfg,
+ const Eigen::VectorXf&
+ run(const Config& cfg,
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors,
std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors,
- std::vector<const SensorValue1DoFActuatorPosition*> positionSensors
- );
+ std::vector<const SensorValue1DoFActuatorPosition*> positionSensors);
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
index a83b78a522cf70993e5f98cfcffca04981942567..0ed24cb2fb0459f76a6a373d9c1cd3768c733181 100644
--- a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.cpp
@@ -24,9 +24,8 @@
namespace armarx::WidgetDescription
{
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options)
+ StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name, std::vector<std::string> options)
{
StringComboBoxPtr rns = new StringComboBox;
rns->name = std::move(name);
@@ -35,10 +34,10 @@ namespace armarx::WidgetDescription
return rns;
}
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet)
+ StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet)
{
StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options));
for (std::size_t i = 0; i < rns->options.size(); ++i)
@@ -52,10 +51,10 @@ namespace armarx::WidgetDescription
return rns;
}
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::string& mostPreferred)
+ StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred)
{
StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options));
for (std::size_t i = 0; i < rns->options.size(); ++i)
@@ -69,13 +68,14 @@ namespace armarx::WidgetDescription
return rns;
}
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet,
- const std::string& mostPreferred)
+ StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet,
+ const std::string& mostPreferred)
{
- StringComboBoxPtr rns = makeStringSelectionComboBox(std::move(name), std::move(options), preferredSet);
+ StringComboBoxPtr rns =
+ makeStringSelectionComboBox(std::move(name), std::move(options), preferredSet);
for (std::size_t i = 0; i < rns->options.size(); ++i)
{
if (mostPreferred == rns->options.at(i))
@@ -86,4 +86,4 @@ namespace armarx::WidgetDescription
}
return rns;
}
-}
+} // namespace armarx::WidgetDescription
diff --git a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
index ec9f4c590b56e5b1f984a76c1a7ff2fc92764a8e..e276010f22b2d70b6b6b37fac642991020df968c 100644
--- a/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
+++ b/source/RobotAPI/components/units/RobotUnit/DefaultWidgetDescriptions.h
@@ -22,8 +22,8 @@
#pragma once
-#include <string>
#include <set>
+#include <string>
#include <VirtualRobot/Robot.h>
@@ -32,58 +32,58 @@
namespace armarx::WidgetDescription
{
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options);
+ StringComboBoxPtr makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options);
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet);
+ StringComboBoxPtr makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet);
- inline StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::initializer_list<std::string>& preferredSet)
+ inline StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::initializer_list<std::string>& preferredSet)
{
- return makeStringSelectionComboBox(std::move(name), std::move(options), std::set<std::string> {preferredSet});
+ return makeStringSelectionComboBox(
+ std::move(name), std::move(options), std::set<std::string>{preferredSet});
}
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::string& mostPreferred);
+ StringComboBoxPtr makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred);
- StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::set<std::string>& preferredSet,
- const std::string& mostPreferred);
+ StringComboBoxPtr makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::set<std::string>& preferredSet,
+ const std::string& mostPreferred);
- inline StringComboBoxPtr makeStringSelectionComboBox(
- std::string name,
- std::vector<std::string> options,
- const std::string& mostPreferred,
- const std::set<std::string>& preferredSet)
+ inline StringComboBoxPtr
+ makeStringSelectionComboBox(std::string name,
+ std::vector<std::string> options,
+ const std::string& mostPreferred,
+ const std::set<std::string>& preferredSet)
{
- return makeStringSelectionComboBox(std::move(name), std::move(options), preferredSet, mostPreferred);
+ return makeStringSelectionComboBox(
+ std::move(name), std::move(options), preferredSet, mostPreferred);
}
- inline StringComboBoxPtr makeRNSComboBox(
- const VirtualRobot::RobotPtr& robot,
- std::string name = "RobotNodeSet",
- const std::set<std::string>& preferredSet = {},
- const std::string& mostPreferred = "")
+ inline StringComboBoxPtr
+ makeRNSComboBox(const VirtualRobot::RobotPtr& robot,
+ std::string name = "RobotNodeSet",
+ const std::set<std::string>& preferredSet = {},
+ const std::string& mostPreferred = "")
{
- return makeStringSelectionComboBox(std::move(name), robot->getRobotNodeSetNames(), preferredSet, mostPreferred);
+ return makeStringSelectionComboBox(
+ std::move(name), robot->getRobotNodeSetNames(), preferredSet, mostPreferred);
}
- inline StringComboBoxPtr makeRobotNodeComboBox(
- const VirtualRobot::RobotPtr& robot,
- std::string name = "RobotNode",
- const std::set<std::string>& preferredSet = {},
- const std::string& mostPreferred = "")
+ inline StringComboBoxPtr
+ makeRobotNodeComboBox(const VirtualRobot::RobotPtr& robot,
+ std::string name = "RobotNode",
+ const std::set<std::string>& preferredSet = {},
+ const std::string& mostPreferred = "")
{
- return makeStringSelectionComboBox(std::move(name), robot->getRobotNodeNames(), preferredSet, mostPreferred);
+ return makeStringSelectionComboBox(
+ std::move(name), robot->getRobotNodeNames(), preferredSet, mostPreferred);
}
-}
+} // namespace armarx::WidgetDescription
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp
index dbb46ec53cf9dbb6bd086d4639cc077d2eee168d..cdc7bba5ed94a59d8468708a3765a0967a257927 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp
@@ -26,49 +26,50 @@
namespace armarx
{
- const ControlDevicePtr ControlDevice::NullPtr
- {
- nullptr
- };
+ const ControlDevicePtr ControlDevice::NullPtr{nullptr};
- JointController* ControlDevice::getJointEmergencyStopController()
+ JointController*
+ ControlDevice::getJointEmergencyStopController()
{
- ARMARX_CHECK_EXPRESSION(
- jointEmergencyStopController) <<
- "ControlDevice::getJointEmergencyStopController called for a ControlDevice ('"
- << getDeviceName() << "') without a JointController "
- "with the ControlMode ControlModes::EmergencyStop"
- " (add a JointController with this ControlMode)";
+ ARMARX_CHECK_EXPRESSION(jointEmergencyStopController)
+ << "ControlDevice::getJointEmergencyStopController called for a ControlDevice ('"
+ << getDeviceName()
+ << "') without a JointController "
+ "with the ControlMode ControlModes::EmergencyStop"
+ " (add a JointController with this ControlMode)";
return jointEmergencyStopController;
}
- JointController* ControlDevice::getJointStopMovementController()
+ JointController*
+ ControlDevice::getJointStopMovementController()
{
- ARMARX_CHECK_EXPRESSION(
- jointStopMovementController) <<
- "ControlDevice::getJointStopMovementController called for a ControlDevice ('"
- << getDeviceName() << "') without a JointController "
- "with the ControlMode ControlModes::StopMovement"
- " (add a JointController with this ControlMode)";
+ ARMARX_CHECK_EXPRESSION(jointStopMovementController)
+ << "ControlDevice::getJointStopMovementController called for a ControlDevice ('"
+ << getDeviceName()
+ << "') without a JointController "
+ "with the ControlMode ControlModes::StopMovement"
+ " (add a JointController with this ControlMode)";
return jointStopMovementController;
}
- void ControlDevice::rtSetActiveJointController(JointController* jointCtrl)
+ void
+ ControlDevice::rtSetActiveJointController(JointController* jointCtrl)
{
- ARMARX_CHECK_EXPRESSION(jointCtrl) << "Called ControlDevice::rtSetActiveJointController with a nullptr (Don't do this)";
+ ARMARX_CHECK_EXPRESSION(jointCtrl)
+ << "Called ControlDevice::rtSetActiveJointController with a nullptr (Don't do this)";
if (activeJointController == jointCtrl)
{
return;
}
if (jointCtrl->parent != this)
{
- throw std::logic_error
- {
- "ControlDevice(" + getDeviceName() + ")::rtSetActiveJointController: "
+ throw std::logic_error{
+ "ControlDevice(" + getDeviceName() +
+ ")::rtSetActiveJointController: "
"tried to switch to a joint controller from a different ControlDevice "
"(You should only call ControlDevice::rtSetActiveJointController with "
- "JointControllers added to the ControlDevice via ControlDevice::addJointController)"
- };
+ "JointControllers added to the ControlDevice via "
+ "ControlDevice::addJointController)"};
}
if (activeJointController)
{
@@ -78,7 +79,9 @@ namespace armarx
activeJointController = jointCtrl;
}
- void ControlDevice::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlDevice::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
auto activejointCtrl = rtGetActiveJointController();
ARMARX_CHECK_EXPRESSION(activejointCtrl);
@@ -86,38 +89,36 @@ namespace armarx
rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
}
- void ControlDevice::addJointController(JointController* jointCtrl)
+ void
+ ControlDevice::addJointController(JointController* jointCtrl)
{
- ARMARX_CHECK_IS_NULL(
- owner) <<
- "The ControlDevice '" << getDeviceName()
- << "' was already added to a RobotUnit! Call addJointController before calling addControlDevice.";
+ ARMARX_CHECK_IS_NULL(owner) << "The ControlDevice '" << getDeviceName()
+ << "' was already added to a RobotUnit! Call "
+ "addJointController before calling addControlDevice.";
ARMARX_DEBUG << "adding Controller " << jointCtrl;
if (!jointCtrl)
{
- throw std::invalid_argument
- {
- "ControlDevice(" + getDeviceName() + ")::addJointController: joint controller is nullptr (Don't try nullptrs as JointController)"
- };
+ throw std::invalid_argument{"ControlDevice(" + getDeviceName() +
+ ")::addJointController: joint controller is nullptr (Don't "
+ "try nullptrs as JointController)"};
}
if (jointCtrl->parent)
{
- throw std::logic_error
- {
- "ControlDevice(" + getDeviceName() + ")::addJointController: tried to add a joint controller multiple times" +
- "(Don't try to add a JointController multiple to a ControlDevice)"
- };
+ throw std::logic_error{
+ "ControlDevice(" + getDeviceName() +
+ ")::addJointController: tried to add a joint controller multiple times" +
+ "(Don't try to add a JointController multiple to a ControlDevice)"};
}
ARMARX_DEBUG << "getControlMode of " << jointCtrl;
const std::string& mode = jointCtrl->getControlMode();
- ARMARX_VERBOSE << "adding joint controller for device '" << getDeviceName() << "' with mode '" << mode << "'" ;
+ ARMARX_VERBOSE << "adding joint controller for device '" << getDeviceName()
+ << "' with mode '" << mode << "'";
if (jointControllers.has(mode))
{
- throw std::invalid_argument
- {
- "ControlDevice(" + getDeviceName() + ")::addJointController: joint controller for mode " + mode + " was already added" +
- "(Don't try to add multiple JointController with the same ControlMode)"
- };
+ throw std::invalid_argument{
+ "ControlDevice(" + getDeviceName() +
+ ")::addJointController: joint controller for mode " + mode + " was already added" +
+ "(Don't try to add multiple JointController with the same ControlMode)"};
}
jointCtrl->parent = this;
ARMARX_DEBUG << "resetting target";
@@ -133,13 +134,12 @@ namespace armarx
}
if (!jointCtrl->getControlTarget())
{
- throw std::logic_error
- {
- "ControlDevice(" + getDeviceName() + ")::addJointController: The JointController has no ControlTarget. (mode = " + mode + ")" +
- "(Don't try to add JointController without a ControlTarget)"
- };
+ throw std::logic_error{
+ "ControlDevice(" + getDeviceName() +
+ ")::addJointController: The JointController has no ControlTarget. (mode = " + mode +
+ ")" + "(Don't try to add JointController without a ControlTarget)"};
}
- const std::hash<std::string> hash {};
+ const std::hash<std::string> hash{};
jointCtrl->controlModeHash = hash(jointCtrl->getControlMode());
jointCtrl->hardwareControlModeHash = hash(jointCtrl->getHardwareControlMode());
jointControllers.add(mode, std::move(jointCtrl));
@@ -147,12 +147,14 @@ namespace armarx
ARMARX_CHECK_IS_NULL(owner);
}
- RobotUnitModule::Devices* ControlDevice::getOwner() const
+ RobotUnitModule::Devices*
+ ControlDevice::getOwner() const
{
return owner;
}
- std::map<std::string, std::string> ControlDevice::getJointControllerToTargetTypeNameMap() const
+ std::map<std::string, std::string>
+ ControlDevice::getJointControllerToTargetTypeNameMap() const
{
std::map<std::string, std::string> map;
for (const auto& name : jointControllers.keys())
@@ -162,4 +164,4 @@ namespace armarx
return map;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
index 9ad894135f395848ec4c5e6aaba6288e98a6f930..692d032deeccd2624d4d97ab5b22dce4a451b172 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
@@ -22,14 +22,14 @@
#pragma once
-#include <string>
-#include <memory>
#include <atomic>
+#include <memory>
+#include <string>
-#include"DeviceBase.h"
-#include "../util.h"
#include "../ControlModes.h"
#include "../JointControllers/JointController.h"
+#include "../util.h"
+#include "DeviceBase.h"
namespace armarx::RobotUnitModule
{
@@ -40,13 +40,16 @@ namespace armarx::ControlDeviceTags
{
using namespace DeviceTags;
const static std::string PeriodicPosition = "ControlDeviceTag_PeriodicPosition";
- const static std::string CreateNoDefaultController = "ControlDeviceTag_CreateNoDefaultController";
- const static std::string ForcePositionThroughVelocity = "ControlDeviceTag_ForcePositionThroughVelocity";
-}
+ const static std::string CreateNoDefaultController =
+ "ControlDeviceTag_CreateNoDefaultController";
+ const static std::string ForcePositionThroughVelocity =
+ "ControlDeviceTag_ForcePositionThroughVelocity";
+} // namespace armarx::ControlDeviceTags
namespace armarx
{
TYPEDEF_PTRS_SHARED(ControlDevice);
+
/**
* @brief The ControlDevice class represents a logical unit accepting commands via its JointControllers.
*
@@ -57,14 +60,17 @@ namespace armarx
* It holds a set of tags.
* These tags are used by different components to create default controllers, check for abilities or present output to the user.
*/
- class ControlDevice: public virtual DeviceBase
+ class ControlDevice : public virtual DeviceBase
{
public:
/// @brief A static const nullptr in case a const ref to a nullptr needs to be returned
static const ControlDevicePtr NullPtr;
/// @brief Create a ControlDevice with the given name
- ControlDevice(const std::string& name): DeviceBase {name} {}
+ ControlDevice(const std::string& name) : DeviceBase{name}
+ {
+ }
+
//controller management
/// @return the jointEmergencyStopController of this ControlDevice
JointController* rtGetJointEmergencyStopController();
@@ -97,17 +103,24 @@ namespace armarx
* @param timeSinceLastIteration
* @see writeTargetValues
*/
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/**
* @brief This is a hook for implementations to write the setpoints to a bus.
* @param sensorValuesTimestamp The timestamp of the current \ref SensorValueBase "SensorValues"
* @param timeSinceLastIteration The time delta between the last two updates of \ref SensorValueBase "SensorValues"
*/
- virtual void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) {}
+ virtual void
+ rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ }
+
RobotUnitModule::Devices* getOwner() const;
std::map<std::string, std::string> getJointControllerToTargetTypeNameMap() const;
+
protected:
/**
* @brief adds the Joint controller to this ControlDevice
@@ -116,81 +129,93 @@ namespace armarx
*/
void addJointController(JointController* jointCtrl);
- ControlTargetBase::ControlDeviceAccessToken getControlTargetAccessToken() const
+ ControlTargetBase::ControlDeviceAccessToken
+ getControlTargetAccessToken() const
{
return {};
}
+
private:
friend class RobotUnitModule::Devices;
/// @brief The owning \ref RobotUnit. (is set when this \ref SensorDevice is added to the \ref RobotUnit)
- std::atomic<RobotUnitModule::Devices*> owner {nullptr};
+ std::atomic<RobotUnitModule::Devices*> owner{nullptr};
/// @brief The \ref JointController "JointControllers" managed by this \ref ControlDevice
KeyValueVector<std::string, JointController*> jointControllers;
/// @brief The currently executed \ref JointController
- JointController* activeJointController {nullptr};
+ JointController* activeJointController{nullptr};
/// @brief This \ref ControlDevice "ControlDevice's" emergency stop controller
- JointController* jointEmergencyStopController {nullptr};
+ JointController* jointEmergencyStopController{nullptr};
/// @brief This \ref ControlDevice "ControlDevice's" stop movement controller
- JointController* jointStopMovementController {nullptr};
+ JointController* jointStopMovementController{nullptr};
};
-}
+} // namespace armarx
//inline functions
namespace armarx
{
- inline JointController* ControlDevice::rtGetJointEmergencyStopController()
+ inline JointController*
+ ControlDevice::rtGetJointEmergencyStopController()
{
return jointEmergencyStopController;
}
- inline JointController* ControlDevice::rtGetJointStopMovementController()
+ inline JointController*
+ ControlDevice::rtGetJointStopMovementController()
{
return jointStopMovementController;
}
- inline JointController* ControlDevice::rtGetActiveJointController()
+ inline JointController*
+ ControlDevice::rtGetActiveJointController()
{
return activeJointController;
}
- inline const std::vector<JointController*>& ControlDevice::rtGetJointControllers()
+ inline const std::vector<JointController*>&
+ ControlDevice::rtGetJointControllers()
{
return jointControllers.values();
}
- inline std::vector<const JointController*> ControlDevice::getJointControllers() const
+ inline std::vector<const JointController*>
+ ControlDevice::getJointControllers() const
{
return {jointControllers.values().begin(), jointControllers.values().end()};
}
- inline JointController* ControlDevice::getJointController(const std::string& mode)
+ inline JointController*
+ ControlDevice::getJointController(const std::string& mode)
{
return jointControllers.at(mode, nullptr);
}
- inline const JointController* ControlDevice::getJointController(const std::string& mode) const
+ inline const JointController*
+ ControlDevice::getJointController(const std::string& mode) const
{
return jointControllers.at(mode, nullptr);
}
- inline JointController* ControlDevice::getJointController(std::size_t i)
+ inline JointController*
+ ControlDevice::getJointController(std::size_t i)
{
return jointControllers.at(i);
}
- inline const JointController* ControlDevice::getJointController(std::size_t i) const
+ inline const JointController*
+ ControlDevice::getJointController(std::size_t i) const
{
return jointControllers.at(i);
}
- inline bool ControlDevice::hasJointController(const std::string& mode) const
+ inline bool
+ ControlDevice::hasJointController(const std::string& mode) const
{
return jointControllers.has(mode);
}
- inline const std::vector<std::string>& ControlDevice::getControlModes() const
+ inline const std::vector<std::string>&
+ ControlDevice::getControlModes() const
{
return jointControllers.keys();
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
index 35958d52d5f984b14e5644e2372990d103eb935b..252be38871a65753da6966130a246436368a71ff 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
@@ -33,6 +33,7 @@ namespace armarx
DeviceBase(const std::string& name) : deviceName{name}
{
}
+
virtual ~DeviceBase() = default;
/// @return The Device's name
const std::string& getDeviceName() const;
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
index 57f7cf102806a02d1bf502eaf5920e6b43dfff3f..2bb041a7b577bf9a9443dcb4f405949e840a636e 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
@@ -1,8 +1,9 @@
#include "GlobalRobotPoseSensorDevice.h"
-#include "ArmarXCore/core/logging/Logging.h"
#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
+#include "ArmarXCore/core/logging/Logging.h"
+
namespace armarx
{
SensorValueBase::SensorValueInfo<SensorValueGlobalPoseCorrection>
@@ -14,7 +15,6 @@ namespace armarx
return svi;
}
-
bool
SensorValueGlobalPoseCorrection::isAvailable() const
{
@@ -30,42 +30,36 @@ namespace armarx
return svi;
}
-
bool
SensorValueGlobalRobotPose::isAvailable() const
{
return not global_T_root.isIdentity();
}
-
std::string
GlobalRobotPoseCorrectionSensorDevice::DeviceName()
{
return "GlobalRobotPoseCorrectionSensorDevice";
}
-
GlobalRobotPoseCorrectionSensorDevice::GlobalRobotPoseCorrectionSensorDevice() :
DeviceBase(DeviceName()), SensorDevice(DeviceName())
{
transformationBuffer.reinitAllBuffers(SensorValueType::Transformation::Identity());
}
-
const SensorValueBase*
GlobalRobotPoseCorrectionSensorDevice::getSensorValue() const
{
return &sensor;
}
-
std::string
GlobalRobotPoseCorrectionSensorDevice::getReportingFrame() const
{
return GlobalFrame;
}
-
void
GlobalRobotPoseCorrectionSensorDevice::rtReadSensorValues(
const IceUtil::Time& sensorValuesTimestamp,
@@ -74,7 +68,6 @@ namespace armarx
sensor.global_T_odom = transformationBuffer.getUpToDateReadBuffer();
}
-
void
GlobalRobotPoseCorrectionSensorDevice::updateGlobalPositionCorrection(
const SensorValueType::Transformation& global_T_odom)
@@ -83,63 +76,59 @@ namespace armarx
transformationBuffer.commitWrite();
}
-
std::string
GlobalRobotPoseSensorDevice::DeviceName()
{
return "GlobalRobotPoseSensorDevice";
}
-
GlobalRobotPoseSensorDevice::GlobalRobotPoseSensorDevice() :
DeviceBase(DeviceName()), SensorDevice(DeviceName())
{
}
-
const SensorValueBase*
GlobalRobotPoseSensorDevice::getSensorValue() const
{
return &sensor;
}
-
std::string
GlobalRobotPoseSensorDevice::getReportingFrame() const
{
return GlobalFrame;
}
-
void
GlobalRobotPoseSensorDevice::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
{
}
-
std::string
GlobalRobotLocalizationSensorDevice::DeviceName()
{
return "GlobalRobotLocalizationSensorDevice";
}
-
GlobalRobotLocalizationSensorDevice::GlobalRobotLocalizationSensorDevice() :
DeviceBase(DeviceName()), SensorDevice(DeviceName())
{
}
-
void
GlobalRobotLocalizationSensorDevice::rtReadSensorValues(
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/Devices/GlobalRobotPoseSensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
index c3cd19f4a3777a6903ceca8b4a1e8660f25b617b..aa6a2bce15c463861983fe97f12becd4bf4dd918 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
@@ -38,7 +38,6 @@ namespace armarx
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
};
-
class SensorValueGlobalRobotPose : virtual public SensorValueBase
{
public:
@@ -53,6 +52,7 @@ namespace armarx
};
TYPEDEF_PTRS_SHARED(GlobalRobotPoseCorrectionSensorDevice);
+
class GlobalRobotPoseCorrectionSensorDevice : virtual public SensorDevice
{
public:
@@ -77,8 +77,8 @@ namespace armarx
TripleBuffer<SensorValueType::Transformation> transformationBuffer;
};
-
TYPEDEF_PTRS_SHARED(GlobalRobotPoseSensorDevice);
+
class GlobalRobotPoseSensorDevice : virtual public SensorDevice
{
public:
@@ -99,8 +99,8 @@ namespace armarx
SensorValueType sensor;
};
-
TYPEDEF_PTRS_SHARED(GlobalRobotPoseSensorDevice);
+
class GlobalRobotLocalizationSensorDevice : virtual public GlobalRobotPoseSensorDevice
{
public:
@@ -113,7 +113,6 @@ namespace armarx
const SensorValueGlobalPoseCorrection* sensorGlobalPositionCorrection;
const SensorValueHolonomicPlatformRelativePosition* sensorRelativePosition;
-
};
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
index 89c0fab1ba6d9f042a7351233a14a86d7641edc0..5af57f010d19837ecb8af20260191c1faee3ff54 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
@@ -22,8 +22,9 @@
#pragma once
-#include"SensorDevice.h"
#include "../SensorValues/SensorValueRTThreadTimings.h"
+#include "../util/RtTiming.h"
+#include "SensorDevice.h"
namespace armarx::RobotUnitModule
{
@@ -34,10 +35,13 @@ namespace armarx
{
TYPEDEF_PTRS_SHARED(RTThreadTimingsSensorDevice);
- class RTThreadTimingsSensorDevice: virtual public SensorDevice
+ class RTThreadTimingsSensorDevice : virtual public SensorDevice
{
public:
- RTThreadTimingsSensorDevice(const std::string& name): DeviceBase(name), SensorDevice(name) {}
+ RTThreadTimingsSensorDevice(const std::string& name) : DeviceBase(name), SensorDevice(name)
+ {
+ }
+
const SensorValueBase* getSensorValue() const override = 0;
virtual void rtMarkRtLoopStart() = 0;
@@ -45,6 +49,7 @@ namespace armarx
virtual void rtMarkRtBusSendReceiveStart() = 0;
virtual void rtMarkRtBusSendReceiveEnd() = 0;
+
protected:
friend class RobotUnit;
friend class RobotUnitModule::ControlThread;
@@ -70,66 +75,80 @@ namespace armarx
virtual void rtMarkRtResetAllTargetsStart() = 0;
virtual void rtMarkRtResetAllTargetsEnd() = 0;
-
};
- template<class SensorValueType = SensorValueRTThreadTimings>
- class RTThreadTimingsSensorDeviceImpl: public RTThreadTimingsSensorDevice
+ template <class SensorValueType = SensorValueRTThreadTimings>
+ class RTThreadTimingsSensorDeviceImpl : public RTThreadTimingsSensorDevice
{
public:
- static_assert(
- std::is_base_of<SensorValueRTThreadTimings, SensorValueType>::value,
- "The template parameter of RTThreadTimingsSensorDeviceImpl should be derived from SensorValueRTThreadTimings"
- );
- RTThreadTimingsSensorDeviceImpl(const std::string& name): DeviceBase(name), SensorDevice(name), RTThreadTimingsSensorDevice(name) {}
- const SensorValueBase* getSensorValue() const override
+ static_assert(std::is_base_of<SensorValueRTThreadTimings, SensorValueType>::value,
+ "The template parameter of RTThreadTimingsSensorDeviceImpl should be derived "
+ "from SensorValueRTThreadTimings");
+
+ RTThreadTimingsSensorDeviceImpl(const std::string& name) :
+ DeviceBase(name), SensorDevice(name), RTThreadTimingsSensorDevice(name)
+ {
+ }
+
+ const SensorValueBase*
+ getSensorValue() const override
{
return &value;
}
SensorValueType value;
- void rtMarkRtLoopStart() override
+ void
+ rtMarkRtLoopStart() override
{
- const IceUtil::Time now = TimeUtil::GetTime(true);
+ const auto now = armarx::rtNow();
value.rtLoopRoundTripTime = now - rtLoopStart;
rtLoopStart = now;
}
- void rtMarkRtLoopPreSleep() override
+
+ void
+ rtMarkRtLoopPreSleep() override
{
- value.rtLoopDuration = TimeUtil::GetTime(true) - rtLoopStart;
+ value.rtLoopDuration = armarx::rtNow() - rtLoopStart;
}
- void rtMarkRtBusSendReceiveStart() override
+
+ void
+ rtMarkRtBusSendReceiveStart() override
{
- const IceUtil::Time now = TimeUtil::GetTime(true);
+ const IceUtil::Time now = armarx::rtNow();
value.rtBusSendReceiveRoundTripTime = now - rtBusSendReceiveStart;
rtBusSendReceiveStart = now;
}
- void rtMarkRtBusSendReceiveEnd() override
+
+ void
+ rtMarkRtBusSendReceiveEnd() override
{
- value.rtBusSendReceiveDuration = TimeUtil::GetTime(true) - rtBusSendReceiveStart;
+ value.rtBusSendReceiveDuration = armarx::rtNow() - rtBusSendReceiveStart;
}
+
protected:
-#define make_markRT_X_Start_End(name) \
- virtual void rtMarkRt##name##Start() override \
- { \
- const IceUtil::Time now = TimeUtil::GetTime(true); \
- value.rt##name##RoundTripTime = now - rt##name##Start; \
- rt##name##Start=now; \
- } \
- virtual void rtMarkRt##name##End() override \
- { \
- value.rt##name##Duration=TimeUtil::GetTime(true) - rt##name##Start; \
- }
-
- make_markRT_X_Start_End(SwitchControllerSetup)
- make_markRT_X_Start_End(RunNJointControllers)
- make_markRT_X_Start_End(HandleInvalidTargets)
- make_markRT_X_Start_End(RunJointControllers)
- make_markRT_X_Start_End(ReadSensorDeviceValues)
- make_markRT_X_Start_End(UpdateSensorAndControlBuffer)
- make_markRT_X_Start_End(ResetAllTargets)
+#define make_markRT_X_Start_End(name) \
+ virtual void rtMarkRt##name##Start() override \
+ { \
+ const IceUtil::Time now = armarx::rtNow(); \
+ value.rt##name##RoundTripTime = now - rt##name##Start; \
+ rt##name##Start = now; \
+ } \
+ virtual void rtMarkRt##name##End() override \
+ { \
+ value.rt##name##Duration = armarx::rtNow() - rt##name##Start; \
+ } \
+ static_assert(true, "force trailing semi-colon")
+
+ make_markRT_X_Start_End(SwitchControllerSetup);
+ make_markRT_X_Start_End(RunNJointControllers);
+ make_markRT_X_Start_End(HandleInvalidTargets);
+ make_markRT_X_Start_End(RunJointControllers);
+ make_markRT_X_Start_End(ReadSensorDeviceValues);
+ make_markRT_X_Start_End(UpdateSensorAndControlBuffer);
+ make_markRT_X_Start_End(ResetAllTargets);
#undef make_markRT_X_Start_End
+
protected:
IceUtil::Time rtSwitchControllerSetupStart;
IceUtil::Time rtRunNJointControllersStart;
@@ -142,5 +161,4 @@ namespace armarx
IceUtil::Time rtLoopStart;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp
index 956e334a81e46441e3850779a0d40e3650c7fe6c..db2c8c52d33f2f036baa8c70001615e18b10d0fe 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp
@@ -24,9 +24,6 @@
namespace armarx
{
- const SensorDevicePtr SensorDevice::NullPtr
- {
- nullptr
- };
+ const SensorDevicePtr SensorDevice::NullPtr{nullptr};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
index a35ae8b8d359fffc4c963b1a0a426b68902eea51..c0a7ed43b74b8375a208c2afbe53dc205b26b7a8 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
@@ -22,10 +22,11 @@
#pragma once
-#include"DeviceBase.h"
+#include <IceUtil/Time.h>
+
#include "../SensorValues/SensorValueBase.h"
#include "../util.h"
-#include <IceUtil/Time.h>
+#include "DeviceBase.h"
namespace armarx::RobotUnitModule
{
@@ -40,6 +41,7 @@ namespace armarx::SensorDeviceTags
namespace armarx
{
TYPEDEF_PTRS_SHARED(SensorDevice);
+
/**
* @brief This class represents some part of the robot providing sensor values.
*
@@ -54,20 +56,24 @@ namespace armarx
* \li Level of a battery power supply
* \li 3d Velocity of a mobile Platform
*/
- class SensorDevice: public virtual DeviceBase
+ class SensorDevice : public virtual DeviceBase
{
public:
/// @brief A static const nullptr in case a const ref to a nullptr needs to be returned
static const SensorDevicePtr NullPtr;
+
/// @brief Create a SensorDevice with the given name
- SensorDevice(const std::string& name): DeviceBase {name} {}
+ SensorDevice(const std::string& name) : DeviceBase{name}
+ {
+ }
/// @return The SensorDevice's sensor value
virtual const SensorValueBase* getSensorValue() const = 0;
/// @return The SensorDevice's sensor value casted to the given type (may be nullptr)
- template<class T>
- const T* getSensorValue() const
+ template <class T>
+ const T*
+ getSensorValue() const
{
return getSensorValue()->asA<const T*>();
}
@@ -89,44 +95,54 @@ namespace armarx
* @param sensorValuesTimestamp The current timestamp
* @param timeSinceLastIteration The time delta since the last call
*/
- virtual void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) {}
+ virtual void
+ rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ }
private:
friend class RobotUnitModule::Devices;
/// @brief The owning \ref RobotUnit. (is set when this \ref SensorDevice is added to the \ref RobotUnit)
- std::atomic<RobotUnitModule::Devices*> owner {nullptr};
+ std::atomic<RobotUnitModule::Devices*> owner{nullptr};
};
- template<class SensorValueType>
+ template <class SensorValueType>
class SensorDeviceTemplate : public virtual SensorDevice
{
- static_assert(std::is_base_of<SensorValueBase, SensorValueType>::value, "SensorValueType has to inherit SensorValueBase");
+ static_assert(std::is_base_of<SensorValueBase, SensorValueType>::value,
+ "SensorValueType has to inherit SensorValueBase");
+
public:
- SensorDeviceTemplate(const std::string& name): DeviceBase(name), SensorDevice(name) {}
+ SensorDeviceTemplate(const std::string& name) : DeviceBase(name), SensorDevice(name)
+ {
+ }
const SensorValueType* getSensorValue() const final override;
SensorValueType sensorValue;
};
-}
+} // namespace armarx
//inline functions
namespace armarx
{
- inline std::string SensorDevice::getSensorValueType(bool withoutNamespaceSpecifier) const
+ inline std::string
+ SensorDevice::getSensorValueType(bool withoutNamespaceSpecifier) const
{
return getSensorValue()->getSensorValueType(withoutNamespaceSpecifier);
}
- inline std::string SensorDevice::getReportingFrame() const
+ inline std::string
+ SensorDevice::getReportingFrame() const
{
return {};
}
- template<class SensorValueType>
- inline const SensorValueType* SensorDeviceTemplate<SensorValueType>::getSensorValue() const
+ template <class SensorValueType>
+ inline const SensorValueType*
+ SensorDeviceTemplate<SensorValueType>::getSensorValue() const
{
return &sensorValue;
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp
index 84e9891176c90ba59443714c444276357f4f30f9..425a51ee3b525253da475dccb6d66bb4728f06c0 100644
--- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp
@@ -26,14 +26,17 @@
namespace armarx
{
- ControlDevice& JointController::getParent() const
+ ControlDevice&
+ JointController::getParent() const
{
ARMARX_CHECK_EXPRESSION(parent) << "This JointController is not owned by a ControlDevice";
return *parent;
}
- StringVariantBaseMap JointController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const
+ StringVariantBaseMap
+ JointController::publish(const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer) const
{
- return StringVariantBaseMap {};
+ return StringVariantBaseMap{};
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
index 869ad5163e781a218795dcedf119f1b096be9ccb..b72d8a939c137c94e14ff8fa8da7163e13d95365 100644
--- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
@@ -22,24 +22,26 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
+#include <atomic>
+#include <memory>
#include <Ice/ProxyHandle.h>
-#include <memory>
-#include <atomic>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h>
namespace IceProxy::armarx
{
class DebugDrawerInterface;
class DebugObserverInterface;
-}
+} // namespace IceProxy::armarx
namespace armarx
{
class ControlDevice;
- typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugDrawerInterface> DebugDrawerInterfacePrx;
- typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugObserverInterface> DebugObserverInterfacePrx;
+ typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface>
+ DebugDrawerInterfacePrx;
+ typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface>
+ DebugObserverInterfacePrx;
/**
* @brief The JointController class represents one joint in one control mode.
@@ -56,8 +58,9 @@ namespace armarx
virtual const ControlTargetBase* getControlTarget() const;
/// @return The \ref JointController's \ref ControlTargetBase "ControlTarget" casted to the given type (may be nullptr)
- template<class T>
- const T* getControlTarget() const
+ template <class T>
+ const T*
+ getControlTarget() const
{
return getControlTarget()->asA<const T*>();
}
@@ -88,36 +91,43 @@ namespace armarx
* with "{ControlDeviceName}_{ControlModeName}_" and are used as keys of the datafields.
*
*/
- virtual StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const;
+ virtual StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer) const;
ControlDevice& rtGetParent() const;
- template<class T>
- T& rtGetParent() const
+
+ template <class T>
+ T&
+ rtGetParent() const
{
return dynamic_cast<T&>(rtGetParent);
}
+
protected:
//called by the owning ControlDevice
/// @brief called when this JointController is run
- virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+ virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) = 0;
/// @brief called when this JointController is activated
virtual void rtPreActivateController();
virtual void rtPostDeactivateController();
/// @brief called when this JointController is deactivated
private:
friend class ControlDevice;
- std::atomic_bool activated {false};
- ControlDevice* parent {nullptr};
- std::size_t controlModeHash {0};
- std::size_t hardwareControlModeHash {0};
+ std::atomic_bool activated{false};
+ ControlDevice* parent{nullptr};
+ std::size_t controlModeHash{0};
+ std::size_t hardwareControlModeHash{0};
void rtActivate();
void rtDeactivate();
};
- template<class ControlTargetType>
+ template <class ControlTargetType>
class JointControllerTemplate : public virtual JointController
{
- static_assert(std::is_base_of<ControlTargetBase, ControlTargetType>::value, "ControlTargetType has to inherit SensorValueBase");
+ static_assert(std::is_base_of<ControlTargetBase, ControlTargetType>::value,
+ "ControlTargetType has to inherit SensorValueBase");
+
public:
using JointController::JointController;
@@ -125,87 +135,101 @@ namespace armarx
ControlTargetType controlTarget;
};
-}
+} // namespace armarx
//inline functions
namespace armarx
{
- inline const ControlTargetBase* JointController::getControlTarget() const
+ inline const ControlTargetBase*
+ JointController::getControlTarget() const
{
- return const_cast<const ControlTargetBase*>(const_cast<JointController*>(this)->getControlTarget());
+ return const_cast<const ControlTargetBase*>(
+ const_cast<JointController*>(this)->getControlTarget());
}
- inline void JointController::rtResetTarget()
+ inline void
+ JointController::rtResetTarget()
{
getControlTarget()->reset();
}
- inline bool JointController::rtIsTargetValid() const
+ inline bool
+ JointController::rtIsTargetValid() const
{
return getControlTarget()->isValid();
}
- inline const std::string& JointController::getControlMode() const
+ inline const std::string&
+ JointController::getControlMode() const
{
return getControlTarget()->getControlMode();
}
- inline std::string JointController::getHardwareControlMode() const
+ inline std::string
+ JointController::getHardwareControlMode() const
{
return "";
}
- inline std::size_t JointController::getControlModeHash() const
+ inline std::size_t
+ JointController::getControlModeHash() const
{
return controlModeHash;
}
- inline std::size_t JointController::rtGetControlModeHash() const
+ inline std::size_t
+ JointController::rtGetControlModeHash() const
{
return controlModeHash;
}
- inline std::size_t JointController::getHardwareControlModeHash() const
+ inline std::size_t
+ JointController::getHardwareControlModeHash() const
{
return hardwareControlModeHash;
}
- inline std::size_t JointController::rtGetHardwareControlModeHash() const
+ inline std::size_t
+ JointController::rtGetHardwareControlModeHash() const
{
return hardwareControlModeHash;
}
- inline ControlDevice& JointController::rtGetParent() const
+ inline ControlDevice&
+ JointController::rtGetParent() const
{
return *parent;
}
- inline void JointController::rtPreActivateController()
+ inline void
+ JointController::rtPreActivateController()
{
-
}
- inline void JointController::rtPostDeactivateController()
+ inline void
+ JointController::rtPostDeactivateController()
{
-
}
- inline void JointController::rtActivate()
+ inline void
+ JointController::rtActivate()
{
rtPreActivateController();
activated = true;
}
- inline void JointController::rtDeactivate()
+ inline void
+ JointController::rtDeactivate()
{
activated = false;
rtPostDeactivateController();
rtResetTarget();
}
- template<class ControlTargetType>
- inline ControlTargetType* JointControllerTemplate<ControlTargetType>::getControlTarget()
+ template <class ControlTargetType>
+ inline ControlTargetType*
+ JointControllerTemplate<ControlTargetType>::getControlTarget()
{
return &controlTarget;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index ff1afaf689c23ebd13e118965ff0848ebf1300c7..15baf198eb03202a929341675f01ef1ab831d234 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -1,22 +1,22 @@
+#include "NJointCartesianNaturalPositionController.h"
+
+#include <iomanip>
+
+#include <VirtualRobot/math/Helpers.h>
+
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include <VirtualRobot/math/Helpers.h>
-
-#include <iomanip>
-
-#include "NJointCartesianNaturalPositionController.h"
-
namespace armarx
{
- std::string vec2str(const std::vector<float>& vec)
+ std::string
+ vec2str(const std::vector<float>& vec)
{
std::stringstream ss;
for (size_t i = 0; i < vec.size(); i++)
@@ -29,7 +29,9 @@ namespace armarx
}
return ss.str();
}
- std::ostream& operator<<(std::ostream& out, const CartesianNaturalPositionControllerConfig& cfg)
+
+ std::ostream&
+ operator<<(std::ostream& out, const CartesianNaturalPositionControllerConfig& cfg)
{
out << "maxPositionAcceleration " << cfg.maxPositionAcceleration << '\n'
<< "maxOrientationAcceleration " << cfg.maxOrientationAcceleration << '\n'
@@ -53,10 +55,12 @@ namespace armarx
return out;
}
+ NJointControllerRegistration<NJointCartesianNaturalPositionController>
+ registrationControllerNJointCartesianNaturalPositionController(
+ "NJointCartesianNaturalPositionController");
- NJointControllerRegistration<NJointCartesianNaturalPositionController> registrationControllerNJointCartesianNaturalPositionController("NJointCartesianNaturalPositionController");
-
- std::string NJointCartesianNaturalPositionController::getClassName(const Ice::Current&) const
+ std::string
+ NJointCartesianNaturalPositionController::getClassName(const Ice::Current&) const
{
return "NJointCartesianNaturalPositionController";
}
@@ -77,14 +81,11 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(_rtRobot);
_rtRns = _rtRobot->getRobotNodeSet(config->rns);
- ARMARX_CHECK_NOT_NULL(_rtRns)
- << "No rns " << config->rns;
+ ARMARX_CHECK_NOT_NULL(_rtRns) << "No rns " << config->rns;
_rtTcp = _rtRns->getTCP();
- ARMARX_CHECK_NOT_NULL(_rtTcp)
- << "No tcp in rns " << config->rns;
+ ARMARX_CHECK_NOT_NULL(_rtTcp) << "No tcp in rns " << config->rns;
_rtElbow = _rtRobot->getRobotNode(config->elbowNode);
- ARMARX_CHECK_NOT_NULL(_rtElbow)
- << "No elbow node " << config->elbowNode;
+ ARMARX_CHECK_NOT_NULL(_rtElbow) << "No elbow node " << config->elbowNode;
_rtRobotRoot = _rtRobot->getRootNode();
}
@@ -95,8 +96,7 @@ namespace armarx
if (!config->ftName.empty())
{
const auto svalFT = useSensorValue<SensorValueForceTorque>(config->ftName);
- ARMARX_CHECK_NOT_NULL(svalFT)
- << "No sensor value of name " << config->ftName;
+ ARMARX_CHECK_NOT_NULL(svalFT) << "No sensor value of name " << config->ftName;
_rtForceSensor = &(svalFT->force);
_rtTorqueSensor = &(svalFT->force);
@@ -105,26 +105,25 @@ namespace armarx
const auto reportFrameName = sdev->getReportingFrame();
ARMARX_CHECK_EXPRESSION(!reportFrameName.empty())
- << VAROUT(sdev->getReportingFrame());
+ << VAROUT(sdev->getReportingFrame());
_rtFT = _rtRobot->getRobotNode(reportFrameName);
ARMARX_CHECK_NOT_NULL(_rtFT)
- << "No sensor report frame '" << reportFrameName
- << "' in robot";
+ << "No sensor report frame '" << reportFrameName << "' in robot";
}
}
//ARMARX_IMPORTANT << "got sensor";
//ctrl
{
- _rtJointVelocityFeedbackBuffer = Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
+ _rtJointVelocityFeedbackBuffer =
+ Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
_rtPosController = std::make_unique<CartesianNaturalPositionController>(
- _rtRns,
- _rtElbow,
- config->runCfg.maxPositionAcceleration,
- config->runCfg.maxOrientationAcceleration,
- config->runCfg.maxNullspaceAcceleration
- );
+ _rtRns,
+ _rtElbow,
+ config->runCfg.maxPositionAcceleration,
+ config->runCfg.maxOrientationAcceleration,
+ config->runCfg.maxNullspaceAcceleration);
_rtPosController->setConfig(config->runCfg);
ARMARX_IMPORTANT << "controller config: " << config->runCfg;
@@ -132,7 +131,8 @@ namespace armarx
for (size_t i = 0; i < _rtRns->getSize(); ++i)
{
std::string jointName = _rtRns->getNode(i)->getName();
- auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(jointName, ControlModes::Velocity1DoF);
+ auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(
+ jointName, ControlModes::Velocity1DoF);
auto sv = useSensorValue<SensorValue1DoFActuatorVelocity>(jointName);
ARMARX_CHECK_NOT_NULL(ct);
ARMARX_CHECK_NOT_NULL(sv);
@@ -155,7 +155,8 @@ namespace armarx
//ARMARX_IMPORTANT << "got visu";
}
- void NJointCartesianNaturalPositionController::rtPreActivateController()
+ void
+ NJointCartesianNaturalPositionController::rtPreActivateController()
{
//_publishIsAtTarget = false;
//_rtForceOffset = Eigen::Vector3f::Zero();
@@ -169,11 +170,13 @@ namespace armarx
_publish.elbPosVel = 0;
//_publish.targetReached = false;
//_publishForceThreshold = _rtForceThreshold;
- _rtPosController->setTarget(_rtTcp->getPoseInRootFrame(), _rtElbow->getPositionInRootFrame());
-
+ _rtPosController->setTarget(_rtTcp->getPoseInRootFrame(),
+ _rtElbow->getPositionInRootFrame());
}
- void NJointCartesianNaturalPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointCartesianNaturalPositionController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();
@@ -218,7 +221,6 @@ namespace armarx
_rtPosController->setNullspaceTarget(r.nullspaceTarget);
}
r.clearRequested = false;
-
}
}
if (_tripBufFFvel.updateReadBuffer())
@@ -286,7 +288,8 @@ namespace armarx
rt2nonrtBuf.currentFT = ft;
rt2nonrtBuf.averageFT = ftAvg;
- if ((_rtFTlimit.force > 0 && ft.force.norm() > _rtFTlimit.force) || (_rtFTlimit.torque > 0 && ft.torque.norm() > _rtFTlimit.torque))
+ if ((_rtFTlimit.force > 0 && ft.force.norm() > _rtFTlimit.force) ||
+ (_rtFTlimit.torque > 0 && ft.torque.norm() > _rtFTlimit.torque))
{
_rtStopConditionReached = true;
_publishIsAtForceLimit = true;
@@ -319,7 +322,8 @@ namespace armarx
rt2nonrtBuf.tcp = _rtTcp->getPoseInRootFrame();
rt2nonrtBuf.elb = _rtElbow->getPoseInRootFrame();
rt2nonrtBuf.tcpTarg = _rtPosController->getCurrentTarget();
- rt2nonrtBuf.elbTarg = ::math::Helpers::CreatePose(_rtPosController->getCurrentElbowTarget(), Eigen::Matrix3f::Identity());
+ rt2nonrtBuf.elbTarg = ::math::Helpers::CreatePose(_rtPosController->getCurrentElbowTarget(),
+ Eigen::Matrix3f::Identity());
if (_rtStopConditionReached)
@@ -328,12 +332,14 @@ namespace armarx
}
else
{
- VirtualRobot::IKSolver::CartesianSelection mode = _rtSetOrientation ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
+ VirtualRobot::IKSolver::CartesianSelection mode =
+ _rtSetOrientation ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
_rtPosController->setNullSpaceControl(_nullspaceControlEnabled);
if (_rtFFvel.use)
{
- if (_rtFFvelLastUpdateMS + _rtFFvelMaxAgeMS > sensorValuesTimestamp.toMilliSeconds())
+ if (_rtFFvelLastUpdateMS + _rtFFvelMaxAgeMS >
+ sensorValuesTimestamp.toMilliSeconds())
{
_rtPosController->setFeedForwardVelocity(_rtFFvel.feedForwardVelocity);
}
@@ -344,7 +350,8 @@ namespace armarx
}
}
- const Eigen::VectorXf& goal = _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble(), mode);
+ const Eigen::VectorXf& goal =
+ _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble(), mode);
const Eigen::VectorXf actualTcpVel = _rtPosController->actualTcpVel(goal);
const Eigen::VectorXf actualElbVel = _rtPosController->actualElbVel(goal);
@@ -364,12 +371,15 @@ namespace armarx
_tripRt2NonRt.commitWrite();
}
- void NJointCartesianNaturalPositionController::rtPostDeactivateController()
+ void
+ NJointCartesianNaturalPositionController::rtPostDeactivateController()
{
-
}
- void NJointCartesianNaturalPositionController::setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setConfig(
+ const CartesianNaturalPositionControllerConfig& cfg,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufCfg.getWriteBuffer();
@@ -379,7 +389,11 @@ namespace armarx
ARMARX_IMPORTANT << "set new config\n" << cfg;
}
- void NJointCartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::Vector3f& elbowTarget,
+ bool setOrientation,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufTarget.getWriteBuffer();
@@ -388,10 +402,17 @@ namespace armarx
w.setOrientation = setOrientation;
w.updated = true;
_tripBufTarget.commitWrite();
- ARMARX_IMPORTANT << "set new target\n" << tcpTarget << "\nelbow: " << elbowTarget.transpose();
+ ARMARX_IMPORTANT << "set new target\n"
+ << tcpTarget << "\nelbow: " << elbowTarget.transpose();
}
- void NJointCartesianNaturalPositionController::setTargetFeedForward(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Eigen::Vector6f& ffVel, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setTargetFeedForward(
+ const Eigen::Matrix4f& tcpTarget,
+ const Eigen::Vector3f& elbowTarget,
+ bool setOrientation,
+ const Eigen::Vector6f& ffVel,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
{
@@ -410,7 +431,9 @@ namespace armarx
_tripBufTarget.commitWrite();
}
- void NJointCartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector6f& vel, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector6f& vel,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFFvel.getWriteBuffer();
@@ -421,7 +444,8 @@ namespace armarx
ARMARX_IMPORTANT << "set new FeedForwardVelocity\n" << vel.transpose();
}
- void NJointCartesianNaturalPositionController::clearFeedForwardVelocity(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::clearFeedForwardVelocity(const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFFvel.getWriteBuffer();
@@ -431,7 +455,9 @@ namespace armarx
_tripBufFFvel.commitWrite();
ARMARX_IMPORTANT << "cleared FeedForwardVelocity";
}
- void NJointCartesianNaturalPositionController::setNullVelocity()
+
+ void
+ NJointCartesianNaturalPositionController::setNullVelocity()
{
for (auto ptr : _rtVelTargets)
{
@@ -445,26 +471,34 @@ namespace armarx
// return _publishIsAtForceLimit;
//}
- void NJointCartesianNaturalPositionController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setVisualizationRobotGlobalPose(
+ const Eigen::Matrix4f& p,
+ const Ice::Current&)
{
std::lock_guard g{_tripFakeRobotGPWriteMutex};
_tripFakeRobotGP.getWriteBuffer() = p;
_tripFakeRobotGP.commitWrite();
}
- void NJointCartesianNaturalPositionController::resetVisualizationRobotGlobalPose(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::resetVisualizationRobotGlobalPose(const Ice::Current&)
{
std::lock_guard g{_tripFakeRobotGPWriteMutex};
_tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
_tripFakeRobotGP.commitWrite();
}
- void NJointCartesianNaturalPositionController::stopMovement(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::stopMovement(const Ice::Current&)
{
_stopNowRequested = true;
}
- void NJointCartesianNaturalPositionController::setNullspaceTarget(const Ice::FloatSeq& nullspaceTarget, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setNullspaceTarget(
+ const Ice::FloatSeq& nullspaceTarget,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufNullspaceTarget.getWriteBuffer();
@@ -475,7 +509,8 @@ namespace armarx
ARMARX_IMPORTANT << "set new nullspaceTarget\n" << nullspaceTarget;
}
- void NJointCartesianNaturalPositionController::clearNullspaceTarget(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::clearNullspaceTarget(const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufNullspaceTarget.getWriteBuffer();
@@ -485,12 +520,15 @@ namespace armarx
ARMARX_IMPORTANT << "reset nullspaceTarget";
}
- void NJointCartesianNaturalPositionController::setNullspaceControlEnabled(bool enabled, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setNullspaceControlEnabled(bool enabled,
+ const Ice::Current&)
{
_nullspaceControlEnabled = enabled;
}
- FTSensorValue NJointCartesianNaturalPositionController::frost(const FTval& ft)
+ FTSensorValue
+ NJointCartesianNaturalPositionController::frost(const FTval& ft)
{
FTSensorValue r;
r.force = ft.force;
@@ -498,19 +536,23 @@ namespace armarx
return r;
}
- FTSensorValue NJointCartesianNaturalPositionController::getCurrentFTValue(const Ice::Current&)
+ FTSensorValue
+ NJointCartesianNaturalPositionController::getCurrentFTValue(const Ice::Current&)
{
std::lock_guard g{_tripRt2NonRtMutex};
return frost(_tripRt2NonRt.getUpToDateReadBuffer().currentFT);
}
- FTSensorValue NJointCartesianNaturalPositionController::getAverageFTValue(const Ice::Current&)
+ FTSensorValue
+ NJointCartesianNaturalPositionController::getAverageFTValue(const Ice::Current&)
{
std::lock_guard g{_tripRt2NonRtMutex};
return frost(_tripRt2NonRt.getUpToDateReadBuffer().averageFT);
}
- void NJointCartesianNaturalPositionController::setFTOffset(const FTSensorValue& offset, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setFTOffset(const FTSensorValue& offset,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFToffset.getWriteBuffer();
@@ -518,9 +560,13 @@ namespace armarx
w.torque = offset.torque;
w.updated = true;
_tripBufFToffset.commitWrite();
- ARMARX_IMPORTANT << "set FT offset:\n" << offset.force.transpose() << "\n" << offset.torque.transpose();
+ ARMARX_IMPORTANT << "set FT offset:\n"
+ << offset.force.transpose() << "\n"
+ << offset.torque.transpose();
}
- void NJointCartesianNaturalPositionController::resetFTOffset(const Ice::Current&)
+
+ void
+ NJointCartesianNaturalPositionController::resetFTOffset(const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFToffset.getWriteBuffer();
@@ -531,9 +577,10 @@ namespace armarx
ARMARX_IMPORTANT << "reset FT offset";
}
-
-
- void NJointCartesianNaturalPositionController::setFTLimit(float force, float torque, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setFTLimit(float force,
+ float torque,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFTlimit.getWriteBuffer();
@@ -544,7 +591,8 @@ namespace armarx
ARMARX_IMPORTANT << "set FT limit:" << force << " " << torque;
}
- void NJointCartesianNaturalPositionController::clearFTLimit(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::clearFTLimit(const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFTlimit.getWriteBuffer();
@@ -555,7 +603,9 @@ namespace armarx
ARMARX_IMPORTANT << "reset FT limit";
}
- void NJointCartesianNaturalPositionController::setFakeFTValue(const FTSensorValue& ftValue, const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::setFakeFTValue(const FTSensorValue& ftValue,
+ const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFTfake.getWriteBuffer();
@@ -564,11 +614,13 @@ namespace armarx
w.use = true;
w.updated = true;
_tripBufFTfake.commitWrite();
- ARMARX_IMPORTANT << "set fake FT value:\n" << ftValue.force.transpose() << "\n" << ftValue.torque.transpose();
-
+ ARMARX_IMPORTANT << "set fake FT value:\n"
+ << ftValue.force.transpose() << "\n"
+ << ftValue.torque.transpose();
}
- void NJointCartesianNaturalPositionController::clearFakeFTValue(const Ice::Current&)
+ void
+ NJointCartesianNaturalPositionController::clearFakeFTValue(const Ice::Current&)
{
std::lock_guard g{_mutexSetTripBuf};
auto& w = _tripBufFTfake.getWriteBuffer();
@@ -580,16 +632,17 @@ namespace armarx
ARMARX_IMPORTANT << "cleared fake FT value";
}
- bool NJointCartesianNaturalPositionController::isAtForceLimit(const Ice::Current&)
+ bool
+ NJointCartesianNaturalPositionController::isAtForceLimit(const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_rtForceSensor);
return _publishIsAtForceLimit;
}
- void NJointCartesianNaturalPositionController::onPublish(
- const SensorAndControl&,
- const DebugDrawerInterfacePrx& drawer,
- const DebugObserverInterfacePrx& obs)
+ void
+ NJointCartesianNaturalPositionController::onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx& drawer,
+ const DebugObserverInterfacePrx& obs)
{
const float errorPos = _publish.errorPos;
const float errorOri = _publish.errorOri;
@@ -619,10 +672,8 @@ namespace armarx
obs->setDebugChannel(getInstanceName(), datafields);
}
- ARMARX_INFO << deactivateSpam(1) << std::setprecision(4)
- << "perror " << errorPos << " / " << errorPosMax
- << ", oerror " << errorOri << " / " << errorOriMax
- << ')';
+ ARMARX_INFO << deactivateSpam(1) << std::setprecision(4) << "perror " << errorPos << " / "
+ << errorPosMax << ", oerror " << errorOri << " / " << errorOriMax << ')';
{
std::lock_guard g{_tripRt2NonRtMutex};
@@ -639,16 +690,18 @@ namespace armarx
const Eigen::Matrix4f gelbtarg = gp * buf.elbTarg;
drawer->setPoseVisu(getName(), "tcp", new Pose(gtcp));
drawer->setPoseVisu(getName(), "tcptarg", new Pose(gtcptarg));
- drawer->setLineVisu(
- getName(), "tcp2targ",
- new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
- new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
- 3, {0, 0, 1, 1});
- drawer->setLineVisu(
- getName(), "elb2targ",
- new Vector3(Eigen::Vector3f{gelb.topRightCorner<3, 1>()}),
- new Vector3(Eigen::Vector3f{gelbtarg.topRightCorner<3, 1>()}),
- 3, {0, 0, 1, 1});
+ drawer->setLineVisu(getName(),
+ "tcp2targ",
+ new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
+ new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
+ 3,
+ {0, 0, 1, 1});
+ drawer->setLineVisu(getName(),
+ "elb2targ",
+ new Vector3(Eigen::Vector3f{gelb.topRightCorner<3, 1>()}),
+ new Vector3(Eigen::Vector3f{gelbtarg.topRightCorner<3, 1>()}),
+ 3,
+ {0, 0, 1, 1});
}
else
{
@@ -671,8 +724,4 @@ namespace armarx
//}
}
}
-}
-
-
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
index 41276169749fe1156623f14bf4a8589c500b2266..fdf9bbf5df49848fe072c820111c3835ee353960 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
@@ -1,12 +1,12 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
-#include <RobotAPI/libraries/core/CartesianNaturalPositionController.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
+#include <RobotAPI/libraries/core/CartesianNaturalPositionController.h>
namespace armarx
{
@@ -32,17 +32,28 @@ namespace armarx
std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
- void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+ void onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx&) override;
public:
//bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
//bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
- void setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
- void setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Ice::Current& = Ice::emptyCurrent) override;
- void setTargetFeedForward(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Eigen::Vector6f& ffVel, const Ice::Current&) override;
+ void setConfig(const CartesianNaturalPositionControllerConfig& cfg,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setTarget(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::Vector3f& elbowTarget,
+ bool setOrientation,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setTargetFeedForward(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::Vector3f& elbowTarget,
+ bool setOrientation,
+ const Eigen::Vector6f& ffVel,
+ const Ice::Current&) override;
void setFeedForwardVelocity(const Eigen::Vector6f& vel, const Ice::Current&) override;
void clearFeedForwardVelocity(const Ice::Current&) override;
void setNullspaceTarget(const Ice::FloatSeq& nullspaceTarget, const Ice::Current&) override;
@@ -61,7 +72,8 @@ namespace armarx
void stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
- void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current& = Ice::emptyCurrent) override;
+ void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
+ const Ice::Current& = Ice::emptyCurrent) override;
void resetVisualizationRobotGlobalPose(const Ice::Current& = Ice::emptyCurrent) override;
protected:
@@ -70,7 +82,6 @@ namespace armarx
//structs
private:
-
struct TB_Target
{
Eigen::Matrix4f tcpTarget;
@@ -78,45 +89,52 @@ namespace armarx
bool setOrientation;
bool updated = false;
};
+
struct TB_FFvel
{
Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
bool use = false;
bool updated = false;
};
+
struct TB_Cfg
{
CartesianNaturalPositionControllerConfig cfg;
bool updated = false;
};
+
struct TB_NT
{
std::vector<float> nullspaceTarget;
bool clearRequested = false;
bool updated = false;
};
+
struct TB_FT
{
- Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
Eigen::Vector3f torque = Eigen::Vector3f::Zero();
bool updated = false;
};
+
struct TB_FTlimit
{
float force = -1;
float torque = -1;
bool updated = false;
};
+
struct TB_FTfake
{
- Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
Eigen::Vector3f torque = Eigen::Vector3f::Zero();
bool use = false;
bool updated = false;
};
+
struct FTval
{
- Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
Eigen::Vector3f torque = Eigen::Vector3f::Zero();
};
@@ -155,72 +173,69 @@ namespace armarx
using Ctrl = CartesianNaturalPositionController;
//rt data
- VirtualRobot::RobotPtr _rtRobot;
- VirtualRobot::RobotNodeSetPtr _rtRns;
- VirtualRobot::RobotNodePtr _rtTcp;
- VirtualRobot::RobotNodePtr _rtElbow;
- VirtualRobot::RobotNodePtr _rtFT;
- VirtualRobot::RobotNodePtr _rtRobotRoot;
+ VirtualRobot::RobotPtr _rtRobot;
+ VirtualRobot::RobotNodeSetPtr _rtRns;
+ VirtualRobot::RobotNodePtr _rtTcp;
+ VirtualRobot::RobotNodePtr _rtElbow;
+ VirtualRobot::RobotNodePtr _rtFT;
+ VirtualRobot::RobotNodePtr _rtRobotRoot;
- std::unique_ptr<Ctrl> _rtPosController;
- bool _rtSetOrientation = true;
- Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
- TB_FFvel _rtFFvel;
- int _rtFFvelMaxAgeMS;
- long _rtFFvelLastUpdateMS = 0;
+ std::unique_ptr<Ctrl> _rtPosController;
+ bool _rtSetOrientation = true;
+ Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
+ TB_FFvel _rtFFvel;
+ int _rtFFvelMaxAgeMS;
+ long _rtFFvelLastUpdateMS = 0;
- std::vector<const float*> _rtVelSensors;
- std::vector<float*> _rtVelTargets;
+ std::vector<const float*> _rtVelSensors;
+ std::vector<float*> _rtVelTargets;
- const Eigen::Vector3f* _rtForceSensor = nullptr;
- const Eigen::Vector3f* _rtTorqueSensor = nullptr;
+ const Eigen::Vector3f* _rtForceSensor = nullptr;
+ const Eigen::Vector3f* _rtTorqueSensor = nullptr;
- std::vector<FTval> _rtFTHistory;
- size_t _rtFTHistoryIndex = 0;
+ std::vector<FTval> _rtFTHistory;
+ size_t _rtFTHistoryIndex = 0;
TB_FT _rtFTOffset;
- TB_FTlimit _rtFTlimit;
- TB_FTfake _rtFTfake;
+ TB_FTlimit _rtFTlimit;
+ TB_FTfake _rtFTfake;
//Eigen::Vector3f _rtForceOffset;
//float _rtForceThreshold = -1;
- bool _rtStopConditionReached = false;
+ bool _rtStopConditionReached = false;
- std::atomic_bool _nullspaceControlEnabled{true};
+ std::atomic_bool _nullspaceControlEnabled{true};
//flags
//std::atomic_bool _publishIsAtTarget{false};
- std::atomic_bool _publishIsAtForceLimit{false};
+ std::atomic_bool _publishIsAtForceLimit{false};
//std::atomic_bool _setFTOffset{false};
- std::atomic_bool _stopNowRequested{false};
+ std::atomic_bool _stopNowRequested{false};
//buffers
- mutable std::recursive_mutex _mutexSetTripBuf;
+ mutable std::recursive_mutex _mutexSetTripBuf;
//TripleBuffer<CtrlData> _tripBufPosCtrl;
- TripleBuffer<TB_Target> _tripBufTarget;
- TripleBuffer<TB_NT> _tripBufNullspaceTarget;
- TripleBuffer<TB_FFvel> _tripBufFFvel;
- TripleBuffer<TB_Cfg> _tripBufCfg;
- TripleBuffer<TB_FT> _tripBufFToffset;
- TripleBuffer<TB_FTlimit> _tripBufFTlimit;
- TripleBuffer<TB_FTfake> _tripBufFTfake;
+ TripleBuffer<TB_Target> _tripBufTarget;
+ TripleBuffer<TB_NT> _tripBufNullspaceTarget;
+ TripleBuffer<TB_FFvel> _tripBufFFvel;
+ TripleBuffer<TB_Cfg> _tripBufCfg;
+ TripleBuffer<TB_FT> _tripBufFToffset;
+ TripleBuffer<TB_FTlimit> _tripBufFTlimit;
+ TripleBuffer<TB_FTfake> _tripBufFTfake;
- mutable std::recursive_mutex _tripRt2NonRtMutex;
- TripleBuffer<RtToNonRtData> _tripRt2NonRt;
+ mutable std::recursive_mutex _tripRt2NonRtMutex;
+ TripleBuffer<RtToNonRtData> _tripRt2NonRt;
- mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
- TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
+ mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
+ TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
//publish data
- PublishData _publish;
+ PublishData _publish;
//std::atomic<float> _publishForceThreshold{0};
//std::atomic<float> _publishForceCurrent{0};
//std::atomic_bool _publishForceThresholdInRobotRootZ{0};
-
-
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
index 11b3246034d8b9f7ae38f6d60244e0b1f9f5f9cb..ec93568c8452b7268c5ad2212ddb9df954e8b9ce 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
@@ -22,11 +22,12 @@
* GNU General Public License
*/
#include "NJointCartesianTorqueController.h"
-#include "../RobotUnit.h"
+
+#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include "../RobotUnit.h"
#define DEFAULT_TCP_STRING "default TCP"
@@ -34,14 +35,19 @@
namespace armarx
{
- NJointControllerRegistration<NJointCartesianTorqueController> registrationControllerNJointCartesianTorqueController("NJointCartesianTorqueController");
+ NJointControllerRegistration<NJointCartesianTorqueController>
+ registrationControllerNJointCartesianTorqueController("NJointCartesianTorqueController");
- std::string NJointCartesianTorqueController::getClassName(const Ice::Current&) const
+ std::string
+ NJointCartesianTorqueController::getClassName(const Ice::Current&) const
{
return "NJointCartesianTorqueController";
}
- NJointCartesianTorqueController::NJointCartesianTorqueController(RobotUnit*, NJointCartesianTorqueControllerConfigPtr config, const VirtualRobot::RobotPtr& r)
+ NJointCartesianTorqueController::NJointCartesianTorqueController(
+ RobotUnit*,
+ NJointCartesianTorqueControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r)
{
ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
useSynchronizedRtRobot();
@@ -68,12 +74,15 @@ namespace armarx
gravityTorqueSensors.push_back(gravityTorqueSensor);*/
};
- tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
+ tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
+ ? rns->getTCP()
+ : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
nodeSetName = config->nodeSetName;
- ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ ik.reset(new VirtualRobot::DifferentialIK(
+ rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
/*NJointCartesianTorqueControllerControlData initData;
@@ -83,18 +92,22 @@ namespace armarx
reinitTripleBuffer(initData);*/
}
-
- void NJointCartesianTorqueController::rtPreActivateController()
+ void
+ NJointCartesianTorqueController::rtPreActivateController()
{
}
- void NJointCartesianTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointCartesianTorqueController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
Eigen::VectorXf cartesianFT(6);
- cartesianFT << rtGetControlStruct().forceX / 1000, rtGetControlStruct().forceY / 1000, rtGetControlStruct().forceZ / 1000,
- rtGetControlStruct().torqueX, rtGetControlStruct().torqueY, rtGetControlStruct().torqueZ;
+ cartesianFT << rtGetControlStruct().forceX / 1000, rtGetControlStruct().forceY / 1000,
+ rtGetControlStruct().forceZ / 1000, rtGetControlStruct().torqueX,
+ rtGetControlStruct().torqueY, rtGetControlStruct().torqueZ;
- Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::MatrixXf jacobi =
+ ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
Eigen::MatrixXf jacobiT = jacobi.transpose();
@@ -108,12 +121,12 @@ namespace armarx
}
}
-
- WidgetDescription::HBoxLayoutPtr NJointCartesianTorqueController::createSliders()
+ WidgetDescription::HBoxLayoutPtr
+ NJointCartesianTorqueController::createSliders()
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
- auto addSlider = [&](const std::string & label, float min, float max, float defaultValue)
+ auto addSlider = [&](const std::string& label, float min, float max, float defaultValue)
{
layout->children.emplace_back(new Label(false, label));
FloatSliderPtr floatSlider = new FloatSlider;
@@ -157,7 +170,11 @@ namespace armarx
return layout;
}*/
- WidgetDescription::WidgetPtr NJointCartesianTorqueController::GenerateConfigDescription(const VirtualRobot::RobotPtr& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
+ WidgetDescription::WidgetPtr
+ NJointCartesianTorqueController::GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>&)
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
@@ -198,37 +215,41 @@ namespace armarx
return layout;
}
- NJointCartesianTorqueControllerConfigPtr NJointCartesianTorqueController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ NJointCartesianTorqueControllerConfigPtr
+ NJointCartesianTorqueController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
{
- return new NJointCartesianTorqueControllerConfig(values.at("nodeSetName")->getString(), values.at("tcpName")->getString());
+ return new NJointCartesianTorqueControllerConfig(values.at("nodeSetName")->getString(),
+ values.at("tcpName")->getString());
}
- std::string NJointCartesianTorqueController::getNodeSetName() const
+ std::string
+ NJointCartesianTorqueController::getNodeSetName() const
{
return nodeSetName;
}
- void NJointCartesianTorqueController::rtPostDeactivateController()
+ void
+ NJointCartesianTorqueController::rtPostDeactivateController()
{
-
}
- WidgetDescription::StringWidgetDictionary NJointCartesianTorqueController::getFunctionDescriptions(const Ice::Current&) const
+ WidgetDescription::StringWidgetDictionary
+ NJointCartesianTorqueController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = createSliders();
- return
- {
- {"ControllerTarget", layout}
- };
+ return {{"ControllerTarget", layout}};
}
- void NJointCartesianTorqueController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ NJointCartesianTorqueController::callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "ControllerTarget")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().forceX = valueMap.at("forceX")->getFloat();
getWriterControlStruct().forceY = valueMap.at("forceY")->getFloat();
getWriterControlStruct().forceZ = valueMap.at("forceZ")->getFloat();
@@ -244,11 +265,16 @@ namespace armarx
}
} // namespace armarx
-
-
-void armarx::NJointCartesianTorqueController::setControllerTarget(float forceX, float forceY, float forceZ, float torqueX, float torqueY, float torqueZ, const Ice::Current&)
+void
+armarx::NJointCartesianTorqueController::setControllerTarget(float forceX,
+ float forceY,
+ float forceZ,
+ float torqueX,
+ float torqueY,
+ float torqueZ,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().forceX = forceX;
getWriterControlStruct().forceY = forceY;
getWriterControlStruct().forceZ = forceZ;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
index 2bb10f999e6cf7c9a0a873ce1316e5148550732f..91326e4935517bb6b05457e2843615647a7fa05c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
@@ -24,12 +24,14 @@
#pragma once
-#include "NJointControllerWithTripleBuffer.h"
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianTorqueController.h>
+
#include "../ControlTargets/ControlTarget1DoFActuator.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianTorqueController.h>
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
@@ -52,6 +54,7 @@ namespace armarx
};*/
TYPEDEF_PTRS_HANDLE(NJointCartesianTorqueControllerControlData);
+
class NJointCartesianTorqueControllerControlData
{
public:
@@ -65,7 +68,6 @@ namespace armarx
//VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
};
-
/**
* @brief The NJointCartesianTorqueController class
* @ingroup Library-RobotUnit-NJointControllers
@@ -76,25 +78,31 @@ namespace armarx
{
public:
using ConfigPtrT = NJointCartesianTorqueControllerConfigPtr;
- NJointCartesianTorqueController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointCartesianTorqueController(RobotUnit* prov,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- static WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&);
-
- static NJointCartesianTorqueControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
- NJointCartesianTorqueController(
- RobotUnit* prov,
- NJointCartesianTorqueControllerConfigPtr config,
- const VirtualRobot::RobotPtr& r);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ static WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+
+ static NJointCartesianTorqueControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ NJointCartesianTorqueController(RobotUnit* prov,
+ NJointCartesianTorqueControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r);
std::string getNodeSetName() const;
static ::armarx::WidgetDescription::HBoxLayoutPtr createSliders();
@@ -103,6 +111,7 @@ namespace armarx
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
std::vector<ControlTarget1DoFActuatorTorque*> targets;
//std::vector<const SensorValue1DoFActuatorPosition*> sensors;
@@ -117,8 +126,13 @@ namespace armarx
// NJointCartesianTorqueControllerInterface interface
public:
- void setControllerTarget(float forceX, float forceY, float forceZ, float torqueX, float torqueY, float torqueZ, const Ice::Current&) override;
+ void setControllerTarget(float forceX,
+ float forceY,
+ float forceZ,
+ float torqueX,
+ float torqueY,
+ float torqueZ,
+ const Ice::Current&) override;
};
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
index 2b55cc15c11be833e581c36eafbdf31cdffec55c..486eaa77564de408fcd7c57da71eb1c73e6e93f1 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
@@ -24,24 +24,30 @@
#include "NJointCartesianVelocityController.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
-#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#define DEFAULT_TCP_STRING "default TCP"
namespace armarx
{
- NJointControllerRegistration<NJointCartesianVelocityController> registrationControllerNJointCartesianVelocityController("NJointCartesianVelocityController");
+ NJointControllerRegistration<NJointCartesianVelocityController>
+ registrationControllerNJointCartesianVelocityController(
+ "NJointCartesianVelocityController");
- std::string NJointCartesianVelocityController::getClassName(const Ice::Current&) const
+ std::string
+ NJointCartesianVelocityController::getClassName(const Ice::Current&) const
{
return "NJointCartesianVelocityController";
}
- NJointCartesianVelocityController::NJointCartesianVelocityController(RobotUnit* robotUnit, const NJointCartesianVelocityControllerConfigPtr& config, const VirtualRobot::RobotPtr& r)
+ NJointCartesianVelocityController::NJointCartesianVelocityController(
+ RobotUnit* robotUnit,
+ const NJointCartesianVelocityControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& r)
{
useSynchronizedRtRobot();
ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
@@ -54,8 +60,10 @@ namespace armarx
const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFGravityTorque* gravityTorqueSensor = sv->asA<SensorValue1DoFGravityTorque>();
+ const SensorValue1DoFActuatorTorque* torqueSensor =
+ sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFGravityTorque* gravityTorqueSensor =
+ sv->asA<SensorValue1DoFGravityTorque>();
if (!torqueSensor)
{
ARMARX_WARNING << "No Torque sensor available for " << jointName;
@@ -68,7 +76,10 @@ namespace armarx
gravityTorqueSensors.push_back(gravityTorqueSensor);
};
- VirtualRobot::RobotNodePtr tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
+ VirtualRobot::RobotNodePtr tcp =
+ (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
+ ? rns->getTCP()
+ : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
tcpController.reset(new CartesianVelocityController(rns, tcp));
@@ -84,12 +95,14 @@ namespace armarx
reinitTripleBuffer(initData);
}
-
- void NJointCartesianVelocityController::rtPreActivateController()
+ void
+ NJointCartesianVelocityController::rtPreActivateController()
{
}
- void NJointCartesianVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointCartesianVelocityController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
auto mode = rtGetControlStruct().mode;
@@ -97,7 +110,9 @@ namespace armarx
if (mode == VirtualRobot::IKSolver::All)
{
x.resize(6);
- x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel, rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel,
+ rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else if (mode == VirtualRobot::IKSolver::Position)
{
@@ -107,7 +122,8 @@ namespace armarx
else if (mode == VirtualRobot::IKSolver::Orientation)
{
x.resize(3);
- x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else
{
@@ -124,11 +140,14 @@ namespace armarx
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
- if (torqueSensors.at(i) && gravityTorqueSensors.at(i) && rtGetControlStruct().torqueKp.at(i) != 0)
+ if (torqueSensors.at(i) && gravityTorqueSensors.at(i) &&
+ rtGetControlStruct().torqueKp.at(i) != 0)
{
torquePIDs.at(i).Kp = rtGetControlStruct().torqueKp.at(i);
torquePIDs.at(i).Kd = rtGetControlStruct().torqueKd.at(i);
- jnv(i) += torquePIDs.at(i).update(timeSinceLastIteration.toSecondsDouble(), torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
+ jnv(i) += torquePIDs.at(i).update(timeSinceLastIteration.toSecondsDouble(),
+ torqueSensors.at(i)->torque -
+ gravityTorqueSensors.at(i)->gravityTorque);
//jnv(i) += rtGetControlStruct().torqueKp.at(i) * (torqueSensors.at(i)->torque - gravityTorqueSensors.at(i)->gravityTorque);
}
else
@@ -144,12 +163,12 @@ namespace armarx
}
}
-
- ::armarx::WidgetDescription::WidgetSeq NJointCartesianVelocityController::createSliders()
+ ::armarx::WidgetDescription::WidgetSeq
+ NJointCartesianVelocityController::createSliders()
{
using namespace armarx::WidgetDescription;
::armarx::WidgetDescription::WidgetSeq widgets;
- auto addSlider = [&](const std::string & label, float limit)
+ auto addSlider = [&](const std::string& label, float limit)
{
widgets.emplace_back(new Label(false, label));
{
@@ -172,11 +191,14 @@ namespace armarx
return widgets;
}
- WidgetDescription::HBoxLayoutPtr NJointCartesianVelocityController::createJointSlidersLayout(float min, float max, float defaultValue) const
+ WidgetDescription::HBoxLayoutPtr
+ NJointCartesianVelocityController::createJointSlidersLayout(float min,
+ float max,
+ float defaultValue) const
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
- auto addSlider = [&](const std::string & label)
+ auto addSlider = [&](const std::string& label)
{
layout->children.emplace_back(new Label(false, label));
FloatSliderPtr floatSlider = new FloatSlider;
@@ -193,10 +215,13 @@ namespace armarx
}
return layout;
-
}
- WidgetDescription::WidgetPtr NJointCartesianVelocityController::GenerateConfigDescription(const VirtualRobot::RobotPtr& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
+ WidgetDescription::WidgetPtr
+ NJointCartesianVelocityController::GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>&)
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
@@ -241,13 +266,18 @@ namespace armarx
return layout;
}
- NJointCartesianVelocityControllerConfigPtr NJointCartesianVelocityController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ NJointCartesianVelocityControllerConfigPtr
+ NJointCartesianVelocityController::GenerateConfigFromVariants(
+ const StringVariantBaseMap& values)
{
- return new NJointCartesianVelocityControllerConfig(values.at("nodeSetName")->getString(), values.at("tcpName")->getString(),
- IceModeFromString(values.at("mode")->getString()));
+ return new NJointCartesianVelocityControllerConfig(
+ values.at("nodeSetName")->getString(),
+ values.at("tcpName")->getString(),
+ IceModeFromString(values.at("mode")->getString()));
}
- VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityController::ModeFromString(const std::string mode)
+ VirtualRobot::IKSolver::CartesianSelection
+ NJointCartesianVelocityController::ModeFromString(const std::string mode)
{
//ARMARX_IMPORTANT_S << "the mode is " << mode;
if (mode == "Position")
@@ -266,7 +296,8 @@ namespace armarx
return (VirtualRobot::IKSolver::CartesianSelection)0;
}
- NJointCartesianVelocityControllerMode::CartesianSelection NJointCartesianVelocityController::IceModeFromString(const std::string mode)
+ NJointCartesianVelocityControllerMode::CartesianSelection
+ NJointCartesianVelocityController::IceModeFromString(const std::string mode)
{
if (mode == "Position")
{
@@ -284,7 +315,9 @@ namespace armarx
return (NJointCartesianVelocityControllerMode::CartesianSelection)0;
}
- VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityController::ModeFromIce(const NJointCartesianVelocityControllerMode::CartesianSelection mode)
+ VirtualRobot::IKSolver::CartesianSelection
+ NJointCartesianVelocityController::ModeFromIce(
+ const NJointCartesianVelocityControllerMode::CartesianSelection mode)
{
if (mode == NJointCartesianVelocityControllerMode::CartesianSelection::ePosition)
{
@@ -302,10 +335,17 @@ namespace armarx
return (VirtualRobot::IKSolver::CartesianSelection)0;
}
-
- void NJointCartesianVelocityController::setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode)
+ void
+ NJointCartesianVelocityController::setVelocities(
+ float xVel,
+ float yVel,
+ float zVel,
+ float rollVel,
+ float pitchVel,
+ float yawVel,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = xVel;
getWriterControlStruct().yVel = yVel;
getWriterControlStruct().zVel = zVel;
@@ -316,79 +356,84 @@ namespace armarx
writeControlStruct();
}
- void NJointCartesianVelocityController::setAvoidJointLimitsKp(float kp)
+ void
+ NJointCartesianVelocityController::setAvoidJointLimitsKp(float kp)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().avoidJointLimitsKp = kp;
writeControlStruct();
}
- std::string NJointCartesianVelocityController::getNodeSetName() const
+ std::string
+ NJointCartesianVelocityController::getNodeSetName() const
{
return nodeSetName;
}
- void NJointCartesianVelocityController::rtPostDeactivateController()
+ void
+ NJointCartesianVelocityController::rtPostDeactivateController()
{
-
}
- WidgetDescription::StringWidgetDictionary NJointCartesianVelocityController::getFunctionDescriptions(const Ice::Current&) const
+ WidgetDescription::StringWidgetDictionary
+ NJointCartesianVelocityController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
auto sliders = createSliders();
- layout->children.insert(layout->children.end(),
- sliders.begin(),
- sliders.end());
+ layout->children.insert(layout->children.end(), sliders.begin(), sliders.end());
- return
- {
- {"ControllerTarget", layout},
- {"TorqueKp", createJointSlidersLayout(-1, 1, 0)},
- {"TorqueKd", createJointSlidersLayout(-1, 1, 0)},
- {"NullspaceJointVelocities", createJointSlidersLayout(-1, 1, 0)}
- };
+ return {{"ControllerTarget", layout},
+ {"TorqueKp", createJointSlidersLayout(-1, 1, 0)},
+ {"TorqueKd", createJointSlidersLayout(-1, 1, 0)},
+ {"NullspaceJointVelocities", createJointSlidersLayout(-1, 1, 0)}};
}
- void NJointCartesianVelocityController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ NJointCartesianVelocityController::callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "ControllerTarget")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = valueMap.at("x")->getFloat();
getWriterControlStruct().yVel = valueMap.at("y")->getFloat();
getWriterControlStruct().zVel = valueMap.at("z")->getFloat();
getWriterControlStruct().rollVel = valueMap.at("roll")->getFloat();
getWriterControlStruct().pitchVel = valueMap.at("pitch")->getFloat();
getWriterControlStruct().yawVel = valueMap.at("yaw")->getFloat();
- getWriterControlStruct().avoidJointLimitsKp = valueMap.at("avoidJointLimitsKp")->getFloat();
+ getWriterControlStruct().avoidJointLimitsKp =
+ valueMap.at("avoidJointLimitsKp")->getFloat();
writeControlStruct();
}
else if (name == "TorqueKp")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
- getWriterControlStruct().torqueKp.at(i) = valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
+ getWriterControlStruct().torqueKp.at(i) =
+ valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
}
writeControlStruct();
}
else if (name == "TorqueKd")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
- getWriterControlStruct().torqueKd.at(i) = valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
+ getWriterControlStruct().torqueKd.at(i) =
+ valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
}
writeControlStruct();
}
else if (name == "NullspaceJointVelocities")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
- getWriterControlStruct().nullspaceJointVelocities.at(i) = valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
+ getWriterControlStruct().nullspaceJointVelocities.at(i) =
+ valueMap.at(tcpController->rns->getNode(i)->getName())->getFloat();
}
writeControlStruct();
}
@@ -398,7 +443,8 @@ namespace armarx
}
}
- float SimplePID::update(float dt, float error)
+ float
+ SimplePID::update(float dt, float error)
{
float derivative = (error - lastError) / dt;
float retVal = Kp * error + Kd * derivative;
@@ -408,11 +454,19 @@ namespace armarx
} // namespace armarx
-
-
-void armarx::NJointCartesianVelocityController::setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerMode::CartesianSelection mode, const Ice::Current&)
+void
+armarx::NJointCartesianVelocityController::setControllerTarget(
+ float x,
+ float y,
+ float z,
+ float roll,
+ float pitch,
+ float yaw,
+ float avoidJointLimitsKp,
+ NJointCartesianVelocityControllerMode::CartesianSelection mode,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = x;
getWriterControlStruct().yVel = y;
getWriterControlStruct().zVel = z;
@@ -424,22 +478,29 @@ void armarx::NJointCartesianVelocityController::setControllerTarget(float x, flo
writeControlStruct();
}
-void armarx::NJointCartesianVelocityController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
+void
+armarx::NJointCartesianVelocityController::setTorqueKp(const StringFloatDictionary& torqueKp,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
- getWriterControlStruct().torqueKp.at(i) = torqueKp.at(tcpController->rns->getNode(i)->getName());
+ getWriterControlStruct().torqueKp.at(i) =
+ torqueKp.at(tcpController->rns->getNode(i)->getName());
}
writeControlStruct();
}
-void armarx::NJointCartesianVelocityController::setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&)
+void
+armarx::NJointCartesianVelocityController::setNullspaceJointVelocities(
+ const StringFloatDictionary& nullspaceJointVelocities,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
for (size_t i = 0; i < tcpController->rns->getSize(); i++)
{
- getWriterControlStruct().nullspaceJointVelocities.at(i) = nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
+ getWriterControlStruct().nullspaceJointVelocities.at(i) =
+ nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
}
writeControlStruct();
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
index aed56406e6f690a8f58013848b44b6deffd6f9c5..35f3a9375629e1a6b2d42c2d9205290b3d2fd4b1 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
@@ -24,21 +24,21 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/VirtualRobot.h>
-
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityController);
TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityControllerControlData);
+
class NJointCartesianVelocityControllerControlData
{
public:
@@ -64,7 +64,6 @@ namespace armarx
float update(float dt, float error);
};
-
/**
* @brief The NJointCartesianVelocityController class
* @ingroup Library-RobotUnit-NJointControllers
@@ -75,39 +74,56 @@ namespace armarx
{
public:
using ConfigPtrT = NJointCartesianVelocityControllerConfigPtr;
- NJointCartesianVelocityController(RobotUnit* prov, const NJointCartesianVelocityControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointCartesianVelocityController(RobotUnit* prov,
+ const NJointCartesianVelocityControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- static WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&);
-
- static NJointCartesianVelocityControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
- NJointCartesianVelocityController(
- RobotUnitPtr prov,
- NJointCartesianVelocityControllerConfigPtr config,
- const VirtualRobot::RobotPtr& r);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ static WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+
+ static NJointCartesianVelocityControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ NJointCartesianVelocityController(RobotUnitPtr prov,
+ NJointCartesianVelocityControllerConfigPtr config,
+ const VirtualRobot::RobotPtr& r);
// for TCPControlUnit
- void setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode);
+ void setVelocities(float xVel,
+ float yVel,
+ float zVel,
+ float rollVel,
+ float pitchVel,
+ float yawVel,
+ VirtualRobot::IKSolver::CartesianSelection mode);
void setAvoidJointLimitsKp(float kp);
std::string getNodeSetName() const;
static ::armarx::WidgetDescription::WidgetSeq createSliders();
- WidgetDescription::HBoxLayoutPtr createJointSlidersLayout(float min, float max, float defaultValue) const;
+ WidgetDescription::HBoxLayoutPtr
+ createJointSlidersLayout(float min, float max, float defaultValue) const;
static VirtualRobot::IKSolver::CartesianSelection ModeFromString(const std::string mode);
- static NJointCartesianVelocityControllerMode::CartesianSelection IceModeFromString(const std::string mode);
- static VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointCartesianVelocityControllerMode::CartesianSelection mode);
+ static NJointCartesianVelocityControllerMode::CartesianSelection
+ IceModeFromString(const std::string mode);
+ static VirtualRobot::IKSolver::CartesianSelection
+ ModeFromIce(const NJointCartesianVelocityControllerMode::CartesianSelection mode);
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
//std::vector<const SensorValue1DoFActuatorPosition*> sensors;
@@ -122,10 +138,18 @@ namespace armarx
// NJointCartesianVelocityControllerInterface interface
public:
- void setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerMode::CartesianSelection mode, const Ice::Current&) override;
+ void setControllerTarget(float x,
+ float y,
+ float z,
+ float roll,
+ float pitch,
+ float yaw,
+ float avoidJointLimitsKp,
+ NJointCartesianVelocityControllerMode::CartesianSelection mode,
+ const Ice::Current&) override;
void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&) override;
- void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&) override;
+ void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities,
+ const Ice::Current&) override;
};
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index 59f4b58be6c94973b3be1bf8de30e67a6d8f25b6..90ef15b6185b7424ef70714af3eec4b16b6a0ba6 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -23,20 +23,23 @@
*/
#include "NJointCartesianVelocityControllerWithRamp.h"
-#include "../RobotUnit.h"
+#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include "../RobotUnit.h"
#define DEFAULT_TCP_STRING "default TCP"
namespace armarx
{
- NJointControllerRegistration<NJointCartesianVelocityControllerWithRamp> registrationControllerNJointCartesianVelocityControllerWithRamp("NJointCartesianVelocityControllerWithRamp");
+ NJointControllerRegistration<NJointCartesianVelocityControllerWithRamp>
+ registrationControllerNJointCartesianVelocityControllerWithRamp(
+ "NJointCartesianVelocityControllerWithRamp");
- std::string NJointCartesianVelocityControllerWithRamp::getClassName(const Ice::Current&) const
+ std::string
+ NJointCartesianVelocityControllerWithRamp::getClassName(const Ice::Current&) const
{
return "NJointCartesianVelocityControllerWithRamp";
}
@@ -58,8 +61,10 @@ namespace armarx
const SensorValueBase* sv = useSensorValue(jointName);
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- const SensorValue1DoFActuatorFilteredVelocity* filteredVelocitySensor = sv->asA<SensorValue1DoFActuatorFilteredVelocity>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorFilteredVelocity* filteredVelocitySensor =
+ sv->asA<SensorValue1DoFActuatorFilteredVelocity>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor =
+ sv->asA<SensorValue1DoFActuatorVelocity>();
ARMARX_CHECK_EXPRESSION(filteredVelocitySensor || velocitySensor);
if (filteredVelocitySensor)
{
@@ -71,10 +76,12 @@ namespace armarx
ARMARX_IMPORTANT << "Using raw velocity for joint " << jointName;
velocitySensors.push_back(&velocitySensor->velocity);
}
-
};
- VirtualRobot::RobotNodePtr tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
+ VirtualRobot::RobotNodePtr tcp =
+ (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
+ ? rns->getTCP()
+ : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
nodeSetName = config->nodeSetName;
@@ -82,12 +89,17 @@ namespace armarx
KpJointLimitAvoidance = config->KpJointLimitAvoidance;
mode = ModeFromIce(config->mode);
- controller.reset(new CartesianVelocityControllerWithRamp(rns, Eigen::VectorXf::Zero(rns->getSize()), mode,
- config->maxPositionAcceleration, config->maxOrientationAcceleration, config->maxNullspaceAcceleration));
+ controller.reset(
+ new CartesianVelocityControllerWithRamp(rns,
+ Eigen::VectorXf::Zero(rns->getSize()),
+ mode,
+ config->maxPositionAcceleration,
+ config->maxOrientationAcceleration,
+ config->maxNullspaceAcceleration));
}
-
- void NJointCartesianVelocityControllerWithRamp::rtPreActivateController()
+ void
+ NJointCartesianVelocityControllerWithRamp::rtPreActivateController()
{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
@@ -101,13 +113,17 @@ namespace armarx
ARMARX_IMPORTANT << "initial joint velocities: " << currentJointVelocities.transpose();
}
- void NJointCartesianVelocityControllerWithRamp::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointCartesianVelocityControllerWithRamp::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
Eigen::VectorXf x;
if (mode == VirtualRobot::IKSolver::All)
{
x.resize(6);
- x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel, rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel,
+ rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else if (mode == VirtualRobot::IKSolver::Position)
{
@@ -117,7 +133,8 @@ namespace armarx
else if (mode == VirtualRobot::IKSolver::Orientation)
{
x.resize(3);
- x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else
{
@@ -125,25 +142,37 @@ namespace armarx
return;
}
- Eigen::VectorXf jnv = KpJointLimitAvoidance * controller->controller.calculateJointLimitAvoidance();
+ Eigen::VectorXf jnv =
+ KpJointLimitAvoidance * controller->controller.calculateJointLimitAvoidance();
jnv += controller->controller.calculateNullspaceVelocity(x, jointLimitAvoidanceScale, mode);
- Eigen::VectorXf jointTargetVelocities = controller->calculate(x, jnv, timeSinceLastIteration.toSecondsDouble());
+ Eigen::VectorXf jointTargetVelocities =
+ controller->calculate(x, jnv, timeSinceLastIteration.toSecondsDouble());
for (size_t i = 0; i < targets.size(); ++i)
{
targets.at(i)->velocity = jointTargetVelocities(i);
}
}
-
-
- void NJointCartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const Ice::Current&)
{
- controller->setMaxAccelerations(maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
+ controller->setMaxAccelerations(
+ maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
}
- void NJointCartesianVelocityControllerWithRamp::setTargetVelocity(float vx, float vy, float vz, float vrx, float vry, float vrz, const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::setTargetVelocity(float vx,
+ float vy,
+ float vz,
+ float vrx,
+ float vry,
+ float vrz,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = vx;
getWriterControlStruct().yVel = vy;
getWriterControlStruct().zVel = vz;
@@ -153,19 +182,25 @@ namespace armarx
writeControlStruct();
}
- void NJointCartesianVelocityControllerWithRamp::setJointLimitAvoidanceScale(float jointLimitAvoidanceScale, const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::setJointLimitAvoidanceScale(
+ float jointLimitAvoidanceScale,
+ const Ice::Current&)
{
this->jointLimitAvoidanceScale = jointLimitAvoidanceScale;
}
- void NJointCartesianVelocityControllerWithRamp::setKpJointLimitAvoidance(float KpJointLimitAvoidance, const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::setKpJointLimitAvoidance(float KpJointLimitAvoidance,
+ const Ice::Current&)
{
this->KpJointLimitAvoidance = KpJointLimitAvoidance;
}
- void NJointCartesianVelocityControllerWithRamp::immediateHardStop(const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::immediateHardStop(const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
controller->setCurrentJointVelocity(Eigen::VectorXf::Zero(velocitySensors.size()));
@@ -179,32 +214,33 @@ namespace armarx
writeControlStruct();
}
-
-
- void NJointCartesianVelocityControllerWithRamp::rtPostDeactivateController()
+ void
+ NJointCartesianVelocityControllerWithRamp::rtPostDeactivateController()
{
-
}
- const std::string& NJointCartesianVelocityControllerWithRamp::getNodeSetName() const
+ const std::string&
+ NJointCartesianVelocityControllerWithRamp::getNodeSetName() const
{
return nodeSetName;
}
- WidgetDescription::StringWidgetDictionary NJointCartesianVelocityControllerWithRamp::getFunctionDescriptions(const Ice::Current&) const
+ WidgetDescription::StringWidgetDictionary
+ NJointCartesianVelocityControllerWithRamp::getFunctionDescriptions(const Ice::Current&) const
{
- return
- {
- {"ControllerTarget", createTargetLayout()},
- {"ControllerParameters", createParameterLayout()}
- };
+ return {{"ControllerTarget", createTargetLayout()},
+ {"ControllerParameters", createParameterLayout()}};
}
- void NJointCartesianVelocityControllerWithRamp::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ NJointCartesianVelocityControllerWithRamp::callDescribedFunction(
+ const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "ControllerTarget")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = valueMap.at("x")->getFloat();
getWriterControlStruct().yVel = valueMap.at("y")->getFloat();
getWriterControlStruct().zVel = valueMap.at("z")->getFloat();
@@ -217,7 +253,7 @@ namespace armarx
}
else if (name == "ControllerParameters")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
setMaxAccelerations(valueMap.at("maxPositionAcceleration")->getFloat(),
valueMap.at("maxOrientationAcceleration")->getFloat(),
valueMap.at("maxNullspaceAcceleration")->getFloat());
@@ -228,7 +264,8 @@ namespace armarx
}
}
- WidgetDescription::VBoxLayoutPtr NJointCartesianVelocityControllerWithRamp::createTargetLayout() const
+ WidgetDescription::VBoxLayoutPtr
+ NJointCartesianVelocityControllerWithRamp::createTargetLayout() const
{
LayoutBuilder layout;
layout.addSlider("x", -100, 100, 0);
@@ -242,17 +279,25 @@ namespace armarx
layout.addSlider("jointLimitAvoidanceScale", -10, 10, jointLimitAvoidanceScale);
return layout.getLayout();
}
- WidgetDescription::VBoxLayoutPtr NJointCartesianVelocityControllerWithRamp::createParameterLayout() const
+
+ WidgetDescription::VBoxLayoutPtr
+ NJointCartesianVelocityControllerWithRamp::createParameterLayout() const
{
LayoutBuilder layout;
- layout.addSlider("maxPositionAcceleration", 0, 1000, controller->getMaxPositionAcceleration());
- layout.addSlider("maxOrientationAcceleration", 0, 100, controller->getMaxOrientationAcceleration());
- layout.addSlider("maxNullspaceAcceleration", 0, 100, controller->getMaxNullspaceAcceleration());
+ layout.addSlider(
+ "maxPositionAcceleration", 0, 1000, controller->getMaxPositionAcceleration());
+ layout.addSlider(
+ "maxOrientationAcceleration", 0, 100, controller->getMaxOrientationAcceleration());
+ layout.addSlider(
+ "maxNullspaceAcceleration", 0, 100, controller->getMaxNullspaceAcceleration());
return layout.getLayout();
}
-
- WidgetDescription::WidgetPtr NJointCartesianVelocityControllerWithRamp::GenerateConfigDescription(const VirtualRobot::RobotPtr& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
+ WidgetDescription::WidgetPtr
+ NJointCartesianVelocityControllerWithRamp::GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>&)
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
@@ -290,7 +335,7 @@ namespace armarx
boxMode->defaultIndex = 2;
- auto addSlider = [&](const std::string & label, float max, float defaultValue)
+ auto addSlider = [&](const std::string& label, float max, float defaultValue)
{
layout->children.emplace_back(new Label(false, label));
FloatSliderPtr floatSlider = new FloatSlider;
@@ -309,18 +354,23 @@ namespace armarx
return layout;
}
- NJointCartesianVelocityControllerWithRampConfigPtr NJointCartesianVelocityControllerWithRamp::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ NJointCartesianVelocityControllerWithRampConfigPtr
+ NJointCartesianVelocityControllerWithRamp::GenerateConfigFromVariants(
+ const StringVariantBaseMap& values)
{
- return new NJointCartesianVelocityControllerWithRampConfig(values.at("nodeSetName")->getString(), values.at("tcpName")->getString(),
- IceModeFromString(values.at("mode")->getString()),
- values.at("maxPositionAcceleration")->getFloat(),
- values.at("maxOrientationAcceleration")->getFloat(),
- values.at("maxNullspaceAcceleration")->getFloat(),
- values.at("KpJointLimitAvoidance")->getFloat(),
- values.at("jointLimitAvoidanceScale")->getFloat());
+ return new NJointCartesianVelocityControllerWithRampConfig(
+ values.at("nodeSetName")->getString(),
+ values.at("tcpName")->getString(),
+ IceModeFromString(values.at("mode")->getString()),
+ values.at("maxPositionAcceleration")->getFloat(),
+ values.at("maxOrientationAcceleration")->getFloat(),
+ values.at("maxNullspaceAcceleration")->getFloat(),
+ values.at("KpJointLimitAvoidance")->getFloat(),
+ values.at("jointLimitAvoidanceScale")->getFloat());
}
- VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityControllerWithRamp::ModeFromString(const std::string mode)
+ VirtualRobot::IKSolver::CartesianSelection
+ NJointCartesianVelocityControllerWithRamp::ModeFromString(const std::string mode)
{
//ARMARX_IMPORTANT_S << "the mode is " << mode;
if (mode == "Position")
@@ -339,7 +389,8 @@ namespace armarx
return (VirtualRobot::IKSolver::CartesianSelection)0;
}
- CartesianSelectionMode::CartesianSelection NJointCartesianVelocityControllerWithRamp::IceModeFromString(const std::string mode)
+ CartesianSelectionMode::CartesianSelection
+ NJointCartesianVelocityControllerWithRamp::IceModeFromString(const std::string mode)
{
if (mode == "Position")
{
@@ -357,7 +408,9 @@ namespace armarx
return (CartesianSelectionMode::CartesianSelection)0;
}
- VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityControllerWithRamp::ModeFromIce(const CartesianSelectionMode::CartesianSelection mode)
+ VirtualRobot::IKSolver::CartesianSelection
+ NJointCartesianVelocityControllerWithRamp::ModeFromIce(
+ const CartesianSelectionMode::CartesianSelection mode)
{
if (mode == CartesianSelectionMode::CartesianSelection::ePosition)
{
@@ -376,11 +429,4 @@ namespace armarx
}
-
-
} // namespace armarx
-
-
-
-
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
index 37a362c9b5215db5367938b136fcb85e9f665622..40bba081c0c63eaa83edb2454c03dfb73ce2bfe3 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
@@ -24,20 +24,21 @@
#pragma once
-#include "NJointControllerWithTripleBuffer.h"
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h>
+
#include "../ControlTargets/ControlTarget1DoFActuator.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointCartesianVelocityControllerWithRamp);
-
class NJointCartesianVelocityControllerWithRampControlData
{
public:
@@ -53,20 +54,23 @@ namespace armarx
{
WidgetDescription::VBoxLayoutPtr vlayout = new WidgetDescription::VBoxLayout;
WidgetDescription::HBoxLayoutPtr hlayout;
+
public:
LayoutBuilder()
{
newLine();
}
- void newLine()
+ void
+ newLine()
{
using namespace WidgetDescription;
hlayout = new HBoxLayout;
vlayout->children.emplace_back(hlayout);
}
- void addSlider(const std::string& label, float min, float max, float value)
+ void
+ addSlider(const std::string& label, float min, float max, float value)
{
using namespace WidgetDescription;
hlayout->children.emplace_back(new Label(false, label));
@@ -78,50 +82,59 @@ namespace armarx
hlayout->children.emplace_back(slider);
}
- WidgetDescription::VBoxLayoutPtr getLayout() const
+ WidgetDescription::VBoxLayoutPtr
+ getLayout() const
{
return vlayout;
}
-
};
-
-
/**
* @brief The NJointCartesianVelocityControllerWithRamp class
* @ingroup Library-RobotUnit-NJointControllers
*/
class NJointCartesianVelocityControllerWithRamp :
- public NJointControllerWithTripleBuffer<NJointCartesianVelocityControllerWithRampControlData>,
+ public NJointControllerWithTripleBuffer<
+ NJointCartesianVelocityControllerWithRampControlData>,
public NJointCartesianVelocityControllerWithRampInterface
{
public:
using ConfigPtrT = NJointCartesianVelocityControllerWithRampConfigPtr;
- NJointCartesianVelocityControllerWithRamp(RobotUnit* robotUnit, const NJointCartesianVelocityControllerWithRampConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointCartesianVelocityControllerWithRamp(
+ RobotUnit* robotUnit,
+ const NJointCartesianVelocityControllerWithRampConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
-
- static WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&);
- static NJointCartesianVelocityControllerWithRampConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+ static WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+ static NJointCartesianVelocityControllerWithRampConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
WidgetDescription::VBoxLayoutPtr createTargetLayout() const;
WidgetDescription::VBoxLayoutPtr createParameterLayout() const;
static VirtualRobot::IKSolver::CartesianSelection ModeFromString(const std::string mode);
static CartesianSelectionMode::CartesianSelection IceModeFromString(const std::string mode);
- static VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const CartesianSelectionMode::CartesianSelection mode);
+ static VirtualRobot::IKSolver::CartesianSelection
+ ModeFromIce(const CartesianSelectionMode::CartesianSelection mode);
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
//std::vector<const SensorValue1DoFActuatorPosition*> sensors;
@@ -137,13 +150,22 @@ namespace armarx
// NJointCartesianVelocityControllerWithRampInterface interface
public:
- void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const Ice::Current& = Ice::emptyCurrent) override;
- void setTargetVelocity(float vx, float vy, float vz, float vrx, float vry, float vrz, const Ice::Current&) override;
- void setJointLimitAvoidanceScale(float jointLimitAvoidanceScale, const Ice::Current&) override;
+ void setMaxAccelerations(float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setTargetVelocity(float vx,
+ float vy,
+ float vz,
+ float vrx,
+ float vry,
+ float vrz,
+ const Ice::Current&) override;
+ void setJointLimitAvoidanceScale(float jointLimitAvoidanceScale,
+ const Ice::Current&) override;
void setKpJointLimitAvoidance(float KpJointLimitAvoidance, const Ice::Current&) override;
void immediateHardStop(const Ice::Current&) override;
const std::string& getNodeSetName() const;
};
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 2b53ca18ef577c4ea609134e9d30351ca85100bc..08870cfcc27981ee9e0e97f096563a35a1f51507 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -1,21 +1,21 @@
+#include "NJointCartesianWaypointController.h"
+
+#include <iomanip>
+
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
-
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include "NJointCartesianWaypointController.h"
-
-#include <iomanip>
-
namespace armarx
{
- std::ostream& operator<<(std::ostream& out, const NJointCartesianWaypointControllerRuntimeConfig& cfg)
+ std::ostream&
+ operator<<(std::ostream& out, const NJointCartesianWaypointControllerRuntimeConfig& cfg)
{
out << "maxPositionAcceleration " << cfg.wpCfg.maxPositionAcceleration << '\n'
<< "maxOrientationAcceleration " << cfg.wpCfg.maxOrientationAcceleration << '\n'
@@ -38,10 +38,12 @@ namespace armarx
return out;
}
+ NJointControllerRegistration<NJointCartesianWaypointController>
+ registrationControllerNJointCartesianWaypointController(
+ "NJointCartesianWaypointController");
- NJointControllerRegistration<NJointCartesianWaypointController> registrationControllerNJointCartesianWaypointController("NJointCartesianWaypointController");
-
- std::string NJointCartesianWaypointController::getClassName(const Ice::Current&) const
+ std::string
+ NJointCartesianWaypointController::getClassName(const Ice::Current&) const
{
return "NJointCartesianWaypointController";
}
@@ -59,11 +61,9 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(_rtRobot);
_rtRns = _rtRobot->getRobotNodeSet(config->rns);
- ARMARX_CHECK_NOT_NULL(_rtRns)
- << "No rns " << config->rns;
+ ARMARX_CHECK_NOT_NULL(_rtRns) << "No rns " << config->rns;
_rtTcp = _rtRns->getTCP();
- ARMARX_CHECK_NOT_NULL(_rtTcp)
- << "No tcp in rns " << config->rns;
+ ARMARX_CHECK_NOT_NULL(_rtTcp) << "No tcp in rns " << config->rns;
_rtRobotRoot = _rtRobot->getRootNode();
}
@@ -73,8 +73,7 @@ namespace armarx
if (!config->ftName.empty())
{
const auto svalFT = useSensorValue<SensorValueForceTorque>(config->ftName);
- ARMARX_CHECK_NOT_NULL(svalFT)
- << "No sensor value of name " << config->ftName;
+ ARMARX_CHECK_NOT_NULL(svalFT) << "No sensor value of name " << config->ftName;
_rtForceSensor = &(svalFT->force);
_rtTorqueSensor = &(svalFT->force);
@@ -83,42 +82,43 @@ namespace armarx
const auto reportFrameName = sdev->getReportingFrame();
ARMARX_CHECK_EXPRESSION(!reportFrameName.empty())
- << VAROUT(sdev->getReportingFrame());
+ << VAROUT(sdev->getReportingFrame());
_rtFT = _rtRobot->getRobotNode(reportFrameName);
ARMARX_CHECK_NOT_NULL(_rtFT)
- << "No sensor report frame '" << reportFrameName
- << "' in robot";
+ << "No sensor report frame '" << reportFrameName << "' in robot";
}
}
//ctrl
{
- _rtJointVelocityFeedbackBuffer = Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
+ _rtJointVelocityFeedbackBuffer =
+ Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
VirtualRobot::RobotNodePtr referenceFrame = nullptr;
if (!config->referenceFrameName.empty())
{
referenceFrame = _rtRobot->getRobotNode(config->referenceFrameName);
ARMARX_CHECK_NOT_NULL(referenceFrame)
- << "No node with name '" << config->referenceFrameName << "' for referenceFrame in robot";
+ << "No node with name '" << config->referenceFrameName
+ << "' for referenceFrame in robot";
}
_rtWpController = std::make_unique<CartesianWaypointController>(
- _rtRns,
- _rtJointVelocityFeedbackBuffer,
- config->runCfg.wpCfg.maxPositionAcceleration,
- config->runCfg.wpCfg.maxOrientationAcceleration,
- config->runCfg.wpCfg.maxNullspaceAcceleration,
- nullptr,
- referenceFrame
- );
+ _rtRns,
+ _rtJointVelocityFeedbackBuffer,
+ config->runCfg.wpCfg.maxPositionAcceleration,
+ config->runCfg.wpCfg.maxOrientationAcceleration,
+ config->runCfg.wpCfg.maxNullspaceAcceleration,
+ nullptr,
+ referenceFrame);
_rtWpController->setConfig({});
for (size_t i = 0; i < _rtRns->getSize(); ++i)
{
std::string jointName = _rtRns->getNode(i)->getName();
- auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(jointName, ControlModes::Velocity1DoF);
+ auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(
+ jointName, ControlModes::Velocity1DoF);
auto sv = useSensorValue<SensorValue1DoFActuatorVelocity>(jointName);
ARMARX_CHECK_NOT_NULL(ct);
ARMARX_CHECK_NOT_NULL(sv);
@@ -129,12 +129,13 @@ namespace armarx
}
//visu
{
- _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
- _tripFakeRobotGP.commitWrite();
+ _tripRt2NonRtRobotGP.getWriteBuffer().setIdentity();
+ _tripRt2NonRtRobotGP.commitWrite();
}
}
- void NJointCartesianWaypointController::rtPreActivateController()
+ void
+ NJointCartesianWaypointController::rtPreActivateController()
{
_publishIsAtTarget = false;
_rtForceOffset = Eigen::Vector3f::Zero();
@@ -159,7 +160,9 @@ namespace armarx
}
}
- void NJointCartesianWaypointController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointCartesianWaypointController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();
@@ -201,9 +204,7 @@ namespace armarx
if (_rtForceSensor)
{
const Eigen::Vector3f ftInRoot =
- _rtFT->getTransformationTo(_rtRobotRoot)
- .topLeftCorner<3, 3>()
- .transpose() *
+ _rtFT->getTransformationTo(_rtRobotRoot).topLeftCorner<3, 3>().transpose() *
(*_rtForceSensor);
rt2nonrtBuf.ft.force = ftInRoot;
rt2nonrtBuf.ft.torque = *_rtTorqueSensor;
@@ -269,7 +270,8 @@ namespace armarx
}
else
{
- const Eigen::VectorXf& goal = _rtWpController->calculate(timeSinceLastIteration.toSecondsDouble());
+ const Eigen::VectorXf& goal =
+ _rtWpController->calculate(timeSinceLastIteration.toSecondsDouble());
//write targets
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(goal.size()), _rtVelTargets.size());
for (std::size_t idx = 0; idx < _rtVelTargets.size(); ++idx)
@@ -279,18 +281,26 @@ namespace armarx
}
}
_tripRt2NonRt.commitWrite();
+
+ _tripRt2NonRtRobotGP.getWriteBuffer() = _rtRobot->getGlobalPose();
+ _tripRt2NonRtRobotGP.commitWrite();
}
- void NJointCartesianWaypointController::rtPostDeactivateController()
+ void
+ NJointCartesianWaypointController::rtPostDeactivateController()
{
-
}
- bool NJointCartesianWaypointController::hasReachedTarget(const Ice::Current&)
+ bool
+ NJointCartesianWaypointController::hasReachedTarget(const Ice::Current&)
{
return _publishIsAtTarget;
}
- void NJointCartesianWaypointController::setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setConfig(
+ const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -299,7 +309,10 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new config\n" << cfg;
}
- void NJointCartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& wps,
+ const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -308,7 +321,9 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new waypoints\n" << wps;
}
- void NJointCartesianWaypointController::setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -318,7 +333,9 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new waypoint\n" << wp;
}
- void NJointCartesianWaypointController::setWaypointAx(const Ice::FloatSeq& data, const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setWaypointAx(const Ice::FloatSeq& data, const Ice::Current&)
{
Eigen::Matrix4f wp(data.data());
std::lock_guard g{_tripBufWpCtrlMut};
@@ -330,10 +347,11 @@ namespace armarx
ARMARX_IMPORTANT << "set new waypoint\n" << wp;
}
-
- void NJointCartesianWaypointController::setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
- const std::vector<Eigen::Matrix4f>& wps,
- const Ice::Current&)
+ void
+ NJointCartesianWaypointController::setConfigAndWaypoints(
+ const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const std::vector<Eigen::Matrix4f>& wps,
+ const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -344,9 +362,12 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new config\n" << cfg << " and new waypoints\n" << wps;
}
- void NJointCartesianWaypointController::setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
- const Eigen::Matrix4f& wp,
- const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setConfigAndWaypoint(
+ const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const Eigen::Matrix4f& wp,
+ const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -358,7 +379,9 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new config\n" << cfg << "and a new waypoint\n" << wp;
}
- void NJointCartesianWaypointController::setNullVelocity()
+
+ void
+ NJointCartesianWaypointController::setNullVelocity()
{
for (auto ptr : _rtVelTargets)
{
@@ -366,38 +389,43 @@ namespace armarx
}
}
- bool NJointCartesianWaypointController::hasReachedForceLimit(const Ice::Current&)
+ bool
+ NJointCartesianWaypointController::hasReachedForceLimit(const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_rtForceSensor);
return _publishIsAtForceLimit;
}
- FTSensorValue NJointCartesianWaypointController::getFTSensorValue(const Ice::Current&)
+
+ FTSensorValue
+ NJointCartesianWaypointController::getFTSensorValue(const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_rtForceSensor);
std::lock_guard g{_tripRt2NonRtMutex};
return _tripRt2NonRt.getUpToDateReadBuffer().ft;
}
- void NJointCartesianWaypointController::setCurrentFTAsOffset(const Ice::Current&)
+
+ void
+ NJointCartesianWaypointController::setCurrentFTAsOffset(const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_rtForceSensor);
_setFTOffset = true;
}
- void NJointCartesianWaypointController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current&)
+ void
+ 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&)
+ void
+ NJointCartesianWaypointController::resetVisualizationRobotGlobalPose(const Ice::Current&)
{
- std::lock_guard g{_tripFakeRobotGPWriteMutex};
- _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
- _tripFakeRobotGP.commitWrite();
+ ; // No longer used ...
}
- void NJointCartesianWaypointController::stopMovement(const Ice::Current&)
+ void
+ NJointCartesianWaypointController::stopMovement(const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -408,10 +436,10 @@ namespace armarx
ARMARX_IMPORTANT << "stop movement by setting 0 waypoints";
}
- void NJointCartesianWaypointController::onPublish(
- const SensorAndControl&,
- const DebugDrawerInterfacePrx& drawer,
- const DebugObserverInterfacePrx& obs)
+ void
+ NJointCartesianWaypointController::onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx& drawer,
+ const DebugObserverInterfacePrx& obs)
{
const std::size_t wpsNum = _publishWpsNum;
const std::size_t wpsCur = wpsNum ? _publishWpsCur + 1 : 0;
@@ -439,19 +467,16 @@ namespace armarx
obs->setDebugChannel(getInstanceName(), datafields);
}
- ARMARX_INFO << deactivateSpam(1) << std::setprecision(4)
- << "At waypoint " << wpsCur << " / " << wpsNum
- << " (last " << _publishIsAtTarget
- << ", ft limit " << _publishIsAtForceLimit
- << ", perror " << errorPos << " / " << errorPosMax
- << ", oerror " << errorOri << " / " << errorOriMax
- << ')';
+ ARMARX_INFO << deactivateSpam(1) << std::setprecision(4) << "At waypoint " << wpsCur
+ << " / " << wpsNum << " (last " << _publishIsAtTarget << ", ft limit "
+ << _publishIsAtForceLimit << ", perror " << errorPos << " / " << errorPosMax
+ << ", oerror " << errorOri << " / " << errorOriMax << ')';
{
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)
@@ -460,11 +485,12 @@ namespace armarx
const Eigen::Matrix4f gtcptarg = gp * buf.tcpTarg;
drawer->setPoseVisu(getName(), "tcp", new Pose(gtcp));
drawer->setPoseVisu(getName(), "tcptarg", new Pose(gtcptarg));
- drawer->setLineVisu(
- getName(), "tcp2targ",
- new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
- new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
- 3, {0, 0, 1, 1});
+ drawer->setLineVisu(getName(),
+ "tcp2targ",
+ new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
+ new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
+ 3,
+ {0, 0, 1, 1});
}
else
{
@@ -476,20 +502,27 @@ namespace armarx
{
const Eigen::Matrix4f gft = gp * buf.ftInRoot;
const Vector3Ptr pos = new Vector3{Eigen::Vector3f{gft.topRightCorner<3, 1>()}};
- drawer->setArrowVisu(
- getName(), "force", pos,
- new Vector3{buf.ft.force.normalized()},
- {1, 1, 0, 1}, buf.ft.force.norm(), 2.5);
- drawer->setArrowVisu(
- getName(), "forceUsed", pos,
- new Vector3{buf.ftUsed.normalized()},
- {1, 0.5, 0, 1}, buf.ftUsed.norm(), 2.5);
+ drawer->setArrowVisu(getName(),
+ "force",
+ pos,
+ new Vector3{buf.ft.force.normalized()},
+ {1, 1, 0, 1},
+ buf.ft.force.norm(),
+ 2.5);
+ drawer->setArrowVisu(getName(),
+ "forceUsed",
+ pos,
+ new Vector3{buf.ftUsed.normalized()},
+ {1, 0.5, 0, 1},
+ buf.ftUsed.norm(),
+ 2.5);
}
}
}
- int NJointCartesianWaypointController::getCurrentWaypointIndex(const Ice::Current&)
+ int
+ NJointCartesianWaypointController::getCurrentWaypointIndex(const Ice::Current&)
{
return _publishWpsCur;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index affada4d1467a0c313a1d32b708469a5c703ca50..59734617b4bb75cd3599f0d2c198beb7e7694bc0 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -1,13 +1,12 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
-
-#include <RobotAPI/libraries/core/CartesianWaypointController.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
+#include <RobotAPI/libraries/core/CartesianWaypointController.h>
namespace armarx
{
@@ -24,10 +23,9 @@ namespace armarx
{
public:
using ConfigPtrT = NJointCartesianWaypointControllerConfigPtr;
- NJointCartesianWaypointController(
- RobotUnit* robotUnit,
- const NJointCartesianWaypointControllerConfigPtr& config,
- const VirtualRobot::RobotPtr&);
+ NJointCartesianWaypointController(RobotUnit* robotUnit,
+ const NJointCartesianWaypointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
@@ -35,7 +33,8 @@ namespace armarx
// void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current& = Ice::emptyCurrent) override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
// static WidgetDescription::WidgetPtr GenerateConfigDescription(
// const VirtualRobot::RobotPtr&,
// const std::map<std::string, ConstControlDevicePtr>&,
@@ -47,26 +46,38 @@ namespace armarx
// NJointCartesianWaypointControllerConfigPtr config,
// const VirtualRobot::RobotPtr& r);
- void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+ void onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx&) override;
public:
bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
int getCurrentWaypointIndex(const Ice::Current& = Ice::emptyCurrent) override;
- void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
- void setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
- void setWaypointAx(const Ice::FloatSeq& data, const Ice::Current& = Ice::emptyCurrent) override;
- void setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
- void setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
- void setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
+ void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setWaypoints(const std::vector<Eigen::Matrix4f>& wps,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setWaypointAx(const Ice::FloatSeq& data,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setWaypoint(const Eigen::Matrix4f& wp,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const std::vector<Eigen::Matrix4f>& wps,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
+ const Eigen::Matrix4f& wp,
+ const Ice::Current& = Ice::emptyCurrent) override;
void stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
FTSensorValue getFTSensorValue(const Ice::Current& = Ice::emptyCurrent) override;
void setCurrentFTAsOffset(const Ice::Current& = Ice::emptyCurrent) override;
- void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current& = Ice::emptyCurrent) override;
+ void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
+ const Ice::Current& = Ice::emptyCurrent) override;
void resetVisualizationRobotGlobalPose(const Ice::Current& = Ice::emptyCurrent) override;
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
@@ -100,53 +111,52 @@ namespace armarx
using Ctrl = CartesianWaypointController;
//rt data
- VirtualRobot::RobotPtr _rtRobot;
- VirtualRobot::RobotNodeSetPtr _rtRns;
- VirtualRobot::RobotNodePtr _rtTcp;
- VirtualRobot::RobotNodePtr _rtFT;
- VirtualRobot::RobotNodePtr _rtRobotRoot;
+ VirtualRobot::RobotPtr _rtRobot;
+ VirtualRobot::RobotNodeSetPtr _rtRns;
+ VirtualRobot::RobotNodePtr _rtTcp;
+ VirtualRobot::RobotNodePtr _rtFT;
+ VirtualRobot::RobotNodePtr _rtRobotRoot;
- std::unique_ptr<Ctrl> _rtWpController;
- Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
+ std::unique_ptr<Ctrl> _rtWpController;
+ Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
- std::vector<const float*> _rtVelSensors;
- std::vector<float*> _rtVelTargets;
+ std::vector<const float*> _rtVelSensors;
+ std::vector<float*> _rtVelTargets;
- const Eigen::Vector3f* _rtForceSensor = nullptr;
- const Eigen::Vector3f* _rtTorqueSensor = nullptr;
+ const Eigen::Vector3f* _rtForceSensor = nullptr;
+ const Eigen::Vector3f* _rtTorqueSensor = nullptr;
- Eigen::Vector3f _rtForceOffset;
+ Eigen::Vector3f _rtForceOffset;
- float _rtForceThreshold = -1;
- bool _rtOptimizeNullspaceIfTargetWasReached = false;
- bool _rtForceThresholdInRobotRootZ = true;
- bool _rtHasWps = false;
- bool _rtStopConditionReached = false;
+ float _rtForceThreshold = -1;
+ bool _rtOptimizeNullspaceIfTargetWasReached = false;
+ bool _rtForceThresholdInRobotRootZ = true;
+ bool _rtHasWps = false;
+ bool _rtStopConditionReached = false;
//flags
- std::atomic_bool _publishIsAtTarget{false};
- std::atomic_bool _publishIsAtForceLimit{false};
- std::atomic_bool _setFTOffset{false};
+ std::atomic_bool _publishIsAtTarget{false};
+ std::atomic_bool _publishIsAtForceLimit{false};
+ std::atomic_bool _setFTOffset{false};
//buffers
- mutable std::recursive_mutex _tripBufWpCtrlMut;
- WriteBufferedTripleBuffer<CtrlData> _tripBufWpCtrl;
+ mutable std::recursive_mutex _tripBufWpCtrlMut;
+ WriteBufferedTripleBuffer<CtrlData> _tripBufWpCtrl;
- mutable std::recursive_mutex _tripRt2NonRtMutex;
- TripleBuffer<RtToNonRtData> _tripRt2NonRt;
+ 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};
- std::atomic_size_t _publishWpsCur{0};
- std::atomic<float> _publishErrorPos{0};
- std::atomic<float> _publishErrorOri{0};
- std::atomic<float> _publishErrorPosMax{0};
- std::atomic<float> _publishErrorOriMax{0};
- std::atomic<float> _publishForceThreshold{0};
- std::atomic<float> _publishForceCurrent{0};
- std::atomic_bool _publishForceThresholdInRobotRootZ{0};
+ std::atomic_size_t _publishWpsNum{0};
+ std::atomic_size_t _publishWpsCur{0};
+ std::atomic<float> _publishErrorPos{0};
+ std::atomic<float> _publishErrorOri{0};
+ std::atomic<float> _publishErrorPosMax{0};
+ std::atomic<float> _publishErrorOriMax{0};
+ std::atomic<float> _publishForceThreshold{0};
+ std::atomic<float> _publishForceCurrent{0};
+ std::atomic_bool _publishForceThresholdInRobotRootZ{0};
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
index a127273c7551352855d23e762cb70b5ff3b5bfba..136df3ceb422d637cad81288ee3082003c322135 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
@@ -20,23 +20,23 @@
* GNU General Public License
*/
-#include "NJointControllerBase.h"
-#include "NJointControllerRegistry.h"
+#include <atomic>
-#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/util/algorithm.h>
#include "../RobotUnit.h"
-
-#include <atomic>
+#include "NJointControllerBase.h"
+#include "NJointControllerRegistry.h"
namespace armarx
{
- RobotUnit* getRobotUnit(RobotUnitModule::ControllerManagement* cmngr)
+ RobotUnit*
+ getRobotUnit(RobotUnitModule::ControllerManagement* cmngr)
{
return cmngr->_modulePtr<RobotUnit>();
}
-}
+} // namespace armarx
namespace armarx::RobotUnitModule
{
@@ -48,7 +48,11 @@ namespace armarx::RobotUnitModule
class DevicesAttorneyForNJointController
{
friend class ::armarx::NJointControllerBase;
- static JointController* GetJointController(Devices& d, const std::string& deviceName, const std::string& controlMode)
+
+ static JointController*
+ GetJointController(Devices& d,
+ const std::string& deviceName,
+ const std::string& controlMode)
{
if (d.controlDevices.has(deviceName))
{
@@ -63,20 +67,16 @@ namespace armarx::RobotUnitModule
return nullptr;
}
};
-}
-
-
+} // namespace armarx::RobotUnitModule
thread_local bool armarx::NJointControllerRegistryEntry::ConstructorIsRunning_ = false;
namespace armarx
{
- const NJointControllerBasePtr NJointControllerBase::NullPtr
- {
- nullptr
- };
+ const NJointControllerBasePtr NJointControllerBase::NullPtr{nullptr};
- NJointControllerDescription NJointControllerBase::getControllerDescription(const Ice::Current&) const
+ NJointControllerDescription
+ NJointControllerBase::getControllerDescription(const Ice::Current&) const
{
ARMARX_TRACE;
NJointControllerDescription d;
@@ -90,7 +90,8 @@ namespace armarx
return d;
}
- std::optional<std::vector<char> > NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
+ std::optional<std::vector<char>>
+ NJointControllerBase::isNotInConflictWith(const std::vector<char>& used) const
{
ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(used.size() == controlDeviceUsedBitmap.size());
@@ -105,12 +106,12 @@ namespace armarx
}
result.at(i) = true;
}
-
}
return result;
}
- NJointControllerStatus NJointControllerBase::getControllerStatus(const Ice::Current&) const
+ NJointControllerStatus
+ NJointControllerBase::getControllerStatus(const Ice::Current&) const
{
ARMARX_TRACE;
NJointControllerStatus s;
@@ -118,11 +119,14 @@ namespace armarx
s.instanceName = getInstanceName();
s.requested = isRequested;
s.error = deactivatedBecauseOfError;
- s.timestampUSec = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch()).count();
+ s.timestampUSec = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now().time_since_epoch())
+ .count();
return s;
}
- NJointControllerDescriptionWithStatus NJointControllerBase::getControllerDescriptionWithStatus(const Ice::Current&) const
+ NJointControllerDescriptionWithStatus
+ NJointControllerBase::getControllerDescriptionWithStatus(const Ice::Current&) const
{
NJointControllerDescriptionWithStatus ds;
ds.description = getControllerDescription();
@@ -130,31 +134,39 @@ namespace armarx
return ds;
}
-// RobotUnitInterfacePrx NJointControllerBase::getRobotUnit(const Ice::Current&) const
-// {
-// return RobotUnitInterfacePrx::uncheckedCast(robotUnit.getProxy(-1));
-// }
+ // RobotUnitInterfacePrx NJointControllerBase::getRobotUnit(const Ice::Current&) const
+ // {
+ // return RobotUnitInterfacePrx::uncheckedCast(robotUnit.getProxy(-1));
+ // }
- void NJointControllerBase::activateController(const Ice::Current&)
+ void
+ NJointControllerBase::activateController(const Ice::Current&)
{
robotUnit.activateNJointController(this);
}
- void NJointControllerBase::deactivateController(const Ice::Current&)
+ void
+ NJointControllerBase::deactivateController(const Ice::Current&)
{
robotUnit.deactivateNJointController(this);
}
- void NJointControllerBase::deleteController(const Ice::Current&)
+ void
+ NJointControllerBase::deleteController(const Ice::Current&)
{
robotUnit.deleteNJointController(this);
}
- void NJointControllerBase::deactivateAndDeleteController(const Ice::Current&)
+
+ void
+ NJointControllerBase::deactivateAndDeleteController(const Ice::Current&)
{
robotUnit.deactivateAndDeleteNJointController(this);
}
- void NJointControllerBase::publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
+ void
+ NJointControllerBase::publish(const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer)
{
ARMARX_TRACE;
const bool active = isActive;
@@ -169,11 +181,14 @@ namespace armarx
}
catch (std::exception& e)
{
- ARMARX_WARNING << "onPublishActivation of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
+ ARMARX_WARNING << "onPublishActivation of '" << getInstanceName()
+ << "' threw an exception!\nWhat:\n"
+ << e.what();
}
catch (...)
{
- ARMARX_WARNING << "onPublishActivation of '" << getInstanceName() << "' threw an exception!";
+ ARMARX_WARNING << "onPublishActivation of '" << getInstanceName()
+ << "' threw an exception!";
}
}
else
@@ -185,11 +200,14 @@ namespace armarx
}
catch (std::exception& e)
{
- ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
+ ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName()
+ << "' threw an exception!\nWhat:\n"
+ << e.what();
}
catch (...)
{
- ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName() << "' threw an exception!";
+ ARMARX_WARNING << "onPublishDeactivation of '" << getInstanceName()
+ << "' threw an exception!";
}
}
}
@@ -201,7 +219,9 @@ namespace armarx
}
catch (std::exception& e)
{
- ARMARX_WARNING << "onPublish of '" << getInstanceName() << "' threw an exception!\nWhat:\n" << e.what();
+ ARMARX_WARNING << "onPublish of '" << getInstanceName()
+ << "' threw an exception!\nWhat:\n"
+ << e.what();
}
catch (...)
{
@@ -210,7 +230,9 @@ namespace armarx
}
}
- void NJointControllerBase::deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
+ void
+ NJointControllerBase::deactivatePublish(const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer)
{
ARMARX_TRACE;
if (publishActive.exchange(false))
@@ -220,7 +242,8 @@ namespace armarx
}
}
- void NJointControllerBase::rtActivateController()
+ void
+ NJointControllerBase::rtActivateController()
{
if (!isActive)
{
@@ -231,7 +254,8 @@ namespace armarx
}
}
- void NJointControllerBase::rtDeactivateController()
+ void
+ NJointControllerBase::rtDeactivateController()
{
if (isActive)
{
@@ -240,58 +264,66 @@ namespace armarx
}
}
- void NJointControllerBase::rtDeactivateControllerBecauseOfError()
+ void
+ NJointControllerBase::rtDeactivateControllerBecauseOfError()
{
deactivatedBecauseOfError = true;
rtDeactivateController();
}
- armarx::ConstSensorDevicePtr armarx::NJointControllerBase::peekSensorDevice(const std::string& deviceName) const
+ armarx::ConstSensorDevicePtr
+ armarx::NJointControllerBase::peekSensorDevice(const std::string& deviceName) const
{
- ARMARX_CHECK_EXPRESSION(
- NJointControllerRegistryEntry::ConstructorIsRunning()) <<
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
+ ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
+ << "The function '" << BOOST_CURRENT_FUNCTION
+ << "' must only be called during the construction of an NJointController.";
return RobotUnitModule::Devices::Instance().getSensorDevice(deviceName);
}
- ConstControlDevicePtr NJointControllerBase::peekControlDevice(const std::string& deviceName) const
+ ConstControlDevicePtr
+ NJointControllerBase::peekControlDevice(const std::string& deviceName) const
{
- ARMARX_CHECK_EXPRESSION(
- NJointControllerRegistryEntry::ConstructorIsRunning()) <<
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
+ ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
+ << "The function '" << BOOST_CURRENT_FUNCTION
+ << "' must only be called during the construction of an NJointController.";
return RobotUnitModule::Devices::Instance().getControlDevice(deviceName);
}
- const VirtualRobot::RobotPtr& NJointControllerBase::useSynchronizedRtRobot(bool updateCollisionModel)
+ const VirtualRobot::RobotPtr&
+ NJointControllerBase::useSynchronizedRtRobot(bool updateCollisionModel)
{
- ARMARX_CHECK_EXPRESSION(
- NJointControllerRegistryEntry::ConstructorIsRunning()) <<
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
+ ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
+ << "The function '" << BOOST_CURRENT_FUNCTION
+ << "' must only be called during the construction of an NJointController.";
ARMARX_CHECK_IS_NULL(rtRobot) << "useSynchronizedRtRobot was already called";
rtRobot = RobotUnitModule::RobotData::Instance().cloneRobot(updateCollisionModel);
rtRobotNodes = rtRobot->getRobotNodes();
return rtRobot;
}
- void NJointControllerBase::onInitComponent()
+ void
+ NJointControllerBase::onInitComponent()
{
ARMARX_TRACE;
onInitNJointController();
}
- void NJointControllerBase::onConnectComponent()
+ void
+ NJointControllerBase::onConnectComponent()
{
ARMARX_TRACE;
onConnectNJointController();
}
- void NJointControllerBase::onDisconnectComponent()
+ void
+ NJointControllerBase::onDisconnectComponent()
{
ARMARX_TRACE;
onDisconnectNJointController();
}
- void NJointControllerBase::onExitComponent()
+ void
+ NJointControllerBase::onExitComponent()
{
ARMARX_TRACE;
onExitNJointController();
@@ -322,14 +354,14 @@ namespace armarx
{
ARMARX_INFO << "Still waiting for " << name << " thread handle!";
}
- else if (status == std::future_status::ready || status == std::future_status::deferred)
+ else if (status == std::future_status::ready ||
+ status == std::future_status::deferred)
{
ARMARX_VERBOSE << "Joining " << name << " task";
handle->join();
handle.reset();
}
- }
- while (status != std::future_status::ready);
+ } while (status != std::future_status::ready);
}
catch (...)
{
@@ -340,19 +372,20 @@ namespace armarx
threadHandles.clear();
}
- ThreadPoolPtr NJointControllerBase::getThreadPool() const
+ ThreadPoolPtr
+ NJointControllerBase::getThreadPool() const
{
ARMARX_CHECK_EXPRESSION(Application::getInstance());
return Application::getInstance()->getThreadPool();
}
-
- const SensorValueBase* NJointControllerBase::useSensorValue(const std::string& deviceName) const
+ const SensorValueBase*
+ NJointControllerBase::useSensorValue(const std::string& deviceName) const
{
ARMARX_TRACE;
- ARMARX_CHECK_EXPRESSION(
- NJointControllerRegistryEntry::ConstructorIsRunning()) <<
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
+ ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
+ << "The function '" << BOOST_CURRENT_FUNCTION
+ << "' must only be called during the construction of an NJointController.";
auto dev = peekSensorDevice(deviceName);
if (!dev)
{
@@ -363,7 +396,7 @@ namespace armarx
}
NJointControllerBase::NJointControllerBase() :
- robotUnit(RobotUnitModule::ModuleBase::Instance< ::armarx::RobotUnit>()),
+ robotUnit(RobotUnitModule::ModuleBase::Instance<::armarx::RobotUnit>()),
controlDeviceUsedBitmap(robotUnit.getNumberOfControlDevices(), 0)
{
controlDeviceUsedIndices.reserve(robotUnit.getNumberOfControlDevices());
@@ -373,21 +406,27 @@ namespace armarx
{
}
- ControlTargetBase* NJointControllerBase::useControlTarget(const std::string& deviceName, const std::string& controlMode)
+ ControlTargetBase*
+ NJointControllerBase::useControlTarget(const std::string& deviceName,
+ const std::string& controlMode)
{
ARMARX_TRACE;
- ARMARX_CHECK_EXPRESSION(
- NJointControllerRegistryEntry::ConstructorIsRunning()) <<
- "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController.";
- ARMARX_CHECK_EQUAL(
- controlDeviceControlModeMap.count(deviceName), 0) <<
- BOOST_CURRENT_FUNCTION << ": Must not request two ControlTargets for the same device. (got = "
- << controlDeviceControlModeMap.at(deviceName) << ", requested " + controlMode + ") "
- << "(You can only have a ControlDevice in one ControlMode. "
- << "To check all available ControlModes for this device, get the ControlDevice via peekControlDevice(" + deviceName + ") and querry it)";
+ ARMARX_CHECK_EXPRESSION(NJointControllerRegistryEntry::ConstructorIsRunning())
+ << "The function '" << BOOST_CURRENT_FUNCTION
+ << "' must only be called during the construction of an NJointController.";
+ ARMARX_CHECK_EQUAL(controlDeviceControlModeMap.count(deviceName), 0)
+ << BOOST_CURRENT_FUNCTION
+ << ": Must not request two ControlTargets for the same device. (got = "
+ << controlDeviceControlModeMap.at(deviceName) << ", requested " + controlMode + ") "
+ << "(You can only have a ControlDevice in one ControlMode. "
+ << "To check all available ControlModes for this device, get the ControlDevice via "
+ "peekControlDevice(" +
+ deviceName + ") and querry it)";
//there is a device and a target was requested:
- JointController* const jointCtrl = RobotUnitModule::DevicesAttorneyForNJointController::GetJointController(RobotUnitModule::Devices::Instance(), deviceName, controlMode);
+ JointController* const jointCtrl =
+ RobotUnitModule::DevicesAttorneyForNJointController::GetJointController(
+ RobotUnitModule::Devices::Instance(), deviceName, controlMode);
if (!jointCtrl)
{
return nullptr;
@@ -402,12 +441,11 @@ namespace armarx
ARMARX_CHECK_EQUAL(0, controlDeviceUsedBitmap.at(devIndex));
controlDeviceUsedBitmap.at(devIndex) = 1;
//we want this vector to be sorted
- controlDeviceUsedIndices.insert
- (
- std::upper_bound(controlDeviceUsedIndices.begin(), controlDeviceUsedIndices.end(), devIndex),
- devIndex
- );
+ controlDeviceUsedIndices.insert(std::upper_bound(controlDeviceUsedIndices.begin(),
+ controlDeviceUsedIndices.end(),
+ devIndex),
+ devIndex);
return jointCtrl->getControlTarget();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
index d393f02ea5f90ff5ed41eab507fb6bf94cc7415a..3239c0e831c96536d442cba4531b2615a1bfe034 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
@@ -22,28 +22,28 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <atomic>
+#include <functional>
+#include <map>
+#include <mutex>
+#include <optional>
+
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h> // for ARMARX_CHECK
-#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <functional>
-#include <optional>
-#include <atomic>
-#include <mutex>
-#include <map>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
namespace IceProxy::armarx
{
class DebugDrawerInterface;
class DebugObserverInterface;
class RobotUnitInterface;
-}
+} // namespace IceProxy::armarx
namespace armarx
{
@@ -52,18 +52,23 @@ namespace armarx
class NJointControllerAttorneyForPublisher;
class NJointControllerAttorneyForControlThread;
class NJointControllerAttorneyForControllerManagement;
- }
+ } // namespace RobotUnitModule
+
namespace detail
{
- template<class> class NJointControllerRegistryEntryHelper;
+ template <class>
+ class NJointControllerRegistryEntryHelper;
class ControlThreadOutputBufferEntry;
- }
+ } // namespace detail
+
namespace WidgetDescription
{
class Widget;
- typedef ::IceInternal::Handle< ::armarx::WidgetDescription::Widget> WidgetPtr;
- typedef ::std::map< ::std::string, ::armarx::WidgetDescription::WidgetPtr> StringWidgetDictionary;
- }
+ typedef ::IceInternal::Handle<::armarx::WidgetDescription::Widget> WidgetPtr;
+ typedef ::std::map<::std::string, ::armarx::WidgetDescription::WidgetPtr>
+ StringWidgetDictionary;
+ } // namespace WidgetDescription
+
using SensorAndControl = detail::ControlThreadOutputBufferEntry;
class JointController;
class SensorValueBase;
@@ -71,9 +76,12 @@ namespace armarx
using ConstControlDevicePtr = std::shared_ptr<const class ControlDevice>;
using ConstSensorDevicePtr = std::shared_ptr<const class SensorDevice>;
- typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugDrawerInterface> DebugDrawerInterfacePrx;
- typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::DebugObserverInterface> DebugObserverInterfacePrx;
- typedef ::IceInternal::ProxyHandle< ::IceProxy::armarx::RobotUnitInterface> RobotUnitInterfacePrx;
+ typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface>
+ DebugDrawerInterfacePrx;
+ typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface>
+ DebugObserverInterfacePrx;
+ typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::RobotUnitInterface>
+ RobotUnitInterfacePrx;
using ThreadPoolPtr = std::shared_ptr<class ThreadPool>;
@@ -245,8 +253,8 @@ namespace armarx
* \subsection nj-example-1-h Header
* Include headers
* \code{.cpp}
- #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+ #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
* \endcode
*
@@ -341,8 +349,9 @@ namespace armarx
*
* Include the required headers.
* \code{.cpp}
- #include "NJointPositionPassThroughController.h"
#include <RobotAPI/libraries/core/Pose.h>
+
+ #include "NJointPositionPassThroughController.h"
* \endcode
*
* Open the namespace
@@ -566,7 +575,7 @@ namespace armarx
}
* \endcode
*/
- class NJointControllerBase:
+ class NJointControllerBase :
virtual public NJointControllerInterface,
virtual public ManagedIceObject
{
@@ -575,14 +584,13 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////// //
public:
using ConfigPtrT = NJointControllerConfigPtr;
- using GenerateConfigDescriptionFunctionSignature = WidgetDescription::WidgetPtr(*)
- (
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>& controlDevices,
- const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
- );
- template<class ConfigPrtType>
- using GenerateConfigFromVariantsFunctionSignature = ConfigPrtType(*)(const StringVariantBaseMap&);
+ using GenerateConfigDescriptionFunctionSignature = WidgetDescription::WidgetPtr (*)(
+ const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices);
+ template <class ConfigPrtType>
+ using GenerateConfigFromVariantsFunctionSignature =
+ ConfigPrtType (*)(const StringVariantBaseMap&);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////// constructor setup functions ////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -593,7 +601,7 @@ namespace armarx
* @param deviceName The \ref SensorDevice's name
* @return A const ptr to the given \ref SensorDevice
*/
- ConstSensorDevicePtr peekSensorDevice(const std::string& deviceName) const;
+ ConstSensorDevicePtr peekSensorDevice(const std::string& deviceName) const;
/**
* @brief Get a const ptr to the given \ref ControlDevice
* \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
@@ -610,7 +618,9 @@ namespace armarx
* @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
* @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
*/
- ControlTargetBase* useControlTarget(const std::string& deviceName, const std::string& controlMode);
+ ControlTargetBase* useControlTarget(const std::string& deviceName,
+ const std::string& controlMode);
+
/**
* @brief Declares to calculate the \ref ControlTargetBase "ControlTarget"
* for the given \ref ControlDevice in the given ControlMode when \ref rtRun is called
@@ -619,8 +629,9 @@ namespace armarx
* @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
* @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
*/
- template<class T>
- T* useControlTarget(const std::string& deviceName, const std::string& controlMode)
+ template <class T>
+ T*
+ useControlTarget(const std::string& deviceName, const std::string& controlMode)
{
static_assert(std::is_base_of<ControlTargetBase, T>::value,
"The given type does not derive ControlTargetBase");
@@ -635,14 +646,16 @@ namespace armarx
* @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
*/
const SensorValueBase* useSensorValue(const std::string& sensorDeviceName) const;
+
/**
* @brief Get a const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
* \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
* @param deviceName The \ref SensorDevice's name
* @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
*/
- template<class T>
- const T* useSensorValue(const std::string& deviceName) const
+ template <class T>
+ const T*
+ useSensorValue(const std::string& deviceName) const
{
static_assert(std::is_base_of<SensorValueBase, T>::value,
"The given type does not derive SensorValueBase");
@@ -666,10 +679,12 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////// //
protected:
/// @see Component::getDefaultName
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return getClassName();
}
+
/// @see Component::onInitComponent
void onInitComponent() final;
/// @see Component::onConnectComponent
@@ -679,11 +694,25 @@ namespace armarx
/// @see Component::onExitComponent
void onExitComponent() final;
+ virtual void
+ onInitNJointController()
+ {
+ }
+
+ virtual void
+ onConnectNJointController()
+ {
+ }
+
+ virtual void
+ onDisconnectNJointController()
+ {
+ }
- virtual void onInitNJointController() {}
- virtual void onConnectNJointController() {}
- virtual void onDisconnectNJointController() {}
- virtual void onExitNJointController() {}
+ virtual void
+ onExitNJointController()
+ {
+ }
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////////// ThreadPool functionality///////////////////////////// //
@@ -719,18 +748,24 @@ namespace armarx
});
* @endcode
*/
- template < typename Task >
- void runTask(const std::string& taskName, Task&& task)
+ template <typename Task>
+ void
+ runTask(const std::string& taskName, Task&& task)
{
std::unique_lock lock(threadHandlesMutex);
ARMARX_CHECK_EXPRESSION(!taskName.empty());
ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName));
ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
- ARMARX_VERBOSE << "Adding NJointControllerBase task named '" << taskName << "' - current available thread count: " << getThreadPool()->getAvailableTaskCount();
+ ARMARX_VERBOSE << "Adding NJointControllerBase task named '" << taskName
+ << "' - current available thread count: "
+ << getThreadPool()->getAvailableTaskCount();
auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task));
- ARMARX_CHECK_EXPRESSION(handlePtr->isValid()) << "Could not add task (" << taskName << " - " << GetTypeString(task) << " ) - available threads: " << getThreadPool()->getAvailableTaskCount();
+ ARMARX_CHECK_EXPRESSION(handlePtr->isValid())
+ << "Could not add task (" << taskName << " - " << GetTypeString(task)
+ << " ) - available threads: " << getThreadPool()->getAvailableTaskCount();
threadHandles[taskName] = handlePtr;
}
+
std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles;
std::mutex threadHandlesMutex;
@@ -738,32 +773,44 @@ namespace armarx
// ///////////////////////////////////// ice interface //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
public:
- bool isControllerActive(const Ice::Current& = Ice::emptyCurrent) const final override
+ bool
+ isControllerActive(const Ice::Current& = Ice::emptyCurrent) const final override
{
return isActive;
}
- bool isControllerRequested(const Ice::Current& = Ice::emptyCurrent) const final override
+
+ bool
+ isControllerRequested(const Ice::Current& = Ice::emptyCurrent) const final override
{
return isRequested;
}
- bool isDeletable(const Ice::Current& = Ice::emptyCurrent) const final override
+
+ bool
+ isDeletable(const Ice::Current& = Ice::emptyCurrent) const final override
{
return deletable;
}
- bool hasControllerError(const Ice::Current& = Ice::emptyCurrent) const final override
+
+ bool
+ hasControllerError(const Ice::Current& = Ice::emptyCurrent) const final override
{
return deactivatedBecauseOfError;
}
- std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override = 0;
- std::string getInstanceName(const Ice::Current& = Ice::emptyCurrent) const final override
+ std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override = 0;
+
+ std::string
+ getInstanceName(const Ice::Current& = Ice::emptyCurrent) const final override
{
return instanceName_;
}
- NJointControllerDescription getControllerDescription(const Ice::Current& = Ice::emptyCurrent) const final override;
- NJointControllerStatus getControllerStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
- NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
+ NJointControllerDescription
+ getControllerDescription(const Ice::Current& = Ice::emptyCurrent) const final override;
+ NJointControllerStatus
+ getControllerStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
+ NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus(
+ const Ice::Current& = Ice::emptyCurrent) const final override;
//RobotUnitInterfacePrx getRobotUnit(const Ice::Current& = Ice::emptyCurrent) const final override;
void activateController(const Ice::Current& = Ice::emptyCurrent) final override;
@@ -771,39 +818,47 @@ namespace armarx
void deleteController(const Ice::Current& = Ice::emptyCurrent) final override;
void deactivateAndDeleteController(const Ice::Current& = Ice::emptyCurrent) final override;
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override
{
return {};
}
- void callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current& = Ice::emptyCurrent) override
- {
+ void
+ callDescribedFunction(const std::string&,
+ const StringVariantBaseMap&,
+ const Ice::Current& = Ice::emptyCurrent) override
+ {
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////// rt interface ///////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
public: ///TODO make protected and use attorneys
-
/**
* @brief Returns the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
* @return The virtual robot used by this \ref NJointControllerBase in the \ref rtRun
* @see useSynchronizedRtRobot
* @see rtGetRobotNodes
*/
- const VirtualRobot::RobotPtr& rtGetRobot()
+ const VirtualRobot::RobotPtr&
+ rtGetRobot()
{
return rtRobot;
}
+
/**
* @brief Returns the nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
* @return The nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
* @see useSynchronizedRtRobot
* @see rtGetRobot
*/
- const std::vector<VirtualRobot::RobotNodePtr>& rtGetRobotNodes()
+ const std::vector<VirtualRobot::RobotNodePtr>&
+ rtGetRobotNodes()
{
return rtRobotNodes;
}
+
/**
* @brief Returns whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
* for the given \ref ControlDevice
@@ -811,7 +866,8 @@ namespace armarx
* @return Whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
* for the given \ref ControlDevice
*/
- bool rtUsesControlDevice(std::size_t deviceIndex) const
+ bool
+ rtUsesControlDevice(std::size_t deviceIndex) const
{
return controlDeviceUsedBitmap.at(deviceIndex);
}
@@ -822,7 +878,8 @@ namespace armarx
* @return The indices of all \ref ControlDevice's this \ref NJointControllerBase
* calculates a \ref ControlTargetBase "ControlTarget" for.
*/
- const std::vector<std::size_t>& rtGetControlDeviceUsedIndices() const
+ const std::vector<std::size_t>&
+ rtGetControlDeviceUsedIndices() const
{
return controlDeviceUsedIndices;
}
@@ -830,7 +887,8 @@ namespace armarx
/**
* @brief Sets the error state to true. This will deactivate this controller!
*/
- bool rtGetErrorState() const
+ bool
+ rtGetErrorState() const
{
return errorState;
}
@@ -839,7 +897,8 @@ namespace armarx
* @brief Returns the number of used \ref ControlDevice "ControlDevices"
* @return The number of used \ref ControlDevice "ControlDevices"
*/
- std::size_t rtGetNumberOfUsedControlDevices() const
+ std::size_t
+ rtGetNumberOfUsedControlDevices() const
{
return controlDeviceUsedIndices.size();
}
@@ -848,7 +907,8 @@ namespace armarx
* @brief Returns the class name. (the c-string may be used for rt message logging)
* @return The class name. (the c-string may be used for rt message logging)
*/
- const std::string& rtGetClassName() const
+ const std::string&
+ rtGetClassName() const
{
return rtClassName_;
}
@@ -857,7 +917,8 @@ namespace armarx
* @brief Returns the instance name. (the c-string may be used for rt message logging)
* @return The instance name. (the c-string may be used for rt message logging)
*/
- const std::string& rtGetInstanceName() const
+ const std::string&
+ rtGetInstanceName() const
{
return instanceName_;
}
@@ -867,20 +928,29 @@ namespace armarx
* @brief This function is called before the controller is activated.
* You can use it to activate a thread again (DO NOT SPAWN NEW THREADS!) e.g. via a std::atomic_bool.
*/
- virtual void rtPreActivateController() {}
+ virtual void
+ rtPreActivateController()
+ {
+ }
+
/**
* @brief This function is called after the controller is deactivated.
* You can use it to deactivate a thread (DO NOT JOIN THREADS!) e.g. via a std::atomic_bool.
*/
- virtual void rtPostDeactivateController() {}
+ virtual void
+ rtPostDeactivateController()
+ {
+ }
/**
* @brief Sets the error state to true. This will deactivate this controller!
*/
- void rtSetErrorState()
+ void
+ rtSetErrorState()
{
errorState.store(true);
}
+
private:
/**
* @brief Activates this controller in the \ref RobotUnitModules::ControlThread "ControlThread".
@@ -914,7 +984,6 @@ namespace armarx
void rtDeactivateControllerBecauseOfError();
public:
-
static const NJointControllerBasePtr NullPtr;
NJointControllerBase();
@@ -928,33 +997,43 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////// //
public:
//used control devices
- StringStringDictionary getControlDeviceUsedControlModeMap(const Ice::Current& = Ice::emptyCurrent) const final override
+ StringStringDictionary
+ getControlDeviceUsedControlModeMap(
+ const Ice::Current& = Ice::emptyCurrent) const final override
{
return controlDeviceControlModeMap;
}
- const std::vector<char>& getControlDeviceUsedBitmap() const
+
+ const std::vector<char>&
+ getControlDeviceUsedBitmap() const
{
return controlDeviceUsedBitmap;
}
- const std::vector<std::size_t>& getControlDeviceUsedIndices() const
+
+ const std::vector<std::size_t>&
+ getControlDeviceUsedIndices() const
{
return controlDeviceUsedIndices;
}
- const std::map<std::string, const JointController*>& getControlDevicesUsedJointController()
+ const std::map<std::string, const JointController*>&
+ getControlDevicesUsedJointController()
{
return controlDeviceUsedJointController;
}
//check for conflict
- std::optional<std::vector<char>> isNotInConflictWith(const NJointControllerBasePtr& other) const
+ std::optional<std::vector<char>>
+ isNotInConflictWith(const NJointControllerBasePtr& other) const
{
return isNotInConflictWith(other->getControlDeviceUsedBitmap());
}
+
std::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
- template<class ItT>
- static std::optional<std::vector<char>> AreNotInConflict(ItT first, ItT last)
+ template <class ItT>
+ static std::optional<std::vector<char>>
+ AreNotInConflict(ItT first, ItT last)
{
if (first == last)
{
@@ -974,17 +1053,35 @@ namespace armarx
}
return inuse;
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////////////// publishing ////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
protected:
//publish thread hooks
- virtual void onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
- virtual void onPublishDeactivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
- virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
+ virtual void
+ onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
+ {
+ }
+
+ virtual void
+ onPublishDeactivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
+ {
+ }
+
+ virtual void
+ onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx&)
+ {
+ }
+
private:
- void publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer);
- void deactivatePublish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer);
+ void publish(const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer);
+ void deactivatePublish(const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -1004,17 +1101,17 @@ namespace armarx
std::string instanceName_;
//this data is filled by the robot unit to provide convenience functions
- std::atomic_bool isActive {false};
- std::atomic_bool isRequested {false};
- std::atomic_bool deactivatedBecauseOfError {false};
- std::atomic_bool errorState {false};
- bool deletable {false};
- bool internal {false};
+ std::atomic_bool isActive{false};
+ std::atomic_bool isRequested{false};
+ std::atomic_bool deactivatedBecauseOfError{false};
+ std::atomic_bool errorState{false};
+ bool deletable{false};
+ bool internal{false};
- std::atomic_bool publishActive {false};
+ std::atomic_bool publishActive{false};
- std::atomic_bool statusReportedActive {false};
- std::atomic_bool statusReportedRequested {false};
+ std::atomic_bool statusReportedActive{false};
+ std::atomic_bool statusReportedRequested{false};
VirtualRobot::RobotPtr rtRobot;
std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
@@ -1041,24 +1138,33 @@ namespace armarx
* \brief This is required for the factory
* \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
*/
- template<class> friend class detail::NJointControllerRegistryEntryHelper;
+ template <class>
+ friend class detail::NJointControllerRegistryEntryHelper;
};
class SynchronousNJointController : public NJointControllerBase
{
public: ///TODO make protected and use attorneys
- virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
- virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) = 0;
+
+ virtual void
+ rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
};
+
class AsynchronousNJointController : public NJointControllerBase
{
public: ///TODO make protected and use attorneys
- virtual void rtRunIterationBegin(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
- virtual void rtRunIterationEnd(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+ virtual void rtRunIterationBegin(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) = 0;
+ virtual void rtRunIterationEnd(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) = 0;
};
+
using NJointController = SynchronousNJointController;
using NJointControllerPtr = SynchronousNJointControllerPtr;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h
index 2b49eb4dd9c039d24b13b6ccacbac05ac4f4a293..0fd471245b9a419a99c476f69d8da7a03bef24a7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h
@@ -22,11 +22,11 @@
#pragma once
-#include "NJointControllerBase.h"
-
-#include <ArmarXCore/util/CPPUtility/TemplateMetaProgramming.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/core/util/Registrar.h>
+#include <ArmarXCore/util/CPPUtility/TemplateMetaProgramming.h>
+
+#include "NJointControllerBase.h"
namespace armarx::RobotUnitModule
{
@@ -60,12 +60,14 @@ namespace armarx
public:
virtual ~NJointControllerRegistryEntry() = default;
+
static bool
ConstructorIsRunning()
{
return ConstructorIsRunning_;
}
};
+
using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
template <class ControllerType>
@@ -80,7 +82,8 @@ namespace armarx
ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
hasGenerateConfigFromVariants,
GenerateConfigFromVariants,
- NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
+ NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<
+ typename T::ConfigPtrT>);
template <class NJointControllerT>
class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
@@ -130,7 +133,8 @@ namespace armarx
GenerateConfigDescription(
const VirtualRobot::RobotPtr& robot,
const std::map<std::string, ConstControlDevicePtr>& controlDevices,
- const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices)
+ const final override
{
if constexpr (hasRemoteConfiguration_)
{
@@ -221,6 +225,7 @@ namespace armarx
} // namespace detail
using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
+
template <class ControllerType>
struct NJointControllerRegistration
{
@@ -241,5 +246,3 @@ namespace armarx
static_assert( \
::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
#T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
-
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h
index cd6befc54b45193149ce7106be84fb644599a8cf..04a4ef03727fded452307224464436b5d9cf5ddb 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h
@@ -1,9 +1,8 @@
#pragma once
-#include "NJointControllerBase.h"
-
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
+#include "NJointControllerBase.h"
namespace armarx
{
@@ -15,29 +14,34 @@ namespace armarx
using LockGuardType = std::lock_guard<std::recursive_mutex>;
NJointControllerWithTripleBuffer(
- const ControlDataStruct& initialCommands = ControlDataStruct()) :
+ const ControlDataStruct& initialCommands = ControlDataStruct()) :
controlDataTripleBuffer{initialCommands}
{
}
- void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override
+ void
+ rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override
{
rtUpdateControlStruct();
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
protected:
- const ControlDataStruct& rtGetControlStruct() const
+ const ControlDataStruct&
+ rtGetControlStruct() const
{
return controlDataTripleBuffer.getReadBuffer();
}
- bool rtUpdateControlStruct()
+
+ bool
+ rtUpdateControlStruct()
{
return controlDataTripleBuffer.updateReadBuffer();
}
- void writeControlStruct()
+ void
+ writeControlStruct()
{
//just lock to be save and reduce the impact of an error
//also this allows code to unlock the mutex before calling this function
@@ -45,19 +49,23 @@ namespace armarx
LockGuardType lock(controlDataMutex);
controlDataTripleBuffer.commitWrite();
}
- ControlDataStruct& getWriterControlStruct()
+
+ ControlDataStruct&
+ getWriterControlStruct()
{
return controlDataTripleBuffer.getWriteBuffer();
}
- void setControlStruct(const ControlDataStruct& newStruct)
+ void
+ setControlStruct(const ControlDataStruct& newStruct)
{
LockGuardType lock{controlDataMutex};
getWriterControlStruct() = newStruct;
writeControlStruct();
}
- void reinitTripleBuffer(const ControlDataStruct& initial)
+ void
+ reinitTripleBuffer(const ControlDataStruct& initial)
{
controlDataTripleBuffer.reinitAllBuffers(initial);
}
@@ -70,6 +78,6 @@ namespace armarx
template <typename ControlDataStruct>
using NJointControllerWithTripleBufferPtr =
- IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
+ IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 074efbc511936171b36d34acd0adbac87a1dde8a..8ce9b397c9147d041290f5f8f73c203d6b8cb326 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -22,33 +22,43 @@
* GNU General Public License
*/
#include "NJointHolonomicPlatformGlobalPositionController.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include "RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <cmath>
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/periodic/periodic_clamp.h>
-#include <cmath>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
- registrationNJointHolonomicPlatformGlobalPositionController("NJointHolonomicPlatformGlobalPositionController");
-
-
- NJointHolonomicPlatformGlobalPositionController::NJointHolonomicPlatformGlobalPositionController(
- RobotUnit*,
- const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
- const VirtualRobot::RobotPtr&) :
+ registrationNJointHolonomicPlatformGlobalPositionController(
+ "NJointHolonomicPlatformGlobalPositionController");
+
+ NJointHolonomicPlatformGlobalPositionController::
+ NJointHolonomicPlatformGlobalPositionController(
+ RobotUnit*,
+ const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&) :
pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
- opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration, true)
+ opid(cfg->p_rot,
+ cfg->i_rot,
+ cfg->d_rot,
+ cfg->maxRotationVelocity,
+ cfg->maxRotationAcceleration,
+ true)
{
- const SensorValueBase* sv = useSensorValue(GlobalRobotLocalizationSensorDevice::DeviceName());
+ const SensorValueBase* sv =
+ useSensorValue(GlobalRobotLocalizationSensorDevice::DeviceName());
this->sv = sv->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
- target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
+ ->asA<ControlTargetHolonomicPlatformVelocity>();
pid.threadSafe = false;
@@ -57,13 +67,16 @@ namespace armarx
opid.threadSafe = false;
}
- void NJointHolonomicPlatformGlobalPositionController::rtRun(const IceUtil::Time& currentTime, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointHolonomicPlatformGlobalPositionController::rtRun(
+ const IceUtil::Time& currentTime,
+ const IceUtil::Time& timeSinceLastIteration)
{
const auto global_T_robot = sv->global_T_root;
const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(global_T_robot);
const float global_orientation = rpy.z();
- const Eigen::Vector2f global_P_robot = global_T_robot.block<2,1>(0,3);
+ const Eigen::Vector2f global_P_robot = global_T_robot.block<2, 1>(0, 3);
if (rtGetControlStruct().newTargetSet)
{
@@ -72,7 +85,7 @@ namespace armarx
getWriterControlStruct().newTargetSet = false;
writeControlStruct();
-
+
isTargetSet = true;
}
@@ -99,7 +112,7 @@ namespace armarx
return;
}
- if(not sv->isAvailable())
+ if (not sv->isAvailable())
{
// ARMARX_RT_LOGF_INFO << deactivateSpam(1) << "global pose not available";
target->velocityX = 0;
@@ -111,8 +124,11 @@ namespace armarx
const float measuredOrientation = global_orientation;
- pid.update(timeSinceLastIteration.toSecondsDouble(), global_P_robot, rtGetControlStruct().target);
- opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
+ pid.update(
+ timeSinceLastIteration.toSecondsDouble(), global_P_robot, rtGetControlStruct().target);
+ opid.update(timeSinceLastIteration.toSecondsDouble(),
+ static_cast<double>(measuredOrientation),
+ rtGetControlStruct().targetOrientation);
const Eigen::Rotation2Df global_R_local(-measuredOrientation);
const Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
@@ -122,25 +138,32 @@ namespace armarx
target->velocityRotation = static_cast<float>(opid.getControlValue());
}
- void NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
+ void
+ NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
{
target->velocityX = 0;
target->velocityY = 0;
target->velocityRotation = 0;
}
- void NJointHolonomicPlatformGlobalPositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
+ void
+ NJointHolonomicPlatformGlobalPositionController::setTarget(float x,
+ float y,
+ float yaw,
+ float translationAccuracy,
+ float rotationAccuracy)
{
// todo do we really need a recursive mutex?
std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
getWriterControlStruct().target << x, y;
- getWriterControlStruct().targetOrientation = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
+ getWriterControlStruct().targetOrientation =
+ simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
getWriterControlStruct().translationAccuracy = translationAccuracy;
getWriterControlStruct().rotationAccuracy = rotationAccuracy;
-
+
getWriterControlStruct().newTargetSet = true;
writeControlStruct();
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index 37a0de6feb1720dc702b1b5e5dd356b36e974399..4e07bdb3e790f36875e2b080b1a1470b461eaa8b 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -30,22 +30,23 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "NJointControllerWithTripleBuffer.h"
-#include "../SensorValues/SensorValueHolonomicPlatform.h"
-
-#include "../ControlTargets/ControlTarget1DoFActuator.h"
#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
-#include <RobotAPI/libraries/core/PIDController.h>
-
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "../SensorValues/SensorValueHolonomicPlatform.h"
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformGlobalPositionControllerConfig);
- class NJointHolonomicPlatformGlobalPositionControllerConfig : virtual public NJointControllerConfig
+
+ class NJointHolonomicPlatformGlobalPositionControllerConfig :
+ virtual public NJointControllerConfig
{
public:
std::string platformName;
@@ -61,7 +62,6 @@ namespace armarx
float maxRotationVelocity = 1.0;
float maxRotationAcceleration = 0.5;
-
};
struct NJointHolonomicPlatformGlobalPositionControllerTarget
@@ -87,8 +87,9 @@ namespace armarx
* @brief The NJointHolonomicPlatformGlobalPositionController class
* @ingroup Library-RobotUnit-NJointControllers
*/
- class NJointHolonomicPlatformGlobalPositionController:
- virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformGlobalPositionControllerTarget>
+ class NJointHolonomicPlatformGlobalPositionController :
+ virtual public NJointControllerWithTripleBuffer<
+ NJointHolonomicPlatformGlobalPositionControllerTarget>
{
public:
using ConfigPtrT = NJointHolonomicPlatformGlobalPositionControllerConfigPtr;
@@ -98,25 +99,27 @@ namespace armarx
const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&);
- inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration) override;
+ inline virtual void rtRun(const IceUtil::Time&,
+ const IceUtil::Time& timeSinceLastIteration) override;
inline virtual void rtPreActivateController() override;
//ice interface
- inline virtual std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ inline virtual std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
{
return "NJointHolonomicPlatformGlobalPositionController";
}
- void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
+ void
+ setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
protected:
const GlobalRobotLocalizationSensorDevice::SensorValueType* sv;
MultiDimPIDController pid;
- PIDController opid;
+ PIDController opid;
ControlTargetHolonomicPlatformVelocity* target;
bool isTargetSet = false;
-
};
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index 40d37c7ae18892dfe18697ada68dcde1297e79ab..cca564afe0ad449f947c3e5be351f9b7ce7952ba 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -23,26 +23,30 @@
*/
#include "NJointHolonomicPlatformRelativePositionController.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include <Eigen/Geometry>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformRelativePositionController>
- registrationNJointHolonomicPlatformRelativePositionController("NJointHolonomicPlatformRelativePositionController");
-
-
- NJointHolonomicPlatformRelativePositionController::NJointHolonomicPlatformRelativePositionController(
- RobotUnit*,
- const NJointHolonomicPlatformRelativePositionControllerConfigPtr& cfg,
- const VirtualRobot::RobotPtr&) :
+ registrationNJointHolonomicPlatformRelativePositionController(
+ "NJointHolonomicPlatformRelativePositionController");
+
+ NJointHolonomicPlatformRelativePositionController::
+ NJointHolonomicPlatformRelativePositionController(
+ RobotUnit*,
+ const NJointHolonomicPlatformRelativePositionControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&) :
pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)
{
const SensorValueBase* sv = useSensorValue(cfg->platformName);
this->sv = sv->asA<SensorValueHolonomicPlatform>();
- target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
- ARMARX_CHECK_EXPRESSION(target) << "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity;
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
+ ->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION(target)
+ << "The actuator '" << cfg->platformName << "' has no control mode "
+ << ControlModes::HolonomicPlatformVelocity;
pid.threadSafe = false;
@@ -56,7 +60,10 @@ namespace armarx
pid.preallocate(2);
}
- void NJointHolonomicPlatformRelativePositionController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointHolonomicPlatformRelativePositionController::rtRun(
+ const IceUtil::Time&,
+ const IceUtil::Time& timeSinceLastIteration)
{
currentPosition << sv->relativePositionX, sv->relativePositionY;
currentOrientation = sv->relativePositionRotation;
@@ -71,12 +78,16 @@ namespace armarx
//position pid
Eigen::Vector2f relativeCurrentPosition = currentPosition - startPosition;
- pid.update(timeSinceLastIteration.toSecondsDouble(), relativeCurrentPosition, rtGetControlStruct().target);
+ pid.update(timeSinceLastIteration.toSecondsDouble(),
+ relativeCurrentPosition,
+ rtGetControlStruct().target);
float relativeOrientation = currentOrientation - startOrientation;
//rotation pid
// Revert the rotation by rotating by the negative angle
- Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-currentOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ Eigen::Vector2f localTargetVelocity =
+ Eigen::Rotation2Df(-currentOrientation) *
+ Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
//ARMARX_RT_LOGF_INFO("global target vel x: %.2f y: %2.f, local target vel x: %.2f y: %2.f rotation: %2.f", pid.getControlValue()[0], pid.getControlValue()[1], localTargetVelocity[0], localTargetVelocity[1], sv->relativePositionRotation).deactivateSpam(0.1);
target->velocityX = localTargetVelocity[0];
@@ -105,16 +116,22 @@ namespace armarx
// ARMARX_RT_LOGF_INFO("new target vel: %2.f, %2.f current vel: %2.f, %2.f", target->velocityX, target->velocityY, sv->velocityX, sv->velocityY).deactivateSpam(0.1);
}
- void NJointHolonomicPlatformRelativePositionController::rtPreActivateController()
+ void
+ NJointHolonomicPlatformRelativePositionController::rtPreActivateController()
{
startPosition[0] = sv->relativePositionX;
startPosition[1] = sv->relativePositionY;
startOrientation = sv->relativePositionRotation;
}
- void NJointHolonomicPlatformRelativePositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
+ void
+ NJointHolonomicPlatformRelativePositionController::setTarget(float x,
+ float y,
+ float yaw,
+ float translationAccuracy,
+ float rotationAccuracy)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().target << x, y;
getWriterControlStruct().targetOrientation = yaw;
getWriterControlStruct().translationAccuracy = translationAccuracy;
@@ -126,4 +143,3 @@ namespace armarx
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index 788a45607361a8fd3f8a36aeb54cbf9042416236..ad5b035266524a4079f84811ad8677279019f856 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -30,20 +30,21 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "NJointControllerWithTripleBuffer.h"
-#include "../SensorValues/SensorValueHolonomicPlatform.h"
-
-#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "../SensorValues/SensorValueHolonomicPlatform.h"
+#include "NJointControllerWithTripleBuffer.h"
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformRelativePositionControllerConfig);
- class NJointHolonomicPlatformRelativePositionControllerConfig : virtual public NJointControllerConfig
+
+ class NJointHolonomicPlatformRelativePositionControllerConfig :
+ virtual public NJointControllerConfig
{
public:
std::string platformName;
@@ -72,8 +73,9 @@ namespace armarx
* @brief The NJointHolonomicPlatformRelativePositionController class
* @ingroup Library-RobotUnit-NJointControllers
*/
- class NJointHolonomicPlatformRelativePositionController:
- virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformRelativePositionControllerTarget>
+ class NJointHolonomicPlatformRelativePositionController :
+ virtual public NJointControllerWithTripleBuffer<
+ NJointHolonomicPlatformRelativePositionControllerTarget>
{
public:
using ConfigPtrT = NJointHolonomicPlatformRelativePositionControllerConfigPtr;
@@ -83,16 +85,19 @@ namespace armarx
const NJointHolonomicPlatformRelativePositionControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&);
- inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration) override;
+ inline virtual void rtRun(const IceUtil::Time&,
+ const IceUtil::Time& timeSinceLastIteration) override;
inline virtual void rtPreActivateController() override;
//ice interface
- inline virtual std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ inline virtual std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
{
return "NJointHolonomicPlatformRelativePositionController";
}
- void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
+ void
+ setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
protected:
@@ -106,5 +111,3 @@ namespace armarx
// float rad2MMFactor;
};
} // namespace armarx
-
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index fa4b4ad337ed17d35b464a6485ecf45c81f63960..bd440c1b0b124795cbe519299bfca7931a64d3bf 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -22,65 +22,83 @@
#include "NJointHolonomicPlatformUnitVelocityPassThroughController.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
namespace armarx
{
- NJointHolonomicPlatformUnitVelocityPassThroughController::NJointHolonomicPlatformUnitVelocityPassThroughController(
- RobotUnit* prov,
- NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr
- cfg, const VirtualRobot::RobotPtr&) :
+ NJointHolonomicPlatformUnitVelocityPassThroughController::
+ NJointHolonomicPlatformUnitVelocityPassThroughController(
+ RobotUnit* prov,
+ NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr cfg,
+ const VirtualRobot::RobotPtr&) :
maxCommandDelay(IceUtil::Time::milliSeconds(500))
{
- target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
- ARMARX_CHECK_EXPRESSION(target) << "The actuator " << cfg->platformName << " has no control mode " << ControlModes::HolonomicPlatformVelocity;
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
+ ->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION(target)
+ << "The actuator " << cfg->platformName << " has no control mode "
+ << ControlModes::HolonomicPlatformVelocity;
- initialSettings.velocityX = cfg->initialVelocityX;
- initialSettings.velocityY = cfg->initialVelocityY;
+ initialSettings.velocityX = cfg->initialVelocityX;
+ initialSettings.velocityY = cfg->initialVelocityY;
initialSettings.velocityRotation = cfg->initialVelocityRotation;
reinitTripleBuffer(initialSettings);
}
- void NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time&)
+ void
+ NJointHolonomicPlatformUnitVelocityPassThroughController::rtRun(
+ const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time&)
{
auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
- if (commandAge > maxCommandDelay && // command must be recent
- (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f || rtGetControlStruct().velocityRotation != 0.0f)) // only throw error if any command is not zero
+ if (commandAge > maxCommandDelay && // command must be recent
+ (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
+ rtGetControlStruct().velocityRotation !=
+ 0.0f)) // only throw error if any command is not zero
{
- throw LocalException("Platform target velocity was not set for a too long time: delay: ") << commandAge.toSecondsDouble() << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
+ throw LocalException(
+ "Platform target velocity was not set for a too long time: delay: ")
+ << commandAge.toSecondsDouble()
+ << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
}
else
{
- target->velocityX = rtGetControlStruct().velocityX;
- target->velocityY = rtGetControlStruct().velocityY;
+ target->velocityX = rtGetControlStruct().velocityX;
+ target->velocityY = rtGetControlStruct().velocityY;
target->velocityRotation = rtGetControlStruct().velocityRotation;
}
}
- void NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX, float velocityY,
- float velocityRotation)
+ void
+ NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX,
+ float velocityY,
+ float velocityRotation)
{
- LockGuardType guard {controlDataMutex};
- getWriterControlStruct().velocityX = velocityX;
- getWriterControlStruct().velocityY = velocityY;
+ LockGuardType guard{controlDataMutex};
+ getWriterControlStruct().velocityX = velocityX;
+ getWriterControlStruct().velocityY = velocityY;
getWriterControlStruct().velocityRotation = velocityRotation;
getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
writeControlStruct();
}
- IceUtil::Time NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
+ IceUtil::Time
+ NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
{
return maxCommandDelay;
}
- void NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(const IceUtil::Time& value)
+ void
+ NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(
+ const IceUtil::Time& value)
{
maxCommandDelay = value;
}
NJointControllerRegistration<NJointHolonomicPlatformUnitVelocityPassThroughController>
- registrationNJointHolonomicPlatformUnitVelocityPassThroughController("NJointHolonomicPlatformUnitVelocityPassThroughController");
-}
+ registrationNJointHolonomicPlatformUnitVelocityPassThroughController(
+ "NJointHolonomicPlatformUnitVelocityPassThroughController");
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index e2c4a3aa4146a573c7fe4e92239272fd02d0372c..37df1f055d7daeb61630e62698585e484ed6cc75 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -24,15 +24,17 @@
#include <VirtualRobot/Robot.h>
-#include "NJointControllerWithTripleBuffer.h"
#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
#include "../util.h"
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig);
- class NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig: virtual public NJointControllerConfig
+
+ class NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig :
+ virtual public NJointControllerConfig
{
public:
std::string platformName;
@@ -42,6 +44,7 @@ namespace armarx
};
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
+
class NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData
{
public:
@@ -52,12 +55,14 @@ namespace armarx
};
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
+
/**
* @brief The NJointHolonomicPlatformUnitVelocityPassThroughController class
* @ingroup Library-RobotUnit-NJointControllers
*/
- class NJointHolonomicPlatformUnitVelocityPassThroughController:
- virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData>
+ class NJointHolonomicPlatformUnitVelocityPassThroughController :
+ virtual public NJointControllerWithTripleBuffer<
+ NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData>
{
public:
using ConfigPtrT = NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr;
@@ -73,10 +78,12 @@ namespace armarx
void setVelocites(float velocityX, float velocityY, float velocityRotation);
//ice interface
- std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
{
return "NJointHolonomicPlatformUnitVelocityPassThroughController";
}
+
IceUtil::Time getMaxCommandDelay() const;
void setMaxCommandDelay(const IceUtil::Time& value);
@@ -86,4 +93,4 @@ namespace armarx
ControlTargetHolonomicPlatformVelocity* target;
NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData initialSettings;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
index 0d8b30e6b30fc0ab29a6183de6f55a73e01b30c3..e51de4dbdb2c7b903d83aeb462d9b9fd8f6a695d 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
@@ -26,7 +26,9 @@
namespace armarx
{
- NJointControllerRegistration<NJointKinematicUnitPassThroughController> registrationControllerNJointKinematicUnitPassThroughController("NJointKinematicUnitPassThroughController");
+ NJointControllerRegistration<NJointKinematicUnitPassThroughController>
+ registrationControllerNJointKinematicUnitPassThroughController(
+ "NJointKinematicUnitPassThroughController");
NJointKinematicUnitPassThroughController::NJointKinematicUnitPassThroughController(
RobotUnit* prov,
@@ -72,7 +74,7 @@ namespace armarx
}
else
{
- throw InvalidArgumentException {"Unsupported control mode: " + cfg->controlMode};
+ throw InvalidArgumentException{"Unsupported control mode: " + cfg->controlMode};
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index cbbb83f65750f6aa9e9d1de7d0f7c2e1abe3fdf4..ab280b2245ff23cecbb5062969168dca312abc2f 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -28,14 +28,14 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "NJointControllerBase.h"
-#include "../SensorValues/SensorValue1DoFActuator.h"
-
#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
+#include "NJointControllerBase.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(NJointKinematicUnitPassThroughControllerConfig);
+
class NJointKinematicUnitPassThroughControllerConfig : virtual public NJointControllerConfig
{
public:
@@ -43,14 +43,13 @@ namespace armarx
std::string controlMode;
};
-
TYPEDEF_PTRS_HANDLE(NJointKinematicUnitPassThroughController);
+
/**
* @brief The NJointKinematicUnitPassThroughController class
* @ingroup Library-RobotUnit-NJointControllers
*/
- class NJointKinematicUnitPassThroughController:
- virtual public NJointController
+ class NJointKinematicUnitPassThroughController : virtual public NJointController
{
public:
using ConfigPtrT = NJointKinematicUnitPassThroughControllerConfigPtr;
@@ -60,16 +59,19 @@ namespace armarx
const NJointKinematicUnitPassThroughControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&);
- inline void rtRun(const IceUtil::Time&, const IceUtil::Time&) override
+ inline void
+ rtRun(const IceUtil::Time&, const IceUtil::Time&) override
{
target.setValue(control);
}
- inline void rtPreActivateController() override
- {
+ inline void
+ rtPreActivateController() override
+ {
}
- void reset()
+ void
+ reset()
{
control = (sensor.getValue()) * sensorToControlOnActivateFactor;
if (std::abs(control) < resetZeroThreshold)
@@ -79,23 +81,26 @@ namespace armarx
}
//ice interface
- inline std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ inline std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
{
return "NJointKinematicUnitPassThroughController";
}
- void set(float val)
+ void
+ set(float val)
{
control = val;
}
protected:
- std::atomic<float> control {0};
+ std::atomic<float> control{0};
- template<bool ConstPtr>
+ template <bool ConstPtr>
struct ptr_wrapper
{
- float getValue() const
+ float
+ getValue() const
{
if (_float)
{
@@ -104,8 +109,9 @@ namespace armarx
return *_int16;
}
- template < bool constPtr = ConstPtr, std::enable_if_t < !constPtr, int > = 0 >
- void setValue(float val)
+ template <bool constPtr = ConstPtr, std::enable_if_t<!constPtr, int> = 0>
+ void
+ setValue(float val)
{
if (_float)
{
@@ -116,15 +122,15 @@ namespace armarx
*_int16 = static_cast<std::int16_t>(val);
}
}
- template<class T>
+ template <class T>
using maybe_const_ptr = std::conditional_t<ConstPtr, const T*, T*>;
- maybe_const_ptr<float> _float{nullptr};
+ maybe_const_ptr<float> _float{nullptr};
maybe_const_ptr<std::int16_t> _int16{nullptr};
};
ptr_wrapper<false> target;
ptr_wrapper<true> sensor;
- float sensorToControlOnActivateFactor {0};
- float resetZeroThreshold {0};
+ float sensorToControlOnActivateFactor{0};
+ float resetZeroThreshold{0};
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
index fcedf27a6a87d533ce5cd67a23cd7c9d36e7774b..12ca8b5a1ec6146599034643989c0a668704d888 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -22,11 +22,12 @@
* GNU General Public License
*/
#include "NJointTCPController.h"
-#include "../RobotUnit.h"
+
+#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include "../RobotUnit.h"
#define DEFAULT_TCP_STRING "default TCP"
@@ -34,19 +35,24 @@
namespace armarx
{
- NJointControllerRegistration<NJointTCPController> registrationControllerNJointTCPController("NJointTCPController");
+ NJointControllerRegistration<NJointTCPController>
+ registrationControllerNJointTCPController("NJointTCPController");
- std::string NJointTCPController::getClassName(const Ice::Current&) const
+ std::string
+ NJointTCPController::getClassName(const Ice::Current&) const
{
return "NJointTCPController";
}
- void NJointTCPController::rtPreActivateController()
+ void
+ NJointTCPController::rtPreActivateController()
{
reinitTripleBuffer(NJointTCPControllerControlData());
}
- void NJointTCPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointTCPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
auto mode = rtGetControlStruct().mode;
@@ -54,7 +60,9 @@ namespace armarx
if (mode == VirtualRobot::IKSolver::All)
{
x.resize(6);
- x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel, rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel,
+ rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else if (mode == VirtualRobot::IKSolver::Position)
{
@@ -64,7 +72,8 @@ namespace armarx
else if (mode == VirtualRobot::IKSolver::Orientation)
{
x.resize(3);
- x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel,
+ rtGetControlStruct().yawVel;
}
else
{
@@ -81,12 +90,12 @@ namespace armarx
}
}
-
- ::armarx::WidgetDescription::WidgetSeq NJointTCPController::createSliders()
+ ::armarx::WidgetDescription::WidgetSeq
+ NJointTCPController::createSliders()
{
using namespace armarx::WidgetDescription;
::armarx::WidgetDescription::WidgetSeq widgets;
- auto addSlider = [&](const std::string & label, float limit)
+ auto addSlider = [&](const std::string& label, float limit)
{
widgets.emplace_back(new Label(false, label));
{
@@ -108,7 +117,11 @@ namespace armarx
return widgets;
}
- WidgetDescription::WidgetPtr NJointTCPController::GenerateConfigDescription(const VirtualRobot::RobotPtr& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
+ WidgetDescription::WidgetPtr
+ NJointTCPController::GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>&)
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
@@ -153,12 +166,16 @@ namespace armarx
return layout;
}
- NJointTCPControllerConfigPtr NJointTCPController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ NJointTCPControllerConfigPtr
+ NJointTCPController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
{
- return new NJointTCPControllerConfig {values.at("nodeSetName")->getString(), values.at("tcpName")->getString()};
+ return new NJointTCPControllerConfig{values.at("nodeSetName")->getString(),
+ values.at("tcpName")->getString()};
}
- NJointTCPController::NJointTCPController(RobotUnit* robotUnit, const NJointTCPControllerConfigPtr& config, const VirtualRobot::RobotPtr& r)
+ NJointTCPController::NJointTCPController(RobotUnit* robotUnit,
+ const NJointTCPControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& r)
{
ARMARX_CHECK_EXPRESSION(robotUnit);
ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
@@ -173,16 +190,26 @@ namespace armarx
targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
sensors.push_back(sv->asA<SensorValue1DoFActuatorPosition>());
};
- ik.reset(new VirtualRobot::DifferentialIK(nodeset, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
- tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? nodeset->getTCP() : rtGetRobot()->getRobotNode(config->tcpName);
+ ik.reset(new VirtualRobot::DifferentialIK(
+ nodeset, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING)
+ ? nodeset->getTCP()
+ : rtGetRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName;
nodeSetName = config->nodeSetName;
}
- void NJointTCPController::setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode)
+ void
+ NJointTCPController::setVelocities(float xVel,
+ float yVel,
+ float zVel,
+ float rollVel,
+ float pitchVel,
+ float yawVel,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = xVel;
getWriterControlStruct().yVel = yVel;
getWriterControlStruct().zVel = zVel;
@@ -193,32 +220,35 @@ namespace armarx
writeControlStruct();
}
- std::string NJointTCPController::getNodeSetName() const
+ std::string
+ NJointTCPController::getNodeSetName() const
{
return nodeSetName;
}
- void NJointTCPController::rtPostDeactivateController()
+ void
+ NJointTCPController::rtPostDeactivateController()
{
-
}
- WidgetDescription::StringWidgetDictionary NJointTCPController::getFunctionDescriptions(const Ice::Current&) const
+ WidgetDescription::StringWidgetDictionary
+ NJointTCPController::getFunctionDescriptions(const Ice::Current&) const
{
using namespace armarx::WidgetDescription;
HBoxLayoutPtr layout = new HBoxLayout;
auto sliders = createSliders();
- layout->children.insert(layout->children.end(),
- sliders.begin(),
- sliders.end());
+ layout->children.insert(layout->children.end(), sliders.begin(), sliders.end());
return {{"ControllerTarget", layout}};
}
- void NJointTCPController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ void
+ NJointTCPController::callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&)
{
if (name == "ControllerTarget")
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().xVel = valueMap.at("x")->getFloat();
getWriterControlStruct().yVel = valueMap.at("y")->getFloat();
getWriterControlStruct().zVel = valueMap.at("z")->getFloat();
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
index 41a93a0c368288536ca2eca179b8532fe1d6e01a..ecf1fc2666dded6b34c9f46850d1b00f7b0c09de 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -24,11 +24,12 @@
#pragma once
-#include "NJointControllerWithTripleBuffer.h"
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
+
#include "../ControlTargets/ControlTarget1DoFActuator.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
-#include <VirtualRobot/IK/DifferentialIK.h>
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
@@ -39,16 +40,17 @@ namespace armarx
class NJointTCPControllerConfig : virtual public NJointControllerConfig
{
public:
- NJointTCPControllerConfig(std::string const& nodeSetName, const std::string& tcpName):
- nodeSetName(nodeSetName),
- tcpName(tcpName)
- {}
+ NJointTCPControllerConfig(std::string const& nodeSetName, const std::string& tcpName) :
+ nodeSetName(nodeSetName), tcpName(tcpName)
+ {
+ }
const std::string nodeSetName;
const std::string tcpName;
};
TYPEDEF_PTRS_HANDLE(NJointTCPControllerControlData);
+
class NJointTCPControllerControlData
{
public:
@@ -61,7 +63,6 @@ namespace armarx
VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
};
-
/**
* @brief The NJointTCPController class
* @ingroup Library-RobotUnit-NJointControllers
@@ -71,29 +72,44 @@ namespace armarx
{
public:
using ConfigPtrT = NJointTCPControllerConfigPtr;
- NJointTCPController(RobotUnit* prov, const NJointTCPControllerConfigPtr& config, const VirtualRobot::RobotPtr& r);
+ NJointTCPController(RobotUnit* prov,
+ const NJointTCPControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& r);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
- WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
- void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+ WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name,
+ const StringVariantBaseMap& valueMap,
+ const Ice::Current&) override;
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
- static WidgetDescription::WidgetPtr GenerateConfigDescription(
- const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&);
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+ static WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
- static NJointTCPControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ static NJointTCPControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& values);
// for TCPControlUnit
- void setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode);
+ void setVelocities(float xVel,
+ float yVel,
+ float zVel,
+ float rollVel,
+ float pitchVel,
+ float yawVel,
+ VirtualRobot::IKSolver::CartesianSelection mode);
std::string getNodeSetName() const;
static ::armarx::WidgetDescription::WidgetSeq createSliders();
+
protected:
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
private:
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
std::vector<const SensorValue1DoFActuatorPosition*> sensors;
@@ -105,4 +121,3 @@ namespace armarx
};
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index 124d66adbc8b24f176cdb7d6acf5df4d3815eccc..ee59658b4c78c60e3187d4e4a4c1db21a0bf25d7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -22,19 +22,24 @@
#include "NJointTaskSpaceImpedanceController.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
using namespace armarx;
-NJointControllerRegistration<NJointTaskSpaceImpedanceController> registrationControllerDSRTController("NJointTaskSpaceImpedanceController");
+NJointControllerRegistration<NJointTaskSpaceImpedanceController>
+ registrationControllerDSRTController("NJointTaskSpaceImpedanceController");
-NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(
+ const RobotUnitPtr& robotUnit,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&)
{
- NJointTaskSpaceImpedanceControlConfigPtr cfg = NJointTaskSpaceImpedanceControlConfigPtr::dynamicCast(config);
+ NJointTaskSpaceImpedanceControlConfigPtr cfg =
+ NJointTaskSpaceImpedanceControlConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(cfg);
ARMARX_CHECK_EXPRESSION(robotUnit);
@@ -56,9 +61,12 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob
ARMARX_CHECK_EXPRESSION(casted_ct);
targets.push_back(casted_ct);
- const SensorValue1DoFActuatorTorque* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
- const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
- const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
+ const SensorValue1DoFActuatorTorque* torqueSensor =
+ sv->asA<SensorValue1DoFActuatorTorque>();
+ const SensorValue1DoFActuatorVelocity* velocitySensor =
+ sv->asA<SensorValue1DoFActuatorVelocity>();
+ const SensorValue1DoFActuatorPosition* positionSensor =
+ sv->asA<SensorValue1DoFActuatorPosition>();
if (!torqueSensor)
{
ARMARX_WARNING << "No Torque sensor available for " << jointName;
@@ -89,15 +97,13 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob
reinitTripleBuffer(initData);
}
-void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/, const IceUtil::Time& /*timeSinceLastIteration*/)
+void
+NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
{
rtUpdateControlStruct();
- const auto& jointDesiredTorques = controller.run(
- rtGetControlStruct(),
- torqueSensors,
- velocitySensors,
- positionSensors
- );
+ const auto& jointDesiredTorques =
+ controller.run(rtGetControlStruct(), torqueSensors, velocitySensors, positionSensors);
ARMARX_CHECK_EQUAL(targets.size(), static_cast<size_t>(jointDesiredTorques.size()));
for (size_t i = 0; i < targets.size(); ++i)
@@ -118,10 +124,12 @@ void NJointTaskSpaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValu
debugDataInfo.getWriteBuffer().quatError = controller.dbg.errorAngle;
debugDataInfo.getWriteBuffer().posiError = controller.dbg.posiError;
debugDataInfo.commitWrite();
-
}
-void NJointTaskSpaceImpedanceController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
+void
+NJointTaskSpaceImpedanceController::onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx& debugObs)
{
StringVariantBaseMap datafields;
auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
@@ -130,55 +138,69 @@ void NJointTaskSpaceImpedanceController::onPublish(const SensorAndControl&, cons
datafields[pair.first] = new Variant(pair.second);
}
- datafields["desiredForce_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
- datafields["desiredForce_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
- datafields["desiredForce_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
+ datafields["desiredForce_x"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_x);
+ datafields["desiredForce_y"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_y);
+ datafields["desiredForce_z"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().desiredForce_z);
- datafields["tcpDesiredTorque_x"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
- datafields["tcpDesiredTorque_y"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
- datafields["tcpDesiredTorque_z"] = new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
+ datafields["tcpDesiredTorque_x"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_x);
+ datafields["tcpDesiredTorque_y"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_y);
+ datafields["tcpDesiredTorque_z"] =
+ new Variant(debugDataInfo.getUpToDateReadBuffer().tcpDesiredTorque_z);
datafields["quatError"] = new Variant(debugDataInfo.getUpToDateReadBuffer().quatError);
datafields["posiError"] = new Variant(debugDataInfo.getUpToDateReadBuffer().posiError);
debugObs->setDebugChannel("DSControllerOutput", datafields);
-
-
}
-void NJointTaskSpaceImpedanceController::setPosition(const Eigen::Vector3f& target, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setPosition(const Eigen::Vector3f& target, const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredPosition = target;
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setOrientation(const Eigen::Quaternionf& target, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setOrientation(const Eigen::Quaternionf& target,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredOrientation = target;
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setPositionOrientation(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setPositionOrientation(const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredPosition = pos;
getWriterControlStruct().desiredOrientation = ori;
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setPose(const Eigen::Matrix4f& mat, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setPose(const Eigen::Matrix4f& mat, const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
getWriterControlStruct().desiredPosition = simox::math::mat4f_to_pos(mat);
getWriterControlStruct().desiredOrientation = simox::math::mat4f_to_quat(mat);
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::string& name, const Ice::FloatSeq& vals, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::string& name,
+ const Ice::FloatSeq& vals,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
if (name == "Kpos")
{
ARMARX_CHECK_EQUAL(vals.size(), 3);
@@ -250,12 +272,13 @@ void NJointTaskSpaceImpedanceController::setImpedanceParameters(const std::strin
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setNullspaceConfig(
- const Eigen::VectorXf& joint,
- const Eigen::VectorXf& knull,
- const Eigen::VectorXf& dnull, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setNullspaceConfig(const Eigen::VectorXf& joint,
+ const Eigen::VectorXf& knull,
+ const Eigen::VectorXf& dnull,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(joint.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(knull.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(dnull.size()), targets.size());
@@ -265,9 +288,12 @@ void NJointTaskSpaceImpedanceController::setNullspaceConfig(
writeControlStruct();
}
-void NJointTaskSpaceImpedanceController::setConfig(const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg, const Ice::Current&)
+void
+NJointTaskSpaceImpedanceController::setConfig(
+ const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg,
+ const Ice::Current&)
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.Knull.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.Dnull.size()), targets.size());
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(cfg.desiredJointPositions.size()), targets.size());
@@ -285,7 +311,7 @@ void NJointTaskSpaceImpedanceController::setConfig(const NJointTaskSpaceImpedanc
writeControlStruct();
}
-void armarx::NJointTaskSpaceImpedanceController::rtPreActivateController()
+void
+armarx::NJointTaskSpaceImpedanceController::rtPreActivateController()
{
-
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
index 06d725f856583029d4c99a781b4832a3ee4f3c80..42ce54179acab6ab646191155c259a8a05f1e38a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -22,13 +22,13 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-
#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
#include "../ControllerParts/CartesianImpedanceController.h"
@@ -53,28 +53,41 @@ namespace armarx
public:
using ConfigPtrT = NJointTaskSpaceImpedanceControlConfigPtr;
- NJointTaskSpaceImpedanceController(const RobotUnitPtr& robotUnit, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointTaskSpaceImpedanceController(const RobotUnitPtr& robotUnit,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
-
- std::string getClassName(const Ice::Current&) const override
+ std::string
+ getClassName(const Ice::Current&) const override
{
return "TaskSpaceImpedanceController";
}
// NJointController interface
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
protected:
- void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+ void onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx&) override;
void setPosition(const Eigen::Vector3f&, const Ice::Current&) override;
void setOrientation(const Eigen::Quaternionf&, const Ice::Current&) override;
- void setPositionOrientation(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const Ice::Current&) override;
+ void setPositionOrientation(const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
+ const Ice::Current&) override;
void setPose(const Eigen::Matrix4f& mat, const Ice::Current&) override;
- void setImpedanceParameters(const std::string&, const Ice::FloatSeq&, const Ice::Current&) override;
- void setNullspaceConfig(const Eigen::VectorXf& joint, const Eigen::VectorXf& knull, const Eigen::VectorXf& dnull, const Ice::Current&) override;
- void setConfig(const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg, const Ice::Current&) override;
+ void setImpedanceParameters(const std::string&,
+ const Ice::FloatSeq&,
+ const Ice::Current&) override;
+ void setNullspaceConfig(const Eigen::VectorXf& joint,
+ const Eigen::VectorXf& knull,
+ const Eigen::VectorXf& dnull,
+ const Ice::Current&) override;
+ void setConfig(const NJointTaskSpaceImpedanceControlRuntimeConfig& cfg,
+ const Ice::Current&) override;
private:
std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors;
@@ -108,6 +121,7 @@ namespace armarx
float quatError;
float posiError;
};
+
TripleBuffer<NJointTaskSpaceImpedanceControllerDebugInfo> debugDataInfo;
std::vector<std::string> jointNames;
@@ -118,4 +132,4 @@ namespace armarx
protected:
void rtPreActivateController() override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index 3b270bd0516b1e00dedfe3c6024c16ecbd6948bc..4e79acb8ff78a77e739ac8b2ee867ca8b64df701 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -1,19 +1,22 @@
#include "NJointTrajectoryController.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
namespace armarx
{
- NJointControllerRegistration<NJointTrajectoryController> registrationControllerNJointTrajectoryController("NJointTrajectoryController");
+ NJointControllerRegistration<NJointTrajectoryController>
+ registrationControllerNJointTrajectoryController("NJointTrajectoryController");
- NJointTrajectoryController::NJointTrajectoryController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
+ NJointTrajectoryController::NJointTrajectoryController(RobotUnit* prov,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& robot)
{
cfg = NJointTrajectoryControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(cfg) << "Needed type: NJointTrajectoryControllerConfigPtr";
@@ -47,27 +50,30 @@ namespace armarx
NJointTrajectoryController::~NJointTrajectoryController()
{
-
}
- std::string NJointTrajectoryController::getClassName(const Ice::Current&) const
+ std::string
+ NJointTrajectoryController::getClassName(const Ice::Current&) const
{
return "NJointTrajectoryController";
}
- void NJointTrajectoryController::onInitNJointController()
+ void
+ NJointTrajectoryController::onInitNJointController()
{
offeringTopic("StaticPlotter");
}
- void NJointTrajectoryController::onConnectNJointController()
+ void
+ NJointTrajectoryController::onConnectNJointController()
{
plotter = getTopic<StaticPlotterInterfacePrx>("StaticPlotter");
dbgObs = getTopic<DebugObserverInterfacePrx>("DebugObserver");
}
-
- void NJointTrajectoryController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ NJointTrajectoryController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (rtGetControlStruct().trajectoryCtrl)
{
@@ -75,13 +81,14 @@ namespace armarx
ARMARX_CHECK_EQUAL(sensors.size(), targets.size());
const auto& dimNames = contr.getTraj()->getDimensionNames();
for (size_t i = 0; i < sensors.size(); i++)
- // for(auto& s : sensors)
+ // for(auto& s : sensors)
{
ARMARX_CHECK_EQUAL(dimNames.at(i), cfg->jointNames.at(i));
currentPos(i) = sensors.at(i)->position;
}
- auto& newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
+ auto& newVelocities =
+ contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
// StringVariantBaseMap positions;
// ARMARX_RT_LOGF_INFO("Current error %.3f", contr.getCurrentError()(1)).deactivateSpam(0.1);
@@ -92,13 +99,16 @@ namespace armarx
// }
// dbgObs->setDebugChannel("positions", positions);
currentTimestamp = contr.getCurrentTimestamp();
- finished = (currentTimestamp >= contr.getTraj()->rbegin()->getTimestamp() && direction == 1.0)
- || (currentTimestamp <= contr.getTraj()->begin()->getTimestamp() && direction == -1.0);
+ finished =
+ (currentTimestamp >= contr.getTraj()->rbegin()->getTimestamp() &&
+ direction == 1.0) ||
+ (currentTimestamp <= contr.getTraj()->begin()->getTimestamp() && direction == -1.0);
for (size_t i = 0; i < targets.size(); ++i)
{
{
- targets.at(i)->velocity = (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
+ targets.at(i)->velocity =
+ (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
}
}
@@ -109,7 +119,8 @@ namespace armarx
}
}
- void NJointTrajectoryController::setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&)
+ void
+ NJointTrajectoryController::setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&)
{
TIMING_START(TrajectoryOptimization);
ARMARX_CHECK_EXPRESSION(t);
@@ -123,8 +134,10 @@ namespace armarx
{
std::list<Eigen::VectorXd> pathPoints;
double timeStep = cfg->preSamplingStepMs / 1000.0;
- Eigen::VectorXd maxVelocities = Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxVelocity);
- Eigen::VectorXd maxAccelerations = Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxAcceleration);
+ Eigen::VectorXd maxVelocities =
+ Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxVelocity);
+ Eigen::VectorXd maxAccelerations =
+ Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxAcceleration);
TrajectoryController::UnfoldLimitlessJointPositions(trajectory);
for (const Trajectory::TrajData& element : *trajectory)
{
@@ -137,7 +150,8 @@ namespace armarx
}
VirtualRobot::Path p(pathPoints, cfg->maxDeviation);
- VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(p, maxVelocities, maxAccelerations, timeStep);
+ VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(
+ p, maxVelocities, maxAccelerations, timeStep);
TrajectoryPtr newTraj = new Trajectory();
@@ -171,7 +185,6 @@ namespace armarx
derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
newTraj->addDerivationsToDimension(d, duration, derivs);
-
}
newTraj->setDimensionNames(trajectory->getDimensionNames());
newTraj->setLimitless(limitlessStates);
@@ -194,9 +207,11 @@ namespace armarx
for (size_t d = 0; d < trajectory->dim(); ++d)
{
auto positions = trajectory->getDimensionData(d, 0);
- posData[trajectory->getDimensionName(d)] = Ice::FloatSeq(positions.begin(), positions.end());
+ posData[trajectory->getDimensionName(d)] =
+ Ice::FloatSeq(positions.begin(), positions.end());
auto velocities = trajectory->getDimensionData(d, 1);
- velData[trajectory->getDimensionName(d)] = Ice::FloatSeq(velocities.begin(), velocities.end());
+ velData[trajectory->getDimensionName(d)] =
+ Ice::FloatSeq(velocities.begin(), velocities.end());
}
auto timestampsDouble = trajectory->getTimestamps();
Ice::FloatSeq timestamps(timestampsDouble.begin(), timestampsDouble.end());
@@ -209,32 +224,37 @@ namespace armarx
}
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
ARMARX_INFO << VAROUT(cfg->PID_p) << VAROUT(cfg->PID_i) << VAROUT(cfg->PID_d);
- trajectoryCtrl.reset(new TrajectoryController(trajectory, cfg->PID_p, cfg->PID_i, cfg->PID_d, false));
+ trajectoryCtrl.reset(
+ new TrajectoryController(trajectory, cfg->PID_p, cfg->PID_i, cfg->PID_d, false));
getWriterControlStruct().trajectoryCtrl = trajectoryCtrl;
writeControlStruct();
}
- bool NJointTrajectoryController::isFinished(const Ice::Current&)
+ bool
+ NJointTrajectoryController::isFinished(const Ice::Current&)
{
return finished;
}
- double NJointTrajectoryController::getCurrentTimestamp(const Ice::Current&)
+ double
+ NJointTrajectoryController::getCurrentTimestamp(const Ice::Current&)
{
return currentTimestamp;
}
- float NJointTrajectoryController::getCurrentProgressFraction(const Ice::Current&)
+ float
+ NJointTrajectoryController::getCurrentProgressFraction(const Ice::Current&)
{
return math::MathUtils::ILerp(startTimestamp, endTimestamp, currentTimestamp);
}
- double NJointTrajectoryController::getCurrentTrajTime() const
+ double
+ NJointTrajectoryController::getCurrentTrajTime() const
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
if (trajectoryCtrl)
{
return trajectoryCtrl->getCurrentTimestamp();
@@ -245,19 +265,22 @@ namespace armarx
}
}
- void NJointTrajectoryController::setLooping(bool looping)
+ void
+ NJointTrajectoryController::setLooping(bool looping)
{
this->looping = looping;
}
- double NJointTrajectoryController::getTrajEndTime() const
+ double
+ NJointTrajectoryController::getTrajEndTime() const
{
return trajEndTime;
}
- TrajectoryPtr NJointTrajectoryController::getTrajectoryCopy() const
+ TrajectoryPtr
+ NJointTrajectoryController::getTrajectoryCopy() const
{
- LockGuardType guard {controlDataMutex};
+ LockGuardType guard{controlDataMutex};
if (trajectoryCtrl)
{
return TrajectoryPtr::dynamicCast(trajectoryCtrl->getTraj()->clone());
@@ -268,12 +291,14 @@ namespace armarx
}
}
- void NJointTrajectoryController::rtPreActivateController()
+ void
+ NJointTrajectoryController::rtPreActivateController()
{
startTime = IceUtil::Time();
}
- void NJointTrajectoryController::rtPostDeactivateController()
+ void
+ NJointTrajectoryController::rtPostDeactivateController()
{
for (auto& target : targets)
{
@@ -281,4 +306,4 @@ namespace armarx
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
index 072f02567b233234a6669cc1a0b4d366676d0759..eb13d7c90f16f4fe14ce3b70b26928f569aa8343 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -1,13 +1,15 @@
#pragma once
-#include "NJointControllerWithTripleBuffer.h"
#include <VirtualRobot/Robot.h>
-#include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
-#include <RobotAPI/libraries/core/TrajectoryController.h>
+
+#include <ArmarXGui/interface/StaticPlotterInterface.h>
+
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
-#include <ArmarXGui/interface/StaticPlotterInterface.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
+#include <RobotAPI/libraries/core/TrajectoryController.h>
+#include "NJointControllerWithTripleBuffer.h"
namespace armarx
{
@@ -28,7 +30,9 @@ namespace armarx
public NJointTrajectoryControllerInterface
{
public:
- NJointTrajectoryController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointTrajectoryController(RobotUnit* prov,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
~NJointTrajectoryController();
@@ -40,7 +44,8 @@ namespace armarx
// NJointController interface
void rtPreActivateController() override;
void rtPostDeactivateController() override;
- void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
// NJointTrajectoryControllerInterface interface
void setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&) override;
@@ -52,6 +57,7 @@ namespace armarx
void setLooping(bool looping);
double getTrajEndTime() const;
TrajectoryPtr getTrajectoryCopy() const;
+
private:
IceUtil::Time startTime;
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
@@ -70,7 +76,5 @@ namespace armarx
double currentTimestamp = 0;
double startTimestamp = 0, endTimestamp = 1;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/PDController.h b/source/RobotAPI/components/units/RobotUnit/PDController.h
index 87abee65875d8827f49dbd1426ec20965cdb8ad8..4643f7724b1a13daa5c9aeca5a52d747b0fabf77 100644
--- a/source/RobotAPI/components/units/RobotUnit/PDController.h
+++ b/source/RobotAPI/components/units/RobotUnit/PDController.h
@@ -26,13 +26,15 @@
namespace armarx
{
- template<class FType>
+ template <class FType>
class PDController
{
public:
using FloatingType = FType;
- PDController(FloatingType p = 0, FloatingType d = 0): Kp {p}, Kd {d} {}
+ PDController(FloatingType p = 0, FloatingType d = 0) : Kp{p}, Kd{d}
+ {
+ }
// setters / getters
/// @see Kp
@@ -117,179 +119,215 @@ namespace armarx
private:
/// @brief p portion of the controller
- FloatingType Kp {0};
+ FloatingType Kp{0};
/// @brief d portion of the controller
- FloatingType Kd {0};
+ FloatingType Kd{0};
/// @brief High limit for the output value
- FloatingType limitOutHi {0};
+ FloatingType limitOutHi{0};
/// @brief Low limit for the output value
- FloatingType limitOutLo {0};
+ FloatingType limitOutLo{0};
/// @brief The target's radius.
/// if abs(error) <= targetRadius the error is 0,
/// otherwise the errors magnitude is reduced by targetRadius
- FloatingType targetRadius {0};
+ FloatingType targetRadius{0};
/// @brief The output can change maximally by this magnitude every time
- FloatingType outRampDelta {0};
+ FloatingType outRampDelta{0};
- FloatingType lastOutInterpolation {0};
+ FloatingType lastOutInterpolation{0};
/// @brief The error is clamped to -maxError, +maxError
- FloatingType maxError {0};
+ FloatingType maxError{0};
//controller state
/// @brief the number of control iterations
- std::uint64_t iterationCount {0};
+ std::uint64_t iterationCount{0};
/// @brief the last control target
- FloatingType lastTarget {0};
+ FloatingType lastTarget{0};
/// @brief the last control input value
- FloatingType lastInValue {0};
+ FloatingType lastInValue{0};
/// @brief the last control output value
- FloatingType lastOutValue {0};
+ FloatingType lastOutValue{0};
};
-}
+} // namespace armarx
//functions
namespace armarx
{
// setters / getters
- template<class FType>
- inline void PDController<FType>::setP(FType p)
+ template <class FType>
+ inline void
+ PDController<FType>::setP(FType p)
{
Kp = std::abs(p);
}
- template<class FType>
- inline FType PDController<FType>::getP() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getP() const
{
return Kp;
}
- template<class FType>
- inline void PDController<FType>::setD(FType d)
+ template <class FType>
+ inline void
+ PDController<FType>::setD(FType d)
{
Kd = std::abs(d);
}
- template<class FType>
- inline FType PDController<FType>::getD() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getD() const
{
return Kd;
}
- template<class FType>
- inline void PDController<FType>::limitOut(FType out)
+ template <class FType>
+ inline void
+ PDController<FType>::limitOut(FType out)
{
limitOut(-out, out);
}
- template<class FType>
- inline void PDController<FType>::limitOut(FType min, FType max)
+
+ template <class FType>
+ inline void
+ PDController<FType>::limitOut(FType min, FType max)
{
std::tie(limitOutLo, limitOutHi) = std::minmax(min, max);
}
- template<class FType>
- inline FType PDController<FType>::getLoOutLimit() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getLoOutLimit() const
{
return limitOutLo;
}
- template<class FType>
- inline FType PDController<FType>::getHiOutLimit() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getHiOutLimit() const
{
return limitOutHi;
}
- template<class FType>
- inline void PDController<FType>::setOutRampDelta(FType delta)
+ template <class FType>
+ inline void
+ PDController<FType>::setOutRampDelta(FType delta)
{
outRampDelta = delta;
}
- template<class FType>
- inline FType PDController<FType>::getOutRampDelta() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getOutRampDelta() const
{
return outRampDelta;
}
- template<class FType>
- inline void PDController<FType>::setMaxError(FType error)
+ template <class FType>
+ inline void
+ PDController<FType>::setMaxError(FType error)
{
maxError = std::abs(error);
}
- template<class FType>
- inline FType PDController<FType>::getMaxError() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getMaxError() const
{
return maxError;
}
- template<class FType>
- inline void PDController<FType>::setTargetRadius(FType radius)
+ template <class FType>
+ inline void
+ PDController<FType>::setTargetRadius(FType radius)
{
targetRadius = std::abs(radius);
}
- template<class FType>
- inline FType PDController<FType>::getTargetRadius() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getTargetRadius() const
{
return targetRadius;
}
- template<class FType>
- inline void PDController<FType>::setLastOutInterpolation(FType lastOutFactor)
+ template <class FType>
+ inline void
+ PDController<FType>::setLastOutInterpolation(FType lastOutFactor)
{
if (!(0 >= lastOutFactor && lastOutFactor < 1))
{
- throw std::invalid_argument
- {
- "the interpolation factor has to be in [0, 1)! factor = " +
- to_string(lastOutFactor)
- };
+ throw std::invalid_argument{"the interpolation factor has to be in [0, 1)! factor = " +
+ to_string(lastOutFactor)};
}
lastOutInterpolation = lastOutFactor;
}
- template<class FType>
- inline FType PDController<FType>::getLastOutInterpolationFactor() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getLastOutInterpolationFactor() const
{
return lastOutInterpolation;
}
//get controller state
- template<class FType>
- inline FType PDController<FType>::getLastTarget() const
+ template <class FType>
+ inline FType
+ PDController<FType>::getLastTarget() const
{
return lastTarget;
}
- template<class FType>
- inline FType PDController<FType>::getLastInValue() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getLastInValue() const
{
return lastInValue;
}
- template<class FType>
- inline FType PDController<FType>::getLastOutValue() const
+
+ template <class FType>
+ inline FType
+ PDController<FType>::getLastOutValue() const
{
return lastOutValue;
}
- template<class FType>
- inline uint64_t PDController<FType>::getIterationCount() const
+
+ template <class FType>
+ inline uint64_t
+ PDController<FType>::getIterationCount() const
{
return iterationCount;
}
// updating functions
- template<class FType>
- inline void PDController<FType>::reset()
+ template <class FType>
+ inline void
+ PDController<FType>::reset()
{
iterationCount = 0;
}
- template<class FType>
- inline FType PDController<FType>::update()
+
+ template <class FType>
+ inline FType
+ PDController<FType>::update()
{
return update(lastInValue, lastTarget);
}
- template<class FType>
- inline FType PDController<FType>::update(FType inValue)
+
+ template <class FType>
+ inline FType
+ PDController<FType>::update(FType inValue)
{
return update(inValue, lastTarget);
}
- template<class FType>
- inline FType PDController<FType>::update(FType inValue, FType target)
+
+ template <class FType>
+ inline FType
+ PDController<FType>::update(FType inValue, FType target)
{
lastTarget = target;
@@ -337,4 +375,4 @@ namespace armarx
lastOutValue = out;
return out;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 17bdc45e13085d67407d061256319f526e4355c1..29f4b175adc0de5bbbca50aa92730d8871c31947 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -25,9 +25,8 @@
namespace armarx
{
-RobotUnit::~RobotUnit()
-{
-
-}
+ RobotUnit::~RobotUnit()
+ {
+ }
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 22ffd80aefe3191b42626aa0af7875a30188b14e..0c97bcd05f4e4d6f4231839b23a99bcec8131c0d 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -42,7 +42,7 @@ namespace armarx
* @ingroup Library-RobotUnit
* @brief
*/
- class RobotUnitPropertyDefinitions:
+ class RobotUnitPropertyDefinitions :
public RobotUnitModule::UnitsPropertyDefinitions,
public RobotUnitModule::DevicesPropertyDefinitions,
public RobotUnitModule::LoggingPropertyDefinitions,
@@ -53,7 +53,7 @@ namespace armarx
public RobotUnitModule::SelfCollisionCheckerPropertyDefinitions
{
public:
- RobotUnitPropertyDefinitions(std::string prefix):
+ RobotUnitPropertyDefinitions(std::string prefix) :
UnitsPropertyDefinitions(prefix),
DevicesPropertyDefinitions(prefix),
LoggingPropertyDefinitions(prefix),
@@ -62,9 +62,10 @@ namespace armarx
ManagementPropertyDefinitions(prefix),
ControlThreadPropertyDefinitions(prefix),
SelfCollisionCheckerPropertyDefinitions(prefix)
- {}
+ {
+ }
};
-}
+} // namespace armarx
namespace armarx
{
@@ -192,14 +193,17 @@ namespace armarx
public:
~RobotUnit();
- static RobotUnit& Instance()
+ static RobotUnit&
+ Instance()
{
return ModuleBase::Instance<RobotUnit>();
}
+
/// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
return PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
index 90910c9f28b246abac25b5b15f0c609bdf33fe3e..732a0e2c479ed852a1162551f881c67c3fa36489 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
@@ -20,18 +20,20 @@
* GNU General Public License
*/
+#include "RobotUnitModuleBase.h"
+
#include <sstream>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/Preprocessor.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-#include "RobotUnitModuleBase.h"
#include "RobotUnitModules.h"
namespace armarx
{
- std::string to_string(RobotUnitState s)
+ std::string
+ to_string(RobotUnitState s)
{
switch (s)
{
@@ -48,124 +50,135 @@ namespace armarx
case armarx::RobotUnitState::Exiting:
return "RobotUnitState::Exiting";
}
- throw std::invalid_argument
- {
- "Unknown state " + to_string(static_cast<std::size_t>(s)) +
- "\nStacktrace\n" + LogSender::CreateBackTrace()
- };
+ throw std::invalid_argument{"Unknown state " + to_string(static_cast<std::size_t>(s)) +
+ "\nStacktrace\n" + LogSender::CreateBackTrace()};
}
-}
+} // namespace armarx
namespace armarx::RobotUnitModule
{
// //////////////////////////////////////////////////////////////////////////// //
// /////////////////////////// call for each module /////////////////////////// //
// //////////////////////////////////////////////////////////////////////////// //
-#define cast_to_and_call(Type, fn, rethrow) \
- ARMARX_TRACE; \
- try \
- { \
- dynamic_cast<Type*>(this)->Type::fn; \
- } \
- catch (Ice::Exception& e) \
- { \
- ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" \
- << e.what() \
- << "\n\tid : " << e.ice_id() \
- << "\n\tfile : " << e.ice_file() \
- << "\n\tline : " << e.ice_line() \
- << "\n\tstack: " << e.ice_stackTrace(); \
- if(rethrow) { throw;} \
- } \
- catch (std::exception& e) \
- { \
- ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" << e.what(); \
- if(rethrow) { throw;} \
- } \
- catch (...) \
- { \
- ARMARX_ERROR << "exception in " << #Type "::" #fn << "!"; \
- if(rethrow) { throw;} \
+#define cast_to_and_call(Type, fn, rethrow) \
+ ARMARX_TRACE; \
+ try \
+ { \
+ dynamic_cast<Type*>(this)->Type::fn; \
+ } \
+ catch (Ice::Exception & e) \
+ { \
+ ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" \
+ << e.what() << "\n\tid : " << e.ice_id() << "\n\tfile : " << e.ice_file() \
+ << "\n\tline : " << e.ice_line() << "\n\tstack: " << e.ice_stackTrace(); \
+ if (rethrow) \
+ { \
+ throw; \
+ } \
+ } \
+ catch (std::exception & e) \
+ { \
+ ARMARX_ERROR << "exception in " << #Type "::" #fn << "!\nwhat:\n" << e.what(); \
+ if (rethrow) \
+ { \
+ throw; \
+ } \
+ } \
+ catch (...) \
+ { \
+ ARMARX_ERROR << "exception in " << #Type "::" #fn << "!"; \
+ if (rethrow) \
+ { \
+ throw; \
+ } \
}
#define for_each_module_apply(r, data, elem) data(elem)
-#define for_each_module(macro) \
+#define for_each_module(macro) \
BOOST_PP_SEQ_FOR_EACH(for_each_module_apply, macro, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
-#define check_base(Type) \
- static_assert( \
- std::is_base_of<ModuleBase, Type>::value, \
- "The RobotUnitModule '" #Type "' has to derived ModuleBase" \
- );
+#define check_base(Type) \
+ static_assert(std::is_base_of<ModuleBase, Type>::value, \
+ "The RobotUnitModule '" #Type "' has to derived ModuleBase");
for_each_module(check_base)
#undef check_base
- void ModuleBase::checkDerivedClasses() const
+ void ModuleBase::checkDerivedClasses() const
{
-#define check_deriving(Type) \
- ARMARX_CHECK_NOT_NULL( \
- dynamic_cast<const Type*>(this)) << \
- "This class does not derive from " << GetTypeString<Type>();
+#define check_deriving(Type) \
+ ARMARX_CHECK_NOT_NULL(dynamic_cast<const Type*>(this)) \
+ << "This class does not derive from " << GetTypeString<Type>();
for_each_module(check_deriving)
#undef check_deriving
}
+
// //////////////////////////////////////////////////////////////////////////// //
// ManagedIceObject life cycle
- void ModuleBase::onInitComponent()
+ void
+ ModuleBase::onInitComponent()
{
ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
-
+ ARMARX_IMPORTANT << "Starting initialization of RobotUnitModule '" << getName();
cast_to_and_call(::armarx::RobotUnitModule::RobotData, _initVirtualRobot(), true);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnInitRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnInitRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- onInitRobotUnit();
+ onInitRobotUnit();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnInitRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnInitRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- finishComponentInitialization();
-
+ finishComponentInitialization();
}
- void ModuleBase::onConnectComponent()
+ void
+ ModuleBase::onConnectComponent()
{
ARMARX_TRACE;
checkDerivedClasses();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnConnectRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnConnectRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- onConnectRobotUnit();
+ onConnectRobotUnit();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnConnectRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnConnectRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::onDisconnectComponent()
+
+ void
+ ModuleBase::onDisconnectComponent()
{
ARMARX_TRACE;
checkDerivedClasses();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnDisconnectRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- onDisconnectRobotUnit();
+ onDisconnectRobotUnit();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnDisconnectRobotUnit(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnDisconnectRobotUnit(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::onExitComponent()
+ void
+ ModuleBase::onExitComponent()
{
ARMARX_TRACE;
checkDerivedClasses();
@@ -173,122 +186,150 @@ namespace armarx::RobotUnitModule
finishRunning();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnExitRobotUnit(), false)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preOnExitRobotUnit(), false)
for_each_module(call_module_hook)
#undef call_module_hook
- onExitRobotUnit();
+ onExitRobotUnit();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnExitRobotUnit(), false)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postOnExitRobotUnit(), false)
for_each_module(call_module_hook)
#undef call_module_hook
}
+
// //////////////////////////////////////////////////////////////////////////// //
// other ManagedIceObject / Component methods
- void ModuleBase::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+ void
+ ModuleBase::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
{
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _componentPropertiesUpdated(changedProperties), true)
+#define call_module_hook(Type) \
+ cast_to_and_call( \
+ ::armarx::RobotUnitModule::Type, _componentPropertiesUpdated(changedProperties), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::icePropertiesInitialized()
+
+ void
+ ModuleBase::icePropertiesInitialized()
{
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _icePropertiesInitialized(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _icePropertiesInitialized(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
+
// //////////////////////////////////////////////////////////////////////////// //
// RobotUnit life cycle
- void ModuleBase::finishComponentInitialization()
+ void
+ ModuleBase::finishComponentInitialization()
{
ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingComponent, __FUNCTION__);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishComponentInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishComponentInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::InitializingDevices);
+ setRobotUnitState(RobotUnitState::InitializingDevices);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishComponentInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishComponentInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::finishDeviceInitialization()
+
+ void
+ ModuleBase::finishDeviceInitialization()
{
ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishDeviceInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishDeviceInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::InitializingUnits);
+ setRobotUnitState(RobotUnitState::InitializingUnits);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishDeviceInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishDeviceInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::finishUnitInitialization()
+ void
+ ModuleBase::finishUnitInitialization()
{
ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishUnitInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishUnitInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::InitializingControlThread);
+ setRobotUnitState(RobotUnitState::InitializingControlThread);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishUnitInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishUnitInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::finishControlThreadInitialization()
+ void
+ ModuleBase::finishControlThreadInitialization()
{
ARMARX_TRACE;
checkDerivedClasses();
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingControlThread, __FUNCTION__);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishControlThreadInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishControlThreadInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::Running);
+ setRobotUnitState(RobotUnitState::Running);
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishControlThreadInitialization(), true)
+#define call_module_hook(Type) \
+ cast_to_and_call( \
+ ::armarx::RobotUnitModule::Type, _postFinishControlThreadInitialization(), true)
for_each_module(call_module_hook)
#undef call_module_hook
}
- void ModuleBase::finishRunning()
+ void
+ ModuleBase::finishRunning()
{
ARMARX_TRACE;
checkDerivedClasses();
shutDown();
- GuardType guard {dataMutex}; //DO NOT USE getGuard here!
+ GuardType guard{dataMutex}; //DO NOT USE getGuard here!
if (getRobotUnitState() != RobotUnitState::Running)
{
- ARMARX_ERROR << "called finishRunning when state was " << to_string(getRobotUnitState());
+ ARMARX_ERROR << "called finishRunning when state was "
+ << to_string(getRobotUnitState());
}
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishRunning(), false)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _preFinishRunning(), false)
for_each_module(call_module_hook)
#undef call_module_hook
- setRobotUnitState(RobotUnitState::Exiting);
+ setRobotUnitState(RobotUnitState::Exiting);
joinControlThread();
-#define call_module_hook(Type) cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishRunning(), false)
+#define call_module_hook(Type) \
+ cast_to_and_call(::armarx::RobotUnitModule::Type, _postFinishRunning(), false)
for_each_module(call_module_hook)
#undef call_module_hook
}
@@ -300,37 +341,37 @@ namespace armarx::RobotUnitModule
// ////////////////////////////////// other /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////// //
- const std::set<RobotUnitState> ModuleBase::DevicesReadyStates
- {
+ const std::set<RobotUnitState> ModuleBase::DevicesReadyStates{
RobotUnitState::InitializingUnits,
RobotUnitState::InitializingControlThread,
- RobotUnitState::Running
- };
+ RobotUnitState::Running};
- void ModuleBase::setRobotUnitState(RobotUnitState to)
+ void
+ ModuleBase::setRobotUnitState(RobotUnitState to)
{
ARMARX_TRACE;
auto guard = getGuard();
- const auto transitionErrorMessage = [ =, this ](RobotUnitState expectedFrom)
+ const auto transitionErrorMessage = [=, this](RobotUnitState expectedFrom)
{
return "Can't switch to state " + to_string(to) + " from " + to_string(state) +
"! The expected source state is " + to_string(expectedFrom) + ".";
};
- const auto transitionErrorThrow = [ =, this ](RobotUnitState expectedFrom)
+ const auto transitionErrorThrow = [=, this](RobotUnitState expectedFrom)
{
if (state != expectedFrom)
{
const auto msg = transitionErrorMessage(expectedFrom);
ARMARX_ERROR << msg;
- throw std::invalid_argument {msg};
+ throw std::invalid_argument{msg};
}
};
switch (to)
{
case armarx::RobotUnitState::InitializingComponent:
- throw std::invalid_argument {"Can't switch to state RobotUnitState::InitializingComponent"};
+ throw std::invalid_argument{
+ "Can't switch to state RobotUnitState::InitializingComponent"};
case armarx::RobotUnitState::InitializingDevices:
transitionErrorThrow(RobotUnitState::InitializingComponent);
break;
@@ -350,71 +391,74 @@ namespace armarx::RobotUnitModule
}
break;
default:
- throw std::invalid_argument
- {
- "setRobotUnitState: Unknown target state " + to_string(static_cast<std::size_t>(to)) +
- "\nStacktrace\n" + LogSender::CreateBackTrace()
- };
+ throw std::invalid_argument{"setRobotUnitState: Unknown target state " +
+ to_string(static_cast<std::size_t>(to)) +
+ "\nStacktrace\n" + LogSender::CreateBackTrace()};
}
ARMARX_INFO << "switching state from " << to_string(state) << " to " << to_string(to);
state = to;
}
-
- bool ModuleBase::areDevicesReady() const
+ bool
+ ModuleBase::areDevicesReady() const
{
return DevicesReadyStates.count(state);
}
- bool ModuleBase::inControlThread() const
+ bool
+ ModuleBase::inControlThread() const
{
return getRobotUnitState() == RobotUnitState::Running &&
std::this_thread::get_id() == _module<ControlThread>().getControlThreadId();
}
- void ModuleBase::throwIfInControlThread(const std::string& fnc) const
+ void
+ ModuleBase::throwIfInControlThread(const std::string& fnc) const
{
ARMARX_TRACE;
if (inControlThread())
{
std::stringstream str;
- str << "Called function '" << fnc << "' in the Control Thread\nStack trace:\n" << LogSender::CreateBackTrace();
+ str << "Called function '" << fnc << "' in the Control Thread\nStack trace:\n"
+ << LogSender::CreateBackTrace();
ARMARX_ERROR << str.str();
- throw LogicError {str.str()};
+ throw LogicError{str.str()};
}
}
- ModuleBase::GuardType ModuleBase::getGuard() const
+ ModuleBase::GuardType
+ ModuleBase::getGuard() const
{
ARMARX_TRACE;
if (inControlThread())
{
- throw LogicError
- {
- "Attempted to lock mutex in Control Thread\nStack trace:\n" +
- LogSender::CreateBackTrace()
- };
+ throw LogicError{"Attempted to lock mutex in Control Thread\nStack trace:\n" +
+ LogSender::CreateBackTrace()};
}
- GuardType guard {dataMutex, std::defer_lock};
+ GuardType guard{dataMutex, std::defer_lock};
if (guard.try_lock())
{
return guard;
}
- while (!guard.try_lock_for(std::chrono::microseconds {100}))
+ while (!guard.try_lock_for(std::chrono::microseconds{100}))
{
if (isShuttingDown())
{
- throw LogicError {"Attempting to lock mutex during shutdown\nStacktrace\n" + LogSender::CreateBackTrace()};
+ throw LogicError{"Attempting to lock mutex during shutdown\nStacktrace\n" +
+ LogSender::CreateBackTrace()};
}
}
return guard;
}
- void ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
+ void
+ ModuleBase::throwIfStateIsNot(const std::set<RobotUnitState>& stateSet,
+ const std::string& fnc,
+ bool onlyWarn) const
{
ARMARX_TRACE;
- if (! stateSet.count(state))
+ if (!stateSet.count(state))
{
std::stringstream ss;
ss << fnc << ": Can't be called if state is not in {";
@@ -429,20 +473,26 @@ namespace armarx::RobotUnitModule
fst = false;
}
ss << "} (current state " << getRobotUnitState() << ")\n"
- << "Stacktrace:\n" << LogSender::CreateBackTrace();
+ << "Stacktrace:\n"
+ << LogSender::CreateBackTrace();
ARMARX_ERROR << ss.str();
if (!onlyWarn)
{
- throw LogicError {ss.str()};
+ throw LogicError{ss.str()};
}
}
}
- void ModuleBase::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
+
+ void
+ ModuleBase::throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
{
- throwIfStateIsNot(std::set<RobotUnitState> {s}, fnc, onlyWarn);
+ throwIfStateIsNot(std::set<RobotUnitState>{s}, fnc, onlyWarn);
}
- void ModuleBase::throwIfStateIs(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn) const
+ void
+ ModuleBase::throwIfStateIs(const std::set<RobotUnitState>& stateSet,
+ const std::string& fnc,
+ bool onlyWarn) const
{
ARMARX_TRACE;
if (stateSet.count(state))
@@ -460,36 +510,40 @@ namespace armarx::RobotUnitModule
fst = false;
}
ss << "} (current state " << getRobotUnitState() << ")\n"
- << "Stacktrace:\n" << LogSender::CreateBackTrace();
+ << "Stacktrace:\n"
+ << LogSender::CreateBackTrace();
ARMARX_ERROR << ss.str();
if (!onlyWarn)
{
- throw LogicError {ss.str()};
+ throw LogicError{ss.str()};
}
}
}
- void ModuleBase::throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
+
+ void
+ ModuleBase::throwIfStateIs(RobotUnitState s, const std::string& fnc, bool onlyWarn) const
{
- throwIfStateIs(std::set<RobotUnitState> {s}, fnc, onlyWarn);
+ throwIfStateIs(std::set<RobotUnitState>{s}, fnc, onlyWarn);
}
- void ModuleBase::throwIfDevicesNotReady(const std::string& fnc) const
+ void
+ ModuleBase::throwIfDevicesNotReady(const std::string& fnc) const
{
throwIfStateIsNot(DevicesReadyStates, fnc);
}
-
- std::atomic<ModuleBase*> ModuleBase::Instance_ {nullptr};
+ std::atomic<ModuleBase*> ModuleBase::Instance_{nullptr};
ModuleBase::ModuleBase()
{
ARMARX_TRACE;
if (Instance_ && this != Instance_)
{
- ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = " << Instance_ << ", this = " << this << ")";
+ ARMARX_FATAL << "This class is a Singleton. It was already instantiated. (Instance = "
+ << Instance_ << ", this = " << this << ")";
std::terminate();
}
Instance_ = this;
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
index 474c2d611ba4d12b20cd402851b3ab26cc84cba1..b76f54dc8b0c5de7052552f202f62c9eefcce05a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h
@@ -26,20 +26,20 @@
#include <chrono>
#include <mutex>
-#include <boost/preprocessor/variadic/to_seq.hpp>
-#include <boost/preprocessor/seq/for_each.hpp>
#include <boost/current_function.hpp>
+#include <boost/preprocessor/seq/for_each.hpp>
+#include <boost/preprocessor/variadic/to_seq.hpp>
#include <VirtualRobot/Robot.h>
-#include <ArmarXCore/util/CPPUtility/Pointer.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/util/CPPUtility/Pointer.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "../JointControllers/JointController.h"
-#include "../util/JointAndNJointControllers.h"
#include "../util.h"
+#include "../util/JointAndNJointControllers.h"
/**
* @defgroup Library-RobotUnit-Modules RobotUnitModules
@@ -51,23 +51,21 @@ namespace armarx
TYPEDEF_PTRS_HANDLE(RobotUnit);
TYPEDEF_PTRS_HANDLE(NJointControllerBase);
- template<class Targ, class Src>
- Targ& CheckedCastAndDeref(Src* src)
+ template <class Targ, class Src>
+ Targ&
+ CheckedCastAndDeref(Src* src)
{
Targ* ptr = dynamic_cast<Targ*>(src);
if (!ptr)
{
if (!src)
{
- throw std::invalid_argument {"Ptr passed to CheckedCastAndDeref is NULL"};
+ throw std::invalid_argument{"Ptr passed to CheckedCastAndDeref is NULL"};
}
- throw std::invalid_argument
- {
- "Ptr of type '" +
- GetTypeString<Src>() +
+ throw std::invalid_argument{
+ "Ptr of type '" + GetTypeString<Src>() +
"' passed to CheckedCastAndDeref is is not related to target type '" +
- GetTypeString<Targ>()
- };
+ GetTypeString<Targ>()};
}
return *ptr;
}
@@ -85,24 +83,29 @@ namespace armarx
};
std::string to_string(armarx::RobotUnitState s);
- inline std::ostream& operator<<(std::ostream& o, RobotUnitState s)
+
+ inline std::ostream&
+ operator<<(std::ostream& o, RobotUnitState s)
{
return o << to_string(s);
}
-}
+} // namespace armarx
namespace armarx::RobotUnitModule::detail
{
- class ModuleBaseIntermediatePropertyDefinitions: public ComponentPropertyDefinitions
+ class ModuleBaseIntermediatePropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- ModuleBaseIntermediatePropertyDefinitions(): ComponentPropertyDefinitions("") {}
+ ModuleBaseIntermediatePropertyDefinitions() : ComponentPropertyDefinitions("")
+ {
+ }
};
-}
+} // namespace armarx::RobotUnitModule::detail
namespace armarx::RobotUnitModule
{
- class ModuleBasePropertyDefinitions: virtual public detail::ModuleBaseIntermediatePropertyDefinitions
+ class ModuleBasePropertyDefinitions :
+ virtual public detail::ModuleBaseIntermediatePropertyDefinitions
{
public:
ModuleBasePropertyDefinitions(std::string prefix)
@@ -112,9 +115,8 @@ namespace armarx::RobotUnitModule
}
};
-
#define forward_declare(r, data, Mod) TYPEDEF_PTRS_HANDLE(Mod);
- BOOST_PP_SEQ_FOR_EACH(forward_declare,, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
+ BOOST_PP_SEQ_FOR_EACH(forward_declare, , BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
#undef forward_declare
/**
@@ -182,27 +184,33 @@ namespace armarx::RobotUnitModule
{
/// @brief The singleton instance
static std::atomic<ModuleBase*> Instance_;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static ModuleBase& Instance()
+ static ModuleBase&
+ Instance()
{
ARMARX_CHECK_NOT_NULL(Instance_);
return *Instance_;
}
+
/**
* @brief Returns the singleton instance of the given class
* @return The singleton instance of the given class
*/
- template<class T>
- static T& Instance()
+ template <class T>
+ static T&
+ Instance()
{
- static_assert(std::is_base_of<ModuleBase, T>::value, "The type has to derive ModuleBase");
+ static_assert(std::is_base_of<ModuleBase, T>::value,
+ "The type has to derive ModuleBase");
ARMARX_CHECK_NOT_NULL(Instance_);
return dynamic_cast<T&>(*Instance_);
}
+
protected:
/// @brief Ctor
ModuleBase();
@@ -220,8 +228,8 @@ namespace armarx::RobotUnitModule
using TimePointType = std::chrono::high_resolution_clock::time_point;
#define using_module_types(...) using_module_types_impl(__VA_ARGS__)
-#define using_module_types_impl(r, data, Mod) using Module ## Mod = Mod;
- BOOST_PP_SEQ_FOR_EACH(using_module_types,, BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
+#define using_module_types_impl(r, data, Mod) using Module##Mod = Mod;
+ BOOST_PP_SEQ_FOR_EACH(using_module_types, , BOOST_PP_VARIADIC_TO_SEQ(RobotUnitModules))
#undef using_module_types
#undef using_module_types_impl
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -230,39 +238,52 @@ namespace armarx::RobotUnitModule
protected:
/// @brief Checks whether the implementing class derives all modules.
void checkDerivedClasses() const;
+
public:
/**
* @brief Returns this as ref to the given type.
* @return This as ref to the given type.
*/
- template<class T> T& _module()
+ template <class T>
+ T&
+ _module()
{
- return dynamic_cast< T&>(*this);
+ return dynamic_cast<T&>(*this);
}
+
/**
* @brief Returns this as ref to the given type.
* @return This as ref to the given type.
*/
- template<class T> const T& _module() const
+ template <class T>
+ const T&
+ _module() const
{
return dynamic_cast<const T&>(*this);
}
+
/**
* @brief Returns this as ptr to the given type.
* @return This as ptr to the given type.
*/
- template<class T> T* _modulePtr()
+ template <class T>
+ T*
+ _modulePtr()
{
- return &_module< T>();
+ return &_module<T>();
}
+
/**
* @brief Returns this as ptr to the given type.
* @return This as ptr to the given type.
*/
- template<class T> const T* _modulePtr() const
+ template <class T>
+ const T*
+ _modulePtr() const
{
return &_module<const T>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////// ManagedIceObject life cycle ///////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -272,12 +293,12 @@ namespace armarx::RobotUnitModule
* @brief \note This function is protected with \ref getGuard()
* @see \ref ManagedIceObject::onInitComponent
*/
- void onInitComponent() final override;
+ void onInitComponent() final override;
/**
* @brief \warning This function is not protected with \ref getGuard()
* @see \ref ManagedIceObject::onConnectComponent
*/
- void onConnectComponent() final override;
+ void onConnectComponent() final override;
/**
* @brief \warning This function is not protected with \ref getGuard()
* @see \ref ManagedIceObject::onDisconnectComponent
@@ -287,7 +308,7 @@ namespace armarx::RobotUnitModule
* @brief \note This function is protected with \ref getGuard()
* @see \ref ManagedIceObject::onExitComponent
*/
- void onExitComponent() final override;
+ void onExitComponent() final override;
//ManagedIceObject life cycle module hooks
/**
@@ -295,52 +316,81 @@ namespace armarx::RobotUnitModule
* \note This function is protected with \ref getGuard()
* @see onInitComponent
*/
- void _preOnInitRobotUnit() {}
+ void
+ _preOnInitRobotUnit()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref onInitComponent (called after \ref onInitRobotUnit was called)
* \note This function is protected with \ref getGuard()
* @see onInitComponent
*/
- void _postOnInitRobotUnit() {}
+ void
+ _postOnInitRobotUnit()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref onConnectComponent (called before \ref onConnectRobotUnit was called)
* \warning This function is not protected with \ref getGuard()
* @see onConnectComponent
*/
- void _preOnConnectRobotUnit() {}
+ void
+ _preOnConnectRobotUnit()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref onConnectComponent (called after \ref onConnectRobotUnit was called)
* \warning This function is not protected with \ref getGuard()
* @see onConnectComponent
*/
- void _postOnConnectRobotUnit() {}
+ void
+ _postOnConnectRobotUnit()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref onDisconnectComponent (called before \ref onDisconnectRobotUnit was called)
* \warning This function is not protected with \ref getGuard()
* @see onDisconnectComponent
*/
- void _preOnDisconnectRobotUnit() {}
+ void
+ _preOnDisconnectRobotUnit()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref onDisconnectComponent (called after \ref onDisconnectRobotUnit was called)
* \warning This function is not protected with \ref getGuard()
* @see onDisconnectComponent
*/
- void _postOnDisconnectRobotUnit() {}
+ void
+ _postOnDisconnectRobotUnit()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref onExitComponent (called before \ref onExitRobotUnit was called)
* \note This function is protected with \ref getGuard()
* @see onExitComponent
*/
- void _preOnExitRobotUnit() {}
+ void
+ _preOnExitRobotUnit()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref onExitComponent (called after \ref onExitRobotUnit was called)
* \note This function is protected with \ref getGuard()
* @see onExitComponent
*/
- void _postOnExitRobotUnit() {}
+ void
+ _postOnExitRobotUnit()
+ {
+ }
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////// other ManagedIceObject / Component methods ////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -349,17 +399,27 @@ namespace armarx::RobotUnitModule
void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
/// @see \ref ManagedIceObject::icePropertiesInitialized
void icePropertiesInitialized() override;
+
/// @see \ref ManagedIceObject::getDefaultName
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotUnit";
}
//module hooks
/// @brief Hook for deriving RobotUnitModules called in \ref componentPropertiesUpdated
- void _componentPropertiesUpdated(const std::set<std::string>& /*changedProperties*/) {}
+ void
+ _componentPropertiesUpdated(const std::set<std::string>& /*changedProperties*/)
+ {
+ }
+
/// @brief Hook for deriving RobotUnitModules called in \ref icePropertiesInitialized
- void _icePropertiesInitialized() {}
+ void
+ _icePropertiesInitialized()
+ {
+ }
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ////////////////////////////// state and state transition ////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -369,6 +429,7 @@ namespace armarx::RobotUnitModule
* @param s The state to set
*/
void setRobotUnitState(RobotUnitState s);
+
protected:
//state transitions
/**
@@ -413,65 +474,101 @@ namespace armarx::RobotUnitModule
* \note This function is protected with \ref getGuard()
* @see finishComponentInitialization
*/
- void _preFinishComponentInitialization() {}
+ void
+ _preFinishComponentInitialization()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishComponentInitialization (called after the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishComponentInitialization
*/
- void _postFinishComponentInitialization() {}
+ void
+ _postFinishComponentInitialization()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishDeviceInitialization (called before the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishDeviceInitialization
*/
- void _preFinishDeviceInitialization() {}
+ void
+ _preFinishDeviceInitialization()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishDeviceInitialization (called after the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishDeviceInitialization
*/
- void _postFinishDeviceInitialization() {}
+ void
+ _postFinishDeviceInitialization()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishUnitInitialization (called before the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishUnitInitialization
*/
- void _preFinishUnitInitialization() {}
+ void
+ _preFinishUnitInitialization()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishUnitInitialization (called after the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishUnitInitialization
*/
- void _postFinishUnitInitialization() {}
+ void
+ _postFinishUnitInitialization()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishControlThreadInitialization (called before the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishControlThreadInitialization
*/
- void _preFinishControlThreadInitialization() {}
+ void
+ _preFinishControlThreadInitialization()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishControlThreadInitialization (called after the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishControlThreadInitialization
*/
- void _postFinishControlThreadInitialization() {}
+ void
+ _postFinishControlThreadInitialization()
+ {
+ }
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishRunning (called before the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishRunning
*/
- void _preFinishRunning() {}
+ void
+ _preFinishRunning()
+ {
+ }
+
/**
* @brief Hook for deriving RobotUnitModules called in \ref finishRunning (called after the state has changed)
* \note This function is protected with \ref getGuard()
* @see finishRunning
*/
- void _postFinishRunning() {}
+ void
+ _postFinishRunning()
+ {
+ }
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////////// utility //////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -488,21 +585,26 @@ namespace armarx::RobotUnitModule
* @param fnc The callers function name.
* @param onlyWarn Only print a warning instead of throwing an exception.
*/
- void throwIfStateIsNot(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn = false) const;
+ void throwIfStateIsNot(const std::set<RobotUnitState>& stateSet,
+ const std::string& fnc,
+ bool onlyWarn = false) const;
/**
* @brief Throws an exception if the current state is not \param state
* @param state The acceptable \ref RobotUnitState
* @param fnc The callers function name.
* @param onlyWarn Only print a warning instead of throwing an exception.
*/
- void throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn = false) const;
+ void
+ throwIfStateIsNot(RobotUnitState s, const std::string& fnc, bool onlyWarn = false) const;
/**
* @brief Throws an exception if the current state is in \param stateSet
* @param stateSet The set of forbidden \ref RobotUnitState "RobotUnitStates"
* @param fnc The callers function name.
* @param onlyWarn Only print a warning instead of throwing an exception.
*/
- void throwIfStateIs(const std::set<RobotUnitState>& stateSet, const std::string& fnc, bool onlyWarn = false) const;
+ void throwIfStateIs(const std::set<RobotUnitState>& stateSet,
+ const std::string& fnc,
+ bool onlyWarn = false) const;
/**
* @brief Throws an exception if the current state is \param state
* @param state The forbidden \ref RobotUnitState
@@ -541,18 +643,22 @@ namespace armarx::RobotUnitModule
* @return Whether the \ref RobotUnit is shutting down
* @see shutDown
*/
- bool isShuttingDown() const ///TODO use this function in implementations
+ bool
+ isShuttingDown() const ///TODO use this function in implementations
{
return isShuttingDown_.load();
}
+
/**
* @brief Requests the \ref RobotUnit to shut down.
* @see isShuttingDown
*/
- void shutDown() ///TODO use this function in implementations
+ void
+ shutDown() ///TODO use this function in implementations
{
isShuttingDown_.store(true);
}
+
// //////////////////////////////////////////////////////////////////////////// //
// /////////////////////////// robot unit impl hooks ////////////////////////// //
// //////////////////////////////////////////////////////////////////////////// //
@@ -561,26 +667,41 @@ namespace armarx::RobotUnitModule
* \note This function is protected with \ref getGuard()
* @see onInitComponent
*/
- virtual void onInitRobotUnit() {}
+ virtual void
+ onInitRobotUnit()
+ {
+ }
+
/**
* @brief called in onConnectComponent
* \warning This function is not protected with \ref getGuard()
* @see onConnectComponent
*/
- virtual void onConnectRobotUnit() {}
+ virtual void
+ onConnectRobotUnit()
+ {
+ }
+
/**
* @brief called in onDisconnecComponent
* \warning This function is not protected with \ref getGuard()
* @see onDisconnecComponent
*/
- virtual void onDisconnectRobotUnit() {}
+ virtual void
+ onDisconnectRobotUnit()
+ {
+ }
+
/**
* @brief called in onExitComponent before calling finishRunning
* \note This function is protected with \ref getGuard()
* @see finishRunning
* @see onExitComponent
*/
- virtual void onExitRobotUnit() {}
+ virtual void
+ onExitRobotUnit()
+ {
+ }
/// @brief Implementations have to join their \ref ControlThread in this hook. (used by RobotUnit::finishRunning())
virtual void joinControlThread() = 0;
@@ -595,10 +716,12 @@ namespace armarx::RobotUnitModule
* @brief Returns a sentinel value for an index (std::numeric_limits<std::size_t>::max())
* @return A sentinel value for an index (std::numeric_limits<std::size_t>::max())
*/
- static constexpr std::size_t IndexSentinel()
+ static constexpr std::size_t
+ IndexSentinel()
{
return std::numeric_limits<std::size_t>::max();
}
+
/**
* @brief Returns a guard to the \ref RobotUnits mutex.
* @return A guard to the \ref RobotUnits mutex.
@@ -614,11 +737,10 @@ namespace armarx::RobotUnitModule
/// @brief Set of \ref RobotUnitState "RobotUnitStates" where \ref Device "Devices" are usable
static const std::set<RobotUnitState> DevicesReadyStates;
/// @brief The \ref RobotUnit's current state
- std::atomic<RobotUnitState> state {RobotUnitState::InitializingComponent};
+ std::atomic<RobotUnitState> state{RobotUnitState::InitializingComponent};
/// @brief Whether the \ref RobotUnit is shutting down
- std::atomic_bool isShuttingDown_ {false};
+ std::atomic_bool isShuttingDown_{false};
};
-}
+} // namespace armarx::RobotUnitModule
#include "RobotUnitModuleBase.ipp"
-
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 5d7d7d01ece91cb274e2f4e6a6f0619c06f4a65e..e0131a416855174e5571d22d9e44cfac50f76954 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -29,16 +29,17 @@
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
-#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
#include "../Devices/RTThreadTimingsSensorDevice.h"
@@ -51,22 +52,26 @@ namespace armarx::RobotUnitModule
class NJointControllerAttorneyForControlThread
{
friend class ControlThread;
+
static void
RtDeactivateController(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtDeactivateController();
}
+
static void
RtActivateController(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtActivateController();
}
+
static void
RtDeactivateControllerBecauseOfError(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtDeactivateControllerBecauseOfError();
}
};
+
/**
* \brief This class allows minimal access to private members of \ref ControlThreadDataBuffer in a sane fashion for \ref ControlThread.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -74,6 +79,7 @@ namespace armarx::RobotUnitModule
class ControlThreadDataBufferAttorneyForControlThread
{
friend class ControlThread;
+
static std::vector<JointController*>&
GetActivatedJointControllers(ControlThread* p)
{
@@ -81,6 +87,7 @@ namespace armarx::RobotUnitModule
.controllersActivated.getWriteBuffer()
.jointControllers;
}
+
static std::vector<NJointControllerBase*>&
GetActivatedNJointControllers(ControlThread* p)
{
@@ -88,6 +95,7 @@ namespace armarx::RobotUnitModule
.controllersActivated.getWriteBuffer()
.nJointControllers;
}
+
static std::vector<std::size_t>&
GetActivatedJointToNJointControllerAssignement(ControlThread* p)
{
@@ -95,6 +103,7 @@ namespace armarx::RobotUnitModule
.controllersActivated.getWriteBuffer()
.jointToNJointControllerAssignement;
}
+
static const std::vector<JointController*>&
GetActivatedJointControllers(const ControlThread* p)
{
@@ -102,6 +111,7 @@ namespace armarx::RobotUnitModule
.controllersActivated.getWriteBuffer()
.jointControllers;
}
+
static const std::vector<NJointControllerBase*>&
GetActivatedNJointControllers(const ControlThread* p)
{
@@ -109,6 +119,7 @@ namespace armarx::RobotUnitModule
.controllersActivated.getWriteBuffer()
.nJointControllers;
}
+
static const std::vector<std::size_t>&
GetActivatedJointToNJointControllerAssignement(const ControlThread* p)
{
@@ -126,11 +137,13 @@ namespace armarx::RobotUnitModule
.controllersRequested.getReadBuffer()
.jointToNJointControllerAssignement;
}
+
static void
CommitActivatedControllers(ControlThread* p)
{
return p->_module<ControlThreadDataBuffer>().controllersActivated.commitWrite();
}
+
static void
ResetActivatedControllerAssignement(ControlThread* p)
{
@@ -147,6 +160,7 @@ namespace armarx::RobotUnitModule
.controllersRequested.getReadBuffer()
.jointControllers;
}
+
static const std::vector<NJointControllerBase*>&
GetRequestedNJointControllers(const ControlThread* p)
{
@@ -155,6 +169,7 @@ namespace armarx::RobotUnitModule
.controllersRequested.getReadBuffer()
.nJointControllers;
}
+
static const std::vector<std::size_t>&
GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
{
@@ -199,6 +214,7 @@ namespace armarx::RobotUnitModule
j.at(index) = c;
}
};
+
/**
* \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -206,11 +222,13 @@ namespace armarx::RobotUnitModule
class DevicesAttorneyForControlThread
{
friend class ControlThread;
+
static const std::vector<ControlDevicePtr>&
GetControlDevices(const ControlThread* p)
{
return p->_module<Devices>().controlDevices.values();
}
+
static const std::vector<SensorDevicePtr>&
GetSensorDevices(const ControlThread* p)
{
@@ -222,11 +240,13 @@ namespace armarx::RobotUnitModule
{
return p->_module<Devices>().sensorValues;
}
+
static const std::vector<std::vector<PropagateConst<ControlTargetBase*>>>&
GetControlTargets(const ControlThread* p)
{
return p->_module<Devices>().controlTargets;
}
+
static RTThreadTimingsSensorDevice&
GetThreadTimingsSensorDevice(const ControlThread* p)
{
@@ -242,6 +262,7 @@ namespace armarx::RobotUnitModule
robot, robotNodes, p->_module<Devices>().sensorValues);
}
};
+
/**
* \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -249,6 +270,7 @@ namespace armarx::RobotUnitModule
class ManagementAttorneyForControlThread
{
friend class ControlThread;
+
static bool
HeartbeatMissing(const ControlThread* p)
{
@@ -1099,7 +1121,6 @@ namespace armarx::RobotUnitModule
}
}
-
void
ControlThread::dumpRtControllerSetup(
std::ostream& out,
@@ -1179,14 +1200,16 @@ namespace armarx::RobotUnitModule
void
ControlThread::rtUpdateRobotGlobalPose()
{
- const ConstSensorDevicePtr globalPoseSensorDevice = _module<Devices>().getSensorDevice(GlobalRobotPoseSensorDevice::DeviceName());
- if(not globalPoseSensorDevice)
+ const ConstSensorDevicePtr globalPoseSensorDevice =
+ _module<Devices>().getSensorDevice(GlobalRobotPoseSensorDevice::DeviceName());
+ if (not globalPoseSensorDevice)
{
return;
}
- const auto *const sensorValue = globalPoseSensorDevice->getSensorValue()->asA<SensorValueGlobalRobotPose>();
- if(sensorValue == nullptr)
+ const auto* const sensorValue =
+ globalPoseSensorDevice->getSensorValue()->asA<SensorValueGlobalRobotPose>();
+ if (sensorValue == nullptr)
{
return;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
index 9b14a13ba370166793e188c8c8577fcc5ea19516..af1990164ed130b335b6e8c26b540d165694b51e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -28,44 +28,47 @@
#include "../Devices/ControlDevice.h"
#include "../Devices/SensorDevice.h"
-
#include "RobotUnitModuleBase.h"
namespace armarx
{
class DynamicsHelper;
class RTThreadTimingsSensorDevice;
-}
+} // namespace armarx
namespace armarx::RobotUnitModule
{
- class ControlThreadPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class ControlThreadPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- ControlThreadPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ ControlThreadPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
defineOptionalProperty<std::size_t>(
- "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning", 2,
- "A Warning will be printed, If the execution time in micro seconds of a NJointControllerBase "
- "exceeds this parameter times the number of ControlDevices."
- );
+ "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning",
+ 2,
+ "A Warning will be printed, If the execution time in micro seconds of a "
+ "NJointControllerBase "
+ "exceeds this parameter times the number of ControlDevices.");
defineOptionalProperty<std::size_t>(
- "NjointController_AllowedExecutionTimePerControlDeviceUntilError", 20,
- "An Error will be printed, If the execution time in micro seconds of a NJointControllerBase "
- "exceeds this parameter times the number of ControlDevices."
- );
- defineOptionalProperty<bool>("EnableInverseDynamics", false,
- "If true, inverse dynamics are calculated for all joints given in the "
- "InverseDynamicsRobotJointSets property during each loop of "
- "the rt control thread.");
- defineOptionalProperty<std::string>(
- "InverseDynamicsRobotJointSets", "LeftArm,RightArm",
- "Comma separated list of robot nodesets for which the inverse dynamics should be calculcated."
- );
+ "NjointController_AllowedExecutionTimePerControlDeviceUntilError",
+ 20,
+ "An Error will be printed, If the execution time in micro seconds of a "
+ "NJointControllerBase "
+ "exceeds this parameter times the number of ControlDevices.");
+ defineOptionalProperty<bool>(
+ "EnableInverseDynamics",
+ false,
+ "If true, inverse dynamics are calculated for all joints given in the "
+ "InverseDynamicsRobotJointSets property during each loop of "
+ "the rt control thread.");
+ defineOptionalProperty<std::string>("InverseDynamicsRobotJointSets",
+ "LeftArm,RightArm",
+ "Comma separated list of robot nodesets for which "
+ "the inverse dynamics should be calculcated.");
defineOptionalProperty<std::string>(
- "InverseDynamicsRobotBodySet", "RobotCol",
- "Robot nodeset of which the masses should be used for the inverse dynamics"
- );
+ "InverseDynamicsRobotBodySet",
+ "RobotCol",
+ "Robot nodeset of which the masses should be used for the inverse dynamics");
}
};
@@ -86,6 +89,7 @@ namespace armarx::RobotUnitModule
RequestInactive,
RequestNone,
};
+
protected:
enum class SwitchControllerMode
{
@@ -96,15 +100,18 @@ namespace armarx::RobotUnitModule
RTThread,
IceRequests
};
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static ControlThread& Instance()
+ static ControlThread&
+ Instance()
{
return ModuleBase::Instance<ControlThread>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -121,17 +128,20 @@ namespace armarx::RobotUnitModule
* @brief Sets the \ref EmergencyStopState
* @param state The \ref EmergencyStopState to set
*/
- void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = Ice::emptyCurrent) override;
+ void setEmergencyStopState(EmergencyStopState state,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns the \ref ControlThread's target \ref EmergencyStopState
* @return The \ref EmergencyStopState
*/
- EmergencyStopState getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
+ EmergencyStopState
+ getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the \ref ControlThread's \ref EmergencyStopState
* @return The \ref EmergencyStopState
*/
- EmergencyStopState getRtEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
+ EmergencyStopState
+ getRtEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -141,10 +151,12 @@ namespace armarx::RobotUnitModule
* @return The \ref ControlThread's thread id
* @see controlThreadId
*/
- std::thread::id getControlThreadId() const
+ std::thread::id
+ getControlThreadId() const
{
return controlThreadId;
}
+
private:
/**
* @brief Sets the \ref EmergencyStopState without updating the topic.
@@ -166,10 +178,12 @@ namespace armarx::RobotUnitModule
* @brief Returns the simox robot used in the control thread
* @return The simox robot used in the control thread
*/
- const VirtualRobot::RobotPtr& rtGetRobot() const
+ const VirtualRobot::RobotPtr&
+ rtGetRobot() const
{
return rtRobot;
}
+
/**
* @brief Changes the set of active controllers.
* Changes can be caused by a new set of requested controllers or emergency stop
@@ -194,29 +208,34 @@ namespace armarx::RobotUnitModule
* @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
- void rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/**
* @brief Runs Joint controllers and writes target values to ControlDevices
* @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
- void rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/**
* @brief Calls \ref SensorDevice::rtReadSensorValues for all \ref SensorDevice "SensorDevices".
* @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
- void rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/**
* @brief Execute code after updating the sensor values. Default implementation calculates the inverse dynamics.
* @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
- void virtual rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void virtual rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/// @brief Deactivates all NJointControllers generating invalid targets.
void rtHandleInvalidTargets();
/// @brief Calls \ref JointController::rtResetTarget for all active Joint controllers
void rtResetAllTargets();
+
/**
* @brief Hook for deriving classes (called AFTER a NJointControllerBase is deactivated due to an error)
*
@@ -226,14 +245,19 @@ namespace armarx::RobotUnitModule
* Since this hook is called after the controller with an error was deactivated an other call to
* \ref rtDeactivateAssignedNJointControllerBecauseOfError can't lead to a cyclic execution.
*/
- virtual void rtDeactivatedNJointControllerBecauseOfError(const NJointControllerBasePtr& /*nJointCtrl*/) {}
+ virtual void
+ rtDeactivatedNJointControllerBecauseOfError(const NJointControllerBasePtr& /*nJointCtrl*/)
+ {
+ }
+
/**
* @brief Updates the current \ref SensorValueBase "SensorValues" and \ref ControlTargetBase "ControlTargets"
* for code running outside of the \ref ControlThread
* @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
* @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
*/
- void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
/**
* @brief Returns all \ref ControlDevice "ControlDevices"
@@ -255,17 +279,21 @@ namespace armarx::RobotUnitModule
* @brief Returns whether the rt thread is in emergency stop. (This function is for use in the \ref ControlThread)
* @return Whether the rt thread is in emergency stop.
*/
- bool rtIsInEmergencyStop() const
+ bool
+ rtIsInEmergencyStop() const
{
return rtIsInEmergencyStop_;
}
+
/**
* @brief Returns the \ref ControlThread's emergency stop state. (This function is for use in the \ref ControlThread)
* @return The \ref ControlThread's emergency stop state
*/
- EmergencyStopState rtGetEmergencyStopState() const
+ EmergencyStopState
+ rtGetEmergencyStopState() const
{
- return rtIsInEmergencyStop_ ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ return rtIsInEmergencyStop_ ? EmergencyStopState::eEmergencyStopActive
+ : EmergencyStopState::eEmergencyStopInactive;
}
/**
@@ -327,21 +355,20 @@ namespace armarx::RobotUnitModule
void rtSyncNJointControllerRobot(NJointControllerBase* njctrl);
- void dumpRtControllerSetup(
- std::ostream& out,
- const std::string& indent,
- const std::vector<JointController*>& activeCdevs,
- const std::vector<std::size_t>& activeJCtrls,
- const std::vector<NJointControllerBase*>& assignement) const;
+ void dumpRtControllerSetup(std::ostream& out,
+ const std::string& indent,
+ const std::vector<JointController*>& activeCdevs,
+ const std::vector<std::size_t>& activeJCtrls,
+ const std::vector<NJointControllerBase*>& assignement) const;
std::string dumpRtState() const;
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
///@brief Whether the ControlThread is in emergency stop
- std::atomic_bool rtIsInEmergencyStop_ {false};
+ std::atomic_bool rtIsInEmergencyStop_{false};
///@brief Whether the ControlThread is supposed to be in emergency stop
- std::atomic_bool emergencyStop {false};
+ std::atomic_bool emergencyStop{false};
///@brief The \ref VirtualRobot::Robot used in the \ref ControlThread
VirtualRobot::RobotPtr rtRobot;
@@ -356,8 +383,8 @@ namespace armarx::RobotUnitModule
/// @brief An Error will be printed, if the execution time per ControlDev of a NJointControllerBase exceeds this parameter
std::size_t usPerDevUntilError;
- bool rtSwitchControllerSetupChangedControllers {false};
- std::size_t numberOfInvalidTargetsInThisIteration {0};
+ bool rtSwitchControllerSetupChangedControllers{false};
+ std::size_t numberOfInvalidTargetsInThisIteration{0};
std::vector<JointController*> preSwitchSetup_ActivatedJointControllers;
std::vector<std::size_t> preSwitchSetup_ActivatedJointToNJointControllerAssignement;
@@ -370,7 +397,8 @@ namespace armarx::RobotUnitModule
std::vector<std::size_t> _activatedSynchronousNJointControllersIdx;
std::vector<std::size_t> _activatedAsynchronousNJointControllersIdx;
- std::atomic<EmergencyStopStateRequest> emergencyStopStateRequest {EmergencyStopStateRequest::RequestNone};
+ std::atomic<EmergencyStopStateRequest> emergencyStopStateRequest{
+ EmergencyStopStateRequest::RequestNone};
std::shared_ptr<DynamicsHelper> dynamicsHelpers;
@@ -390,4 +418,4 @@ namespace armarx::RobotUnitModule
*/
friend class ControlThreadAttorneyForRobotUnitEmergencyStopMaster;
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
index 891c8fbf11edd151630ebb3e6fa4d244b6bc71ea..99b4a64e42605d0449e62145b869d8d64972aba7 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
@@ -22,11 +22,11 @@
#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+
#include "RobotUnitModuleControllerManagement.h"
#include "RobotUnitModuleDevices.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
-
namespace armarx::RobotUnitModule
{
/**
@@ -36,11 +36,14 @@ namespace armarx::RobotUnitModule
class DevicesAttorneyForControlThreadDataBuffer
{
friend class ControlThreadDataBuffer;
- static std::vector<JointController*> GetStopMovementJointControllers(ControlThreadDataBuffer* p)
+
+ static std::vector<JointController*>
+ GetStopMovementJointControllers(ControlThreadDataBuffer* p)
{
return p->_module<Devices>().getStopMovementJointControllers();
}
};
+
/**
* \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref ControlThreadDataBuffer.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -49,121 +52,134 @@ namespace armarx::RobotUnitModule
{
friend class ControlThreadDataBuffer;
- static void UpdateNJointControllerRequestedState(ControlThreadDataBuffer* p, const std::set<NJointControllerBasePtr>& request)
+ static void
+ UpdateNJointControllerRequestedState(ControlThreadDataBuffer* p,
+ const std::set<NJointControllerBasePtr>& request)
{
p->_module<ControllerManagement>().updateNJointControllerRequestedState(request);
}
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- JointAndNJointControllers ControlThreadDataBuffer::getActivatedControllers() const
+ JointAndNJointControllers
+ ControlThreadDataBuffer::getActivatedControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
return controllersActivated.getReadBuffer();
}
- std::vector<JointController*> ControlThreadDataBuffer::getActivatedJointControllers() const
+ std::vector<JointController*>
+ ControlThreadDataBuffer::getActivatedJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
return controllersActivated.getReadBuffer().jointControllers;
}
- std::vector<NJointControllerBase*> ControlThreadDataBuffer::getActivatedNJointControllers() const
+ std::vector<NJointControllerBase*>
+ ControlThreadDataBuffer::getActivatedNJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
return controllersActivated.getReadBuffer().nJointControllers;
}
- bool ControlThreadDataBuffer::activatedControllersChanged() const
+
+ bool
+ ControlThreadDataBuffer::activatedControllersChanged() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersActivatedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersActivatedMutex};
return controllersActivated.updateReadBuffer();
}
- JointAndNJointControllers ControlThreadDataBuffer::copyRequestedControllers() const
+ JointAndNJointControllers
+ ControlThreadDataBuffer::copyRequestedControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
return controllersRequested.getWriteBuffer();
}
- std::vector<JointController*> ControlThreadDataBuffer::copyRequestedJointControllers() const
+ std::vector<JointController*>
+ ControlThreadDataBuffer::copyRequestedJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
return controllersRequested.getWriteBuffer().jointControllers;
}
- std::vector<NJointControllerBase*> ControlThreadDataBuffer::copyRequestedNJointControllers() const
+ std::vector<NJointControllerBase*>
+ ControlThreadDataBuffer::copyRequestedNJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
return controllersRequested.getWriteBuffer().nJointControllers;
}
- bool ControlThreadDataBuffer::sensorAndControlBufferChanged() const
+ bool
+ ControlThreadDataBuffer::sensorAndControlBufferChanged() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
return controlThreadOutputBuffer.updateReadBuffer();
}
- const SensorAndControl& ControlThreadDataBuffer::getSensorAndControlBuffer() const
+ const SensorAndControl&
+ ControlThreadDataBuffer::getSensorAndControlBuffer() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
return controlThreadOutputBuffer.getReadBuffer();
}
- void ControlThreadDataBuffer::writeRequestedControllers(JointAndNJointControllers&& setOfControllers)
+ void
+ ControlThreadDataBuffer::writeRequestedControllers(JointAndNJointControllers&& setOfControllers)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
//check NJoint
const auto& nJointCtrls = setOfControllers.nJointControllers;
- std::set<NJointControllerBasePtr> nJointSet {nJointCtrls.begin(), nJointCtrls.end()};
+ std::set<NJointControllerBasePtr> nJointSet{nJointCtrls.begin(), nJointCtrls.end()};
nJointSet.erase(nullptr);
//# NJoint
- const std::size_t nNJointCtrls = std::count_if(nJointCtrls.begin(), nJointCtrls.end(), [](const NJointControllerBasePtr & p)
- {
- return p;
- });
+ const std::size_t nNJointCtrls =
+ std::count_if(nJointCtrls.begin(),
+ nJointCtrls.end(),
+ [](const NJointControllerBasePtr& p) { return p; });
ARMARX_CHECK_EXPRESSION(nNJointCtrls == nJointSet.size());
- ARMARX_CHECK_EXPRESSION(nJointCtrls.size() == _module<Devices>().getNumberOfControlDevices());
+ ARMARX_CHECK_EXPRESSION(nJointCtrls.size() ==
+ _module<Devices>().getNumberOfControlDevices());
//first nNJointCtrls not null
- ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls, [](NJointControllerBase * p)
- {
- return p;
- }));
+ ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin(),
+ nJointCtrls.begin() + nNJointCtrls,
+ [](NJointControllerBase* p) { return p; }));
//last getNumberOfControlDevices()-nNJointCtrls null
- ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin() + nNJointCtrls, nJointCtrls.end(), [](NJointControllerBase * p)
- {
- return !p;
- }));
+ ARMARX_CHECK_EXPRESSION(std::all_of(nJointCtrls.begin() + nNJointCtrls,
+ nJointCtrls.end(),
+ [](NJointControllerBase* p) { return !p; }));
//conflict free and sorted
- ARMARX_CHECK_EXPRESSION(std::is_sorted(nJointCtrls.begin(), nJointCtrls.end(), std::greater<NJointControllerBase*> {}));
- ARMARX_CHECK_EXPRESSION(NJointControllerBase::AreNotInConflict(nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls));
+ ARMARX_CHECK_EXPRESSION(std::is_sorted(
+ nJointCtrls.begin(), nJointCtrls.end(), std::greater<NJointControllerBase*>{}));
+ ARMARX_CHECK_EXPRESSION(NJointControllerBase::AreNotInConflict(
+ nJointCtrls.begin(), nJointCtrls.begin() + nNJointCtrls));
//check JointCtrl
const auto& jointCtrls = setOfControllers.jointControllers;
- ARMARX_CHECK_EXPRESSION(jointCtrls.size() == _module<Devices>().getNumberOfControlDevices());
- ARMARX_CHECK_EXPRESSION(std::all_of(jointCtrls.begin(), jointCtrls.end(), [](JointController * p)
- {
- return p;
- }));
+ ARMARX_CHECK_EXPRESSION(jointCtrls.size() ==
+ _module<Devices>().getNumberOfControlDevices());
+ ARMARX_CHECK_EXPRESSION(std::all_of(
+ jointCtrls.begin(), jointCtrls.end(), [](JointController* p) { return p; }));
//check groups
{
@@ -172,13 +188,17 @@ namespace armarx::RobotUnitModule
for (const auto& group : _module<Devices>().getControlModeGroups().groups)
{
ARMARX_CHECK_EXPRESSION(!group.empty());
- const auto hwMode = setOfControllers.jointControllers.at(_module<Devices>().getControlDeviceIndex(*group.begin()))->getHardwareControlMode();
+ const auto hwMode =
+ setOfControllers.jointControllers
+ .at(_module<Devices>().getControlDeviceIndex(*group.begin()))
+ ->getHardwareControlMode();
ARMARX_DEBUG << "---- group " << grpIdx << " mode '" << hwMode << "'";
for (const auto& other : group)
{
const auto& dev = _module<Devices>().getControlDeviceIndex(other);
ARMARX_CHECK_EXPRESSION(dev);
- const auto otherHwMode = setOfControllers.jointControllers.at(dev)->getHardwareControlMode();
+ const auto otherHwMode =
+ setOfControllers.jointControllers.at(dev)->getHardwareControlMode();
ARMARX_DEBUG << "-------- '" << other << "'' mode '" << otherHwMode << "'";
ARMARX_CHECK_EXPRESSION(otherHwMode == hwMode);
}
@@ -189,9 +209,12 @@ namespace armarx::RobotUnitModule
{
ARMARX_DEBUG << "setup assignement index";
setOfControllers.resetAssignement();
- for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < setOfControllers.nJointControllers.size(); ++nJointCtrlIndex)
+ for (std::size_t nJointCtrlIndex = 0;
+ nJointCtrlIndex < setOfControllers.nJointControllers.size();
+ ++nJointCtrlIndex)
{
- const NJointControllerBase* nJoint = setOfControllers.nJointControllers.at(nJointCtrlIndex);
+ const NJointControllerBase* nJoint =
+ setOfControllers.nJointControllers.at(nJointCtrlIndex);
if (!nJoint)
{
break;
@@ -199,9 +222,14 @@ namespace armarx::RobotUnitModule
ARMARX_DEBUG << "---- " << nJoint->getInstanceName();
for (std::size_t jointIndex : nJoint->getControlDeviceUsedIndices())
{
- ARMARX_DEBUG << "-------- Index " << jointIndex << ": " << setOfControllers.jointToNJointControllerAssignement.at(jointIndex) << "-> " << nJointCtrlIndex;
- ARMARX_CHECK_EXPRESSION(setOfControllers.jointToNJointControllerAssignement.at(jointIndex) == IndexSentinel());
- setOfControllers.jointToNJointControllerAssignement.at(jointIndex) = nJointCtrlIndex;
+ ARMARX_DEBUG << "-------- Index " << jointIndex << ": "
+ << setOfControllers.jointToNJointControllerAssignement.at(
+ jointIndex)
+ << "-> " << nJointCtrlIndex;
+ ARMARX_CHECK_EXPRESSION(setOfControllers.jointToNJointControllerAssignement.at(
+ jointIndex) == IndexSentinel());
+ setOfControllers.jointToNJointControllerAssignement.at(jointIndex) =
+ nJointCtrlIndex;
}
}
}
@@ -211,40 +239,45 @@ namespace armarx::RobotUnitModule
checkSetOfControllersToActivate(setOfControllers);
}
//set requested state
- ControllerManagementAttorneyForControlThreadDataBuffer::UpdateNJointControllerRequestedState(this, nJointSet);
+ ControllerManagementAttorneyForControlThreadDataBuffer::
+ UpdateNJointControllerRequestedState(this, nJointSet);
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.jointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.nJointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), setOfControllers.jointToNJointControllerAssignement.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ setOfControllers.jointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ setOfControllers.nJointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ setOfControllers.jointToNJointControllerAssignement.size());
{
- std::lock_guard<std::recursive_mutex> guard {controllersRequestedMutex};
+ std::lock_guard<std::recursive_mutex> guard{controllersRequestedMutex};
controllersRequested.getWriteBuffer() = std::move(setOfControllers);
controllersRequested.commitWrite();
}
- ARMARX_DEBUG << "writeRequestedControllers(JointAndNJointControllers&& setOfControllers)...done!";
+ ARMARX_DEBUG
+ << "writeRequestedControllers(JointAndNJointControllers&& setOfControllers)...done!";
}
- void ControlThreadDataBuffer::setActivateControllersRequest(std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls)
+ void
+ ControlThreadDataBuffer::setActivateControllersRequest(
+ std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::recursive_mutex> guardRequested {controllersRequestedMutex};
+ std::lock_guard<std::recursive_mutex> guardRequested{controllersRequestedMutex};
throwIfDevicesNotReady(__FUNCTION__);
//erase nullptr
ctrls.erase(nullptr);
ARMARX_CHECK_EXPRESSION(ctrls.size() <= _module<Devices>().getNumberOfControlDevices());
- const std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> setOfRequestedCtrls
- {
- controllersRequested.getWriteBuffer().nJointControllers.begin(),
- controllersRequested.getWriteBuffer().nJointControllers.end()
- };
+ const std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>>
+ setOfRequestedCtrls{controllersRequested.getWriteBuffer().nJointControllers.begin(),
+ controllersRequested.getWriteBuffer().nJointControllers.end()};
if (setOfRequestedCtrls == ctrls)
{
//redirty the flag to swap in the same set again
controllersRequested.commitWrite();
return;
}
- JointAndNJointControllers request {_module<Devices>().getNumberOfControlDevices()};
+ JointAndNJointControllers request{_module<Devices>().getNumberOfControlDevices()};
std::size_t idx = 0;
for (const NJointControllerBasePtr& nJointCtrl : ctrls)
{
@@ -252,19 +285,25 @@ namespace armarx::RobotUnitModule
//add Joint for this ctrl
for (const auto& cd2cm : nJointCtrl->getControlDeviceUsedControlModeMap())
{
- std::size_t jointCtrlIndex = _module<Devices>().getControlDevices().index(cd2cm.first);
+ std::size_t jointCtrlIndex =
+ _module<Devices>().getControlDevices().index(cd2cm.first);
if (request.jointControllers.at(jointCtrlIndex))
{
std::stringstream ss;
- ss << "RobotUnit:setActivateControllersRequest controllers to activate are in conflict!\ncontrollers:";
+ ss << "RobotUnit:setActivateControllersRequest controllers to activate are in "
+ "conflict!\ncontrollers:";
for (const auto& ctrl : ctrls)
{
ss << "\n" << ctrl->getInstanceName();
}
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
- request.jointControllers.at(jointCtrlIndex) = _module<Devices>().getControlDevices().at(jointCtrlIndex)->getJointController(cd2cm.second);
+ request.jointControllers.at(jointCtrlIndex) =
+ _module<Devices>()
+ .getControlDevices()
+ .at(jointCtrlIndex)
+ ->getJointController(cd2cm.second);
}
}
@@ -274,8 +313,10 @@ namespace armarx::RobotUnitModule
JointController*& jointCtrl = request.jointControllers.at(i);
if (!jointCtrl)
{
- JointController* jointCtrlOld = controllersRequested.getWriteBuffer().jointControllers.at(i);
- if (jointCtrlOld == _module<Devices>().getControlDevices().at(i)->getJointStopMovementController())
+ JointController* jointCtrlOld =
+ controllersRequested.getWriteBuffer().jointControllers.at(i);
+ if (jointCtrlOld ==
+ _module<Devices>().getControlDevices().at(i)->getJointStopMovementController())
{
//don't replace this controller with emergency stop
jointCtrl = jointCtrlOld;
@@ -283,7 +324,10 @@ namespace armarx::RobotUnitModule
else
{
//no one controls this device -> emergency stop
- jointCtrl = _module<Devices>().getControlDevices().at(i)->getJointEmergencyStopController();
+ jointCtrl = _module<Devices>()
+ .getControlDevices()
+ .at(i)
+ ->getJointEmergencyStopController();
}
}
}
@@ -295,43 +339,61 @@ namespace armarx::RobotUnitModule
const auto& ctrl = request.nJointControllers.at(i);
if (ctrl)
{
- out << i << "\t'" << ctrl->getInstanceName() << "' \t of class '" << ctrl->getClassName() << "'\n";
+ out << i << "\t'" << ctrl->getInstanceName() << "' \t of class '"
+ << ctrl->getClassName() << "'\n";
}
}
};
writeRequestedControllers(std::move(request));
}
- void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes) const
+ void
+ ControlThreadDataBuffer::updateVirtualRobot(
+ const VirtualRobot::RobotPtr& robot,
+ const std::vector<VirtualRobot::RobotNodePtr>& nodes) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- _module<Devices>().updateVirtualRobotFromSensorValues(robot, nodes, controlThreadOutputBuffer.getReadBuffer().sensors);
+ _module<Devices>().updateVirtualRobotFromSensorValues(
+ robot, nodes, controlThreadOutputBuffer.getReadBuffer().sensors);
}
- void ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const
+ void
+ ControlThreadDataBuffer::updateVirtualRobot(const VirtualRobot::RobotPtr& robot) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- _module<Devices>().updateVirtualRobotFromSensorValues(robot, controlThreadOutputBuffer.getReadBuffer().sensors);
+ _module<Devices>().updateVirtualRobotFromSensorValues(
+ robot, controlThreadOutputBuffer.getReadBuffer().sensors);
}
- void ControlThreadDataBuffer::_postFinishDeviceInitialization()
+
+ void
+ ControlThreadDataBuffer::_postFinishDeviceInitialization()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "initializing buffers:";
{
- ARMARX_DEBUG << "----initializing controller buffers for " << _module<Devices>().getNumberOfControlDevices() << " control devices";
+ ARMARX_DEBUG << "----initializing controller buffers for "
+ << _module<Devices>().getNumberOfControlDevices() << " control devices";
JointAndNJointControllers ctrlinitReq(_module<Devices>().getNumberOfControlDevices());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.nJointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointToNJointControllerAssignement.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.jointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.nJointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.jointToNJointControllerAssignement.size());
controllersActivated.reinitAllBuffers(ctrlinitReq);
- ctrlinitReq.jointControllers = DevicesAttorneyForControlThreadDataBuffer::GetStopMovementJointControllers(this);
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.nJointControllers.size());
- ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(), ctrlinitReq.jointToNJointControllerAssignement.size());
+ ctrlinitReq.jointControllers =
+ DevicesAttorneyForControlThreadDataBuffer::GetStopMovementJointControllers(this);
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.jointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.nJointControllers.size());
+ ARMARX_CHECK_EQUAL(_module<Devices>().getNumberOfControlDevices(),
+ ctrlinitReq.jointToNJointControllerAssignement.size());
controllersRequested.reinitAllBuffers(ctrlinitReq);
- controllersRequested.commitWrite();//to make sure the diry bit is activated (to trigger rt switch)
+ controllersRequested
+ .commitWrite(); //to make sure the diry bit is activated (to trigger rt switch)
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
index 66597aee2f9b7d14f597fa4fcaf0b2e533179db7..1833042321debe6dbd1487e9d5af4a673c670ac5 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
@@ -28,11 +28,10 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include "RobotUnitModuleBase.h"
-#include "../util/JointAndNJointControllers.h"
-#include "../util/ControlThreadOutputBuffer.h"
-
#include "../SensorValues/SensorValue1DoFActuator.h"
+#include "../util/ControlThreadOutputBuffer.h"
+#include "../util/JointAndNJointControllers.h"
+#include "RobotUnitModuleBase.h"
namespace armarx
{
@@ -80,15 +79,18 @@ namespace armarx::RobotUnitModule
class ControlThreadDataBuffer : virtual public ModuleBase
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static ControlThreadDataBuffer& Instance()
+ static ControlThreadDataBuffer&
+ Instance()
{
return ModuleBase::Instance<ControlThreadDataBuffer>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -109,7 +111,8 @@ namespace armarx::RobotUnitModule
* @param robot The robot to update
* @param nodes The robot's nodes
*/
- void updateVirtualRobot(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes) const;
+ void updateVirtualRobot(const VirtualRobot::RobotPtr& robot,
+ const std::vector<VirtualRobot::RobotNodePtr>& nodes) const;
/**
* @brief Returns whether the set of activated \ref JointController "Joint-" and
@@ -199,7 +202,8 @@ namespace armarx::RobotUnitModule
* \warning This function is dangerous to use.
* @return The \ref ControlThreadOutputBuffer
*/
- ControlThreadOutputBuffer& getControlThreadOutputBuffer() ///TODO refactor logging and remove this function
+ ControlThreadOutputBuffer&
+ getControlThreadOutputBuffer() ///TODO refactor logging and remove this function
{
return controlThreadOutputBuffer;
}
@@ -208,14 +212,17 @@ namespace armarx::RobotUnitModule
* @brief Returns the writer buffer for sensor and control values.
* @return The writer buffer for sensor and control values.
*/
- SensorAndControl& rtGetSensorAndControlBuffer() ///TODO move to attorney
+ SensorAndControl&
+ rtGetSensorAndControlBuffer() ///TODO move to attorney
{
return controlThreadOutputBuffer.getWriteBuffer();
}
+
/**
* @brief Commits the writer buffer for sensor and control values.
*/
- void rtSensorAndControlBufferCommitWrite() ///TODO move to attorney
+ void
+ rtSensorAndControlBufferCommitWrite() ///TODO move to attorney
{
return controlThreadOutputBuffer.commitWrite();
}
@@ -230,7 +237,8 @@ namespace armarx::RobotUnitModule
* @see NJointControllerBase::activateController
* @see NJointControllerBase::deactivateController
*/
- void setActivateControllersRequest(std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr> > ctrls);
+ void setActivateControllersRequest(
+ std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls);
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -242,7 +250,11 @@ namespace armarx::RobotUnitModule
* If any check fails, throw an exception with a descriptive message.
* @param controllers The controllers to activate
*/
- virtual void checkSetOfControllersToActivate(const JointAndNJointControllers& /*controllers*/) const {}
+ virtual void
+ checkSetOfControllersToActivate(const JointAndNJointControllers& /*controllers*/) const
+ {
+ }
+
private:
void writeRequestedControllers(JointAndNJointControllers&& setOfControllers);
@@ -268,7 +280,8 @@ namespace armarx::RobotUnitModule
/// \warning DO NOT ACCESS EXCEPT YOU KNOW WHAT YOU ARE DOING!
ControlThreadOutputBuffer controlThreadOutputBuffer;
/// @brief Mutex guarding \ref controlThreadOutputBuffer
- mutable std::recursive_mutex controlThreadOutputBufferMutex; /// TODO use this mutex instead of the global one
+ mutable std::recursive_mutex
+ controlThreadOutputBufferMutex; /// TODO use this mutex instead of the global one
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////////// Attorneys ////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -279,5 +292,4 @@ namespace armarx::RobotUnitModule
*/
friend class ControlThreadDataBufferAttorneyForControlThread;
};
-}
-
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index 60ebf6dbf533c27820ee7b4b950e858004cac60f..6f0001391b69aa246edc781fee744997f214f988 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -23,23 +23,23 @@
#include "RobotUnitModuleControllerManagement.h"
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
-#include <ArmarXCore/core/ArmarXManager.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/core/util/algorithm.h>
-
namespace armarx::RobotUnitModule
{
- template<class Cont>
- static Ice::StringSeq GetNonNullNames(const Cont& c)
+ template <class Cont>
+ static Ice::StringSeq
+ GetNonNullNames(const Cont& c)
{
Ice::StringSeq result;
result.reserve(c.size());
@@ -60,42 +60,50 @@ namespace armarx::RobotUnitModule
class NJointControllerAttorneyForControllerManagement
{
friend class ControllerManagement;
- static void SetRequested(const NJointControllerBasePtr& nJointCtrl, bool requested)
+
+ static void
+ SetRequested(const NJointControllerBasePtr& nJointCtrl, bool requested)
{
nJointCtrl->isRequested = requested;
}
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- Ice::StringSeq ControllerManagement::getRequestedNJointControllerNames(const Ice::Current&) const
+ Ice::StringSeq
+ ControllerManagement::getRequestedNJointControllerNames(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return GetNonNullNames(_module<ControlThreadDataBuffer>().copyRequestedNJointControllers());
}
- Ice::StringSeq ControllerManagement::getActivatedNJointControllerNames(const Ice::Current&) const
+ Ice::StringSeq
+ ControllerManagement::getActivatedNJointControllerNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return GetNonNullNames(_module<ControlThreadDataBuffer>().getActivatedNJointControllers());
}
- void ControllerManagement::checkNJointControllerClassName(const std::string& className) const
+ void
+ ControllerManagement::checkNJointControllerClassName(const std::string& className) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!NJointControllerRegistry::has(className))
{
std::stringstream ss;
- ss << "Requested controller class '" << className << "' unknown! Known classes:" << NJointControllerRegistry::getKeys()
- << " (If this class exists in a different lib then load it via loadLibFromPath(path) or loadLibFromPackage(package, lib))";
+ ss << "Requested controller class '" << className
+ << "' unknown! Known classes:" << NJointControllerRegistry::getKeys()
+ << " (If this class exists in a different lib then load it via "
+ "loadLibFromPath(path) or loadLibFromPackage(package, lib))";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
}
- std::vector<NJointControllerBasePtr> ControllerManagement::getNJointControllersNotNull(const std::vector<std::string>& names) const
+ std::vector<NJointControllerBasePtr>
+ ControllerManagement::getNJointControllersNotNull(const std::vector<std::string>& names) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -109,7 +117,8 @@ namespace armarx::RobotUnitModule
return ctrl;
}
- const NJointControllerBasePtr& ControllerManagement::getNJointControllerNotNull(const std::string& name) const
+ const NJointControllerBasePtr&
+ ControllerManagement::getNJointControllerNotNull(const std::string& name) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -120,26 +129,29 @@ namespace armarx::RobotUnitModule
std::stringstream ss;
ss << "RobotUnit: there is no NJointControllerBase with name '" << name
<< "'. Existing NJointControllers are: " << getNJointControllerNames();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
if (!it->second)
{
std::stringstream ss;
ss << "RobotUnit: The NJointControllerBase with name '" << name
- << "'. Is a nullptr! This should never be the case (invariant)! \nMap:\n" << nJointControllers;
+ << "'. Is a nullptr! This should never be the case (invariant)! \nMap:\n"
+ << nJointControllers;
ARMARX_FATAL << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return it->second;
}
- void ControllerManagement::deleteNJointController(const NJointControllerBasePtr& ctrl)
+ void
+ ControllerManagement::deleteNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deleteNJointControllers(std::vector<NJointControllerBasePtr> {ctrl});
+ deleteNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
}
- StringNJointControllerPrxDictionary ControllerManagement::getAllNJointControllers(const Ice::Current&) const
+ StringNJointControllerPrxDictionary
+ ControllerManagement::getAllNJointControllers(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
@@ -151,24 +163,27 @@ namespace armarx::RobotUnitModule
StringNJointControllerPrxDictionary result;
for (const auto& pair : nJointControllersCopy)
{
- result[pair.first] = NJointControllerInterfacePrx::uncheckedCast(pair.second->getProxy(-1, true));
+ result[pair.first] =
+ NJointControllerInterfacePrx::uncheckedCast(pair.second->getProxy(-1, true));
}
return result;
}
- NJointControllerInterfacePrx ControllerManagement::createNJointController(
- const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- const Ice::Current&)
+ NJointControllerInterfacePrx
+ ControllerManagement::createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//no lock required
return NJointControllerInterfacePrx::uncheckedCast(
- createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
+ createNJointController(className, instanceName, config, true, false)
+ ->getProxy(-1, true));
}
- NJointControllerInterfacePrx ControllerManagement::createNJointControllerFromVariantConfig(
+ NJointControllerInterfacePrx
+ ControllerManagement::createNJointControllerFromVariantConfig(
const std::string& className,
const std::string& instanceName,
const StringVariantBaseMap& variants,
@@ -180,24 +195,28 @@ namespace armarx::RobotUnitModule
if (!NJointControllerRegistry::get(className)->hasRemoteConfiguration())
{
std::stringstream ss;
- ss << "Requested controller class '" << className << "' allows no remote configuration" << NJointControllerRegistry::getKeys()
- << " (Implement 'static WidgetDescription::WidgetPtr " << className << "::GenerateConfigDescription()'"
+ ss << "Requested controller class '" << className << "' allows no remote configuration"
+ << NJointControllerRegistry::getKeys()
+ << " (Implement 'static WidgetDescription::WidgetPtr " << className
+ << "::GenerateConfigDescription()'"
<< " and 'static NJointControllerConfigPtr " << className
- << "::GenerateConfigFromVariants(const StringVariantBaseMap&)' to allow remote configuration";
+ << "::GenerateConfigFromVariants(const StringVariantBaseMap&)' to allow remote "
+ "configuration";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return createNJointController(
- className, instanceName,
- NJointControllerRegistry::get(className)->GenerateConfigFromVariants(variants),
- Ice::emptyCurrent/*to select ice overload*/);
+ className,
+ instanceName,
+ NJointControllerRegistry::get(className)->GenerateConfigFromVariants(variants),
+ Ice::emptyCurrent /*to select ice overload*/);
}
- NJointControllerInterfacePrx ControllerManagement::createOrReplaceNJointController(
- const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- const Ice::Current&)
+ NJointControllerInterfacePrx
+ ControllerManagement::createOrReplaceNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -232,15 +251,16 @@ namespace armarx::RobotUnitModule
ARMARX_INFO << "wiating until controller '" << instanceName << "' is removed from ice";
}
return NJointControllerInterfacePrx::uncheckedCast(
- createNJointController(className, instanceName, config, true, false)->getProxy(-1, true));
+ createNJointController(className, instanceName, config, true, false)
+ ->getProxy(-1, true));
}
- const NJointControllerBasePtr& ControllerManagement::createNJointController(
- const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- bool deletable,
- bool internal)
+ const NJointControllerBasePtr&
+ ControllerManagement::createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool deletable,
+ bool internal)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -248,7 +268,7 @@ namespace armarx::RobotUnitModule
if (instanceName.empty())
{
ARMARX_ERROR << "The instance name is empty! (give a unique name)";
- throw InvalidArgumentException {"The instance name is empty! (give a unique name)"};
+ throw InvalidArgumentException{"The instance name is empty! (give a unique name)"};
}
//check if we would be able to create the class
checkNJointControllerClassName(className);
@@ -258,24 +278,20 @@ namespace armarx::RobotUnitModule
if (nJointControllers.count(instanceName))
{
std::stringstream ss;
- ss << "There already is a controller instance with the name '" << instanceName << "'. Use a different instance name instead."
+ ss << "There already is a controller instance with the name '" << instanceName
+ << "'. Use a different instance name instead."
<< " Other used instance names are " << getNJointControllerNames();
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
//create the controller
ARMARX_CHECK_EXPRESSION(factory);
- NJointControllerBasePtr nJointCtrl = factory->create(
- this,
- config,
- controllerCreateRobot,
- deletable, internal,
- instanceName
- );
- ARMARX_CHECK_NOT_EQUAL(
- nJointCtrl->getControlDeviceUsedIndices().size(), 0) <<
- "The NJointControllerBase '" << nJointCtrl->getName() << "' uses no ControlDevice! (It has to use at least one)";
+ NJointControllerBasePtr nJointCtrl =
+ factory->create(this, config, controllerCreateRobot, deletable, internal, instanceName);
+ ARMARX_CHECK_NOT_EQUAL(nJointCtrl->getControlDeviceUsedIndices().size(), 0)
+ << "The NJointControllerBase '" << nJointCtrl->getName()
+ << "' uses no ControlDevice! (It has to use at least one)";
getArmarXManager()->addObject(nJointCtrl, instanceName, false, false);
nJointControllers[instanceName] = std::move(nJointCtrl);
@@ -283,8 +299,8 @@ namespace armarx::RobotUnitModule
return nJointControllers.at(instanceName);
}
-
- bool ControllerManagement::loadLibFromPath(const std::string& path, const Ice::Current&)
+ bool
+ ControllerManagement::loadLibFromPath(const std::string& path, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
const bool result = getArmarXManager()->loadLibFromPath(path);
@@ -292,7 +308,10 @@ namespace armarx::RobotUnitModule
return result;
}
- bool ControllerManagement::loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current&)
+ bool
+ ControllerManagement::loadLibFromPackage(const std::string& package,
+ const std::string& lib,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
const bool result = getArmarXManager()->loadLibFromPackage(package, lib);
@@ -300,19 +319,24 @@ namespace armarx::RobotUnitModule
return result;
}
- Ice::StringSeq ControllerManagement::getNJointControllerClassNames(const Ice::Current&) const
+ Ice::StringSeq
+ ControllerManagement::getNJointControllerClassNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return NJointControllerRegistry::getKeys();
}
- Ice::StringSeq ControllerManagement::getNJointControllerNames(const Ice::Current&) const
+ Ice::StringSeq
+ ControllerManagement::getNJointControllerNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
return getMapKeys(nJointControllers);
}
- std::vector<std::string> ControllerManagement::getNJointControllerNames(const std::vector<NJointControllerBasePtr>& ctrls) const
+
+ std::vector<std::string>
+ ControllerManagement::getNJointControllerNames(
+ const std::vector<NJointControllerBasePtr>& ctrls) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::vector<std::string> result;
@@ -327,22 +351,31 @@ namespace armarx::RobotUnitModule
return result;
}
- void ControllerManagement::activateNJointController(const std::string& name, const Ice::Current&)
+ void
+ ControllerManagement::activateNJointController(const std::string& name, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
activateNJointControllers(getNJointControllersNotNull({name}));
}
- void ControllerManagement::activateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+
+ void
+ ControllerManagement::activateNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
activateNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::activateNJointController(const NJointControllerBasePtr& ctrl)
+
+ void
+ ControllerManagement::activateNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- activateNJointControllers(std::vector<NJointControllerBasePtr> {ctrl});
+ activateNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
}
- void ControllerManagement::activateNJointControllers(const std::vector<NJointControllerBasePtr>& ctrlsToActVec)
+
+ void
+ ControllerManagement::activateNJointControllers(
+ const std::vector<NJointControllerBasePtr>& ctrlsToActVec)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (ctrlsToActVec.empty())
@@ -352,24 +385,21 @@ namespace armarx::RobotUnitModule
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
//if not activate them
- std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrlsToAct {ctrlsToActVec.begin(), ctrlsToActVec.end()};
+ std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrlsToAct{
+ ctrlsToActVec.begin(), ctrlsToActVec.end()};
ARMARX_CHECK_EXPRESSION(!ctrlsToAct.count(nullptr));
//check if all already active
- if (
- std::all_of(
- ctrlsToActVec.begin(), ctrlsToActVec.end(),
- [](const NJointControllerBasePtr & ctrl)
- {
- return ctrl->isControllerActive();
- }
- )
- )
+ if (std::all_of(ctrlsToActVec.begin(),
+ ctrlsToActVec.end(),
+ [](const NJointControllerBasePtr& ctrl)
+ { return ctrl->isControllerActive(); }))
{
return;
}
//get already requested
const auto ctrlVector = _module<ControlThreadDataBuffer>().copyRequestedNJointControllers();
- std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrlsAlreadyRequested {ctrlVector.begin(), ctrlVector.end()};
+ std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>>
+ ctrlsAlreadyRequested{ctrlVector.begin(), ctrlVector.end()};
ctrlsAlreadyRequested.erase(nullptr);
//check for conflict
std::vector<char> inuse;
@@ -379,10 +409,11 @@ namespace armarx::RobotUnitModule
if (!r)
{
std::stringstream ss;
- ss << "activateNJointControllers: requested controllers are in conflict!\ncontrollers:\n"
+ ss << "activateNJointControllers: requested controllers are in "
+ "conflict!\ncontrollers:\n"
<< getNJointControllerNames(ctrlsToActVec);
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
inuse = std::move(*r);
}
@@ -411,14 +442,16 @@ namespace armarx::RobotUnitModule
if (r)
{
ARMARX_DEBUG << "keeping already requested NJointControllerBase '"
- << nJointCtrl->getInstanceName() << "' in list of requested controllers";
+ << nJointCtrl->getInstanceName()
+ << "' in list of requested controllers";
ctrlsToAct.insert(nJointCtrl);
inuse = std::move(*r);
}
else
{
ARMARX_INFO << "removing already requested NJointControllerBase '"
- << nJointCtrl->getInstanceName() << "' from list of requested controllers";
+ << nJointCtrl->getInstanceName()
+ << "' from list of requested controllers";
}
}
ARMARX_DEBUG << "inuse field (all)\n" << printInUse;
@@ -426,22 +459,31 @@ namespace armarx::RobotUnitModule
_module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrlsToAct);
}
- void ControllerManagement::deactivateNJointController(const std::string& name, const Ice::Current&)
+ void
+ ControllerManagement::deactivateNJointController(const std::string& name, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateNJointControllers(getNJointControllersNotNull({name}));
}
- void ControllerManagement::deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+
+ void
+ ControllerManagement::deactivateNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::deactivateNJointController(const NJointControllerBasePtr& ctrl)
+
+ void
+ ControllerManagement::deactivateNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- deactivateNJointControllers(std::vector<NJointControllerBasePtr> {ctrl});
+ deactivateNJointControllers(std::vector<NJointControllerBasePtr>{ctrl});
}
- void ControllerManagement::deactivateNJointControllers(const std::vector<NJointControllerBasePtr>& ctrlsDeacVec)
+
+ void
+ ControllerManagement::deactivateNJointControllers(
+ const std::vector<NJointControllerBasePtr>& ctrlsDeacVec)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -451,7 +493,8 @@ namespace armarx::RobotUnitModule
return;
}
const auto ctrlVector = _module<ControlThreadDataBuffer>().copyRequestedNJointControllers();
- std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls {ctrlVector.begin(), ctrlVector.end()};
+ std::set<NJointControllerBasePtr, std::greater<NJointControllerBasePtr>> ctrls{
+ ctrlVector.begin(), ctrlVector.end()};
const std::size_t ctrlsNum = ctrls.size();
for (const auto& nJointCtrlToDeactivate : ctrlsDeacVec)
{
@@ -464,25 +507,36 @@ namespace armarx::RobotUnitModule
_module<ControlThreadDataBuffer>().setActivateControllersRequest(ctrls);
}
- void ControllerManagement::deleteNJointController(const std::string& name, const Ice::Current&)
+ void
+ ControllerManagement::deleteNJointController(const std::string& name, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deleteNJointControllers(getNJointControllersNotNull({name}));
}
- void ControllerManagement::deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+
+ void
+ ControllerManagement::deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deleteNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::switchNJointControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current&)
+
+ void
+ ControllerManagement::switchNJointControllerSetup(const Ice::StringSeq& newSetup,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
throwIfDevicesNotReady(__FUNCTION__);
- auto ctrlsToActVec = getNJointControllersNotNull(newSetup); //also checks if these controllers exist
- _module<ControlThreadDataBuffer>().setActivateControllersRequest({ctrlsToActVec.begin(), ctrlsToActVec.end()});
+ auto ctrlsToActVec =
+ getNJointControllersNotNull(newSetup); //also checks if these controllers exist
+ _module<ControlThreadDataBuffer>().setActivateControllersRequest(
+ {ctrlsToActVec.begin(), ctrlsToActVec.end()});
}
- void ControllerManagement::deleteNJointControllers(const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
+
+ void
+ ControllerManagement::deleteNJointControllers(
+ const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -496,19 +550,15 @@ namespace armarx::RobotUnitModule
{
if (!nJointCtrl->isDeletable())
{
- throw LogicError
- {
- "The NJointControllerBase '" + nJointCtrl->getInstanceName() +
- "' can't be deleted since this operation is not allowed for this controller! (no NJointControllerBase was deleted)"
- };
+ throw LogicError{"The NJointControllerBase '" + nJointCtrl->getInstanceName() +
+ "' can't be deleted since this operation is not allowed for this "
+ "controller! (no NJointControllerBase was deleted)"};
}
if (nJointCtrl->isControllerActive() || nJointCtrl->isControllerRequested())
{
- throw LogicError
- {
- "The NJointControllerBase '" + nJointCtrl->getInstanceName() +
- "' can't be deleted since it is active or requested! (no NJointControllerBase was deleted)"
- };
+ throw LogicError{"The NJointControllerBase '" + nJointCtrl->getInstanceName() +
+ "' can't be deleted since it is active or requested! (no "
+ "NJointControllerBase was deleted)"};
}
}
for (const auto& nJointCtrl : ctrlsToDelVec)
@@ -521,22 +571,32 @@ namespace armarx::RobotUnitModule
}
}
- void ControllerManagement::deactivateAndDeleteNJointController(const std::string& name, const Ice::Current&)
+ void
+ ControllerManagement::deactivateAndDeleteNJointController(const std::string& name,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers(getNJointControllersNotNull({name}));
}
- void ControllerManagement::deactivateAndDeleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&)
+
+ void
+ ControllerManagement::deactivateAndDeleteNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers(getNJointControllersNotNull(names));
}
- void ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerBasePtr& ctrl)
+
+ void
+ ControllerManagement::deactivateAndDeleteNJointController(const NJointControllerBasePtr& ctrl)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
deactivateAndDeleteNJointControllers({ctrl});
}
- void ControllerManagement::deactivateAndDeleteNJointControllers(const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
+
+ void
+ ControllerManagement::deactivateAndDeleteNJointControllers(
+ const std::vector<NJointControllerBasePtr>& ctrlsToDelVec)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -546,31 +606,27 @@ namespace armarx::RobotUnitModule
return;
}
deactivateNJointControllers(ctrlsToDelVec);
- while (
- std::any_of(
- ctrlsToDelVec.begin(),
- ctrlsToDelVec.end(),
- [](const NJointControllerBasePtr & ctrl)
- {
- return ctrl->isControllerActive();
- }
- )
- )
+ while (std::any_of(ctrlsToDelVec.begin(),
+ ctrlsToDelVec.end(),
+ [](const NJointControllerBasePtr& ctrl)
+ { return ctrl->isControllerActive(); }))
{
if (isShuttingDown())
{
return;
}
- std::this_thread::sleep_for(std::chrono::microseconds {100});
+ std::this_thread::sleep_for(std::chrono::microseconds{100});
}
deleteNJointControllers(ctrlsToDelVec);
}
- NJointControllerClassDescription ControllerManagement::getNJointControllerClassDescription(
- const std::string& className, const Ice::Current&) const
+ NJointControllerClassDescription
+ ControllerManagement::getNJointControllerClassDescription(const std::string& className,
+ const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- while (getRobotUnitState() == RobotUnitState::InitializingComponent || getRobotUnitState() == RobotUnitState::InitializingDevices)
+ while (getRobotUnitState() == RobotUnitState::InitializingComponent ||
+ getRobotUnitState() == RobotUnitState::InitializingDevices)
{
//this phase should only last short so busy waiting is ok
std::this_thread::sleep_for(std::chrono::milliseconds(50));
@@ -586,23 +642,25 @@ namespace armarx::RobotUnitModule
NJointControllerRegistry::get(className)->GenerateConfigDescription(
controllerCreateRobot,
_module<Devices>().getControlDevicesConstPtr(),
- _module<Devices>().getSensorDevicesConstPtr()
- );
+ _module<Devices>().getSensorDevicesConstPtr());
}
return data;
}
- NJointControllerClassDescriptionSeq ControllerManagement::getNJointControllerClassDescriptions(const Ice::Current&) const
+ NJointControllerClassDescriptionSeq
+ ControllerManagement::getNJointControllerClassDescriptions(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::size_t tries = 200;
- while (getRobotUnitState() == RobotUnitState::InitializingComponent || getRobotUnitState() == RobotUnitState::InitializingDevices)
+ while (getRobotUnitState() == RobotUnitState::InitializingComponent ||
+ getRobotUnitState() == RobotUnitState::InitializingDevices)
{
//this phase should only last short so busy waiting is ok
std::this_thread::sleep_for(std::chrono::milliseconds(50));
- if (! --tries)
+ if (!--tries)
{
- throw RuntimeError {"RobotUnit::getNJointControllerClassDescriptions: it took too long to for the unit to get in a valid state"};
+ throw RuntimeError{"RobotUnit::getNJointControllerClassDescriptions: it took too "
+ "long to for the unit to get in a valid state"};
}
}
auto guard = getGuard();
@@ -615,7 +673,8 @@ namespace armarx::RobotUnitModule
return r;
}
- NJointControllerInterfacePrx ControllerManagement::getNJointController(const std::string& name, const Ice::Current&) const
+ NJointControllerInterfacePrx
+ ControllerManagement::getNJointController(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
NJointControllerBasePtr ctrl;
@@ -631,7 +690,9 @@ namespace armarx::RobotUnitModule
return NJointControllerInterfacePrx::uncheckedCast(ctrl->getProxy(-1, true));
}
- NJointControllerStatus ControllerManagement::getNJointControllerStatus(const std::string& name, const Ice::Current&) const
+ NJointControllerStatus
+ ControllerManagement::getNJointControllerStatus(const std::string& name,
+ const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -639,7 +700,8 @@ namespace armarx::RobotUnitModule
return getNJointControllerNotNull(name)->getControllerStatus();
}
- NJointControllerStatusSeq ControllerManagement::getNJointControllerStatuses(const Ice::Current&) const
+ NJointControllerStatusSeq
+ ControllerManagement::getNJointControllerStatuses(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -656,7 +718,9 @@ namespace armarx::RobotUnitModule
return r;
}
- NJointControllerDescription ControllerManagement::getNJointControllerDescription(const std::string& name, const Ice::Current&) const
+ NJointControllerDescription
+ ControllerManagement::getNJointControllerDescription(const std::string& name,
+ const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
NJointControllerBasePtr nJointCtrl;
@@ -668,7 +732,8 @@ namespace armarx::RobotUnitModule
return nJointCtrl->getControllerDescription();
}
- NJointControllerDescriptionSeq ControllerManagement::getNJointControllerDescriptions(const Ice::Current&) const
+ NJointControllerDescriptionSeq
+ ControllerManagement::getNJointControllerDescriptions(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
@@ -689,8 +754,9 @@ namespace armarx::RobotUnitModule
return r;
}
- NJointControllerDescriptionWithStatus ControllerManagement::getNJointControllerDescriptionWithStatus(
- const std::string& name, const Ice::Current&) const
+ NJointControllerDescriptionWithStatus
+ ControllerManagement::getNJointControllerDescriptionWithStatus(const std::string& name,
+ const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
NJointControllerBasePtr nJointCtrl;
@@ -702,7 +768,8 @@ namespace armarx::RobotUnitModule
return nJointCtrl->getControllerDescriptionWithStatus();
}
- NJointControllerDescriptionWithStatusSeq ControllerManagement::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
+ NJointControllerDescriptionWithStatusSeq
+ ControllerManagement::getNJointControllerDescriptionsWithStatuses(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::map<std::string, NJointControllerBasePtr> nJointControllersCopy;
@@ -723,7 +790,11 @@ namespace armarx::RobotUnitModule
return r;
}
- void ControllerManagement::removeNJointControllers(std::map<std::string, NJointControllerBasePtr>& ctrls, bool blocking, RobotUnitListenerPrx l)
+ void
+ ControllerManagement::removeNJointControllers(
+ std::map<std::string, NJointControllerBasePtr>& ctrls,
+ bool blocking,
+ RobotUnitListenerPrx l)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
for (auto& n2NJointCtrl : ctrls)
@@ -747,13 +818,15 @@ namespace armarx::RobotUnitModule
ctrls.clear();
}
- void ControllerManagement::removeNJointControllersToBeDeleted(bool blocking, RobotUnitListenerPrx l)
+ void
+ ControllerManagement::removeNJointControllersToBeDeleted(bool blocking, RobotUnitListenerPrx l)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
removeNJointControllers(nJointControllersToBeDeleted, blocking, l);
}
- void ControllerManagement::_preFinishRunning()
+ void
+ ControllerManagement::_preFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//NJoint queued for deletion (some could still be in the queue)
@@ -766,7 +839,8 @@ namespace armarx::RobotUnitModule
ARMARX_DEBUG << "remove NJointControllers...done";
}
- void ControllerManagement::_postFinishRunning()
+ void
+ ControllerManagement::_postFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
nJointControllers.clear();
@@ -774,22 +848,25 @@ namespace armarx::RobotUnitModule
ControllerManagement::~ControllerManagement()
{
-
}
- void ControllerManagement::_preOnInitRobotUnit()
+ void
+ ControllerManagement::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controllerCreateRobot = _module<RobotData>().cloneRobot();
}
- void ControllerManagement::updateNJointControllerRequestedState(const std::set<NJointControllerBasePtr>& request)
+ void
+ ControllerManagement::updateNJointControllerRequestedState(
+ const std::set<NJointControllerBasePtr>& request)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "set requested state for NJoint controllers";
for (const auto& name2NJoint : nJointControllers)
{
- NJointControllerAttorneyForControllerManagement::SetRequested(name2NJoint.second, request.count(name2NJoint.second));
+ NJointControllerAttorneyForControllerManagement::SetRequested(
+ name2NJoint.second, request.count(name2NJoint.second));
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
index 3317540bc5e9adf326ba8410d06df2576d907ec3..9541d6c791b508c7c4b844803455cf69c7bee55f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h
@@ -36,15 +36,19 @@ namespace armarx::RobotUnitModule
*
* @see ModuleBase
*/
- class ControllerManagement : virtual public ModuleBase, virtual public RobotUnitControllerManagementInterface
+ class ControllerManagement :
+ virtual public ModuleBase,
+ virtual public RobotUnitControllerManagementInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static ControllerManagement& Instance()
+ static ControllerManagement&
+ Instance()
{
return ModuleBase::Instance<ControllerManagement>();
}
@@ -71,13 +75,16 @@ namespace armarx::RobotUnitModule
* @return A proxy to the \ref NJointControllerBase.
* @see getAllNJointControllers
*/
- NJointControllerInterfacePrx getNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerInterfacePrx
+ getNJointController(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns proxies to all \ref NJointControllerBase "NJointControllers"
* @return Proxies to all \ref NJointControllerBase "NJointControllers"
* @see getNJointController
*/
- StringNJointControllerPrxDictionary getAllNJointControllers(const Ice::Current& = Ice::emptyCurrent) const override;
+ StringNJointControllerPrxDictionary
+ getAllNJointControllers(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the status of the \ref NJointControllerBase.
@@ -88,7 +95,9 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescriptionWithStatus
* @see getNJointControllerDescriptionsWithStatuses
*/
- NJointControllerStatus getNJointControllerStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerStatus
+ getNJointControllerStatus(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the status of all \ref NJointControllerBase "NJointControllers".
* @return The status of all \ref NJointControllerBase "NJointControllers".
@@ -97,7 +106,8 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescriptionWithStatus
* @see getNJointControllerDescriptionsWithStatuses
*/
- NJointControllerStatusSeq getNJointControllerStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerStatusSeq
+ getNJointControllerStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the description of the \ref NJointControllerBase.
@@ -108,7 +118,9 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescriptionWithStatus
* @see getNJointControllerDescriptionsWithStatuses
*/
- NJointControllerDescription getNJointControllerDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerDescription
+ getNJointControllerDescription(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the description of all \ref NJointControllerBase "NJointControllers".
* @return The description of all \ref NJointControllerBase "NJointControllers".
@@ -118,7 +130,8 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescriptionsWithStatuses
*/
- NJointControllerDescriptionSeq getNJointControllerDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerDescriptionSeq
+ getNJointControllerDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the status and description of the \ref NJointControllerBase.
@@ -133,7 +146,8 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescriptions
*/
NJointControllerDescriptionWithStatus getNJointControllerDescriptionWithStatus(
- const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the status and description of all \ref NJointControllerBase "NJointControllers".
* @return The status and description of all \ref NJointControllerBase "NJointControllers".
@@ -145,27 +159,31 @@ namespace armarx::RobotUnitModule
* @see getNJointControllerDescription
* @see getNJointControllerDescriptions
*/
- NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses(
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief getNJointControllerClassDescription
* @param className
* @return
*/
- NJointControllerClassDescription getNJointControllerClassDescription(
- const std::string& className, const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerClassDescription
+ getNJointControllerClassDescription(const std::string& className,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief getNJointControllerClassDescriptions
* @return
*/
- NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions(
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Loads the given lib. (calls `getArmarXManager()->loadLibFromPath(path)`)
* @param path Path to the lib to load.
* @return Whether loading the lib was successful.
* @see ArmarXManager::loadLibFromPath
*/
- bool loadLibFromPath(const std::string& path, const Ice::Current& = Ice::emptyCurrent) override;
+ bool loadLibFromPath(const std::string& path,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Loads the given lib. (calls `getArmarXManager()->loadLibFromPath(package, lib)`)
* @param package The armarx package containing the lib
@@ -173,27 +191,33 @@ namespace armarx::RobotUnitModule
* @return Whether loading the lib was successful.
* @see ArmarXManager::loadLibFromPackage
*/
- bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = Ice::emptyCurrent) override;
+ bool loadLibFromPackage(const std::string& package,
+ const std::string& lib,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns the names of all available classes of \ref NJointControllerBase.
* @return The names of all available classes of \ref NJointControllerBase.
*/
- Ice::StringSeq getNJointControllerClassNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ Ice::StringSeq
+ getNJointControllerClassNames(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the names of all \ref NJointControllerBase "NJointControllers"
* @return The names of all \ref NJointControllerBase "NJointControllers"
*/
- Ice::StringSeq getNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ Ice::StringSeq
+ getNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the names of all requested \ref NJointControllerBase "NJointControllers"
* @return The names of all requested \ref NJointControllerBase "NJointControllers"
*/
- Ice::StringSeq getRequestedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ Ice::StringSeq
+ getRequestedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the names of all activated \ref NJointControllerBase "NJointControllers"
* @return The names of all activated \ref NJointControllerBase "NJointControllers"
*/
- Ice::StringSeq getActivatedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ Ice::StringSeq
+ getActivatedNJointControllerNames(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Queues the given \ref NJointControllerBase for deletion.
@@ -202,7 +226,8 @@ namespace armarx::RobotUnitModule
* @see nJointControllersToBeDeleted
* @see deleteNJointControllers
*/
- void deleteNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void deleteNJointController(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Queues the given \ref NJointControllerBase "NJointControllers" for deletion.
* @param names The \ref NJointControllerBase "NJointControllers" to delete.
@@ -210,7 +235,8 @@ namespace armarx::RobotUnitModule
* @see nJointControllersToBeDeleted
* @see deleteNJointController
*/
- void deleteNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
+ void deleteNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Queues the given \ref NJointControllerBase for deletion and deactivates it if necessary.
* @param name The \ref NJointControllerBase to delete.
@@ -220,7 +246,8 @@ namespace armarx::RobotUnitModule
* @see deleteNJointControllers
* @see deactivateAnddeleteNJointControllers
*/
- void deactivateAndDeleteNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void deactivateAndDeleteNJointController(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Queues the given \ref NJointControllerBase "NJointControllers" for deletion and deactivates them if necessary.
* @param names The \ref NJointControllerBase "NJointControllers" to delete.
@@ -230,33 +257,38 @@ namespace armarx::RobotUnitModule
* @see deleteNJointControllers
* @see deactivateAnddeleteNJointController
*/
- void deactivateAndDeleteNJointControllers(const Ice::StringSeq& names, const Ice::Current&) override;
+ void deactivateAndDeleteNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current&) override;
/**
* @brief Requests activation for the given \ref NJointControllerBase.
* @param name The requested \ref NJointControllerBase.
* @see activateNJointControllers
*/
- void activateNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void activateNJointController(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Requests activation for the given \ref NJointControllerBase "NJointControllers".
* @param names The requested \ref NJointControllerBase "NJointControllers".
* @see activateNJointController
*/
- void activateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
+ void activateNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Requests deactivation for the given \ref NJointControllerBase.
* @param name The \ref NJointControllerBase to be deactivated.
* @see deactivateNJointControllers
*/
- void deactivateNJointController(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void deactivateNJointController(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Requests deactivation for the given \ref NJointControllerBase "NJointControllers".
* @param names The \ref NJointControllerBase "NJointControllers" to be deactivated.
* @see deactivateNJointController
*/
- void deactivateNJointControllers(const Ice::StringSeq& names, const Ice::Current& = Ice::emptyCurrent) override;
+ void deactivateNJointControllers(const Ice::StringSeq& names,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Cretes a \ref NJointControllerBase.
@@ -265,9 +297,11 @@ namespace armarx::RobotUnitModule
* @param config A config passed to the \ref NJointControllerBase's ctor.
* @return A proxy to the created \ref NJointControllerBase.
*/
- NJointControllerInterfacePrx createNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, const Ice::Current& = Ice::emptyCurrent) override;
+ NJointControllerInterfacePrx
+ createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Cretes a \ref NJointControllerBase.
* @param className The \ref NJointControllerBase's class.
@@ -275,9 +309,11 @@ namespace armarx::RobotUnitModule
* @param variants A map of \ref Variant "Variants" passed to the \ref NJointControllerBase's ctor.
* @return A proxy to the created \ref NJointControllerBase.
*/
- NJointControllerInterfacePrx createNJointControllerFromVariantConfig(
- const std::string& className, const std::string& instanceName,
- const StringVariantBaseMap& variants, const Ice::Current& = Ice::emptyCurrent) override;
+ NJointControllerInterfacePrx
+ createNJointControllerFromVariantConfig(const std::string& className,
+ const std::string& instanceName,
+ const StringVariantBaseMap& variants,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Deletes any \ref NJointControllerBase with the given name and creates a new one.
* @param className The \ref NJointControllerBase's class.
@@ -285,15 +321,18 @@ namespace armarx::RobotUnitModule
* @param config A config passed to the \ref NJointControllerBase's ctor.
* @return A proxy to the created \ref NJointControllerBase.
*/
- NJointControllerInterfacePrx createOrReplaceNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, const Ice::Current& = Ice::emptyCurrent) override;
+ NJointControllerInterfacePrx
+ createOrReplaceNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Changes the set of requested \ref NJointControllerBase "NJointControllers" to the given set.
* @param newSetup The new set of requested \ref NJointControllerBase "NJointControllers"
*/
- void switchNJointControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current& = Ice::emptyCurrent) override;
+ void switchNJointControllerSetup(const Ice::StringSeq& newSetup,
+ const Ice::Current& = Ice::emptyCurrent) override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -307,9 +346,12 @@ namespace armarx::RobotUnitModule
* @param internal Whether the \ref NJointControllerBase should be tagged as internal.
* @return A pointer to the created \ref NJointControllerBase
*/
- const NJointControllerBasePtr& createNJointController(
- const std::string& className, const std::string& instanceName,
- const NJointControllerConfigPtr& config, bool deletable, bool internal);
+ const NJointControllerBasePtr&
+ createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool deletable,
+ bool internal);
/**
* @brief Returns pointers to the \ref NJointControllerBase "NJointControllers". (If one does not exist, an exception is thrown.)
@@ -317,7 +359,8 @@ namespace armarx::RobotUnitModule
* @return Pointers to the \ref NJointControllerBase "NJointControllers".
* @throw If there is no \ref NJointControllerBase for \param name
*/
- std::vector<armarx::NJointControllerBasePtr> getNJointControllersNotNull(const std::vector<std::string>& names) const;
+ std::vector<armarx::NJointControllerBasePtr>
+ getNJointControllersNotNull(const std::vector<std::string>& names) const;
/**
* @brief Returns a pointer to the \ref NJointControllerBase. (If it does not exist, an exception is thrown.)
* @param name The \ref NJointControllerBase
@@ -331,7 +374,8 @@ namespace armarx::RobotUnitModule
* @param ctrls The \ref NJointControllerBase "NJointControllers"
* @return The names of given \ref NJointControllerBase "NJointControllers"
*/
- std::vector<std::string> getNJointControllerNames(const std::vector<armarx::NJointControllerBasePtr>& ctrls) const;
+ std::vector<std::string>
+ getNJointControllerNames(const std::vector<armarx::NJointControllerBasePtr>& ctrls) const;
/**
* @brief Queues the given \ref NJointControllerBase for deletion.
@@ -368,7 +412,8 @@ namespace armarx::RobotUnitModule
* @see deleteNJointControllers
* @see deactivateAnddeleteNJointController
*/
- void deactivateAndDeleteNJointControllers(const std::vector<NJointControllerBasePtr>& ctrls);
+ void
+ deactivateAndDeleteNJointControllers(const std::vector<NJointControllerBasePtr>& ctrls);
/**
@@ -408,7 +453,9 @@ namespace armarx::RobotUnitModule
* @param blocking Whether removal from the \ref ArmarXManager should be blocking.
* @param l Proxy to the listener topic all removal events should be sent to.
*/
- void removeNJointControllers(std::map<std::string, NJointControllerBasePtr>& ctrls, bool blocking = true, RobotUnitListenerPrx l = nullptr);
+ void removeNJointControllers(std::map<std::string, NJointControllerBasePtr>& ctrls,
+ bool blocking = true,
+ RobotUnitListenerPrx l = nullptr);
/**
* @brief Calls \ref removeNJointControllers for all \ref NJointControllerBase "NJointControllers"
* queued for deletion (\ref nJointControllersToBeDeleted).
@@ -417,7 +464,8 @@ namespace armarx::RobotUnitModule
* @see removeNJointControllers
* @see nJointControllersToBeDeleted
*/
- void removeNJointControllersToBeDeleted(bool blocking = true, RobotUnitListenerPrx l = nullptr);
+ void removeNJointControllersToBeDeleted(bool blocking = true,
+ RobotUnitListenerPrx l = nullptr);
/**
* @brief Sets the requested flag for all given \ref NJointControllerBase "NJointControllers" and unsets it for the rest.
@@ -458,4 +506,4 @@ namespace armarx::RobotUnitModule
*/
friend class ControllerManagementAttorneyForControlThreadDataBuffer;
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index c29c7ce2d74f3f4ee7f7dc04cc96e48b46315f89..61d0d6c3a63f6a82f981eed63a61d6cabd57105e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -21,40 +21,44 @@
*/
#include "RobotUnitModuleDevices.h"
-#include "RobotUnitModuleControlThreadDataBuffer.h"
-#include "RobotUnitModuleRobotData.h"
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/core/util/algorithm.h>
+
#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
+#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include "RobotUnitModuleRobotData.h"
+
namespace armarx::RobotUnitModule
{
const std::string Devices::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
- ControlDeviceDescription Devices::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
+ ControlDeviceDescription
+ Devices::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
if (!controlDevices.has(name))
{
std::stringstream ss;
ss << "getControlDeviceDescription: There is no ControlDevice '" << name
<< "'. There are these ControlDevices: " << controlDevices.keys();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return getControlDeviceDescription(controlDevices.index(name));
}
- ControlDeviceDescriptionSeq Devices::getControlDeviceDescriptions(const Ice::Current&) const
+ ControlDeviceDescriptionSeq
+ Devices::getControlDeviceDescriptions(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!areDevicesReady())
{
return {};
}
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
ControlDeviceDescriptionSeq r;
r.reserve(getNumberOfControlDevices());
for (auto idx : getIndices(controlDevices.values()))
@@ -65,124 +69,145 @@ namespace armarx::RobotUnitModule
return r;
}
- std::size_t Devices::getNumberOfControlDevices() const
+ std::size_t
+ Devices::getNumberOfControlDevices() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
return controlDevices.size();
}
- std::size_t Devices::getNumberOfSensorDevices() const
+ std::size_t
+ Devices::getNumberOfSensorDevices() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
return sensorDevices.size();
}
- std::size_t Devices::getSensorDeviceIndex(const std::string& deviceName) const
+ std::size_t
+ Devices::getSensorDeviceIndex(const std::string& deviceName) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
ARMARX_CHECK_EXPRESSION(sensorDevices.has(deviceName));
return sensorDevices.index(deviceName);
}
- std::size_t Devices::getControlDeviceIndex(const std::string& deviceName) const
+
+ std::size_t
+ Devices::getControlDeviceIndex(const std::string& deviceName) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
ARMARX_CHECK_EXPRESSION(controlDevices.has(deviceName));
return controlDevices.index(deviceName);
}
- ConstSensorDevicePtr Devices::getSensorDevice(const std::string& sensorDeviceName) const
+ ConstSensorDevicePtr
+ Devices::getSensorDevice(const std::string& sensorDeviceName) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
}
- ConstControlDevicePtr Devices::getControlDevice(const std::string& deviceName) const
+ ConstControlDevicePtr
+ Devices::getControlDevice(const std::string& deviceName) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
return controlDevices.at(deviceName, ControlDevice::NullPtr);
}
- Ice::StringSeq Devices::getControlDeviceNames(const Ice::Current&) const
+ Ice::StringSeq
+ Devices::getControlDeviceNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
return controlDevices.keys();
}
- ControlDeviceDescription Devices::getControlDeviceDescription(std::size_t idx) const
+ ControlDeviceDescription
+ Devices::getControlDeviceDescription(std::size_t idx) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
const ControlDevicePtr& controlDevice = controlDevices.at(idx);
ControlDeviceDescription data;
data.deviceName = controlDevice->getDeviceName();
data.tags.assign(controlDevice->getTags().begin(), controlDevice->getTags().end());
for (const auto& jointCtrl : controlDevice->getJointControllers())
{
- data.contolModeToTargetType[jointCtrl->getControlMode()].targetType = jointCtrl->getControlTarget()->getControlTargetType();
- data.contolModeToTargetType[jointCtrl->getControlMode()].hardwareControlMode = jointCtrl->getHardwareControlMode();
+ data.contolModeToTargetType[jointCtrl->getControlMode()].targetType =
+ jointCtrl->getControlTarget()->getControlTargetType();
+ data.contolModeToTargetType[jointCtrl->getControlMode()].hardwareControlMode =
+ jointCtrl->getHardwareControlMode();
}
throwIfDevicesNotReady(__FUNCTION__);
return data;
}
- ControlDeviceStatus Devices::getControlDeviceStatus(std::size_t idx) const
+ ControlDeviceStatus
+ Devices::getControlDeviceStatus(std::size_t idx) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
const ControlDevicePtr& controlDevice = controlDevices.at(idx);
ControlDeviceStatus status;
- const auto activeJointCtrl = _module<ControlThreadDataBuffer>().getActivatedJointControllers().at(idx);
- status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
+ const auto activeJointCtrl =
+ _module<ControlThreadDataBuffer>().getActivatedJointControllers().at(idx);
+ status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode()
+ : std::string{"!!JointController is nullptr!!"};
status.deviceName = controlDevice->getDeviceName();
- const auto requestedJointControllers = _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
+ const auto requestedJointControllers =
+ _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
ARMARX_CHECK_EXPRESSION(requestedJointControllers.at(idx));
status.requestedControlMode = requestedJointControllers.at(idx)->getControlMode();
- for (const auto& targ : _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().control.at(idx))
+ for (const auto& targ :
+ _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().control.at(idx))
{
- status.controlTargetValues[targ->getControlMode()] = targ->toVariants(_module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensorValuesTimestamp);
+ status.controlTargetValues[targ->getControlMode()] =
+ targ->toVariants(_module<ControlThreadDataBuffer>()
+ .getSensorAndControlBuffer()
+ .sensorValuesTimestamp);
}
status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
throwIfDevicesNotReady(__FUNCTION__);
return status;
}
- ControlDeviceStatus Devices::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
+ ControlDeviceStatus
+ Devices::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
if (!controlDevices.has(name))
{
std::stringstream ss;
ss << "getControlDeviceStatus: There is no ControlDevice '" << name
<< "'. There are these ControlDevices: " << controlDevices.keys();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return getControlDeviceStatus(controlDevices.index(name));
}
- ControlDeviceStatusSeq Devices::getControlDeviceStatuses(const Ice::Current&) const
+ ControlDeviceStatusSeq
+ Devices::getControlDeviceStatuses(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!areDevicesReady())
{
return {};
}
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
ControlDeviceStatusSeq r;
r.reserve(getNumberOfControlDevices());
for (auto idx : getIndices(controlDevices.values()))
@@ -193,19 +218,21 @@ namespace armarx::RobotUnitModule
return r;
}
- Ice::StringSeq Devices::getSensorDeviceNames(const Ice::Current&) const
+ Ice::StringSeq
+ Devices::getSensorDeviceNames(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
return sensorDevices.keys();
}
- SensorDeviceDescription Devices::getSensorDeviceDescription(std::size_t idx) const
+ SensorDeviceDescription
+ Devices::getSensorDeviceDescription(std::size_t idx) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
SensorDeviceDescription data;
data.deviceName = sensorDevice->getDeviceName();
@@ -215,39 +242,47 @@ namespace armarx::RobotUnitModule
return data;
}
- SensorDeviceStatus Devices::getSensorDeviceStatus(std::size_t idx) const
+ SensorDeviceStatus
+ Devices::getSensorDeviceStatus(std::size_t idx) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
SensorDeviceStatus status;
status.deviceName = sensorDevice->getDeviceName();
- status.sensorValue = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensors.at(idx)->toVariants(_module<ControlThreadDataBuffer>().getSensorAndControlBuffer().sensorValuesTimestamp);
+ status.sensorValue = _module<ControlThreadDataBuffer>()
+ .getSensorAndControlBuffer()
+ .sensors.at(idx)
+ ->toVariants(_module<ControlThreadDataBuffer>()
+ .getSensorAndControlBuffer()
+ .sensorValuesTimestamp);
status.timestampUSec = TimeUtil::GetTime(true).toMicroSeconds();
throwIfDevicesNotReady(__FUNCTION__);
return status;
}
- SensorDeviceDescription Devices::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
+ SensorDeviceDescription
+ Devices::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
if (!sensorDevices.has(name))
{
std::stringstream ss;
ss << "getSensorDeviceDescription: There is no SensorDevice '" << name
<< "'. There are these SensorDevices: " << sensorDevices.keys();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return getSensorDeviceDescription(sensorDevices.index(name));
}
- SensorDeviceDescriptionSeq Devices::getSensorDeviceDescriptions(const Ice::Current&) const
+ SensorDeviceDescriptionSeq
+ Devices::getSensorDeviceDescriptions(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
if (!areDevicesReady())
{
return {};
@@ -261,29 +296,31 @@ namespace armarx::RobotUnitModule
return r;
}
- SensorDeviceStatus Devices::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
+ SensorDeviceStatus
+ Devices::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
if (!sensorDevices.has(name))
{
std::stringstream ss;
ss << "getSensorDeviceStatus: There is no SensorDevice '" << name
<< "'. There are these SensorDevices: " << sensorDevices.keys();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
return getSensorDeviceStatus(sensorDevices.index(name));
}
- SensorDeviceStatusSeq Devices::getSensorDeviceStatuses(const Ice::Current&) const
+ SensorDeviceStatusSeq
+ Devices::getSensorDeviceStatuses(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!areDevicesReady())
{
return {};
}
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
SensorDeviceStatusSeq r;
r.reserve(getNumberOfSensorDevices());
for (auto idx : getIndices(sensorDevices.values()))
@@ -294,20 +331,21 @@ namespace armarx::RobotUnitModule
return r;
}
- void Devices::addControlDevice(const ControlDevicePtr& cd)
+ void
+ Devices::addControlDevice(const ControlDevicePtr& cd)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "ControlDevice " << &cd;
+ ARMARX_DEBUG << "ControlDevice " << &cd;
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
{
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
//check it
if (!cd)
{
std::stringstream ss;
ss << "armarx::RobotUnit::addControlDevice: ControlDevice is nullptr";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
if (!cd->getJointEmergencyStopController())
{
@@ -315,7 +353,7 @@ namespace armarx::RobotUnitModule
ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
<< " has null JointEmergencyStopController (this is not allowed)";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
if (!cd->getJointStopMovementController())
{
@@ -323,10 +361,10 @@ namespace armarx::RobotUnitModule
ss << "armarx::RobotUnit::addControlDevice: ControlDevice " << cd->getDeviceName()
<< " has null getJointStopMovementController (this is not allowed)";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
//add it
- ARMARX_DEBUG << "Adding the ControlDevice " << cd->getDeviceName() << " " << &cd ;
+ ARMARX_DEBUG << "Adding the ControlDevice " << cd->getDeviceName() << " " << &cd;
controlDevices.add(cd->getDeviceName(), cd);
cd->owner = this;
ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
@@ -335,20 +373,21 @@ namespace armarx::RobotUnitModule
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
}
- void Devices::addSensorDevice(const SensorDevicePtr& sd)
+ void
+ Devices::addSensorDevice(const SensorDevicePtr& sd)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "SensorDevice " << &sd;
+ ARMARX_DEBUG << "SensorDevice " << &sd;
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
{
- std::lock_guard<MutexType> guard {sensorDevicesMutex};
+ std::lock_guard<MutexType> guard{sensorDevicesMutex};
//check it
if (!sd)
{
std::stringstream ss;
ss << "armarx::RobotUnit::addSensorDevice: SensorDevice is nullptr";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
if (!sd->getSensorValue())
{
@@ -356,76 +395,79 @@ namespace armarx::RobotUnitModule
ss << "armarx::RobotUnit::addSensorDevice: SensorDevice " << sd->getDeviceName()
<< " has null SensorValue (this is not allowed)";
ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
+ throw InvalidArgumentException{ss.str()};
}
//add it
if (sd->getDeviceName() == rtThreadTimingsSensorDeviceName)
{
- ARMARX_DEBUG << "Device is the " << rtThreadTimingsSensorDeviceName;
+ ARMARX_DEBUG << "Device is the " << rtThreadTimingsSensorDeviceName;
if (!std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd))
{
- throw InvalidArgumentException
- {
+ throw InvalidArgumentException{
"You tried to add a SensorDevice with the name " + sd->getDeviceName() +
- " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"
- };
+ " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"};
}
//this checks if we already added such a device (do this before setting timingSensorDevice)
- ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
+ ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
sensorDevices.add(sd->getDeviceName(), sd);
sd->owner = this;
- rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
+ rtThreadTimingsSensorDevice =
+ std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
}
- else if(sd->getDeviceName() == GlobalRobotLocalizationSensorDevice::DeviceName())
+ else if (sd->getDeviceName() == GlobalRobotLocalizationSensorDevice::DeviceName())
{
- ARMARX_DEBUG << "Device is the " << sd->getDeviceName();
+ ARMARX_DEBUG << "Device is the " << sd->getDeviceName();
if (!std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd))
{
- throw InvalidArgumentException
- {
+ throw InvalidArgumentException{
"You tried to add a SensorDevice with the name " + sd->getDeviceName() +
- " which does not derive from GlobalRobotLocalizationSensorDevice. (Don't do this)"
- };
+ " which does not derive from GlobalRobotLocalizationSensorDevice. (Don't "
+ "do this)"};
}
//this checks if we already added such a device (do this before setting timingSensorDevice)
- ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
+ ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
sensorDevices.add(sd->getDeviceName(), sd);
sd->owner = this;
- globalRobotLocalizationSensorDevice = sd; //std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd);
+ globalRobotLocalizationSensorDevice =
+ sd; //std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd);
}
else
{
- ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd ;
+ ARMARX_DEBUG << "Adding the SensorDevice " << sd->getDeviceName() << " " << &sd;
sensorDevices.add(sd->getDeviceName(), sd);
sd->owner = this;
}
}
- ARMARX_INFO << "added SensorDevice " << sd->getDeviceName() << " (valuetype = " << sd->getSensorValueType() << ")";
+ ARMARX_INFO << "added SensorDevice " << sd->getDeviceName()
+ << " (valuetype = " << sd->getSensorValueType() << ")";
throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
}
- RTThreadTimingsSensorDevicePtr Devices::createRTThreadTimingSensorDevice() const
+ RTThreadTimingsSensorDevicePtr
+ Devices::createRTThreadTimingSensorDevice() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
}
- void Devices::_postFinishRunning()
+ void
+ Devices::_postFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guardS {sensorDevicesMutex};
- std::lock_guard<MutexType> guardC {controlDevicesMutex};
+ std::lock_guard<MutexType> guardS{sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC{controlDevicesMutex};
controlDevicesConstPtr.clear();
sensorDevicesConstPtr.clear();
sensorDevices.clear();
controlDevices.clear();
}
- std::vector<JointController*> Devices::getStopMovementJointControllers() const
+ std::vector<JointController*>
+ Devices::getStopMovementJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
std::vector<JointController*> controllers;
controllers.reserve(controlDevices.values().size());
for (const ControlDevicePtr& dev : controlDevices.values())
@@ -439,11 +481,12 @@ namespace armarx::RobotUnitModule
return controllers;
}
- std::vector<JointController*> Devices::getEmergencyStopJointControllers() const
+ std::vector<JointController*>
+ Devices::getEmergencyStopJointControllers() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
throwIfDevicesNotReady(__FUNCTION__);
- std::lock_guard<MutexType> guard {controlDevicesMutex};
+ std::lock_guard<MutexType> guard{controlDevicesMutex};
std::vector<JointController*> controllers;
controllers.reserve(controlDevices.values().size());
for (const ControlDevicePtr& dev : controlDevices.values())
@@ -457,11 +500,12 @@ namespace armarx::RobotUnitModule
return controllers;
}
- void Devices::_preFinishDeviceInitialization()
+ void
+ Devices::_preFinishDeviceInitialization()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guardS {sensorDevicesMutex};
- std::lock_guard<MutexType> guardC {controlDevicesMutex};
+ std::lock_guard<MutexType> guardS{sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC{controlDevicesMutex};
if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
{
addSensorDevice(createRTThreadTimingSensorDevice());
@@ -473,12 +517,13 @@ namespace armarx::RobotUnitModule
addSensorDevice(std::make_shared<GlobalRobotLocalizationSensorDevice>());
}
- void Devices::_postFinishDeviceInitialization()
+ void
+ Devices::_postFinishDeviceInitialization()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<MutexType> guardS {sensorDevicesMutex};
- std::lock_guard<MutexType> guardC {controlDevicesMutex};
+ std::lock_guard<MutexType> guardS{sensorDevicesMutex};
+ std::lock_guard<MutexType> guardC{controlDevicesMutex};
ARMARX_DEBUG << "checking " << controlDevices.size() << " ControlDevices:";
{
ARMARX_TRACE;
@@ -491,21 +536,22 @@ namespace armarx::RobotUnitModule
std::stringstream s;
s << "ControlDevice " << controlDevice->getDeviceName()
<< " has no JointController with ControlMode " << ControlModes::EmergencyStop
- << " (to fix this, add a JointController with this ControlMode to the ControlDevice).\nAvailable controllers: "
- << controlDevice->getControlModes();
+ << " (to fix this, add a JointController with this ControlMode to the "
+ "ControlDevice).\nAvailable controllers: "
+ << controlDevice->getControlModes();
ARMARX_ERROR << "--------" << s.str();
- throw LogicError {s.str()};
-
+ throw LogicError{s.str()};
}
if (!controlDevice->hasJointController(ControlModes::StopMovement))
{
std::stringstream s;
s << "ControlDevice " << controlDevice->getDeviceName()
<< " has no JointController with ControlMode \"" << ControlModes::StopMovement
- << "\" (to fix this, add a JointController with this ControlMode to the ControlDevice) \nAvailable controllers: "
+ << "\" (to fix this, add a JointController with this ControlMode to the "
+ "ControlDevice) \nAvailable controllers: "
<< controlDevice->getControlModes();
ARMARX_ERROR << "--------" << s.str();
- throw LogicError {s.str()};
+ throw LogicError{s.str()};
}
}
}
@@ -519,9 +565,10 @@ namespace armarx::RobotUnitModule
if (!sensorDevice->getSensorValue())
{
std::stringstream s;
- s << "SensorDevice " << sensorDevice->getSensorValue() << " has null SensorValue";
+ s << "SensorDevice " << sensorDevice->getSensorValue()
+ << " has null SensorValue";
ARMARX_ERROR << "--------" << s.str();
- throw LogicError {s.str()};
+ throw LogicError{s.str()};
}
ARMARX_CHECK_EXPRESSION(sensorDevice);
}
@@ -611,8 +658,9 @@ namespace armarx::RobotUnitModule
}
if (!ctrlModeGroups.groupsMerged.empty())
{
- ARMARX_DEBUG << "Checking control modes for ControlDeviceHardwareControlModeGroups ("
- << ctrlModeGroups.groups.size() << ")";
+ ARMARX_DEBUG
+ << "Checking control modes for ControlDeviceHardwareControlModeGroups ("
+ << ctrlModeGroups.groups.size() << ")";
ctrlModeGroups.deviceIndices.resize(ctrlModeGroups.groups.size());
//iterate over groups
for (std::size_t groupIdx = 0; groupIdx < ctrlModeGroups.groups.size(); ++groupIdx)
@@ -622,38 +670,43 @@ namespace armarx::RobotUnitModule
ARMARX_CHECK_EXPRESSION(!group.empty());
ARMARX_DEBUG << "----Group " << groupIdx << " size: " << group.size();
//gets a map of ControlMode->HardwareControlMode for the given device
- const auto getControlModeToHWControlMode = [&](const std::string & devname)
+ const auto getControlModeToHWControlMode = [&](const std::string& devname)
{
std::map<std::string, std::string> controlModeToHWControlMode;
const ControlDevicePtr& cd = controlDevices.at(devname);
for (const auto& jointCtrl : cd->getJointControllers())
{
- controlModeToHWControlMode[jointCtrl->getControlMode()] = jointCtrl->getHardwareControlMode();
+ controlModeToHWControlMode[jointCtrl->getControlMode()] =
+ jointCtrl->getHardwareControlMode();
}
return controlModeToHWControlMode;
};
//get modes of first dev
- const auto controlModeToHWControlMode = getControlModeToHWControlMode(*group.begin());
+ const auto controlModeToHWControlMode =
+ getControlModeToHWControlMode(*group.begin());
//check other devs
for (const auto& devname : group)
{
- ARMARX_CHECK_EXPRESSION(
- controlDevices.has(devname)) <<
- "The ControlDeviceHardwareControlModeGroups property contains device names not existent in the robot: "
- << devname << "\navailable:\n" << controlDevices.keys();
+ ARMARX_CHECK_EXPRESSION(controlDevices.has(devname))
+ << "The ControlDeviceHardwareControlModeGroups property contains "
+ "device names not existent in the robot: "
+ << devname << "\navailable:\n"
+ << controlDevices.keys();
//Assert all devices in a group have the same control modes with the same hw controle modes
- const auto controlModeToHWControlModeForDevice = getControlModeToHWControlMode(devname);
- ARMARX_CHECK_EXPRESSION(
- controlModeToHWControlModeForDevice == controlModeToHWControlMode) <<
- "Error for control modes of device '" << devname << "'\n"
- << "it has the modes: " << controlModeToHWControlModeForDevice
- << "\n but should have the modes: " << controlModeToHWControlMode;
+ const auto controlModeToHWControlModeForDevice =
+ getControlModeToHWControlMode(devname);
+ ARMARX_CHECK_EXPRESSION(controlModeToHWControlModeForDevice ==
+ controlModeToHWControlMode)
+ << "Error for control modes of device '" << devname << "'\n"
+ << "it has the modes: " << controlModeToHWControlModeForDevice
+ << "\n but should have the modes: " << controlModeToHWControlMode;
//insert the device index into the device indices
const auto devIdx = controlDevices.index(devname);
ctrlModeGroups.deviceIndices.at(groupIdx).emplace_back(devIdx);
//insert the group index into the group indices (+ check the current group index is the sentinel
ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.size() > devIdx);
- ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.at(devIdx) == IndexSentinel());
+ ARMARX_CHECK_EXPRESSION(ctrlModeGroups.groupIndices.at(devIdx) ==
+ IndexSentinel());
ctrlModeGroups.groupIndices.at(devIdx) = groupIdx;
ARMARX_DEBUG << "------- " << devname;
}
@@ -669,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))
@@ -684,10 +737,10 @@ namespace armarx::RobotUnitModule
}
else
{
- ARMARX_WARNING << "SensorValue for SensorDevice "
- << name << " is of type "
- << dev->getSensorValueType()
- << " which does not derive SensorValue1DoFActuatorPosition";
+ ARMARX_WARNING
+ << "SensorValue for SensorDevice " << name << " is of type "
+ << dev->getSensorValueType()
+ << " which does not derive SensorValue1DoFActuatorPosition";
}
}
else
@@ -702,19 +755,22 @@ namespace armarx::RobotUnitModule
ARMARX_VERBOSE << "SensorDevices:\n" << sensorDevices.keys();
}
- void Devices::_preOnInitRobotUnit()
+ void
+ Devices::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//ControlDeviceHardwareControlModeGroups
- const std::string controlDeviceHardwareControlModeGroupsStr = getProperty<std::string>("ControlDevices_HardwareControlModeGroups").getValue();
+ const std::string controlDeviceHardwareControlModeGroupsStr =
+ getProperty<std::string>("ControlDevices_HardwareControlModeGroups").getValue();
if (!controlDeviceHardwareControlModeGroupsStr.empty())
{
- const auto numGroups = std::count(
- controlDeviceHardwareControlModeGroupsStr.begin(),
- controlDeviceHardwareControlModeGroupsStr.end(), ';'
- ) + 1;
+ const auto numGroups = std::count(controlDeviceHardwareControlModeGroupsStr.begin(),
+ controlDeviceHardwareControlModeGroupsStr.end(),
+ ';') +
+ 1;
ctrlModeGroups.groups.reserve(numGroups);
- std::vector<std::string> strGroups = Split(controlDeviceHardwareControlModeGroupsStr, ";");
+ std::vector<std::string> strGroups =
+ Split(controlDeviceHardwareControlModeGroupsStr, ";");
for (const auto& gstr : strGroups)
{
bool trimDeviceNames = true;
@@ -722,19 +778,19 @@ namespace armarx::RobotUnitModule
std::set<std::string> group;
for (auto& device : strElems)
{
- ARMARX_CHECK_EXPRESSION(
- !device.empty()) <<
- "The ControlDeviceHardwareControlModeGroups property contains empty device names";
- ARMARX_CHECK_EXPRESSION(
- !ctrlModeGroups.groupsMerged.count(device)) <<
- "The ControlDeviceHardwareControlModeGroups property contains duplicate device names: " << device;
+ ARMARX_CHECK_EXPRESSION(!device.empty())
+ << "The ControlDeviceHardwareControlModeGroups property contains empty "
+ "device names";
+ ARMARX_CHECK_EXPRESSION(!ctrlModeGroups.groupsMerged.count(device))
+ << "The ControlDeviceHardwareControlModeGroups property contains duplicate "
+ "device names: "
+ << device;
ctrlModeGroups.groupsMerged.emplace(device);
group.emplace(std::move(device));
}
if (!group.empty())
{
- ARMARX_DEBUG << "adding device group:\n"
- << ARMARX_STREAM_PRINTER
+ ARMARX_DEBUG << "adding device group:\n" << ARMARX_STREAM_PRINTER
{
for (const auto& elem : group)
{
@@ -746,4 +802,4 @@ namespace armarx::RobotUnitModule
}
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
index 9a32f682c421ca571786037c542d24195f915a35..0c7caccb3cf6fa840928c6a5907f483e21690d5a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
@@ -24,35 +24,37 @@
#include <mutex>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
-#include "RobotUnitModuleBase.h"
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "../Devices/ControlDevice.h"
-#include "../Devices/SensorDevice.h"
#include "../Devices/RTThreadTimingsSensorDevice.h"
-
+#include "../Devices/SensorDevice.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
+#include "RobotUnitModuleBase.h"
namespace armarx::RobotUnitModule
{
- class DevicesPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class DevicesPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- DevicesPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ DevicesPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>(
- "ControlDevices_HardwareControlModeGroups", "",
+ "ControlDevices_HardwareControlModeGroups",
+ "",
"Groups of control devices for which the same hardware control mode has to be set. "
- "For a control mode, the JointControllers of all devices in a group must have the identical hardware control mode. "
+ "For a control mode, the JointControllers of all devices in a group must have the "
+ "identical hardware control mode. "
"The kinematic unit will change the control mode of all joints in a group,"
" if their current hw control mode does not match the new hw control mode. "
- "This can be used if the robot has a multi DOF joint which is represented by multiple 1DOF control devices "
+ "This can be used if the robot has a multi DOF joint which is represented by "
+ "multiple 1DOF control devices "
"(in such a case all of these 1DOF joints should have the same hw control mode, "
"except he multi DOF joint can handle different control modes at once). "
"Syntax: semicolon separated groups , each group is CSV of joints "
- "e.g. j1,j2;j3,j4,j5 -> Group 1 = j1+j2 Group 2 = j3+j4+j5; No joint may be in two groups!"
- );
+ "e.g. j1,j2;j3,j4,j5 -> Group 1 = j1+j2 Group 2 = j3+j4+j5; No joint may be in two "
+ "groups!");
}
};
@@ -65,16 +67,20 @@ namespace armarx::RobotUnitModule
class Devices : virtual public ModuleBase, virtual public RobotUnitDevicesInterface
{
friend class ModuleBase;
+
public:
struct ControlDeviceHardwareControlModeGroups;
+
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static Devices& Instance()
+ static Devices&
+ Instance()
{
return ModuleBase::Instance<Devices>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -95,7 +101,8 @@ namespace armarx::RobotUnitModule
* @brief Returns the names of all \ref ControlDevice "ControlDevices" for the robot.
* @return The names of all \ref ControlDevice "ControlDevices" for the robot.
*/
- Ice::StringSeq getControlDeviceNames(const Ice::Current& = Ice::emptyCurrent) const override;
+ Ice::StringSeq
+ getControlDeviceNames(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref ControlDeviceDescription for the given \ref ControlDevice
@@ -104,14 +111,17 @@ namespace armarx::RobotUnitModule
* @see ControlDeviceDescription
* @see getControlDeviceDescriptions
*/
- ControlDeviceDescription getControlDeviceDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ ControlDeviceDescription
+ getControlDeviceDescription(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref ControlDeviceDescription "ControlDeviceDescriptions" for all \ref ControlDevice "ControlDevices"
* @return The \ref ControlDeviceDescription "ControlDeviceDescriptions"
* @see ControlDeviceDescription
* @see getControlDeviceDescription
*/
- ControlDeviceDescriptionSeq getControlDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ ControlDeviceDescriptionSeq
+ getControlDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref ControlDeviceStatus for the given \ref ControlDevice
@@ -120,14 +130,17 @@ namespace armarx::RobotUnitModule
* @see ControlDeviceStatus
* @see getControlDeviceStatuses
*/
- ControlDeviceStatus getControlDeviceStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ ControlDeviceStatus
+ getControlDeviceStatus(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref ControlDeviceStatus "ControlDeviceStatuses" for all \ref ControlDevice "ControlDevices"
* @return The \ref ControlDeviceStatus "ControlDeviceStatuses"
* @see ControlDeviceStatus
* @see getControlDeviceStatus
*/
- ControlDeviceStatusSeq getControlDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ ControlDeviceStatusSeq
+ getControlDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the names of all \ref SensorDevice "SensorDevices" for the robot.
@@ -142,14 +155,17 @@ namespace armarx::RobotUnitModule
* @see SensorDeviceDescription
* @see getSensorDeviceDescriptions
*/
- SensorDeviceDescription getSensorDeviceDescription(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ SensorDeviceDescription
+ getSensorDeviceDescription(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref SensorDeviceDescription "SensorDeviceDescriptions" for all \ref SensorDevice "SensorDevices"
* @return The \ref SensorDeviceDescription "SensorDeviceDescriptions"
* @see SensorDeviceDescription
* @see getSensorDeviceDescription
*/
- SensorDeviceDescriptionSeq getSensorDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ SensorDeviceDescriptionSeq
+ getSensorDeviceDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref SensorDeviceStatus for the given \ref SensorDevice
@@ -158,14 +174,17 @@ namespace armarx::RobotUnitModule
* @see SensorDeviceStatus
* @see getSensorDeviceStatuses
*/
- SensorDeviceStatus getSensorDeviceStatus(const std::string& name, const Ice::Current& = Ice::emptyCurrent) const override;
+ SensorDeviceStatus
+ getSensorDeviceStatus(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Return the \ref SensorDeviceStatus "SensorDeviceStatuses" for all \ref SensorDevice "SensorDevices"
* @return The \ref SensorDeviceStatus "SensorDeviceStatuses"
* @see SensorDeviceStatus
* @see getSensorDeviceStatus
*/
- SensorDeviceStatusSeq getSensorDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
+ SensorDeviceStatusSeq
+ getSensorDeviceStatuses(const Ice::Current& = Ice::emptyCurrent) const override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -224,11 +243,14 @@ namespace armarx::RobotUnitModule
* @param robot The VirtualRobot to update
* @param sensors The \ref SensorValue "SensorValues"
*/
- template<class PtrT>
- void updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot, const std::vector<PtrT>& sensors) const
+ template <class PtrT>
+ void
+ updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot,
+ const std::vector<PtrT>& sensors) const
{
updateVirtualRobotFromSensorValues(robot, robot->getRobotNodes(), sensors);
}
+
/**
* @brief Updates the given VirtualRobot with the given \ref SensorValue "SensorValues"
*
@@ -237,8 +259,11 @@ namespace armarx::RobotUnitModule
* @param nodes The VirtualRobot's RobotNodes
* @param sensors The \ref SensorValue "SensorValues"
*/
- template<class PtrT>
- void updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes, const std::vector<PtrT>& sensors) const
+ template <class PtrT>
+ void
+ updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot,
+ const std::vector<VirtualRobot::RobotNodePtr>& nodes,
+ const std::vector<PtrT>& sensors) const
{
ARMARX_CHECK_NOT_NULL(robot);
@@ -246,15 +271,18 @@ namespace armarx::RobotUnitModule
{
const SensorValueBase* sv = sensors.at(m.idxSens);
ARMARX_CHECK_NOT_NULL(sv);
- const SensorValue1DoFActuatorPosition* svPos = sv->asA<SensorValue1DoFActuatorPosition>();
+ const SensorValue1DoFActuatorPosition* svPos =
+ sv->asA<SensorValue1DoFActuatorPosition>();
ARMARX_CHECK_NOT_NULL(svPos);
nodes.at(m.idxRobot)->setJointValueNoUpdate(svPos->position);
}
- // update global pose
- if(globalRobotLocalizationSensorDevice != nullptr)
+ // update global pose
+ if (globalRobotLocalizationSensorDevice != nullptr)
{
- const auto sensorValue = globalRobotLocalizationSensorDevice->getSensorValue()->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
+ const auto sensorValue =
+ globalRobotLocalizationSensorDevice->getSensorValue()
+ ->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
const auto global_T_root = sensorValue->global_T_root;
robot->setGlobalPose(global_T_root, false);
}
@@ -268,7 +296,8 @@ namespace armarx::RobotUnitModule
* @return The groups of \ref ControlDevice "ControlDevices" required to be in the same
* hardware control mode.
*/
- const ControlDeviceHardwareControlModeGroups& getControlModeGroups() const
+ const ControlDeviceHardwareControlModeGroups&
+ getControlModeGroups() const
{
return ctrlModeGroups;
}
@@ -291,19 +320,22 @@ namespace armarx::RobotUnitModule
* @param deviceName \ref ControlDevice's name.
* @return The \ref ControlDevice
*/
- ConstControlDevicePtr getControlDevice(const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
+ ConstControlDevicePtr getControlDevice(
+ const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
/**
* @brief Returns the \ref SensorDevice
* @param deviceName \ref SensorDevice's name.
* @return The \ref SensorDevice
*/
- ConstSensorDevicePtr getSensorDevice(const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
+ ConstSensorDevicePtr getSensorDevice(
+ const std::string& deviceName) const; /// TODO move to attorney for NJointControllerBase
/**
* @brief Returns all \ref SensorDevice "SensorDevices"
* @return All \ref SensorDevice "SensorDevices"
*/
- const KeyValueVector<std::string, SensorDevicePtr>& getSensorDevices() const ///TODO remove from interface
+ const KeyValueVector<std::string, SensorDevicePtr>&
+ getSensorDevices() const ///TODO remove from interface
{
return sensorDevices;
}
@@ -312,7 +344,8 @@ namespace armarx::RobotUnitModule
* @brief Returns all \ref ControlDevice "ControlDevices"
* @return All \ref ControlDevice "ControlDevices"
*/
- const KeyValueVector<std::string, ControlDevicePtr>& getControlDevices() const ///TODO remove from interface
+ const KeyValueVector<std::string, ControlDevicePtr>&
+ getControlDevices() const ///TODO remove from interface
{
return controlDevices;
}
@@ -321,18 +354,22 @@ namespace armarx::RobotUnitModule
* @brief Returns const pointers to all \ref SensorDevice "SensorDevices"
* @return Const pointers to all \ref SensorDevice "SensorDevices"
*/
- const std::map<std::string, ConstSensorDevicePtr>& getSensorDevicesConstPtr() const ///TODO rename to getSensorDevices
+ const std::map<std::string, ConstSensorDevicePtr>&
+ getSensorDevicesConstPtr() const ///TODO rename to getSensorDevices
{
return sensorDevicesConstPtr;
}
+
/**
* @brief Returns const pointers to all \ref ControlDevice "ControlDevices"
* @return Const pointers to all \ref ControlDevice "ControlDevices"
*/
- const std::map<std::string, ConstControlDevicePtr>& getControlDevicesConstPtr() const ///TODO rename to getControlDevices
+ const std::map<std::string, ConstControlDevicePtr>&
+ getControlDevicesConstPtr() const ///TODO rename to getControlDevices
{
return controlDevicesConstPtr;
}
+
protected:
/**
* @brief Adds a \ref ControlDevice to the robot.
@@ -353,16 +390,19 @@ namespace armarx::RobotUnitModule
* @see rtGetRTThreadTimingsSensorDevice
*/
virtual RTThreadTimingsSensorDevicePtr createRTThreadTimingSensorDevice() const;
+
/**
* @brief Returns the \ref SensorDevice used to log timings in the \ref ControlThread.
* (This function is supposed to be used in the \ref ControlThread)
* @return The \ref SensorDevice used to log timings in the \ref ControlThread
* @see createRTThreadTimingSensorDevice
*/
- RTThreadTimingsSensorDevice& rtGetRTThreadTimingsSensorDevice()
+ RTThreadTimingsSensorDevice&
+ rtGetRTThreadTimingsSensorDevice()
{
return *rtThreadTimingsSensorDevice;
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -391,12 +431,14 @@ namespace armarx::RobotUnitModule
/// @brief The \ref SensorDevice's index
std::size_t idxSens;
};
+
/// @brief Mapping from RobotNode index to \ref SensorDevice
std::vector<SimoxRobotSensorValueMapping> simoxRobotSensorValueMapping;
/// @brief Pointers to all \ref SensorValueBase "SensorValues"
std::vector<const SensorValueBase*> sensorValues;
/// @brief Pointers to all \ref ControlTargetBase "ControlTargets"
std::vector<std::vector<PropagateConst<ControlTargetBase*>>> controlTargets;
+
public:
/// @brief Holds data about a device group requiring the same hardware control mode. (explained \ref RobotUnitCtrlModeGroups "here")
struct ControlDeviceHardwareControlModeGroups
@@ -412,6 +454,7 @@ namespace armarx::RobotUnitModule
/// @brief contains the group index for each device (or the sentinel)
std::vector<std::size_t> groupIndices;
};
+
private:
//ctrl dev
/// @brief \ref ControlDevice "ControlDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
@@ -426,7 +469,7 @@ namespace armarx::RobotUnitModule
//sens dev
/// @brief \ref SensorDevice "SensorDevices" added to this unit (data may only be added during and only be used after \ref State::InitializingDevices)
- KeyValueVector<std::string, SensorDevicePtr > sensorDevices;
+ KeyValueVector<std::string, SensorDevicePtr> sensorDevices;
/// @brief const pointer to all SensorDevices (passed to GenerateConfigDescription of a NJointControllerBase)
std::map<std::string, ConstSensorDevicePtr> sensorDevicesConstPtr;
/// @brief Guards access to all \ref SensorDevice "SensorDevices"
@@ -464,4 +507,4 @@ namespace armarx::RobotUnitModule
*/
friend class DevicesAttorneyForNJointController;
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 3e50e1069a5ff136d8a4eba9df1c1d982ebccc2e..ee8522d21984cbf6936bd1b2c259e37850398d7f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -25,6 +25,7 @@
#include <regex>
+#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
@@ -396,7 +397,6 @@ namespace armarx::RobotUnitModule
getProperty(streamingEntry.rtStreamMaxClientErrors,
"RTLogging_StreamingDataMaxClientConnectionFailures");
- getProperty(logAllEntries, "RTLogging_LogAllMessages");
ARMARX_INFO << "start data streaming to " << receiver->ice_getIdentity().name
<< ". Values: " << config.loggingNames;
@@ -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;
}
@@ -528,11 +531,13 @@ namespace armarx::RobotUnitModule
ARMARX_INFO << "starting rt logging with timestep " << rtLoggingTimestepMs;
stopRtLoggingTask = false;
+ Metronome metronome(Duration::MilliSeconds(rtLoggingTimestepMs));
rtLoggingTask = std::thread{
- [&]
+ [this]
{
using clock_t = std::chrono::steady_clock;
const auto now = [] { return clock_t::now(); };
+ Metronome metronome(Duration::MilliSeconds(rtLoggingTimestepMs));
while (!stopRtLoggingTask)
{
const auto start = now();
@@ -540,15 +545,13 @@ namespace armarx::RobotUnitModule
const std::uint64_t ms =
std::chrono::duration_cast<std::chrono::milliseconds>(now() - start)
.count();
- if (ms < rtLoggingTimestepMs)
+ if (ms > rtLoggingTimestepMs)
{
- std::this_thread::sleep_for(
- std::chrono::milliseconds{rtLoggingTimestepMs - ms});
- continue;
+ ARMARX_WARNING << deactivateSpam(10) << "logging thread took " << ms
+ << " ms > " << rtLoggingTimestepMs
+ << " ms (message printed every 10 seconds)";
}
- ARMARX_WARNING << deactivateSpam(10) << "logging thread took " << ms << " ms > "
- << rtLoggingTimestepMs
- << " ms (message printed every 10 seconds)";
+ metronome.waitForNextTick();
}
}};
}
@@ -587,13 +590,12 @@ namespace armarx::RobotUnitModule
<< "Number of streams " << rtDataStreamingEntry.size();
}
-
- if (logAllEntries)
+ if (numberOfEntriesToLog < 1)
{
_module<ControlThreadDataBuffer>()
.getControlThreadOutputBuffer()
- .foreachNewLoggingEntry(
- [&](const auto& data, auto i, auto num)
+ .forEachNewLoggingEntry(
+ [this, &dlogdurs, &now](const auto& data, auto i, auto num)
{
ARMARX_TRACE;
doLogging(dlogdurs, now, data, i, num);
@@ -604,11 +606,12 @@ namespace armarx::RobotUnitModule
_module<ControlThreadDataBuffer>()
.getControlThreadOutputBuffer()
.forLatestLoggingEntry(
- [&](const auto& data, auto i, auto num)
+ [this, &dlogdurs, &now](const auto& data, auto i, auto num)
{
ARMARX_TRACE;
doLogging(dlogdurs, now, data, i, num);
- });
+ },
+ numberOfEntriesToLog);
}
}
ARMARX_DEBUG << ::deactivateSpam() << "the last " << backlog.size()
@@ -650,6 +653,8 @@ namespace armarx::RobotUnitModule
toRemove.reserve(rtDataStreamingEntry.size());
for (auto& [prx, data] : rtDataStreamingEntry)
{
+ ARMARX_DEBUG_S << data.entryBuffer.size() << " entries to send to "
+ << prx->ice_toString();
if (data.stopStreaming)
{
toRemove.emplace_back(prx);
@@ -668,7 +673,7 @@ namespace armarx::RobotUnitModule
// clang-format off
const auto end_time = IceUtil::Time::now();
const auto time_total = (end_time - now).toMilliSecondsDouble();
- ARMARX_DEBUG << deactivateSpam(1)
+ ARMARX_DEBUG_S << deactivateSpam(1)
<< "rtlogging time required: " << time_total << "ms\n"
<< " time_remove_backlog_entries "
<< (start_time_log_all - start_time_remove_backlog_entries).toMilliSecondsDouble() << "ms\n"
@@ -911,7 +916,13 @@ Logging::_postOnInitRobotUnit()
IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
rtLoggingBacklogMaxSize = getProperty<std::size_t>("RTLogging_MaxBacklogSize");
rtLoggingBacklogEnabled = getProperty<bool>("RTLogging_EnableBacklog");
+ getProperty(numberOfEntriesToLog, "RTLogging_LogLastNMessagesOnly");
ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
+ ARMARX_IMPORTANT << "Initializing RTLogging with the following parameters: " << VAROUT(rtLoggingTimestepMs)
+ << VAROUT(messageBufferSize) << VAROUT(messageBufferMaxSize)
+ << VAROUT(messageBufferNumberEntries) << VAROUT(messageBufferMaxNumberEntries)
+ << VAROUT(rtLoggingBacklogRetentionTime) << VAROUT(rtLoggingBacklogMaxSize)
+ << VAROUT(rtLoggingBacklogEnabled) << VAROUT(numberOfEntriesToLog);
}
void
@@ -1140,13 +1151,14 @@ Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r,
{
ARMARX_TRACE;
const auto start_send = IceUtil::Time::now();
+ const auto num_timesteps = result.size();
updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
const auto start_clear = IceUtil::Time::now();
clearResult();
const auto end = IceUtil::Time::now();
ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
<< "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms ("
- << result.size() << " timesteps)"
+ << num_timesteps << " timesteps)"
<< "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
//now execute all ready callbacks
@@ -1181,4 +1193,4 @@ Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r,
updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index 7529c75fd0182aa30c7c64ff994df15127b29009..ec1179ae7c5c3e31c8dad8878edce862078c37b6 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -23,17 +23,17 @@
#pragma once
#include <atomic>
-#include <map>
-#include <vector>
#include <deque>
#include <fstream>
+#include <map>
+#include <vector>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include "RobotUnitModuleBase.h"
#include "../util/ControlThreadOutputBuffer.h"
+#include "RobotUnitModuleBase.h"
namespace armarx::RobotUnitModule::details
{
@@ -42,54 +42,68 @@ namespace armarx::RobotUnitModule::details
namespace armarx::RobotUnitModule
{
- class LoggingPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class LoggingPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- LoggingPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ LoggingPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
defineOptionalProperty<std::size_t>(
- "RTLogging_PeriodMs", 10,
+ "RTLogging_PeriodMs",
+ 10,
"Period of the rt-logging thread in milliseconds. "
"A high period can cause spikes in disk activity if data is logged to disk. "
"A low period causes more computation overhead and memory consumption. "
"Must not be 0.");
- defineOptionalProperty<std::string>(
- "RTLogging_DefaultLog", "",
- "If rt logging is active and a file path is given, all data is logged to this file.");
+ defineOptionalProperty<std::string>("RTLogging_DefaultLog",
+ "",
+ "If rt logging is active and a file path is given, "
+ "all data is logged to this file.");
defineOptionalProperty<std::size_t>(
- "RTLogging_MessageNumber", 1000,
+ "RTLogging_MessageNumber",
+ 1000,
"Number of messages that can be logged in the control thread");
defineOptionalProperty<std::size_t>(
- "RTLogging_MessageBufferSize", 1024 * 1024,
+ "RTLogging_MessageBufferSize",
+ 1024 * 1024,
"Number of bytes that can be logged in the control thread");
defineOptionalProperty<std::size_t>(
- "RTLogging_MaxMessageNumber", 16000,
+ "RTLogging_MaxMessageNumber",
+ 16000,
"Max number of messages that can be logged in the control thread");
defineOptionalProperty<std::size_t>(
- "RTLogging_MaxMessageBufferSize", 16 * 1024 * 1024,
+ "RTLogging_MaxMessageBufferSize",
+ 16 * 1024 * 1024,
"Max number of bytes that can be logged in the control thread");
defineOptionalProperty<bool>(
- "RTLogging_EnableBacklog", true,
+ "RTLogging_EnableBacklog",
+ true,
"Enable/Disable the backlog (SensorValues, ControlTargets, Messages) that is kept"
"and can be dumped in case of an error.");
+ defineOptionalProperty<std::size_t>("RTLogging_KeepIterationsForMs",
+ 60 * 1000,
+ "All logging data (SensorValues, ControlTargets, "
+ "Messages) is kept for this duration "
+ "and can be dumped in case of an error.");
defineOptionalProperty<std::size_t>(
- "RTLogging_KeepIterationsForMs", 60 * 1000,
- "All logging data (SensorValues, ControlTargets, Messages) is kept for this duration "
- "and can be dumped in case of an error.");
- defineOptionalProperty<std::size_t>(
- "RTLogging_MaxBacklogSize", 5000,
+ "RTLogging_MaxBacklogSize",
+ 5000,
"Maximum size of the backlog (SensorValues, ControlTargets, Messages) that is kept"
"and can be dumped in case of an error.");
defineOptionalProperty<std::size_t>(
- "RTLogging_StreamingDataMaxClientConnectionFailures", 10,
- "If sending data to a client fails this often, the client is removed from the list");
-
- defineOptionalProperty<bool>("RTLogging_LogAllMessages", true,
- "If true, logs all messages from the control thread. If false, only the newest message will be logged");
+ "RTLogging_StreamingDataMaxClientConnectionFailures",
+ 10,
+ "If sending data to a client fails this often, the client is removed from the "
+ "list");
+
+ defineOptionalProperty<int>(
+ "RTLogging_LogLastNMessagesOnly",
+ -1,
+ "If greater than 0, logs/streams only the last n timesteps that are in the entry "
+ "cue of the ControlThreadOutputBuffer");
}
};
@@ -106,15 +120,18 @@ namespace armarx::RobotUnitModule
class Logging : virtual public ModuleBase, virtual public RobotUnitLoggingInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static Logging& Instance()
+ static Logging&
+ Instance()
{
return ModuleBase::Instance<Logging>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -137,7 +154,10 @@ namespace armarx::RobotUnitModule
* @param loggingNames The data fields to log.
* @return A handle to the log. If it's last copy is deleted, logging is stopped.
*/
- SimpleRemoteReferenceCounterBasePtr startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current& = Ice::emptyCurrent) override;
+ SimpleRemoteReferenceCounterBasePtr
+ startRtLogging(const std::string& formatString,
+ const Ice::StringSeq& loggingNames,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Starts logging to a CSV file
* @param formatString The file to log to.
@@ -145,19 +165,25 @@ namespace armarx::RobotUnitModule
* If value is empty, key is used as heading.
* @return A handle to the log. If it's last copy is deleted, logging is stopped.
*/
- SimpleRemoteReferenceCounterBasePtr startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current& = Ice::emptyCurrent) override;
+ SimpleRemoteReferenceCounterBasePtr
+ startRtLoggingWithAliasNames(const std::string& formatString,
+ const StringStringDictionary& aliasNames,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Stops logging to the given log.
* @param token The log to close.
*/
- void stopRtLogging(const armarx::SimpleRemoteReferenceCounterBasePtr& token, const Ice::Current& = Ice::emptyCurrent) override;
+ void stopRtLogging(const armarx::SimpleRemoteReferenceCounterBasePtr& token,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Adds a string to the log (it is added in a special column).
* @param token The log.
* @param marker The string to add.
*/
- void addMarkerToRtLog(const SimpleRemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current& = Ice::emptyCurrent) override;
+ void addMarkerToRtLog(const SimpleRemoteReferenceCounterBasePtr& token,
+ const std::string& marker,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns the names of all loggable data fields.
@@ -169,16 +195,16 @@ namespace armarx::RobotUnitModule
* This helps debugging.
* @param formatString The file.
*/
- void writeRecentIterationsToFile(const std::string& formatString, const Ice::Current& = Ice::emptyCurrent) const override;
+ void writeRecentIterationsToFile(const std::string& formatString,
+ const Ice::Current& = Ice::emptyCurrent) const override;
- RobotUnitDataStreaming::DataStreamingDescription startDataStreaming(
- const RobotUnitDataStreaming::ReceiverPrx& receiver,
- const RobotUnitDataStreaming::Config& config,
- const Ice::Current& = Ice::emptyCurrent) override;
+ RobotUnitDataStreaming::DataStreamingDescription
+ startDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const RobotUnitDataStreaming::Config& config,
+ const Ice::Current& = Ice::emptyCurrent) override;
- void stopDataStreaming(
- const RobotUnitDataStreaming::ReceiverPrx& receiver,
- const Ice::Current& = Ice::emptyCurrent) override;
+ void stopDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const Ice::Current& = Ice::emptyCurrent) override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -188,7 +214,8 @@ namespace armarx::RobotUnitModule
void doLogging(details::DoLoggingDurations& durations,
const IceUtil::Time& now,
const ControlThreadOutputBuffer::Entry& data,
- std::size_t i, std::size_t num);
+ std::size_t i,
+ std::size_t num);
static bool MatchName(const std::string& pattern, const std::string& name);
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -199,7 +226,7 @@ namespace armarx::RobotUnitModule
struct CSVLoggingEntry
{
/// @brief Whether logging should be stopped.
- std::atomic_bool stopLogging {false};
+ std::atomic_bool stopLogging{false};
/// @brief Bools whether a control device field should be logged.
std::vector<std::vector<std::vector<char>>> loggedControlDeviceValues;
/// @brief Bools whether a sensor device field should be logged.
@@ -225,48 +252,47 @@ namespace armarx::RobotUnitModule
Double,
Skipped
};
+
struct OutVal
{
std::size_t idx;
- ValueT value = ValueT::Skipped;
+ ValueT value = ValueT::Skipped;
};
- bool stopStreaming = false;
+ bool stopStreaming = false;
- std::size_t numBools = 0;
- std::size_t numBytes = 0;
- std::size_t numShorts = 0;
- std::size_t numInts = 0;
- std::size_t numLongs = 0;
- std::size_t numFloats = 0;
+ std::size_t numBools = 0;
+ std::size_t numBytes = 0;
+ std::size_t numShorts = 0;
+ std::size_t numInts = 0;
+ std::size_t numLongs = 0;
+ std::size_t numFloats = 0;
std::size_t numDoubles = 0;
- bool onlyNewestFrame = false;
+ bool onlyNewestFrame = false;
std::size_t connectionFailures = 0;
std::size_t rtStreamMaxClientErrors = 0;
- std::vector<std::vector<OutVal>> sensDevs;
+ std::vector<std::vector<OutVal>> sensDevs;
std::vector<std::vector<std::vector<OutVal>>> ctrlDevs;
- RobotUnitDataStreaming::TimeStepSeq result;
- std::deque<RobotUnitDataStreaming::TimeStep> entryBuffer;
- std::deque<Ice::AsyncResultPtr> updateCalls;
+ RobotUnitDataStreaming::TimeStepSeq result;
+ std::deque<RobotUnitDataStreaming::TimeStep> entryBuffer;
+ std::deque<Ice::AsyncResultPtr> updateCalls;
void clearResult();
RobotUnitDataStreaming::TimeStep getResultElement();
RobotUnitDataStreaming::TimeStep allocateResultElement() const;
-
void processHeader(const ControlThreadOutputBuffer::Entry& e);
void processCtrl(const ControlTargetBase& val,
std::size_t didx,
std::size_t vidx,
std::size_t fidx);
- void processSens(const SensorValueBase& val,
- std::size_t didx,
- std::size_t fidx);
+ void processSens(const SensorValueBase& val, std::size_t didx, std::size_t fidx);
void send(const RobotUnitDataStreaming::ReceiverPrx& r, uint64_t msgId);
};
+
std::map<RobotUnitDataStreaming::ReceiverPrx, DataStreamingEntry> rtDataStreamingEntry;
std::uint64_t rtDataStreamingMsgID = 0;
@@ -278,9 +304,9 @@ namespace armarx::RobotUnitModule
/// @brief Handle for the default log. (This log can be enabled via a property and logs everything)
SimpleRemoteReferenceCounterBasePtr defaultLogHandle;
/// @brief The stored backlog (it is dumped when \ref writeRecentIterationsToFile is called)
- std::deque< ::armarx::detail::ControlThreadOutputBufferEntry > backlog;
+ std::deque<::armarx::detail::ControlThreadOutputBufferEntry> backlog;
/// @brief The control threads unix thread id
- Ice::Int controlThreadId {0};
+ Ice::Int controlThreadId{0};
/// @brief Mutex protecting the data structures of this class
mutable std::mutex rtLoggingMutex;
@@ -288,27 +314,28 @@ namespace armarx::RobotUnitModule
{
struct FieldMetaData
{
- std::string name;
+ std::string name;
const std::type_info* type;
};
+
std::vector<FieldMetaData> fields;
};
/// @brief Data field info for all \ref ControlTargetBase "ControlTargets" (dex x jctrl)
std::vector<std::vector<ValueMetaData>> controlDeviceValueMetaData;
/// @brief Data field info for all \ref SensorValueBase "SensorValues" (dev)
- std::vector< ValueMetaData > sensorDeviceValueMetaData;
+ std::vector<ValueMetaData> sensorDeviceValueMetaData;
/// @brief The initial size of the Message entry buffer
- std::size_t messageBufferSize {0};
+ std::size_t messageBufferSize{0};
/// @brief The initial number of Message entries
- std::size_t messageBufferNumberEntries {0};
+ std::size_t messageBufferNumberEntries{0};
/// @brief The maximal size of the Message entry buffer
- std::size_t messageBufferMaxSize {0};
+ std::size_t messageBufferMaxSize{0};
/// @brief The maximal number of Message entries
- std::size_t messageBufferMaxNumberEntries {0};
+ std::size_t messageBufferMaxNumberEntries{0};
/// @brief The logging thread's period
- std::size_t rtLoggingTimestepMs {0};
+ std::size_t rtLoggingTimestepMs{0};
/// @brief The time an entry should remain in the backlog.
IceUtil::Time rtLoggingBacklogRetentionTime;
/// @brief The maximum number of entries in the backlog.
@@ -316,8 +343,8 @@ namespace armarx::RobotUnitModule
/// @brief Enable/disable the backlog.
bool rtLoggingBacklogEnabled;
- /// @brief
- bool logAllEntries = true;
+ /// @brief
+ int numberOfEntriesToLog{-1};
friend void WriteTo(const auto& dentr,
const Logging::DataStreamingEntry::OutVal& out,
@@ -325,4 +352,4 @@ namespace armarx::RobotUnitModule
std::size_t fidx,
auto& data);
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
index 28a25a4706505ce721037efff9b6d7348531b97c..11a61acc71b3df3ae35b411194741f76cfb38a48 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.cpp
@@ -20,38 +20,42 @@
* GNU General Public License
*/
-#include <ArmarXCore/core/ArmarXManager.h>
-
#include "RobotUnitModuleManagement.h"
+
+#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/time/TimeUtil.h>
namespace armarx::RobotUnitModule
{
- void Management::_preOnInitRobotUnit()
+ void
+ Management::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- additionalObjectSchedulerCount = getProperty<std::uint64_t>("AdditionalObjectSchedulerCount").getValue();
- heartbeatRequired = getProperty<bool>("HeartbeatRequired").getValue();
- heartbeatMaxCycleMS = getProperty<long>("HeartbeatMaxCycleMS").getValue();
- heartbeatStartupMarginMS = getProperty<long>("HeartbeatStartupMarginMS").getValue();
+ additionalObjectSchedulerCount =
+ getProperty<std::uint64_t>("AdditionalObjectSchedulerCount").getValue();
+ heartbeatRequired = getProperty<bool>("HeartbeatRequired").getValue();
+ heartbeatMaxCycleMS = getProperty<long>("HeartbeatMaxCycleMS").getValue();
+ heartbeatStartupMarginMS = getProperty<long>("HeartbeatStartupMarginMS").getValue();
getArmarXManager()->increaseSchedulers(static_cast<int>(additionalObjectSchedulerCount));
usingTopic(getProperty<std::string>("AggregatedRobotHealthTopicName").getValue());
}
- void Management::_postFinishControlThreadInitialization()
+ void
+ Management::_postFinishControlThreadInitialization()
{
controlLoopStartTime = TimeUtil::GetTime(true).toMilliSeconds();
}
-
- void Management::aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&)
+ void
+ Management::aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&)
{
heartbeatRequired = true;
- if (overallHealthState == RobotHealthState::HealthOK || overallHealthState == RobotHealthState::HealthWarning)
+ if (overallHealthState == RobotHealthState::HealthOK ||
+ overallHealthState == RobotHealthState::HealthWarning)
{
lastHeartbeat = TimeUtil::GetTime(true).toMilliSeconds();
}
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
index 71c9f1caad2438d2c0869dc64e70f01440907de6..c92c56d05435c7a9e104837131cc49666a5e6f50 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h
@@ -27,27 +27,22 @@
namespace armarx::RobotUnitModule
{
- class ManagementPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class ManagementPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- ManagementPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ ManagementPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
defineOptionalProperty<std::uint64_t>(
- "AdditionalObjectSchedulerCount", 10,
- "Number of ObjectSchedulers to be added");
+ "AdditionalObjectSchedulerCount", 10, "Number of ObjectSchedulers to be added");
defineOptionalProperty<bool>(
- "HeartbeatRequired", true,
- "Whether the Heatbeat is required.");
+ "HeartbeatRequired", true, "Whether the Heatbeat is required.");
+ defineOptionalProperty<long>("HeartbeatMaxCycleMS", 100, "The heartbeats cycle time");
defineOptionalProperty<long>(
- "HeartbeatMaxCycleMS", 100,
- "The heartbeats cycle time");
- defineOptionalProperty<long>(
- "HeartbeatStartupMarginMS", 1000,
- "Startup time for heartbeats");
- defineOptionalProperty<std::string>(
- "AggregatedRobotHealthTopicName", "AggregatedRobotHealthTopic",
- "Name of the AggregatedRobotHealthTopic");
+ "HeartbeatStartupMarginMS", 1000, "Startup time for heartbeats");
+ defineOptionalProperty<std::string>("AggregatedRobotHealthTopicName",
+ "AggregatedRobotHealthTopic",
+ "Name of the AggregatedRobotHealthTopic");
}
};
@@ -61,15 +56,18 @@ namespace armarx::RobotUnitModule
class Management : virtual public ModuleBase, virtual public RobotUnitManagementInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static Management& Instance()
+ static Management&
+ Instance()
{
return ModuleBase::Instance<Management>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -85,14 +83,17 @@ namespace armarx::RobotUnitModule
* @brief Returns whether the RobotUnit is running.
* @return Whether the RobotUnit is running.
*/
- bool isRunning(const Ice::Current& = Ice::emptyCurrent) const override
+ bool
+ isRunning(const Ice::Current& = Ice::emptyCurrent) const override
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return getRobotUnitState() == RobotUnitState::Running;
}
+
void aggregatedHeartbeat(RobotHealthState overallHealthState, const Ice::Current&) override;
- bool isSimulation(const Ice::Current& = Ice::emptyCurrent) const override
+ bool
+ isSimulation(const Ice::Current& = Ice::emptyCurrent) const override
{
return false;
}
@@ -102,9 +103,9 @@ namespace armarx::RobotUnitModule
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
/// @brief The number of additional object schedulers
- std::int64_t additionalObjectSchedulerCount {0};
- std::int64_t controlLoopStartTime {0};
- std::int64_t heartbeatStartupMarginMS {0};
+ std::int64_t additionalObjectSchedulerCount{0};
+ std::int64_t controlLoopStartTime{0};
+ std::int64_t heartbeatStartupMarginMS{0};
std::atomic_bool heartbeatRequired{false};
std::atomic_long heartbeatMaxCycleMS{100};
std::atomic_long lastHeartbeat{0};
@@ -119,4 +120,4 @@ namespace armarx::RobotUnitModule
*/
friend class ManagementAttorneyForControlThread;
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index 243950a7c5a15f376f1f5c7b09b8b99fac0ea2d2..5d51c4ca2474c8e46bd3be0140ae45ec9e877e55 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -22,20 +22,20 @@
#include "RobotUnitModulePublisher.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
-#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/util/StringHelpers.h"
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
+#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
+
namespace armarx::RobotUnitModule
{
/**
@@ -45,37 +45,44 @@ namespace armarx::RobotUnitModule
class NJointControllerAttorneyForPublisher
{
friend class Publisher;
- static bool GetStatusReportedActive(const NJointControllerBasePtr& nJointCtrl)
+
+ static bool
+ GetStatusReportedActive(const NJointControllerBasePtr& nJointCtrl)
{
return nJointCtrl->statusReportedActive;
}
- static bool GetStatusReportedRequested(const NJointControllerBasePtr& nJointCtrl)
+
+ static bool
+ GetStatusReportedRequested(const NJointControllerBasePtr& nJointCtrl)
{
return nJointCtrl->statusReportedRequested;
}
- static void UpdateStatusReported(const NJointControllerBasePtr& nJointCtrl)
+
+ static void
+ UpdateStatusReported(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->statusReportedActive = nJointCtrl->isActive.load();
nJointCtrl->statusReportedRequested = nJointCtrl->isRequested.load();
}
- static void Publish(
- const NJointControllerBasePtr& nJointCtrl,
- const SensorAndControl& sac,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- )
+
+ static void
+ Publish(const NJointControllerBasePtr& nJointCtrl,
+ const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer)
{
- nJointCtrl ->publish(sac, draw, observer);
+ nJointCtrl->publish(sac, draw, observer);
}
- static void DeactivatePublishing(
- const NJointControllerBasePtr& nJointCtrl,
- const DebugDrawerInterfacePrx& draw,
- const DebugObserverInterfacePrx& observer
- )
+
+ static void
+ DeactivatePublishing(const NJointControllerBasePtr& nJointCtrl,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer)
{
nJointCtrl->deactivatePublish(draw, observer);
}
};
+
/**
* \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref Publisher.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -83,11 +90,14 @@ namespace armarx::RobotUnitModule
class DevicesAttorneyForPublisher
{
friend class Publisher;
- static const std::string& GetSensorDeviceName(Publisher* p, std::size_t idx)
+
+ static const std::string&
+ GetSensorDeviceName(Publisher* p, std::size_t idx)
{
return p->_module<Devices>().sensorDevices.at(idx)->getDeviceName();
}
};
+
/**
* \brief This class allows minimal access to private members of \ref Units in a sane fashion for \ref Publisher.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -95,11 +105,14 @@ namespace armarx::RobotUnitModule
class UnitsAttorneyForPublisher
{
friend class Publisher;
- static const std::vector<RobotUnitSubUnitPtr>& GetSubUnits(Publisher* p)
+
+ static const std::vector<RobotUnitSubUnitPtr>&
+ GetSubUnits(Publisher* p)
{
return p->_module<Units>().subUnits;
}
};
+
/**
* \brief This class allows minimal access to private members of \ref ControllerManagement in a sane fashion for \ref Publisher.
* \warning !! DO NOT ADD ANYTHING IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -107,15 +120,20 @@ namespace armarx::RobotUnitModule
class ControllerManagementAttorneyForPublisher
{
friend class Publisher;
- static const std::map<std::string, NJointControllerBasePtr>& GetNJointControllers(Publisher* p)
+
+ static const std::map<std::string, NJointControllerBasePtr>&
+ GetNJointControllers(Publisher* p)
{
return p->_module<ControllerManagement>().nJointControllers;
}
- static void RemoveNJointControllersToBeDeleted(Publisher* p, bool blocking, RobotUnitListenerPrx l)
+
+ static void
+ RemoveNJointControllersToBeDeleted(Publisher* p, bool blocking, RobotUnitListenerPrx l)
{
p->_module<ControllerManagement>().removeNJointControllersToBeDeleted(blocking, l);
}
};
+
/**
* \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref Publisher.
* \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
@@ -123,16 +141,19 @@ namespace armarx::RobotUnitModule
class ControlThreadAttorneyForPublisher
{
friend class Publisher;
- static void ProcessEmergencyStopRequest(Publisher* p)
+
+ static void
+ ProcessEmergencyStopRequest(Publisher* p)
{
return p->_module<ControlThread>().processEmergencyStopRequest();
}
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- TimedVariantPtr Publisher::publishNJointClassNames()
+ TimedVariantPtr
+ Publisher::publishNJointClassNames()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -152,13 +173,12 @@ namespace armarx::RobotUnitModule
}
const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- TimedVariantPtr Publisher::publishNJointControllerUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer
- )
+ TimedVariantPtr
+ Publisher::publishNJointControllerUpdates(StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -168,24 +188,27 @@ namespace armarx::RobotUnitModule
auto debugDrawerBatchPrx = debugDrawerPrx->ice_batchOneway();
auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
NJointControllerStatusSeq allStatus;
- for (const auto& pair : ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
+ for (const auto& pair :
+ ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
{
const auto begInner = TimeUtil::GetTime(true);
const NJointControllerBasePtr& nJointCtrl = pair.second;
//run some hook for active (used for visu)
- NJointControllerAttorneyForPublisher::Publish(nJointCtrl, controlThreadOutputBuffer, debugDrawerBatchPrx, debugObserverBatchPrx);
- if (
- NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) != nJointCtrl->isControllerActive() ||
- NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) != nJointCtrl->isControllerRequested()
- )
+ NJointControllerAttorneyForPublisher::Publish(
+ nJointCtrl, controlThreadOutputBuffer, debugDrawerBatchPrx, debugObserverBatchPrx);
+ if (NJointControllerAttorneyForPublisher::GetStatusReportedActive(nJointCtrl) !=
+ nJointCtrl->isControllerActive() ||
+ NJointControllerAttorneyForPublisher::GetStatusReportedRequested(nJointCtrl) !=
+ nJointCtrl->isControllerRequested())
{
NJointControllerAttorneyForPublisher::UpdateStatusReported(nJointCtrl);
allStatus.emplace_back(nJointCtrl->getControllerStatus());
}
const auto endInner = TimeUtil::GetTime(true);
- timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] = new TimedVariant {TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
+ timingMap["NJointControllerUpdates_" + nJointCtrl->getInstanceName()] =
+ new TimedVariant{TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
}
robotUnitListenerBatchPrx->nJointControllerStatusChanged(allStatus);
@@ -193,13 +216,13 @@ namespace armarx::RobotUnitModule
debugDrawerBatchPrx->ice_flushBatchRequests();
const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- TimedVariantPtr Publisher::publishUnitUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer,
- const JointAndNJointControllers& activatedControllers)
+ TimedVariantPtr
+ Publisher::publishUnitUpdates(StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer,
+ const JointAndNJointControllers& activatedControllers)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -214,20 +237,21 @@ namespace armarx::RobotUnitModule
const auto begInner = TimeUtil::GetTime(true);
rsu->update(controlThreadOutputBuffer, activatedControllers);
const auto endInner = TimeUtil::GetTime(true);
- timingMap["UnitUpdate_" + rsu->getName()] = new TimedVariant {TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
+ timingMap["UnitUpdate_" + rsu->getName()] = new TimedVariant{
+ TimestampVariant{endInner - begInner}, lastControlThreadTimestamp};
}
}
const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- TimedVariantPtr Publisher::publishControlUpdates(
- const SensorAndControl& controlThreadOutputBuffer,
- bool haveSensorAndControlValsChanged,
- bool publishToObserver,
- const JointAndNJointControllers& activatedControllers,
- const std::vector<JointController*>& requestedJointControllers)
+ TimedVariantPtr
+ Publisher::publishControlUpdates(const SensorAndControl& controlThreadOutputBuffer,
+ bool haveSensorAndControlValsChanged,
+ bool publishToObserver,
+ const JointAndNJointControllers& activatedControllers,
+ const std::vector<JointController*>& requestedJointControllers)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -236,21 +260,26 @@ namespace armarx::RobotUnitModule
StringVariantBaseMap ctrlDevMap;
ControlDeviceStatusSeq allStatus;
- for (std::size_t ctrlidx = 0 ; ctrlidx < _module<Devices>().getNumberOfControlDevices(); ++ctrlidx)
+ for (std::size_t ctrlidx = 0; ctrlidx < _module<Devices>().getNumberOfControlDevices();
+ ++ctrlidx)
{
const ControlDevice& ctrlDev = *(_module<Devices>().getControlDevices().at(ctrlidx));
StringToStringVariantBaseMapMap variants;
ControlDeviceStatus status;
const auto activeJointCtrl = activatedControllers.jointControllers.at(ctrlidx);
- status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"};
+ status.activeControlMode = activeJointCtrl
+ ? activeJointCtrl->getControlMode()
+ : std::string{"!!JointController is nullptr!!"};
status.deviceName = ctrlDev.getDeviceName();
if (activeJointCtrl)
{
- auto additionalDatafields = activeJointCtrl->publish(debugDrawerPrx, debugObserverPrx);
+ auto additionalDatafields =
+ activeJointCtrl->publish(debugDrawerPrx, debugObserverPrx);
for (auto& pair : additionalDatafields)
{
- ctrlDevMap[ctrlDev.getDeviceName() + "_" + activeJointCtrl->getControlMode() + "_" + pair.first] = pair.second;
+ ctrlDevMap[ctrlDev.getDeviceName() + "_" + activeJointCtrl->getControlMode() +
+ "_" + pair.first] = pair.second;
}
}
@@ -258,12 +287,8 @@ namespace armarx::RobotUnitModule
{
const auto& controlMode = ctrlVal->getControlMode();
variants[controlMode] = ctrlVal->toVariants(lastControlThreadTimestamp);
- if (
- haveSensorAndControlValsChanged &&
- !variants[controlMode].empty() &&
- observerPublishControlTargets &&
- publishToObserver
- )
+ if (haveSensorAndControlValsChanged && !variants[controlMode].empty() &&
+ observerPublishControlTargets && publishToObserver)
{
for (const auto& pair : variants[controlMode])
{
@@ -284,17 +309,18 @@ namespace armarx::RobotUnitModule
{
if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
{
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->controlDevicesChannel, std::move(ctrlDevMap));
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
+ robotUnitObserver->controlDevicesChannel, std::move(ctrlDevMap));
}
}
const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- TimedVariantPtr Publisher::publishSensorUpdates(
- bool publishToObserver,
- const SensorAndControl& controlThreadOutputBuffer)
+ TimedVariantPtr
+ Publisher::publishSensorUpdates(bool publishToObserver,
+ const SensorAndControl& controlThreadOutputBuffer)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -302,9 +328,10 @@ namespace armarx::RobotUnitModule
StringVariantBaseMap sensorDevMap;
const auto numSensorDev = _module<Devices>().getNumberOfSensorDevices();
- ARMARX_CHECK_EQUAL(controlThreadOutputBuffer.sensors.size(), _module<Devices>().getNumberOfSensorDevices());
+ ARMARX_CHECK_EQUAL(controlThreadOutputBuffer.sensors.size(),
+ _module<Devices>().getNumberOfSensorDevices());
SensorDeviceStatusSeq allStatus;
- for (std::size_t sensidx = 0 ; sensidx < numSensorDev; ++sensidx)
+ for (std::size_t sensidx = 0; sensidx < numSensorDev; ++sensidx)
{
ARMARX_TRACE;
const SensorValueBase& sensVal = *(controlThreadOutputBuffer.sensors.at(sensidx));
@@ -340,46 +367,60 @@ namespace armarx::RobotUnitModule
std::stringstream s;
for (auto& pair : sensorDevMap)
{
- s << pair.first << ": \t" << (pair.second ? pair.second->ice_id() + "\t with datatype \t" + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
+ s << pair.first << ": \t"
+ << (pair.second ? pair.second->ice_id() + "\t with datatype \t" +
+ Variant::typeToString(pair.second->getType())
+ : "NULL")
+ << "\n";
}
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
+ ARMARX_DEBUG << deactivateSpam(spamdelay)
+ << "Sensor Datafieldnames: " << ARMARX_STREAM_PRINTER
{
for (auto& pair : sensorDevMap)
{
- out << pair.first << ": " << (pair.second ? pair.second->ice_id() + Variant::typeToString(pair.second->getType()) : "NULL") << "\n";
+ out << pair.first << ": "
+ << (pair.second ? pair.second->ice_id() +
+ Variant::typeToString(pair.second->getType())
+ : "NULL")
+ << "\n";
}
};
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->sensorDevicesChannel, std::move(sensorDevMap));
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
+ robotUnitObserver->sensorDevicesChannel, std::move(sensorDevMap));
}
}
ARMARX_TRACE;
const auto end = TimeUtil::GetTime(true);
- return new TimedVariant {TimestampVariant{end - beg}, lastControlThreadTimestamp};
+ return new TimedVariant{TimestampVariant{end - beg}, lastControlThreadTimestamp};
}
- std::string Publisher::getRobotUnitListenerTopicName(const Ice::Current&) const
+ std::string
+ Publisher::getRobotUnitListenerTopicName(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return robotUnitListenerTopicName;
}
- std::string Publisher::getDebugDrawerTopicName(const Ice::Current&) const
+ std::string
+ Publisher::getDebugDrawerTopicName(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return debugDrawerUpdatesTopicName;
}
- std::string Publisher::getDebugObserverTopicName(const Ice::Current&) const
+ std::string
+ Publisher::getDebugObserverTopicName(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return debugObserverTopicName;
}
- RobotUnitListenerPrx Publisher::getRobotUnitListenerProxy(const Ice::Current&) const
+ RobotUnitListenerPrx
+ Publisher::getRobotUnitListenerProxy(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -387,7 +428,8 @@ namespace armarx::RobotUnitModule
return robotUnitListenerPrx;
}
- DebugObserverInterfacePrx Publisher::getDebugObserverProxy(const Ice::Current&) const
+ DebugObserverInterfacePrx
+ Publisher::getDebugObserverProxy(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -395,7 +437,8 @@ namespace armarx::RobotUnitModule
return debugObserverPrx;
}
- DebugDrawerInterfacePrx Publisher::getDebugDrawerProxy(const Ice::Current&) const
+ DebugDrawerInterfacePrx
+ Publisher::getDebugDrawerProxy(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -403,34 +446,45 @@ namespace armarx::RobotUnitModule
return debugDrawerPrx;
}
- void Publisher::_preOnConnectRobotUnit()
+ void
+ Publisher::_preOnConnectRobotUnit()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_DEBUG << "retrieving RobotUnitListenerPrx for topic " << getRobotUnitListenerTopicName();
+ ARMARX_DEBUG << "retrieving RobotUnitListenerPrx for topic "
+ << getRobotUnitListenerTopicName();
robotUnitListenerPrx = getTopic<RobotUnitListenerPrx>(getRobotUnitListenerTopicName());
robotUnitListenerBatchPrx = robotUnitListenerPrx->ice_batchOneway();
- ARMARX_DEBUG << "retrieving DebugDrawerInterfacePrx for topic " << getDebugDrawerTopicName();
+ ARMARX_DEBUG << "retrieving DebugDrawerInterfacePrx for topic "
+ << getDebugDrawerTopicName();
debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getDebugDrawerTopicName());
- ARMARX_DEBUG << "retrieving DebugObserverInterfacePrx for topic " << getDebugObserverTopicName();
+ ARMARX_DEBUG << "retrieving DebugObserverInterfacePrx for topic "
+ << getDebugObserverTopicName();
debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getDebugObserverTopicName());
}
- void Publisher::_preOnInitRobotUnit()
+ void
+ Publisher::_preOnInitRobotUnit()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- robotUnitListenerTopicName = getProperty<std::string>("RobotUnitListenerTopicName").getValue();
- debugDrawerUpdatesTopicName = getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
+ robotUnitListenerTopicName =
+ getProperty<std::string>("RobotUnitListenerTopicName").getValue();
+ debugDrawerUpdatesTopicName =
+ getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue();
debugObserverTopicName = getProperty<std::string>("DebugObserverTopicName").getValue();
observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
- observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
- observerPublishTimingInformation = getProperty<bool>("ObserverPublishTimingInformation").getValue();
- observerPublishAdditionalInformation = getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
- debugObserverSkipIterations = std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());
+ observerPublishControlTargets =
+ getProperty<bool>("ObserverPublishControlTargets").getValue();
+ observerPublishTimingInformation =
+ getProperty<bool>("ObserverPublishTimingInformation").getValue();
+ observerPublishAdditionalInformation =
+ getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
+ debugObserverSkipIterations =
+ std::max(1ul, getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue());
publishPeriodMs = std::max(1ul, getProperty<std::size_t>("PublishPeriodMs").getValue());
@@ -440,41 +494,50 @@ namespace armarx::RobotUnitModule
getArmarXManager()->addObject(robotUnitObserver);
}
- void Publisher::_icePropertiesInitialized()
+ void
+ Publisher::_icePropertiesInitialized()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(getIceProperties(), "", getConfigDomain());
+ robotUnitObserver = Component::create<RobotUnitObserver, RobotUnitObserverPtr>(
+ getIceProperties(), "", getConfigDomain());
addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObserver));
}
- void Publisher::_componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+ void
+ Publisher::_componentPropertiesUpdated(const std::set<std::string>& changedProperties)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (changedProperties.count("ObserverPublishSensorValues"))
{
- observerPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
+ observerPublishSensorValues =
+ getProperty<bool>("ObserverPublishSensorValues").getValue();
}
if (changedProperties.count("ObserverPublishControlTargets"))
{
- observerPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
+ observerPublishControlTargets =
+ getProperty<bool>("ObserverPublishControlTargets").getValue();
}
if (changedProperties.count("ObserverPublishTimingInformation"))
{
- observerPublishTimingInformation = getProperty<bool>("ObserverPublishTimingInformation").getValue();
+ observerPublishTimingInformation =
+ getProperty<bool>("ObserverPublishTimingInformation").getValue();
}
if (changedProperties.count("ObserverPublishAdditionalInformation"))
{
- observerPublishAdditionalInformation = getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
+ observerPublishAdditionalInformation =
+ getProperty<bool>("ObserverPublishAdditionalInformation").getValue();
}
if (changedProperties.count("ObserverPrintEveryNthIterations"))
{
- debugObserverSkipIterations = getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
+ debugObserverSkipIterations =
+ getProperty<std::uint64_t>("ObserverPrintEveryNthIterations").getValue();
}
}
- void Publisher::_preFinishRunning()
+ void
+ Publisher::_preFinishRunning()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -482,35 +545,41 @@ namespace armarx::RobotUnitModule
{
ARMARX_DEBUG << "shutting down publisher task";
publisherTask->stop();
- std::this_thread::sleep_for(std::chrono::milliseconds {10});
+ std::this_thread::sleep_for(std::chrono::milliseconds{10});
while (publisherTask->isFunctionExecuting() || publisherTask->isRunning())
{
- ARMARX_FATAL << deactivateSpam(0.1) << "PUBLISHER TASK IS RUNNING AFTER IT WAS STOPPED!";
+ ARMARX_FATAL << deactivateSpam(0.1)
+ << "PUBLISHER TASK IS RUNNING AFTER IT WAS STOPPED!";
}
publisherTask = nullptr;
//since the drawer queues draw events and we want to clear the layers, we have to sleep here
- std::this_thread::sleep_for(std::chrono::milliseconds {100});
+ std::this_thread::sleep_for(std::chrono::milliseconds{100});
ARMARX_DEBUG << "shutting down publisher task done";
}
- for (const auto& pair : ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
+ for (const auto& pair :
+ ControllerManagementAttorneyForPublisher::GetNJointControllers(this))
{
ARMARX_DEBUG << "forcing deactivation of publishing for " << pair.first;
- NJointControllerAttorneyForPublisher::DeactivatePublishing(pair.second, debugDrawerPrx, debugObserverPrx);
+ NJointControllerAttorneyForPublisher::DeactivatePublishing(
+ pair.second, debugDrawerPrx, debugObserverPrx);
}
}
- void Publisher::_postFinishControlThreadInitialization()
+ void
+ Publisher::_postFinishControlThreadInitialization()
{
ARMARX_TRACE;
//start publisher
publishNewSensorDataTime = TimeUtil::GetTime(true);
- publisherTask = new PublisherTaskT([&] {publish({});}, publishPeriodMs, false, getName() + "_PublisherTask");
+ publisherTask = new PublisherTaskT(
+ [&] { publish({}); }, publishPeriodMs, false, getName() + "_PublisherTask");
ARMARX_INFO << "starting publisher with timestep " << publishPeriodMs;
publisherTask->setDelayWarningTolerance(10 * publishPeriodMs);
publisherTask->start();
}
- void Publisher::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
+ void
+ Publisher::publish(StringVariantBaseMap additionalMap, StringVariantBaseMap timingMap)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -518,7 +587,8 @@ namespace armarx::RobotUnitModule
if (getRobotUnitState() != RobotUnitState::Running)
{
- ARMARX_DEBUG << deactivateSpam(spamdelay) << "skipping publishing in state " << getRobotUnitState();
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "skipping publishing in state "
+ << getRobotUnitState();
return;
}
if (isShuttingDown())
@@ -542,93 +612,113 @@ namespace armarx::RobotUnitModule
//get batch proxies
//delete NJoint queued for deletion
- ControllerManagementAttorneyForPublisher::RemoveNJointControllersToBeDeleted(this, false, robotUnitListenerBatchPrx);
+ ControllerManagementAttorneyForPublisher::RemoveNJointControllersToBeDeleted(
+ this, false, robotUnitListenerBatchPrx);
//swap buffers in
- const bool haveActivatedControllersChanged = _module<ControlThreadDataBuffer>().activatedControllersChanged();
- const bool haveSensorAndControlValsChanged = _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
+ const bool haveActivatedControllersChanged =
+ _module<ControlThreadDataBuffer>().activatedControllersChanged();
+ const bool haveSensorAndControlValsChanged =
+ _module<ControlThreadDataBuffer>().sensorAndControlBufferChanged();
//setup datastructures
- const auto& controlThreadOutputBuffer = _module<ControlThreadDataBuffer>().getSensorAndControlBuffer(); ///TODO copy buffer
+ const auto& controlThreadOutputBuffer =
+ _module<ControlThreadDataBuffer>().getSensorAndControlBuffer(); ///TODO copy buffer
- const auto activatedControllers = _module<ControlThreadDataBuffer>().getActivatedControllers();
- const auto requestedJointControllers = _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
+ const auto activatedControllers =
+ _module<ControlThreadDataBuffer>().getActivatedControllers();
+ const auto requestedJointControllers =
+ _module<ControlThreadDataBuffer>().copyRequestedJointControllers();
lastControlThreadTimestamp = controlThreadOutputBuffer.sensorValuesTimestamp;
const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
//publish publishing meta state
- additionalMap["haveActivatedControllersChanged"] = new TimedVariant {haveActivatedControllersChanged, lastControlThreadTimestamp};
- additionalMap["haveSensorAndControlValsChanged"] = new TimedVariant {haveSensorAndControlValsChanged, lastControlThreadTimestamp};
- additionalMap["publishToObserver" ] = new TimedVariant {publishToObserver, lastControlThreadTimestamp};
- additionalMap["SoftwareEmergencyStopState" ] = new TimedVariant {_module<Units>().getEmergencyStopMaster()->getEmergencyStopState() == EmergencyStopState::eEmergencyStopActive ? "EmergencyStopActive" : "EmergencyStopInactive",
- lastControlThreadTimestamp
- };
+ additionalMap["haveActivatedControllersChanged"] =
+ new TimedVariant{haveActivatedControllersChanged, lastControlThreadTimestamp};
+ additionalMap["haveSensorAndControlValsChanged"] =
+ new TimedVariant{haveSensorAndControlValsChanged, lastControlThreadTimestamp};
+ additionalMap["publishToObserver"] =
+ new TimedVariant{publishToObserver, lastControlThreadTimestamp};
+ additionalMap["SoftwareEmergencyStopState"] =
+ new TimedVariant{_module<Units>().getEmergencyStopMaster()->getEmergencyStopState() ==
+ EmergencyStopState::eEmergencyStopActive
+ ? "EmergencyStopActive"
+ : "EmergencyStopInactive",
+ lastControlThreadTimestamp};
//update
if (haveSensorAndControlValsChanged)
{
- timingMap["UnitUpdate"] = publishUnitUpdates(timingMap, controlThreadOutputBuffer, activatedControllers);
- timingMap["SensorUpdates"] = publishSensorUpdates(publishToObserver, controlThreadOutputBuffer);
+ timingMap["UnitUpdate"] =
+ publishUnitUpdates(timingMap, controlThreadOutputBuffer, activatedControllers);
+ timingMap["SensorUpdates"] =
+ publishSensorUpdates(publishToObserver, controlThreadOutputBuffer);
}
else
{
- const double sensSecondsAgo = (TimeUtil::GetTime() - publishNewSensorDataTime).toSecondsDouble();
+ const double sensSecondsAgo =
+ (TimeUtil::GetTime() - publishNewSensorDataTime).toSecondsDouble();
if (sensSecondsAgo > 1)
{
- ARMARX_WARNING << deactivateSpam(spamdelay) << "armarx::RobotUnit::publish: Last sensor value update is "
+ ARMARX_WARNING << deactivateSpam(spamdelay)
+ << "armarx::RobotUnit::publish: Last sensor value update is "
<< sensSecondsAgo << " seconds ago!";
}
}
if (haveSensorAndControlValsChanged || haveActivatedControllersChanged)
{
- timingMap["ControlUpdates"] = publishControlUpdates(
- controlThreadOutputBuffer,
- haveSensorAndControlValsChanged, publishToObserver,
- activatedControllers, requestedJointControllers);
+ timingMap["ControlUpdates"] = publishControlUpdates(controlThreadOutputBuffer,
+ haveSensorAndControlValsChanged,
+ publishToObserver,
+ activatedControllers,
+ requestedJointControllers);
}
//call publish hook + publish NJointControllerBase changes
- timingMap["NJointControllerUpdates"] = publishNJointControllerUpdates(timingMap, controlThreadOutputBuffer);
+ timingMap["NJointControllerUpdates"] =
+ publishNJointControllerUpdates(timingMap, controlThreadOutputBuffer);
//report new class names
timingMap["ClassNameUpdates"] = publishNJointClassNames();
- timingMap["RobotUnitListenerFlush"] = new TimedVariant
- {
- ARMARX_STOPWATCH(TimestampVariant)
- {
- robotUnitListenerBatchPrx->ice_flushBatchRequests();
- }, lastControlThreadTimestamp
- };
+ timingMap["RobotUnitListenerFlush"] = new TimedVariant{
+ ARMARX_STOPWATCH(TimestampVariant){robotUnitListenerBatchPrx->ice_flushBatchRequests();
+ }
+ , lastControlThreadTimestamp
+}; // namespace armarx::RobotUnitModule
- if (publishToObserver)
+if (publishToObserver)
+{
+ timingMap["LastPublishLoop"] =
+ new TimedVariant{TimestampVariant{lastPublishLoop}, lastControlThreadTimestamp};
+ if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
+ {
+ if (observerPublishTimingInformation)
{
- timingMap["LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop}, lastControlThreadTimestamp};
- if (robotUnitObserver->getState() >= eManagedIceObjectStarted)
- {
- if (observerPublishTimingInformation)
- {
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->additionalChannel, std::move(additionalMap));
- }
- if (observerPublishAdditionalInformation)
- {
- robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(robotUnitObserver->timingChannel, std::move(timingMap));
- }
- }
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
+ robotUnitObserver->additionalChannel, std::move(additionalMap));
}
-
- //warn if there are unset jointCtrl controllers
+ if (observerPublishAdditionalInformation)
{
- const auto& jointCtrls = _module<ControlThreadDataBuffer>().getActivatedJointControllers();
- if (std::any_of(jointCtrls.begin(), jointCtrls.end(), [](const JointController * jointCtrl)
- {
- return !jointCtrl;
- }))
- {
- ARMARX_WARNING << deactivateSpam(5) << "armarx::RobotUnit::publish: Some activated JointControllers are reported to be nullptr! "
- << "(did you forget to call rtSwitchControllerSetup()?)";
- }
+ robotUnitObserver->offerOrUpdateDataFieldsFlatCopy_async(
+ robotUnitObserver->timingChannel, std::move(timingMap));
}
- ++publishIterationCount;
- lastPublishLoop = TimeUtil::GetTime(true) - begPublish;
}
}
+
+//warn if there are unset jointCtrl controllers
+{
+ const auto& jointCtrls = _module<ControlThreadDataBuffer>().getActivatedJointControllers();
+ if (std::any_of(jointCtrls.begin(),
+ jointCtrls.end(),
+ [](const JointController* jointCtrl) { return !jointCtrl; }))
+ {
+ ARMARX_WARNING << deactivateSpam(5)
+ << "armarx::RobotUnit::publish: Some activated JointControllers are "
+ "reported to be nullptr! "
+ << "(did you forget to call rtSwitchControllerSetup()?)";
+ }
+}
+++publishIterationCount;
+lastPublishLoop = TimeUtil::GetTime(true) - begPublish;
+}
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
index 9b68b4c7aeffc6ef77b3aa5e0c2d9ca20c1c8677..9f061fd02cad06539fb919e32087912f72713f6e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h
@@ -28,8 +28,8 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include "RobotUnitModuleBase.h"
#include "../RobotUnitObserver.h"
+#include "RobotUnitModuleBase.h"
namespace armarx::detail
{
@@ -40,42 +40,47 @@ namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnitObserver);
using SensorAndControl = detail::ControlThreadOutputBufferEntry;
-}
+} // namespace armarx
namespace armarx::RobotUnitModule
{
- class PublisherPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class PublisherPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- PublisherPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ PublisherPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>(
- "RobotUnitListenerTopicName", "RobotUnitListenerTopic",
- "The topic receiving events for RobotUnitListener");
- defineOptionalProperty<std::string>(
- "DebugDrawerUpdatesTopicName", "DebugDrawerUpdates",
- "The topic receiving events for debug drawing");
+ defineOptionalProperty<std::string>("RobotUnitListenerTopicName",
+ "RobotUnitListenerTopic",
+ "The topic receiving events for RobotUnitListener");
+ defineOptionalProperty<std::string>("DebugDrawerUpdatesTopicName",
+ "DebugDrawerUpdates",
+ "The topic receiving events for debug drawing");
defineOptionalProperty<std::size_t>(
- "PublishPeriodMs", 10,
- "Milliseconds between each publish");
+ "PublishPeriodMs", 10, "Milliseconds between each publish");
- defineOptionalProperty<bool>(
- "ObserverPublishSensorValues", true,
- "Whether sensor values are send to the observer", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>(
- "ObserverPublishControlTargets", true,
- "Whether control targets are send to the observer", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>(
- "ObserverPublishTimingInformation", true,
- "Whether timing information are send to the observer", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>(
- "ObserverPublishAdditionalInformation", true,
- "Whether additional information are send to the observer", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("ObserverPublishSensorValues",
+ true,
+ "Whether sensor values are send to the observer",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("ObserverPublishControlTargets",
+ true,
+ "Whether control targets are send to the observer",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("ObserverPublishTimingInformation",
+ true,
+ "Whether timing information are send to the observer",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("ObserverPublishAdditionalInformation",
+ true,
+ "Whether additional information are send to the observer",
+ PropertyDefinitionBase::eModifiable);
defineOptionalProperty<std::string>(
"DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
defineOptionalProperty<std::uint64_t>(
- "ObserverPrintEveryNthIterations", 1,
- "Only every nth iteration data is send to the debug observer", PropertyDefinitionBase::eModifiable);
+ "ObserverPrintEveryNthIterations",
+ 1,
+ "Only every nth iteration data is send to the debug observer",
+ PropertyDefinitionBase::eModifiable);
}
};
@@ -89,15 +94,18 @@ namespace armarx::RobotUnitModule
class Publisher : virtual public ModuleBase, virtual public RobotUnitPublishingInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static Publisher& Instance()
+ static Publisher&
+ Instance()
{
return ModuleBase::Instance<Publisher>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -128,28 +136,33 @@ namespace armarx::RobotUnitModule
* @brief Returns the name of the used DebugObserverTopic
* @return The name of the used DebugObserverTopic
*/
- std::string getDebugObserverTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
+ std::string
+ getDebugObserverTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the name of the used RobotUnitListenerTopic
* @return The name of the used RobotUnitListenerTopic
*/
- std::string getRobotUnitListenerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
+ std::string
+ getRobotUnitListenerTopicName(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the used DebugDrawerProxy
* @return The used DebugDrawerProxy
*/
- DebugDrawerInterfacePrx getDebugDrawerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
+ DebugDrawerInterfacePrx
+ getDebugDrawerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the used RobotUnitListenerProxy
* @return The used RobotUnitListenerProxy
*/
- RobotUnitListenerPrx getRobotUnitListenerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
+ RobotUnitListenerPrx
+ getRobotUnitListenerProxy(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the used DebugObserverProxy
* @return The used DebugObserverProxy
*/
- DebugObserverInterfacePrx getDebugObserverProxy(const Ice::Current& = Ice::emptyCurrent) const override;
+ DebugObserverInterfacePrx
+ getDebugObserverProxy(const Ice::Current& = Ice::emptyCurrent) const override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -159,7 +172,8 @@ namespace armarx::RobotUnitModule
* @param additionalMap Data published to the RobotUnitObserver's additional channel (This field is used by deriving classes)
* @param timingMap Data published to the RobotUnitObserver's timing channel (This field is used by deriving classes)
*/
- virtual void publish(StringVariantBaseMap additionalMap = {}, StringVariantBaseMap timingMap = {});
+ virtual void publish(StringVariantBaseMap additionalMap = {},
+ StringVariantBaseMap timingMap = {});
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -168,11 +182,13 @@ namespace armarx::RobotUnitModule
* @brief Returns the used RobotUnitObserver
* @return The used RobotUnitObserver
*/
- const RobotUnitObserverPtr& getRobotUnitObserver() const
+ const RobotUnitObserverPtr&
+ getRobotUnitObserver() const
{
ARMARX_CHECK_EXPRESSION(robotUnitObserver);
return robotUnitObserver;
}
+
/**
* @brief Publishes updtes about new classes od \ref NJointControllerBase "NJointControllers"
* @return The time required by this function as a \ref Variant
@@ -185,9 +201,9 @@ namespace armarx::RobotUnitModule
* @return The time required by this function as a \ref Variant
* @see NJointControllerBase::onPublish
*/
- TimedVariantPtr publishNJointControllerUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer);
+ TimedVariantPtr
+ publishNJointControllerUpdates(StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer);
/**
* @brief Updates all sub units and publishes the timings of these updates
* @param timingMap Timings of this publish iteration (out param)
@@ -196,10 +212,9 @@ namespace armarx::RobotUnitModule
* active in the published \ref ControlThread iteration
* @return The time required by this function as a \ref Variant
*/
- TimedVariantPtr publishUnitUpdates(
- StringVariantBaseMap& timingMap,
- const SensorAndControl& controlThreadOutputBuffer,
- const JointAndNJointControllers& activatedControllers);
+ TimedVariantPtr publishUnitUpdates(StringVariantBaseMap& timingMap,
+ const SensorAndControl& controlThreadOutputBuffer,
+ const JointAndNJointControllers& activatedControllers);
/**
* @brief Publishes data about updates of \ref JointController "JointControllers" and their \ref ControlTargetBase "ControlTargets"
* @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
@@ -210,21 +225,20 @@ namespace armarx::RobotUnitModule
* @param requestedJointControllers
* @return The time required by this function as a \ref Variant
*/
- TimedVariantPtr publishControlUpdates(
- const SensorAndControl& controlThreadOutputBuffer,
- bool haveSensorAndControlValsChanged,
- bool publishToObserver,
- const JointAndNJointControllers& activatedControllers,
- const std::vector<JointController*>& requestedJointControllers);
+ TimedVariantPtr
+ publishControlUpdates(const SensorAndControl& controlThreadOutputBuffer,
+ bool haveSensorAndControlValsChanged,
+ bool publishToObserver,
+ const JointAndNJointControllers& activatedControllers,
+ const std::vector<JointController*>& requestedJointControllers);
/**
* @brief Publishes Updates about \ref SensorValueBase "SensorValues" (To \ref RobotUnitListener and \ref RobotUnitObserver)
* @param publishToObserver Whether data should be published to observers
* @param controlThreadOutputBuffer The output of the published \ref ControlThread iteration
* @return The time required by this function as a \ref Variant
*/
- TimedVariantPtr publishSensorUpdates(
- bool publishToObserver,
- const SensorAndControl& controlThreadOutputBuffer);
+ TimedVariantPtr publishSensorUpdates(bool publishToObserver,
+ const SensorAndControl& controlThreadOutputBuffer);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -246,7 +260,7 @@ namespace armarx::RobotUnitModule
DebugObserverInterfacePrx debugObserverPrx;
/// @brief the number of calles to \ref publish
- std::uint64_t publishIterationCount {0};
+ std::uint64_t publishIterationCount{0};
/// @brief The time required by the last iteration of \ref publish to publish sensor data
IceUtil::Time publishNewSensorDataTime;
@@ -275,7 +289,7 @@ namespace armarx::RobotUnitModule
RobotUnitObserverPtr robotUnitObserver;
/// @brief The period of the publisher loop
- std::size_t publishPeriodMs {1};
+ std::size_t publishPeriodMs{1};
/// @brief A proxy to the used RobotUnitListener topic
RobotUnitListenerPrx robotUnitListenerPrx;
@@ -286,4 +300,4 @@ namespace armarx::RobotUnitModule
/// \warning May only be accessed by the publish thread.
IceUtil::Time lastControlThreadTimestamp;
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index b60019683741f988d4eefcf131cd44df44b1be9c..cc1e89e592642c8adbd137c071023a472284e4ec 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -20,66 +20,116 @@
* GNU General Public License
*/
-#include <VirtualRobot/XML/RobotIO.h>
+#include "RobotUnitModuleRobotData.h"
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <RobotAPI/libraries/core/RobotPool.h>
-#include "RobotUnitModuleRobotData.h"
-
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
namespace armarx::RobotUnitModule
{
- const std::string& RobotData::getRobotPlatformName() const
+
+ 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
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
return robotPlatformName;
}
- std::string RobotData::getRobotPlatformInstanceName() const
+ std::string
+ RobotData::getRobotPlatformInstanceName() const
{
return robotPlatformInstanceName;
}
- const std::string& RobotData::getRobotNodetSeName() const
+ const std::string&
+ RobotData::getRobotNodetSeName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
return robotNodeSetName;
}
- const std::string& RobotData::getRobotProjectName() const
+ const std::string&
+ RobotData::getRobotProjectName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
return robotProjectName;
}
- const std::string& RobotData::getRobotFileName() const
+ const std::string&
+ RobotData::getRobotFileName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
return robotFileName;
}
- std::string RobotData::getRobotName() const
+ std::string
+ RobotData::getRobotName() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {robotMutex};
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ std::lock_guard<std::mutex> guard{robotMutex};
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
return robot->getName();
}
- VirtualRobot::RobotPtr RobotData::cloneRobot(bool updateCollisionModel) const
+ VirtualRobot::RobotPtr
+ RobotData::cloneRobot(bool updateCollisionModel) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {robotMutex};
- ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
+ std::lock_guard<std::mutex> guard{robotMutex};
+ ARMARX_CHECK_EXPRESSION(robot)
+ << __FUNCTION__ << " should only be called after _initVirtualRobot was called";
ARMARX_CHECK_EXPRESSION(robotPool);
const VirtualRobot::RobotPtr clone = robotPool->getRobot();
clone->setUpdateVisualization(false);
@@ -87,21 +137,22 @@ namespace armarx::RobotUnitModule
return clone;
}
-
-
- void RobotData::_initVirtualRobot()
+ void
+ RobotData::_initVirtualRobot()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {robotMutex};
+ std::lock_guard<std::mutex> guard{robotMutex};
ARMARX_CHECK_IS_NULL(robot);
- std::string robotName = getProperty<std::string>("RobotName").getValue();
+ std::string robotName = getProperty<std::string>("RobotName").getValue();
- robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
- robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
- robotFileName = getProperty<std::string>("RobotFileName").getValue();
- robotPlatformName = getProperty<std::string>("PlatformName").getValue();
- robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+ 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
{
@@ -117,13 +168,15 @@ namespace armarx::RobotUnitModule
{
std::stringstream str;
str << "Could not find robot file " + robotFileName
- << "\nCollected paths from RobotFileNameProject '" << robotProjectName << "':" << includePaths;
+ << "\nCollected paths from RobotFileNameProject '" << robotProjectName
+ << "':" << includePaths;
throw UserException(str.str());
}
// read the robot
try
{
- robot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
+ robot =
+ VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
if (robotName.empty())
{
robotName = robot->getName();
@@ -141,10 +194,11 @@ namespace armarx::RobotUnitModule
<< "\n\tProject : " << robotProjectName
<< "\n\tName : " << robotName
<< "\n\tRobot file : " << robotFileName
- << "\n\tRobotNodeSet : " << robotNodeSetName
- << "\n\tNodeNames : " << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
+ << "\n\tRobotNodeSet : " << robotNodeSetName << "\n\tNodeNames : "
+ << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
ARMARX_CHECK_NOT_NULL(robot);
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 126aa742114c889b2cef6eeabfdf70302b91bd07..c6e94bd515bea5a6f88597c935eb277c234a33d2 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h
@@ -24,8 +24,8 @@
#include <VirtualRobot/Robot.h>
-#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/observers/DebugObserver.h>
#include "RobotUnitModuleBase.h"
@@ -37,31 +37,10 @@ namespace armarx
namespace armarx::RobotUnitModule
{
- class RobotDataPropertyDefinitions: public ModuleBasePropertyDefinitions
+ 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);
};
/**
@@ -73,15 +52,18 @@ namespace armarx::RobotUnitModule
class RobotData : virtual public ModuleBase
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static RobotData& Instance()
+ static RobotData&
+ Instance()
{
return ModuleBase::Instance<RobotData>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -92,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
@@ -143,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;
@@ -152,5 +141,4 @@ namespace armarx::RobotUnitModule
/// @brief A mutex guarding \ref robot
mutable std::mutex robotMutex;
};
-}
-
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index 09b19d38a9ede50f8e7f167aad5aee2681bf30e3..698df2657899773a1a111bd02ce561abd66308ef 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -20,33 +20,41 @@
* GNU General Public License
*/
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Obstacle.h>
+#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 "RobotUnitModuleSelfCollisionChecker.h"
-#include "RobotUnitModuleRobotData.h"
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.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>
-#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include "RobotUnitModuleRobotData.h"
#define FLOOR_OBJ_STR "FLOOR"
namespace armarx::RobotUnitModule
{
- void SelfCollisionChecker::_preOnInitRobotUnit()
+ void
+ SelfCollisionChecker::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//get the robot
selfCollisionAvoidanceRobot = _module<RobotData>().cloneRobot(true);
//get pairs to check
{
- const std::string colModelsString = getProperty<std::string>("SelfCollisionCheck_ModelGroupsToCheck").getValue();
+ const std::string colModelsString =
+ getProperty<std::string>("SelfCollisionCheck_ModelGroupsToCheck").getValue();
std::vector<std::string> groups;
if (!colModelsString.empty())
{
@@ -72,50 +80,60 @@ namespace armarx::RobotUnitModule
{
std::set<std::string> nodes;
//the entry is a set
- for (const auto& node : selfCollisionAvoidanceRobot->getRobotNodeSet(subentry)->getAllRobotNodes())
+ for (const auto& node :
+ selfCollisionAvoidanceRobot->getRobotNodeSet(subentry)
+ ->getAllRobotNodes())
{
if (!node->getCollisionModel())
{
- ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '"
+ ARMARX_WARNING << "Self Collision Avoidance: No collision "
+ "model found for '"
<< node->getName() << "'";
continue;
}
nodes.emplace(node->getName());
- ARMARX_DEBUG << "-------- from set: " << subentry << ", node: " << node->getName();
+ ARMARX_DEBUG << "-------- from set: " << subentry
+ << ", node: " << node->getName();
}
setsOfNode.emplace(std::move(nodes));
}
else if (selfCollisionAvoidanceRobot->hasRobotNode(subentry))
{
//the entry is a node
- if (!selfCollisionAvoidanceRobot->getRobotNode(subentry)->getCollisionModel())
+ if (!selfCollisionAvoidanceRobot->getRobotNode(subentry)
+ ->getCollisionModel())
{
- ARMARX_WARNING << "Self Collision Avoidance: No collision model found for '"
- << selfCollisionAvoidanceRobot->getRobotNode(subentry)->getName() << "'";
+ ARMARX_WARNING
+ << "Self Collision Avoidance: No collision model found for '"
+ << selfCollisionAvoidanceRobot->getRobotNode(subentry)
+ ->getName()
+ << "'";
continue;
}
- setsOfNode.emplace(std::set<std::string> {subentry});
+ setsOfNode.emplace(std::set<std::string>{subentry});
ARMARX_DEBUG << "-------- node: " << subentry;
}
else if (subentry == FLOOR_OBJ_STR)
{
//the entry is the floor
- setsOfNode.emplace(std::set<std::string> {subentry});
+ setsOfNode.emplace(std::set<std::string>{subentry});
ARMARX_DEBUG << "-------- floor: " << subentry;
}
else
{
- ARMARX_ERROR << "No RobotNodeSet or RobotNode with name '"
- << subentry
- << "' defined in " << _module<RobotData>().getRobotFileName() << ". Skipping.";
+ ARMARX_ERROR << "No RobotNodeSet or RobotNode with name '" << subentry
+ << "' defined in "
+ << _module<RobotData>().getRobotFileName()
+ << ". Skipping.";
continue;
}
}
}
- auto addCombinationOfSetsToCollisionCheck = [this](const std::set<std::string>& a, const std::set<std::string>& b)
+ auto addCombinationOfSetsToCollisionCheck =
+ [this](const std::set<std::string>& a, const std::set<std::string>& b)
{
for (const auto& nodeA : a)
{
@@ -155,20 +173,22 @@ namespace armarx::RobotUnitModule
}
ARMARX_DEBUG << "Processing groups for self collision checking...DONE!";
}
- setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheck_Frequency").getValue());
- setSelfCollisionAvoidanceDistance(getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
+ setSelfCollisionAvoidanceFrequency(
+ getProperty<float>("SelfCollisionCheck_Frequency").getValue());
+ setSelfCollisionAvoidanceDistance(
+ getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
}
- void SelfCollisionChecker::setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current&)
+ void
+ SelfCollisionChecker::setSelfCollisionAvoidanceDistance(Ice::Float distance,
+ const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
+ std::lock_guard<std::mutex> guard{selfCollisionDataMutex};
if (distance < 0)
{
- throw InvalidArgumentException
- {
- std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal distance:" + std::to_string(distance)
- };
+ throw InvalidArgumentException{std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION +
+ ": illegal distance:" + std::to_string(distance)};
}
if (distance == minSelfDistance && !nodePairsToCheck.empty())
{
@@ -182,11 +202,16 @@ namespace armarx::RobotUnitModule
nodePairsToCheck.clear();
//set floor
{
- floor.reset(new VirtualRobot::SceneObjectSet("FLOORSET", selfCollisionAvoidanceRobot->getCollisionChecker()));
+ floor.reset(new VirtualRobot::SceneObjectSet(
+ "FLOORSET", selfCollisionAvoidanceRobot->getCollisionChecker()));
static constexpr float floorSize = 1e16f;
- VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(floorSize, floorSize, std::min(0.001f, minSelfDistance / 2),
- VirtualRobot::VisualizationFactory::Color::Red(), "",
- selfCollisionAvoidanceRobot->getCollisionChecker());
+ VirtualRobot::ObstaclePtr boxOb = VirtualRobot::Obstacle::createBox(
+ floorSize,
+ floorSize,
+ std::min(0.001f, minSelfDistance / 2),
+ VirtualRobot::VisualizationFactory::Color::Red(),
+ "",
+ selfCollisionAvoidanceRobot->getCollisionChecker());
boxOb->setGlobalPose(Eigen::Matrix4f::Identity());
boxOb->setName(FLOOR_OBJ_STR);
floor->addSceneObject(boxOb);
@@ -199,106 +224,178 @@ 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)
{
- VirtualRobot::SceneObjectPtr first =
- (pair.first == FLOOR_OBJ_STR) ?
- floor->getSceneObject(0) :
- selfCollisionAvoidanceRobot->getRobotNode(pair.first);
+ VirtualRobot::SceneObjectPtr first =
+ (pair.first == FLOOR_OBJ_STR)
+ ? 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);
+ (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 SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
+ void
+ SelfCollisionChecker::setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (freq < 0)
{
- throw InvalidArgumentException
- {
- std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION + ": illegal frequency:" + std::to_string(freq)
- };
+ throw InvalidArgumentException{std::string{__FILE__} + ": " + BOOST_CURRENT_FUNCTION +
+ ": illegal frequency:" + std::to_string(freq)};
}
checkFrequency = freq;
}
- bool SelfCollisionChecker::isSelfCollisionCheckEnabled(const Ice::Current&) const
+ bool
+ SelfCollisionChecker::isSelfCollisionCheckEnabled(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return checkFrequency != 0;
}
- float SelfCollisionChecker::getSelfCollisionAvoidanceFrequency(const Ice::Current&) const
+ float
+ SelfCollisionChecker::getSelfCollisionAvoidanceFrequency(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return checkFrequency;
}
- float SelfCollisionChecker::getSelfCollisionAvoidanceDistance(const Ice::Current&) const
+ float
+ SelfCollisionChecker::getSelfCollisionAvoidanceDistance(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return minSelfDistance;
}
- void SelfCollisionChecker::_componentPropertiesUpdated(const std::set<std::string>& changed)
+ void
+ SelfCollisionChecker::_componentPropertiesUpdated(const std::set<std::string>& changed)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (changed.count("SelfCollisionCheck_Frequency"))
{
- setSelfCollisionAvoidanceFrequency(getProperty<float>("SelfCollisionCheck_Frequency").getValue());
+ setSelfCollisionAvoidanceFrequency(
+ getProperty<float>("SelfCollisionCheck_Frequency").getValue());
}
if (changed.count("SelfCollisionCheck_MinSelfDistance"))
{
- setSelfCollisionAvoidanceDistance(getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
+ setSelfCollisionAvoidanceDistance(
+ getProperty<float>("SelfCollisionCheck_MinSelfDistance").getValue());
}
}
- void SelfCollisionChecker::_postFinishControlThreadInitialization()
+ void
+ SelfCollisionChecker::_postFinishControlThreadInitialization()
{
- selfCollisionAvoidanceThread = std::thread {[&]{selfCollisionAvoidanceTask();}};
+ selfCollisionAvoidanceThread = std::thread{[&] { selfCollisionAvoidanceTask(); }};
}
- void SelfCollisionChecker::selfCollisionAvoidanceTask()
+ void
+ SelfCollisionChecker::selfCollisionAvoidanceTask()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_INFO << "Self collision checker: entered selfCollisionAvoidanceTask";
- ARMARX_ON_SCOPE_EXIT { ARMARX_INFO << "Self collision checker: leaving selfCollisionAvoidanceTask"; };
+ ARMARX_ON_SCOPE_EXIT
+ {
+ ARMARX_INFO << "Self collision checker: leaving selfCollisionAvoidanceTask";
+ };
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;
+ const bool inEmergencyStop =
+ _module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive;
if (inEmergencyStop || freq == 0)
{
- ARMARX_INFO << deactivateSpam() << "Self collision checker: skipping check " << VAROUT(freq) << " " << VAROUT(inEmergencyStop);
+ 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
{
- std::lock_guard<std::mutex> guard {selfCollisionDataMutex};
+ std::lock_guard<std::mutex> guard{selfCollisionDataMutex};
//update robot
- _module<ControlThreadDataBuffer>().updateVirtualRobot(selfCollisionAvoidanceRobot, selfCollisionAvoidanceRobotNodes);
+ _module<ControlThreadDataBuffer>().updateVirtualRobot(
+ selfCollisionAvoidanceRobot, selfCollisionAvoidanceRobotNodes);
//check for all nodes 0
{
bool allJoints0 = true;
for (const auto& node : selfCollisionAvoidanceRobotNodes)
{
+ ARMARX_CHECK_NOT_NULL(node);
if (0 != node->getJointValue())
{
allJoints0 = false;
@@ -315,12 +412,20 @@ namespace armarx::RobotUnitModule
for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
{
const auto& pair = nodePairsToCheck.at(idx);
- if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(pair.first, pair.second))
+
+ ARMARX_CHECK_NOT_NULL(pair.first);
+ ARMARX_CHECK_NOT_NULL(pair.second);
+
+ if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(
+ pair.first, pair.second))
{
collision = true;
lastCollisionPairIndex = idx;
- ARMARX_WARNING << "Self collision checker: COLLISION: '" << pair.first->getName() << "' and '" << pair.second->getName() << "'";
- _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ ARMARX_WARNING << "Self collision checker: COLLISION: '"
+ << pair.first->getName() << "' and '"
+ << pair.second->getName() << "'";
+ _module<Units>().getEmergencyStopMaster()->setEmergencyStopState(
+ EmergencyStopState::eEmergencyStopActive);
// since at least one of the NJoint-Controllers is going to cause a collision, we just kick them all.
_module<ControlThreadDataBuffer>().setActivateControllersRequest({});
break;
@@ -328,15 +433,26 @@ namespace armarx::RobotUnitModule
}
if (!collision)
{
- ARMARX_VERBOSE << deactivateSpam() << "Self collision checker: no collision found between the " << nodePairsToCheck.size() << " pairs";
+ ARMARX_VERBOSE << deactivateSpam()
+ << "Self collision checker: no collision found between the "
+ << 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.";
+ }
}
}
- void SelfCollisionChecker::_preFinishRunning()
+ void
+ SelfCollisionChecker::_preFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_INFO << "Stopping Self Collision Avoidance.";
@@ -345,5 +461,4 @@ namespace armarx::RobotUnitModule
selfCollisionAvoidanceThread.join();
}
}
-}
-
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
index d1d059ea78d90f41b47f3d2aa3f77e9303e73974..fa7e690cd602045f3ed0883e81fb0e3956822b69 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.h
@@ -29,30 +29,38 @@
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
#include "RobotUnitModuleBase.h"
namespace armarx::RobotUnitModule
{
- class SelfCollisionCheckerPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class SelfCollisionCheckerPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- SelfCollisionCheckerPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ SelfCollisionCheckerPropertyDefinitions(std::string prefix) :
+ ModuleBasePropertyDefinitions(prefix)
{
- defineOptionalProperty<float>(
- "SelfCollisionCheck_Frequency", 10,
- "Frequency [Hz] of self collision checking (default = 10). If set to 0, no cllisions will be checked.",
- PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>(
- "SelfCollisionCheck_MinSelfDistance", 20,
- "Minimum allowed distance (mm) between each collision model before entering emergency stop (default = 20).",
- PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("SelfCollisionCheck_Frequency",
+ 10,
+ "Frequency [Hz] of self collision checking (default = "
+ "10). If set to 0, no cllisions will be checked.",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("SelfCollisionCheck_MinSelfDistance",
+ 20,
+ "Minimum allowed distance (mm) between each collision "
+ "model before entering emergency stop (default = 20).",
+ PropertyDefinitionBase::eModifiable);
defineOptionalProperty<std::string>(
- "SelfCollisionCheck_ModelGroupsToCheck", "",
- "Comma seperated list of groups (two or more) of collision models (RobotNodeSets or RobotNodes) that "
+ "SelfCollisionCheck_ModelGroupsToCheck",
+ "",
+ "Comma seperated list of groups (two or more) of collision models (RobotNodeSets "
+ "or RobotNodes) that "
"should be checked against each other by collision avoidance \n"
"(e.g. {rnsColModel1,rnsColModel2};{rnsColModel3,rnsColModel4}; .... "
- "says that rnsColModel1 will be only checked against rnsColModel2 and rnsColModel3 will only be checked against rnsColModel4). \n"
- "Following predefined descriptors can be used: 'FLOOR' which represents a virtual collision model of the floor.");
+ "says that rnsColModel1 will be only checked against rnsColModel2 and rnsColModel3 "
+ "will only be checked against rnsColModel4). \n"
+ "Following predefined descriptors can be used: 'FLOOR' which represents a virtual "
+ "collision model of the floor.");
}
};
@@ -63,18 +71,23 @@ namespace armarx::RobotUnitModule
*
* @see ModuleBase
*/
- class SelfCollisionChecker : virtual public ModuleBase, virtual public RobotUnitSelfCollisionCheckerInterface
+ class SelfCollisionChecker :
+ virtual public ModuleBase,
+ virtual public RobotUnitSelfCollisionCheckerInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
* @return The singleton instance of this class
*/
- static SelfCollisionChecker& Instance()
+ static SelfCollisionChecker&
+ Instance()
{
return ModuleBase::Instance<SelfCollisionChecker>();
}
+
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -95,12 +108,14 @@ namespace armarx::RobotUnitModule
* @brief Sets the minimal distance (mm) between a configured pair of bodies to 'distance'.
* @param distance The minimal distance (mm) between a pair of bodies.
*/
- void setSelfCollisionAvoidanceDistance(Ice::Float distance, const Ice::Current& = Ice::emptyCurrent) override;
+ void setSelfCollisionAvoidanceDistance(Ice::Float distance,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Sets the frequency of self collision checks.
* @param freq The frequency of self collision checks.
*/
- void setSelfCollisionAvoidanceFrequency(Ice::Float freq, const Ice::Current& = Ice::emptyCurrent) override;
+ void setSelfCollisionAvoidanceFrequency(Ice::Float freq,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns whether the frequency of self collision checks is above 0.
@@ -111,12 +126,14 @@ namespace armarx::RobotUnitModule
* @brief Returns the frequency of self collision checks.
* @return The frequency of self collision checks.
*/
- float getSelfCollisionAvoidanceFrequency(const Ice::Current& = Ice::emptyCurrent) const override;
+ float
+ getSelfCollisionAvoidanceFrequency(const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns the minimal distance (mm) between a pair of bodies.
* @return The minimal distance (mm) between a pair of bodies.
*/
- float getSelfCollisionAvoidanceDistance(const Ice::Current& = Ice::emptyCurrent) const override;
+ float
+ getSelfCollisionAvoidanceDistance(const Ice::Current& = Ice::emptyCurrent) const override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -134,9 +151,9 @@ namespace armarx::RobotUnitModule
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
/// @brief The list of pairs of collision models that should be checked agaisnt each other (names only)
- std::set< std::pair<std::string, std::string>> namePairsToCheck;
+ std::set<std::pair<std::string, std::string>> namePairsToCheck;
/// @brief The frequency of self collision checks.
- std::atomic<float> checkFrequency {0};
+ std::atomic<float> checkFrequency{0};
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////////// col data /////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -144,9 +161,10 @@ namespace armarx::RobotUnitModule
///@ brief The mutex guarding data used for self collision checks.
std::mutex selfCollisionDataMutex;
/// @brief Min allowed distance (mm) between each pair of collision models
- std::atomic<float> minSelfDistance {0};
+ std::atomic<float> minSelfDistance{0};
/// @brief The list of pairs of collision models that should be checked agaisnt each other (model nodes only)
- std::vector<std::pair<VirtualRobot::SceneObjectPtr, VirtualRobot::SceneObjectPtr>> nodePairsToCheck;
+ std::vector<std::pair<VirtualRobot::SceneObjectPtr, VirtualRobot::SceneObjectPtr>>
+ nodePairsToCheck;
/// @brief The Robot used for self collison checks
VirtualRobot::RobotPtr selfCollisionAvoidanceRobot;
/// @brief The Robot Nodes of the Robot used for collision checks
@@ -154,6 +172,6 @@ namespace armarx::RobotUnitModule
/// @brief A Collision object for the floor
VirtualRobot::SceneObjectSetPtr floor;
/// @brief The pair of nodes causing the last detected collision (as index in \ref nodePairsToCheck or -1)
- std::atomic_size_t lastCollisionPairIndex {std::numeric_limits<std::size_t>::max()};
+ std::atomic_size_t lastCollisionPairIndex{std::numeric_limits<std::size_t>::max()};
};
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 80efce08aeaefed56bf599ceb928c7e1615e36a0..bd92c93f31a9007c338b9ae4bcd52d9ebe1c4d50 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -20,15 +20,17 @@
* GNU General Public License
*/
-#include <ArmarXCore/core/IceManager.h>
+#include "RobotUnitModuleUnits.h"
+
#include <ArmarXCore/core/IceGridAdmin.h>
+#include <ArmarXCore/core/IceManager.h>
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
#include "RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h"
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
-#include "RobotUnitModuleUnits.h"
-
+#include "../RobotUnit.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
#include "../Units/ForceTorqueSubUnit.h"
#include "../Units/InertialMeasurementSubUnit.h"
#include "../Units/KinematicSubUnit.h"
@@ -36,10 +38,6 @@
#include "../Units/TCPControllerSubUnit.h"
#include "../Units/TrajectoryControllerSubUnit.h"
-#include "../SensorValues/SensorValue1DoFActuator.h"
-
-#include "../RobotUnit.h"
-
namespace armarx::RobotUnitModule
{
/**
@@ -49,12 +47,15 @@ namespace armarx::RobotUnitModule
class ControlThreadAttorneyForRobotUnitEmergencyStopMaster
{
friend class RobotUnitEmergencyStopMaster;
- static void SetEmergencyStopStateNoReportToTopic(ControlThread* controlThreadModule, EmergencyStopState state)
+
+ static void
+ SetEmergencyStopStateNoReportToTopic(ControlThread* controlThreadModule,
+ EmergencyStopState state)
{
controlThreadModule->setEmergencyStopStateNoReportToTopic(state);
}
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
@@ -63,40 +64,50 @@ namespace armarx::RobotUnitModule
virtual public EmergencyStopMasterInterface
{
public:
- RobotUnitEmergencyStopMaster(ControlThread* controlThreadModule, std::string emergencyStopTopicName) :
- controlThreadModule {controlThreadModule},
- emergencyStopTopicName {emergencyStopTopicName}
+ RobotUnitEmergencyStopMaster(ControlThread* controlThreadModule,
+ std::string emergencyStopTopicName) :
+ controlThreadModule{controlThreadModule}, emergencyStopTopicName{emergencyStopTopicName}
{
ARMARX_CHECK_NOT_NULL(controlThreadModule);
ARMARX_CHECK_EXPRESSION(!emergencyStopTopicName.empty());
}
- void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = Ice::emptyCurrent) final override
+ void
+ setEmergencyStopState(EmergencyStopState state,
+ const Ice::Current& = Ice::emptyCurrent) final override
{
if (getEmergencyStopState() == state)
{
return;
}
- ControlThreadAttorneyForRobotUnitEmergencyStopMaster::SetEmergencyStopStateNoReportToTopic(controlThreadModule, state);
+ ControlThreadAttorneyForRobotUnitEmergencyStopMaster::
+ SetEmergencyStopStateNoReportToTopic(controlThreadModule, state);
emergencyStopTopic->reportEmergencyStopState(state);
}
- EmergencyStopState getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const final override
+
+ EmergencyStopState
+ getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const final override
{
return controlThreadModule->getEmergencyStopState();
}
protected:
- void onInitComponent() final override
+ void
+ onInitComponent() final override
{
armarx::ManagedIceObject::offeringTopic(emergencyStopTopicName);
}
- void onConnectComponent() final override
+
+ void
+ onConnectComponent() final override
{
- emergencyStopTopic = armarx::ManagedIceObject::getTopic<EmergencyStopListenerPrx>(emergencyStopTopicName);
+ emergencyStopTopic = armarx::ManagedIceObject::getTopic<EmergencyStopListenerPrx>(
+ emergencyStopTopicName);
setEmergencyStopState(EmergencyStopState::eEmergencyStopInactive);
}
- std::string getDefaultName() const final override
+ std::string
+ getDefaultName() const final override
{
return "EmergencyStopMaster";
}
@@ -106,11 +117,12 @@ namespace armarx::RobotUnitModule
const std::string emergencyStopTopicName;
EmergencyStopListenerPrx emergencyStopTopic;
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- Ice::ObjectProxySeq Units::getUnits(const Ice::Current&) const
+ Ice::ObjectProxySeq
+ Units::getUnits(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::vector<ManagedIceObjectPtr> unitsCopy;
@@ -128,7 +140,8 @@ namespace armarx::RobotUnitModule
return r;
}
- Ice::ObjectPrx Units::getUnit(const std::string& staticIceId, const Ice::Current&) const
+ Ice::ObjectPrx
+ Units::getUnit(const std::string& staticIceId, const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//no lock required
@@ -140,43 +153,71 @@ namespace armarx::RobotUnitModule
return nullptr;
}
- const EmergencyStopMasterInterfacePtr& Units::getEmergencyStopMaster() const
+ const EmergencyStopMasterInterfacePtr&
+ Units::getEmergencyStopMaster() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return emergencyStopMaster;
}
- void Units::initializeDefaultUnits()
+ 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";
+ ARMARX_INFO << "initializing default units...done! "
+ << (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
}
- void Units::initializeKinematicUnit()
+ void
+ Units::initializeKinematicUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using IfaceT = KinematicUnitInterface;
@@ -196,13 +237,12 @@ namespace armarx::RobotUnitModule
}
}
- ManagedIceObjectPtr Units::createKinematicSubUnit(
- const Ice::PropertiesPtr& properties,
- const std::string& positionControlMode,
- const std::string& velocityControlMode,
- const std::string& torqueControlMode,
- const std::string& pwmControlMode
- )
+ ManagedIceObjectPtr
+ Units::createKinematicSubUnit(const Ice::PropertiesPtr& properties,
+ const std::string& positionControlMode,
+ const std::string& velocityControlMode,
+ const std::string& torqueControlMode,
+ const std::string& pwmControlMode)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -212,7 +252,8 @@ namespace armarx::RobotUnitModule
//add ctrl et al
ARMARX_DEBUG << "createKinematicSubUnit...";
std::map<std::string, UnitT::ActuatorData> devs;
- for (const ControlDevicePtr& controlDevice : _module<Devices>().getControlDevices().values())
+ for (const ControlDevicePtr& controlDevice :
+ _module<Devices>().getControlDevices().values())
{
ARMARX_CHECK_NOT_NULL(controlDevice);
const auto& controlDeviceName = controlDevice->getDeviceName();
@@ -222,19 +263,21 @@ namespace armarx::RobotUnitModule
{
UnitT::ActuatorData ad;
ad.name = controlDeviceName;
- ARMARX_DEBUG << "has sensor device: " << _module<Devices>().getSensorDevices().has(controlDeviceName);
+ ARMARX_DEBUG << "has sensor device: "
+ << _module<Devices>().getSensorDevices().has(controlDeviceName);
ad.sensorDeviceIndex =
- _module<Devices>().getSensorDevices().has(controlDeviceName) ?
- _module<Devices>().getSensorDevices().index(controlDeviceName) :
- std::numeric_limits<std::size_t>::max();
+ _module<Devices>().getSensorDevices().has(controlDeviceName)
+ ? _module<Devices>().getSensorDevices().index(controlDeviceName)
+ : std::numeric_limits<std::size_t>::max();
- const auto init_controller = [&](const std::string & requestedControlMode, auto & ctrl, const auto * tpptr)
+ const auto init_controller =
+ [&](const std::string& requestedControlMode, auto& ctrl, const auto* tpptr)
{
auto controlMode = requestedControlMode;
using CtargT = std::remove_const_t<std::remove_reference_t<decltype(*tpptr)>>;
NJointKinematicUnitPassThroughControllerPtr& pt = ctrl;
- auto testMode = [&](const auto & m)
+ auto testMode = [&](const auto& m)
{
JointController* jointCtrl = controlDevice->getJointController(m);
return jointCtrl && jointCtrl->getControlTarget()->isA<CtargT>();
@@ -254,8 +297,7 @@ namespace armarx::RobotUnitModule
{
selected_ctrl = nullptr;
ARMARX_INFO << "Autodetected two controllers supporting "
- << requestedControlMode
- << "! autoselection failed";
+ << requestedControlMode << "! autoselection failed";
break;
}
selected_ctrl = ctrl;
@@ -264,42 +306,49 @@ namespace armarx::RobotUnitModule
if (selected_ctrl)
{
controlMode = selected_ctrl->getControlMode();
- ARMARX_INFO << "Autodetected controller with mode "
- << controlMode << "(the requested mode was "
- << requestedControlMode << ")";
+ ARMARX_INFO << "Autodetected controller with mode " << controlMode
+ << "(the requested mode was " << requestedControlMode
+ << ")";
}
}
if (!controlMode.empty())
{
- NJointKinematicUnitPassThroughControllerConfigPtr config = new NJointKinematicUnitPassThroughControllerConfig;
+ NJointKinematicUnitPassThroughControllerConfigPtr config =
+ new NJointKinematicUnitPassThroughControllerConfig;
config->controlMode = controlMode;
config->deviceName = controlDeviceName;
- auto ctrl_name = getName() + "_" + "NJointKU_PTCtrl_" + controlDeviceName + "_" + controlMode;
+ auto ctrl_name = getName() + "_" + "NJointKU_PTCtrl_" + controlDeviceName +
+ "_" + controlMode;
std::replace(ctrl_name.begin(), ctrl_name.end(), '.', '_');
std::replace(ctrl_name.begin(), ctrl_name.end(), '-', '_');
std::replace(ctrl_name.begin(), ctrl_name.end(), ':', '_');
- const NJointControllerBasePtr& nJointCtrl = _module<ControllerManagement>().createNJointController(
- "NJointKinematicUnitPassThroughController",
- ctrl_name, config, false, true);
+ const NJointControllerBasePtr& nJointCtrl =
+ _module<ControllerManagement>().createNJointController(
+ "NJointKinematicUnitPassThroughController",
+ ctrl_name,
+ config,
+ false,
+ true);
pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl);
ARMARX_CHECK_EXPRESSION(pt);
}
};
- init_controller(positionControlMode, ad.ctrlPos, (ControlTarget1DoFActuatorPosition*)0);
- init_controller(velocityControlMode, ad.ctrlVel, (ControlTarget1DoFActuatorVelocity*)0);
- init_controller(torqueControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorTorque*)0);
+ init_controller(
+ positionControlMode, ad.ctrlPos, (ControlTarget1DoFActuatorPosition*)0);
+ init_controller(
+ velocityControlMode, ad.ctrlVel, (ControlTarget1DoFActuatorVelocity*)0);
+ init_controller(torqueControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorTorque*)0);
if (!ad.ctrlTor)
{
- init_controller(pwmControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorPWM*)0);
+ init_controller(pwmControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorPWM*)0);
}
if (ad.ctrlPos || ad.ctrlTor || ad.ctrlVel)
{
- ARMARX_DEBUG << "created controllers for "
- << controlDeviceName
- << " pos/tor/vel " << ad.ctrlPos.get() << " / "
- << ad.ctrlTor.get() << " / " << ad.ctrlVel.get();
+ ARMARX_DEBUG << "created controllers for " << controlDeviceName
+ << " pos/tor/vel " << ad.ctrlPos.get() << " / " << ad.ctrlTor.get()
+ << " / " << ad.ctrlVel.get();
devs[controlDeviceName] = std::move(ad);
}
}
@@ -310,38 +359,45 @@ namespace armarx::RobotUnitModule
return nullptr;
}
ARMARX_IMPORTANT << "Found " << devs.size() << " joint devices - adding KinematicUnit";
- ARMARX_CHECK_EXPRESSION(!devs.empty()) << "no 1DoF ControlDevice found with matching SensorDevice";
+ ARMARX_CHECK_EXPRESSION(!devs.empty())
+ << "no 1DoF ControlDevice found with matching SensorDevice";
//add it
const std::string configName = getProperty<std::string>("KinematicUnitName");
const std::string confPre = getConfigDomain() + "." + configName + ".";
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
- properties->setProperty(confPre + "RobotNodeSetName", _module<RobotData>().getRobotNodetSeName());
+ properties->setProperty(confPre + "RobotNodeSetName",
+ _module<RobotData>().getRobotNodetSeName());
properties->setProperty(confPre + "RobotFileName", _module<RobotData>().getRobotFileName());
- properties->setProperty(confPre + "RobotFileNameProject", _module<RobotData>().getRobotProjectName());
- properties->setProperty(confPre + "TopicPrefix", getProperty<std::string>("KinematicUnitNameTopicPrefix"));
+ properties->setProperty(confPre + "RobotFileNameProject",
+ _module<RobotData>().getRobotProjectName());
+ properties->setProperty(confPre + "TopicPrefix",
+ getProperty<std::string>("KinematicUnitNameTopicPrefix"));
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ ARMARX_DEBUG << "creating unit " << configName
+ << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
//fill devices (sensor + controller)
- unit->setupData(
- getProperty<std::string>("RobotFileName").getValue(),
- _module<RobotData>().cloneRobot(),
- std::move(devs),
- _module<Devices>().getControlModeGroups().groups,
- _module<Devices>().getControlModeGroups().groupsMerged,
- dynamic_cast<RobotUnit*>(this)
- );
+ unit->setupData(getProperty<std::string>("RobotFileName").getValue(),
+ _module<RobotData>().cloneRobot(),
+ std::move(devs),
+ _module<Devices>().getControlModeGroups().groups,
+ _module<Devices>().getControlModeGroups().groupsMerged,
+ dynamic_cast<RobotUnit*>(this));
return unit;
}
- void Units::initializePlatformUnit()
+ void
+ Units::initializePlatformUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = PlatformSubUnit;
using IfaceT = PlatformUnitInterface;
+ ARMARX_TRACE;
+
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
//check if unit is already added
@@ -349,89 +405,117 @@ 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;
}
- if (!_module<Devices>().getControlDevices().has(_module<RobotData>().getRobotPlatformName()))
+ ARMARX_TRACE;
+ if (!_module<Devices>().getControlDevices().has(
+ _module<RobotData>().getRobotPlatformName()))
{
- ARMARX_WARNING << "no platform unit created since the platform control device with name '"
- << _module<RobotData>().getRobotPlatformName() << "' was not found";
+ ARMARX_WARNING
+ << "no platform unit created since the platform control device with name '"
+ << _module<RobotData>().getRobotPlatformName() << "' was not found";
return;
}
- 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;
+ 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>());
+ 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();
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
- properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformInstanceName());
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ properties->setProperty(confPre + "PlatformName",
+ _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
- NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config = new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
+ ARMARX_TRACE;
+ NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
+ new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
config->initialVelocityX = 0;
config->initialVelocityY = 0;
config->initialVelocityRotation = 0;
config->platformName = _module<RobotData>().getRobotPlatformName();
auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformUnitVelocityPassThroughController",
- getName() + "_" + configName + "_VelPTContoller",
- config, false, true
- )
- );
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformUnitVelocityPassThroughController",
+ getName() + "_" + configName + "_VelPTContoller",
+ config,
+ false,
+ true));
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(ctrl);
unit->pt = ctrl;
+ ARMARX_TRACE;
- NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg = new NJointHolonomicPlatformRelativePositionControllerConfig;
+ NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
+ new NJointHolonomicPlatformRelativePositionControllerConfig;
configRelativePositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
- auto ctrlRelativePosition = NJointHolonomicPlatformRelativePositionControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformRelativePositionController",
- getName() + "_" + configName + "_RelativePositionContoller",
- configRelativePositionCtrlCfg, false, true
- )
- );
+ auto ctrlRelativePosition =
+ NJointHolonomicPlatformRelativePositionControllerPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformRelativePositionController",
+ getName() + "_" + configName + "_RelativePositionContoller",
+ configRelativePositionCtrlCfg,
+ false,
+ true));
ARMARX_CHECK_EXPRESSION(ctrlRelativePosition);
unit->pt = ctrl;
unit->relativePosCtrl = ctrlRelativePosition;
// unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
- NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg = new NJointHolonomicPlatformGlobalPositionControllerConfig;
+ NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg =
+ new NJointHolonomicPlatformGlobalPositionControllerConfig;
configGlobalPositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
auto ctrlGlobalPosition = NJointHolonomicPlatformGlobalPositionControllerPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformGlobalPositionController",
- getName() + "_" + configName + "_GlobalPositionContoller",
- configGlobalPositionCtrlCfg, false, true
- )
- );
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformGlobalPositionController",
+ getName() + "_" + configName + "_GlobalPositionContoller",
+ 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());
+ unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(
+ _module<RobotData>().getRobotPlatformName());
//add
addUnit(std::move(unit));
}
- void Units::initializeLocalizationUnit()
+ 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
@@ -439,11 +523,18 @@ namespace armarx::RobotUnitModule
{
return;
}
+ ARMARX_TRACE;
ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition";
- const SensorDevicePtr& sensorDeviceRelativePosition = _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
- ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<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>());
@@ -455,26 +546,41 @@ namespace armarx::RobotUnitModule
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
// properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformInstanceName());
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ ARMARX_DEBUG << "creating unit " << configName
+ << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
//config
ARMARX_DEBUG << "Setting device GlobalRobotPoseCorrectionSensorDevice";
- unit->globalPositionCorrectionSensorDevice = dynamic_cast<decltype(unit->globalPositionCorrectionSensorDevice)>(
- _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName()).get());
-
-
- const SensorDevicePtr& sensorDeviceGlobalRobotLocalization = _module<Devices>().getSensorDevices().at(GlobalRobotLocalizationSensorDevice::DeviceName());
- ARMARX_CHECK_EXPRESSION(sensorDeviceGlobalRobotLocalization->getSensorValue()->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>());
-
- dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization).sensorRelativePosition = sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueHolonomicPlatformRelativePosition>();
- dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization).sensorGlobalPositionCorrection = unit->globalPositionCorrectionSensorDevice->getSensorValue()->asA<SensorValueGlobalPoseCorrection>();
+ unit->globalPositionCorrectionSensorDevice =
+ dynamic_cast<decltype(unit->globalPositionCorrectionSensorDevice)>(
+ _module<Devices>()
+ .getSensorDevices()
+ .at(GlobalRobotPoseCorrectionSensorDevice::DeviceName())
+ .get());
+
+
+ const SensorDevicePtr& sensorDeviceGlobalRobotLocalization =
+ _module<Devices>().getSensorDevices().at(
+ GlobalRobotLocalizationSensorDevice::DeviceName());
+ ARMARX_CHECK_EXPRESSION(sensorDeviceGlobalRobotLocalization->getSensorValue()
+ ->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>());
+
+ dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization)
+ .sensorRelativePosition = sensorDeviceRelativePosition->getSensorValue()
+ ->asA<SensorValueHolonomicPlatformRelativePosition>();
+ dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization)
+ .sensorGlobalPositionCorrection =
+ unit->globalPositionCorrectionSensorDevice->getSensorValue()
+ ->asA<SensorValueGlobalPoseCorrection>();
//add
addUnit(unit);
}
- void Units::initializeForceTorqueUnit()
+ void
+ Units::initializeForceTorqueUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = ForceTorqueSubUnit;
@@ -494,24 +600,28 @@ namespace armarx::RobotUnitModule
if (ftSensorDevice->getSensorValue()->isA<SensorValueForceTorque>())
{
ForceTorqueSubUnit::DeviceData ftSensorData;
- ftSensorData.sensorIndex = _module<Devices>().getSensorDevices().index(ftSensorDevice->getDeviceName());
+ ftSensorData.sensorIndex =
+ _module<Devices>().getSensorDevices().index(ftSensorDevice->getDeviceName());
ftSensorData.deviceName = ftSensorDevice->getDeviceName();
ftSensorData.frame = ftSensorDevice->getReportingFrame();
- ARMARX_CHECK_EXPRESSION(
- !ftSensorData.frame.empty()) <<
- "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex
- << ") reports force torque values but returns an empty string as reporting frame";
- ARMARX_CHECK_EXPRESSION(
- unitCreateRobot->hasRobotNode(ftSensorData.frame)) <<
- "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex
- << ") reports force torque values but returns a reporting frame '" << ftSensorData.frame
- << "' which is not present in the robot '" << _module<RobotData>().getRobotName() << "'";
+ ARMARX_CHECK_EXPRESSION(!ftSensorData.frame.empty())
+ << "The sensor device '" << ftSensorData.deviceName
+ << "' (index: " << ftSensorData.sensorIndex
+ << ") reports force torque values but returns an empty string as reporting "
+ "frame";
+ ARMARX_CHECK_EXPRESSION(unitCreateRobot->hasRobotNode(ftSensorData.frame))
+ << "The sensor device '" << ftSensorData.deviceName
+ << "' (index: " << ftSensorData.sensorIndex
+ << ") reports force torque values but returns a reporting frame '"
+ << ftSensorData.frame << "' which is not present in the robot '"
+ << _module<RobotData>().getRobotName() << "'";
ftdevs.emplace_back(std::move(ftSensorData));
}
}
if (ftdevs.empty())
{
- ARMARX_IMPORTANT << "no force torque unit created since there are no force torque sensor devices";
+ ARMARX_IMPORTANT
+ << "no force torque unit created since there are no force torque sensor devices";
return;
}
//add it
@@ -521,15 +631,19 @@ namespace armarx::RobotUnitModule
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
properties->setProperty(confPre + "AgentName", _module<RobotData>().getRobotName());
- properties->setProperty(confPre + "ForceTorqueTopicName", getProperty<std::string>("ForceTorqueTopicName").getValue());
- ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ properties->setProperty(confPre + "ForceTorqueTopicName",
+ getProperty<std::string>("ForceTorqueTopicName").getValue());
+ ARMARX_DEBUG << "creating unit " << configName
+ << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
unit->devs = ftdevs;
//add
addUnit(std::move(unit));
}
- void Units::initializeInertialMeasurementUnit()
+ void
+ Units::initializeInertialMeasurementUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
using UnitT = InertialMeasurementSubUnit;
@@ -554,7 +668,8 @@ namespace armarx::RobotUnitModule
}
if (imudevs.empty())
{
- ARMARX_IMPORTANT << "no inertial measurement unit created since there are no imu sensor devices";
+ ARMARX_IMPORTANT
+ << "no inertial measurement unit created since there are no imu sensor devices";
return;
}
//add it
@@ -563,14 +678,17 @@ namespace armarx::RobotUnitModule
Ice::PropertiesPtr properties = getIceProperties()->clone();
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
- properties->setProperty(confPre + "IMUTopicName", getProperty<std::string>("IMUTopicName").getValue());
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ properties->setProperty(confPre + "IMUTopicName",
+ getProperty<std::string>("IMUTopicName").getValue());
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
unit->devs = imudevs;
//add
addUnit(std::move(unit));
}
- void Units::initializeTrajectoryControllerUnit()
+ void
+ Units::initializeTrajectoryControllerUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -586,12 +704,14 @@ namespace armarx::RobotUnitModule
{
return;
}
- (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))->setup(_modulePtr<RobotUnit>());
+ (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))
+ ->setup(_modulePtr<RobotUnit>());
addUnit(trajectoryControllerSubUnit);
trajectoryControllerSubUnit = nullptr;
}
- void Units::initializeTcpControllerUnit()
+ void
+ Units::initializeTcpControllerUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -608,12 +728,14 @@ namespace armarx::RobotUnitModule
{
return;
}
- (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))->setup(_modulePtr<RobotUnit>(), _module<RobotData>().cloneRobot());
+ (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))
+ ->setup(_modulePtr<RobotUnit>(), _module<RobotData>().cloneRobot());
addUnit(tcpControllerSubUnit);
tcpControllerSubUnit = nullptr;
}
- void Units::addUnit(ManagedIceObjectPtr unit)
+ void
+ Units::addUnit(ManagedIceObjectPtr unit)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_CHECK_NOT_NULL(unit);
@@ -629,7 +751,8 @@ namespace armarx::RobotUnitModule
units.emplace_back(std::move(unit));
}
- void Units::_icePropertiesInitialized()
+ void
+ Units::_icePropertiesInitialized()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
Ice::PropertiesPtr properties = getIceProperties()->clone();
@@ -637,23 +760,31 @@ namespace armarx::RobotUnitModule
// create TCPControlSubUnit
{
- const std::string configNameTCPControlUnit = getProperty<std::string>("TCPControlUnitName");
- tcpControllerSubUnit = Component::create<TCPControllerSubUnit, TCPControllerSubUnitPtr>(properties, configNameTCPControlUnit, configDomain);
+ const std::string configNameTCPControlUnit =
+ getProperty<std::string>("TCPControlUnitName");
+ tcpControllerSubUnit = Component::create<TCPControllerSubUnit, TCPControllerSubUnitPtr>(
+ properties, configNameTCPControlUnit, configDomain);
addPropertyUser(PropertyUserPtr::dynamicCast(tcpControllerSubUnit));
}
// create TrajectoryControllerSubUnit
{
- const std::string configNameTrajectoryControllerUnit = getProperty<std::string>("TrajectoryControllerUnitName");
- const std::string confPre = configDomain + "." + configNameTrajectoryControllerUnit + ".";
- properties->setProperty(confPre + "KinematicUnitName", getProperty<std::string>("KinematicUnitName").getValue());
- trajectoryControllerSubUnit = Component::create<TrajectoryControllerSubUnit, TrajectoryControllerSubUnitPtr>(properties, configNameTrajectoryControllerUnit, configDomain);
+ const std::string configNameTrajectoryControllerUnit =
+ getProperty<std::string>("TrajectoryControllerUnitName");
+ const std::string confPre =
+ configDomain + "." + configNameTrajectoryControllerUnit + ".";
+ properties->setProperty(confPre + "KinematicUnitName",
+ getProperty<std::string>("KinematicUnitName").getValue());
+ trajectoryControllerSubUnit =
+ Component::create<TrajectoryControllerSubUnit, TrajectoryControllerSubUnitPtr>(
+ properties, configNameTrajectoryControllerUnit, configDomain);
addPropertyUser(PropertyUserPtr::dynamicCast(trajectoryControllerSubUnit));
}
}
- void Units::_preFinishRunning()
+ void
+ Units::_preFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//remove all units
@@ -665,47 +796,61 @@ namespace armarx::RobotUnitModule
units.clear();
}
- void Units::_preOnInitRobotUnit()
+ void
+ Units::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
unitCreateRobot = _module<RobotData>().cloneRobot();
ARMARX_DEBUG << "add emergency stop master";
{
- emergencyStopMaster = new RobotUnitEmergencyStopMaster {&_module<ControlThread>(), getProperty<std::string>("EmergencyStopTopic").getValue()};
+ emergencyStopMaster = new RobotUnitEmergencyStopMaster{
+ &_module<ControlThread>(),
+ getProperty<std::string>("EmergencyStopTopic").getValue()};
ManagedIceObjectPtr obj = ManagedIceObjectPtr::dynamicCast(emergencyStopMaster);
ARMARX_CHECK_EXPRESSION(obj) << "RobotUnitEmergencyStopMaster";
- getArmarXManager()->addObject(obj, getProperty<std::string>("EmergencyStopMasterName").getValue(), true);
+ getArmarXManager()->addObject(
+ obj, getProperty<std::string>("EmergencyStopMasterName").getValue(), true);
auto prx = obj->getProxy(-1);
try
{
- getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->addObject(prx);
+ getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->addObject(
+ prx);
}
catch (const IceGrid::ObjectExistsException&)
{
- getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->updateObject(prx);
+ getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->updateObject(
+ prx);
}
catch (std::exception& e)
{
- ARMARX_WARNING << "add the robot unit as emergency stop master to the ice grid admin!\nThere was an exception:\n" << e.what();
+ ARMARX_WARNING << "add the robot unit as emergency stop master to the ice grid "
+ "admin!\nThere was an exception:\n"
+ << e.what();
}
}
ARMARX_DEBUG << "add emergency stop master...done!";
}
- void Units::_postOnExitRobotUnit()
+ void
+ Units::_postOnExitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_DEBUG << "remove EmergencyStopMaster";
try
{
- getArmarXManager()->removeObjectBlocking(ManagedIceObjectPtr::dynamicCast(emergencyStopMaster));
- getArmarXManager()->getIceManager()->removeObject(getProperty<std::string>("EmergencyStopMasterName").getValue());
+ getArmarXManager()->removeObjectBlocking(
+ ManagedIceObjectPtr::dynamicCast(emergencyStopMaster));
+ getArmarXManager()->getIceManager()->removeObject(
+ getProperty<std::string>("EmergencyStopMasterName").getValue());
+ }
+ catch (...)
+ {
}
- catch (...) {}
ARMARX_DEBUG << "remove EmergencyStopMaster...done!";
}
- const ManagedIceObjectPtr& Units::getUnit(const std::string& staticIceId) const
+ const ManagedIceObjectPtr&
+ Units::getUnit(const std::string& staticIceId) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto guard = getGuard();
@@ -718,4 +863,4 @@ namespace armarx::RobotUnitModule
}
return ManagedIceObject::NullPtr;
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index ecb6e49b5e3ebd44fb358e976b7ffdfe68d56a82..6a423722690a411ab270477bcacd387118ba8d61 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -24,61 +24,64 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include "RobotUnitModuleBase.h"
#include "../Units/RobotUnitSubUnit.h"
+#include "RobotUnitModuleBase.h"
namespace armarx::RobotUnitModule
{
- class UnitsPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class UnitsPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- UnitsPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ UnitsPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>(
- "KinematicUnitName", "KinematicUnit",
- "The name of the created kinematic unit");
+ "KinematicUnitName", "KinematicUnit", "The name of the created kinematic unit");
defineOptionalProperty<std::string>(
- "KinematicUnitNameTopicPrefix", "",
- "Prefix for the kinematic sensor values topic");
+ "KinematicUnitNameTopicPrefix", "", "Prefix for the kinematic sensor values topic");
defineOptionalProperty<std::string>(
- "PlatformUnitName", "PlatformUnit",
- "The name of the created platform unit");
+ "PlatformUnitName", "PlatformUnit", "The name of the created platform unit");
defineOptionalProperty<std::string>(
- "ForceTorqueUnitName", "ForceTorqueUnit",
+ "ForceTorqueUnitName",
+ "ForceTorqueUnit",
"The name of the created force troque unit (empty = default)");
defineOptionalProperty<std::string>(
- "ForceTorqueTopicName", "ForceTorqueValues",
+ "ForceTorqueTopicName",
+ "ForceTorqueValues",
"Name of the topic on which the force torque sensor values are provided");
defineOptionalProperty<std::string>(
- "InertialMeasurementUnitName", "InertialMeasurementUnit",
+ "InertialMeasurementUnitName",
+ "InertialMeasurementUnit",
"The name of the created inertial measurement unit (empty = default)");
defineOptionalProperty<std::string>(
- "IMUTopicName", "IMUValues",
- "Name of the IMU Topic.");
+ "IMUTopicName", "IMUValues", "Name of the IMU Topic.");
- defineOptionalProperty<bool>(
- "CreateTCPControlUnit", false,
- "If true the TCPControl SubUnit is created and added");
+ defineOptionalProperty<bool>("CreateTCPControlUnit",
+ false,
+ "If true the TCPControl SubUnit is created and added");
defineOptionalProperty<std::string>(
- "TCPControlUnitName", "TCPControlUnit",
- "Name of the TCPControlUnit.");
+ "TCPControlUnitName", "TCPControlUnit", "Name of the TCPControlUnit.");
defineOptionalProperty<bool>(
- "CreateTrajectoryPlayer", true,
+ "CreateTrajectoryPlayer",
+ true,
"If true the TrajectoryPlayer SubUnit is created and added");
defineOptionalProperty<std::string>(
- "TrajectoryControllerUnitName", "TrajectoryPlayer",
- "Name of the TrajectoryControllerUnit. The respective component outside of the RobotUnit is TrajectoryPlayer");
+ "TrajectoryControllerUnitName",
+ "TrajectoryPlayer",
+ "Name of the TrajectoryControllerUnit. The respective component outside of the "
+ "RobotUnit is TrajectoryPlayer");
defineOptionalProperty<std::string>(
- "EmergencyStopMasterName", "EmergencyStopMaster",
+ "EmergencyStopMasterName",
+ "EmergencyStopMaster",
"The name used to register as an EmergencyStopMaster");
defineOptionalProperty<std::string>(
- "EmergencyStopTopic", "EmergencyStop",
+ "EmergencyStopTopic",
+ "EmergencyStop",
"The name of the topic over which changes of the emergencyStopState are sent.");
}
};
@@ -100,6 +103,7 @@ namespace armarx::RobotUnitModule
class Units : virtual public ModuleBase, virtual public RobotUnitUnitInterface
{
friend class ModuleBase;
+
public:
/**
* @brief Returns the singleton instance of this class
@@ -148,7 +152,8 @@ namespace armarx::RobotUnitModule
* @brief Returns a proxy to the InertialMeasurementUnit
* @return A proxy to the InertialMeasurementUnit
*/
- InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current&) const override;
+ InertialMeasurementUnitInterfacePrx
+ getInertialMeasurementUnit(const Ice::Current&) const override;
/**
* @brief Returns a proxy to the PlatformUnit
* @return A proxy to the PlatformUnit
@@ -180,12 +185,14 @@ namespace armarx::RobotUnitModule
* @brief Returns a pointer to the Unit for the given type (or null if there is none)
* @return A pointer to the Unit for the given type (or null if there is none)
*/
- template<class T> typename T::PointerType getUnit() const;
+ template <class T>
+ typename T::PointerType getUnit() const;
/**
* @brief Returns a proxy to the Unit for the given type (or null if there is none)
* @return A proxy to the Unit for the given type (or null if there is none)
*/
- template<class T> typename T::ProxyType getUnitPrx() const;
+ template <class T>
+ typename T::ProxyType getUnitPrx() const;
//specific getters
/**
@@ -254,11 +261,12 @@ namespace armarx::RobotUnitModule
/// @brief Initializes the TcpControllerUnit
virtual void initializeLocalizationUnit();
/// @brief Create the \ref KinematicUnit (this function should be called in \ref initializeKinematicUnit)
- ManagedIceObjectPtr createKinematicSubUnit(const Ice::PropertiesPtr& properties,
- const std::string& positionControlMode = ControlModes::Position1DoF,
- const std::string& velocityControlMode = ControlModes::Velocity1DoF,
- const std::string& torqueControlMode = ControlModes::Torque1DoF,
- const std::string& pwmControlMode = ControlModes::PWM1DoF);
+ ManagedIceObjectPtr
+ createKinematicSubUnit(const Ice::PropertiesPtr& properties,
+ const std::string& positionControlMode = ControlModes::Position1DoF,
+ const std::string& velocityControlMode = ControlModes::Velocity1DoF,
+ const std::string& torqueControlMode = ControlModes::Torque1DoF,
+ const std::string& pwmControlMode = ControlModes::PWM1DoF);
/**
* @brief Registers a unit for management (the Unit is added and removed to the \ref ArmarXManager
* and updated with new values.
@@ -295,6 +303,6 @@ namespace armarx::RobotUnitModule
*/
friend class UnitsAttorneyForPublisher;
};
-}
+} // namespace armarx::RobotUnitModule
#include "RobotUnitModuleUnits.ipp"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h
index 75c57423496559a2a7cca4437f67e0ab3c0542bd..4df7d8cc88aca99131d3587df0443054dd7a2084 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModules.h
@@ -23,14 +23,13 @@
#pragma once
#include "RobotUnitModuleBase.h"
-
-#include "RobotUnitModuleDevices.h"
-#include "RobotUnitModuleControlThreadDataBuffer.h"
#include "RobotUnitModuleControlThread.h"
-#include "RobotUnitModuleUnits.h"
+#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include "RobotUnitModuleControllerManagement.h"
+#include "RobotUnitModuleDevices.h"
#include "RobotUnitModuleLogging.h"
+#include "RobotUnitModuleManagement.h"
#include "RobotUnitModulePublisher.h"
#include "RobotUnitModuleRobotData.h"
-#include "RobotUnitModuleControllerManagement.h"
-#include "RobotUnitModuleManagement.h"
#include "RobotUnitModuleSelfCollisionChecker.h"
+#include "RobotUnitModuleUnits.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp
index 004d3361faf19b3acfdfd99e0278d0246c004449..2af079acbfd5e4bfdd9459de758055b44bcdf641 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.cpp
@@ -4,19 +4,22 @@
namespace armarx
{
- void RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy_async(const std::string& channelName, StringVariantBaseMap&& valueMap)
+ void
+ RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy_async(const std::string& channelName,
+ StringVariantBaseMap&& valueMap)
{
- addWorkerJob("RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy_async", [this, channelName, vmap = std::move(valueMap)]
- {
- offerOrUpdateDataFieldsFlatCopy(channelName, vmap);
- });
+ addWorkerJob("RobotUnitObserver::offerOrUpdateDataFieldsFlatCopy_async",
+ [this, channelName, vmap = std::move(valueMap)]
+ { offerOrUpdateDataFieldsFlatCopy(channelName, vmap); });
}
- void RobotUnitObserver::onConnectObserver()
+ void
+ RobotUnitObserver::onConnectObserver()
{
offerChannel(sensorDevicesChannel, "Devices that provide sensor data");
offerChannel(controlDevicesChannel, "Devices that are controlled by JointControllers");
- offerChannel(timingChannel, "Channel for timings of the different phases of the robot unit");
+ offerChannel(timingChannel,
+ "Channel for timings of the different phases of the robot unit");
offerChannel(additionalChannel, "Additional custom datafields");
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
index 2fdfb990e824b343cb88337c8316363ceb964a3e..36a127da9f45974c4ad7083f7bd2ca3ce00c87ca 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitObserver.h
@@ -23,19 +23,25 @@ namespace armarx
RobotUnitObserver() = default;
- void offerOrUpdateDataFieldsFlatCopy_async(const std::string& channelName, StringVariantBaseMap&& valueMap);
+ void offerOrUpdateDataFieldsFlatCopy_async(const std::string& channelName,
+ StringVariantBaseMap&& valueMap);
// Observer interface
protected:
- void onInitObserver() override {}
+ void
+ onInitObserver() override
+ {
+ }
+
void onConnectObserver() override;
friend class RobotUnitModule::Publisher;
// ManagedIceObject interface
protected:
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotUnitObserver";
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
index cdb365742dad36bc50872c5ba86b3a3e58be9d87..354424245a27f6b086dbff417a25fd3c2e510124 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
@@ -21,25 +21,25 @@
*/
#pragma once
-#include "SensorValueBase.h"
-
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include "SensorValueBase.h"
+
namespace armarx
{
-#define make_SensorValue1DoFActuator(type, name, varname) \
- class name : virtual public SensorValueBase \
- { \
- public: \
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION \
- type varname = 0.0f; \
- static SensorValueInfo<name> GetClassMemberInfo() \
- { \
- SensorValueInfo<name> svi; \
- svi.addMemberVariable(&name::varname, #varname); \
- return svi; \
- } \
+#define make_SensorValue1DoFActuator(type, name, varname) \
+ class name : virtual public SensorValueBase \
+ { \
+ public: \
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION type varname = 0.0f; \
+ static SensorValueInfo<name> \
+ GetClassMemberInfo() \
+ { \
+ SensorValueInfo<name> svi; \
+ svi.addMemberVariable(&name::varname, #varname); \
+ return svi; \
+ } \
}
make_SensorValue1DoFActuator(float, SensorValue1DoFActuatorPosition, position);
@@ -54,24 +54,25 @@ namespace armarx
#undef make_SensorValue1DoFActuator
- class SensorValue1DoFInverseDynamicsTorque :
- virtual public SensorValue1DoFGravityTorque
+ class SensorValue1DoFInverseDynamicsTorque : virtual public SensorValue1DoFGravityTorque
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float inverseDynamicsTorque = 0.0f;
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float inverseDynamicsTorque = 0.0f;
float inertiaTorque = 0.0f;
- static SensorValueInfo<SensorValue1DoFInverseDynamicsTorque> GetClassMemberInfo()
+
+ static SensorValueInfo<SensorValue1DoFInverseDynamicsTorque>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValue1DoFInverseDynamicsTorque> svi;
svi.addBaseClass<SensorValue1DoFGravityTorque>();
- svi.addMemberVariable(&SensorValue1DoFInverseDynamicsTorque::inverseDynamicsTorque, "inverseDynamicsTorque");
- svi.addMemberVariable(&SensorValue1DoFInverseDynamicsTorque::inertiaTorque, "inertiaTorque");
+ svi.addMemberVariable(&SensorValue1DoFInverseDynamicsTorque::inverseDynamicsTorque,
+ "inverseDynamicsTorque");
+ svi.addMemberVariable(&SensorValue1DoFInverseDynamicsTorque::inertiaTorque,
+ "inertiaTorque");
return svi;
}
};
-
class SensorValue1DoFActuator :
virtual public SensorValue1DoFActuatorPosition,
virtual public SensorValue1DoFActuatorVelocity,
@@ -80,8 +81,9 @@ namespace armarx
virtual public SensorValue1DoFInverseDynamicsTorque
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- static SensorValueInfo<SensorValue1DoFActuator> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION static SensorValueInfo<
+ SensorValue1DoFActuator>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValue1DoFActuator> svi;
svi.addBaseClass<SensorValue1DoFActuatorPosition>();
@@ -96,13 +98,17 @@ namespace armarx
class SensorValue1DoFActuatorStatus : virtual public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- armarx::JointStatus status;
- static SensorValueInfo<SensorValue1DoFActuatorStatus> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION armarx::JointStatus status;
+
+ static SensorValueInfo<SensorValue1DoFActuatorStatus>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValue1DoFActuatorStatus> svi;
svi.addMemberVariable(&SensorValue1DoFActuatorStatus::status, "JointStatus")
- .setFieldNames({"JointStatusError", "JointStatusOperation", "JointStatusEnabled", "JointStatusEmergencyStop"});
+ .setFieldNames({"JointStatusError",
+ "JointStatusOperation",
+ "JointStatusEnabled",
+ "JointStatusEmergencyStop"});
return svi;
}
};
@@ -113,8 +119,9 @@ namespace armarx
virtual public SensorValue1DoFActuatorMotorTemperature
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- static SensorValueInfo<SensorValue1DoFRealActuator> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION static SensorValueInfo<
+ SensorValue1DoFRealActuator>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValue1DoFRealActuator> svi;
svi.addBaseClass<SensorValue1DoFActuator>();
@@ -129,8 +136,9 @@ namespace armarx
virtual public SensorValue1DoFActuatorStatus
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- static SensorValueInfo<SensorValue1DoFRealActuatorWithStatus> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION static SensorValueInfo<
+ SensorValue1DoFRealActuatorWithStatus>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValue1DoFRealActuatorWithStatus> svi;
svi.addBaseClass<SensorValue1DoFRealActuator>();
@@ -138,4 +146,4 @@ namespace armarx
return svi;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
index 021ab483d56dc0536e28004102fafaacd629677f..8a9dc1e258078bb856cd4c6596bc67c5ff046b65 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
@@ -21,15 +21,15 @@
*/
#pragma once
-#include <typeinfo>
+#include <map>
#include <memory>
#include <string>
-#include <map>
+#include <typeinfo>
#include <ArmarXCore/util/CPPUtility/TemplateMetaProgramming.h>
-#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
#include <RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h>
+#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
namespace armarx
{
@@ -39,151 +39,165 @@ namespace armarx
*/
class SensorValueBase
{
- template<class...Ts>
+ template <class... Ts>
struct IsAHelper
{
// only called then sizeof...(Ts) == 0
static_assert(sizeof...(Ts) == 0, "called overload for empty pack with params");
- static bool IsA(const SensorValueBase* sv)
+
+ static bool
+ IsA(const SensorValueBase* sv)
{
return true;
}
};
- template<class T, class...Ts>
+
+ template <class T, class... Ts>
struct IsAHelper<T, Ts...>
{
- static bool IsA(const SensorValueBase* sv)
+ static bool
+ IsA(const SensorValueBase* sv)
{
return dynamic_cast<const T*>(sv) && IsAHelper<Ts...>::IsA(sv);
}
};
public:
- template<class DerivedClass>
+ template <class DerivedClass>
using SensorValueInfo = introspection::ClassMemberInfo<SensorValueBase, DerivedClass>;
virtual ~SensorValueBase() = default;
virtual std::string getSensorValueType(bool withoutNamespaceSpecifier) const = 0;
- template<class...Ts>
- bool isA() const
+ template <class... Ts>
+ bool
+ isA() const
{
return IsAHelper<Ts...>::IsA(this);
}
- template<class T>
- const T* asA() const
+ template <class T>
+ const T*
+ asA() const
{
return dynamic_cast<const T*>(this);
}
- template<class T>
- T* asA()
+ template <class T>
+ T*
+ asA()
{
return dynamic_cast<T*>(this);
}
//logging functions
/// @brief used to send the data to the DebugObserverTopic and to other Components (e.g. GUI widgets)
- virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const = 0;
+ virtual std::map<std::string, VariantBasePtr>
+ toVariants(const IceUtil::Time& timestamp) const = 0;
virtual std::size_t getNumberOfDataFields() const = 0;
virtual std::vector<std::string> getDataFieldNames() const = 0;
- virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
- virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
virtual void getDataFieldAs(std::size_t i, Ice::Double& out) const = 0;
virtual void getDataFieldAs(std::size_t i, std::string& out) const = 0;
- template<class T>
- T getDataFieldAs(std::size_t i) const
+
+ template <class T>
+ T
+ getDataFieldAs(std::size_t i) const
{
ARMARX_TRACE;
T t;
this->getDataFieldAs(i, t);
return t;
}
+
virtual const std::type_info& getDataFieldType(std::size_t i) const = 0;
//management functions
- template<class T, class = typename std::enable_if<std::is_base_of<SensorValueBase, T>::value>::type>
- void _copyTo(std::unique_ptr<T>& target) const
+ template <class T,
+ class = typename std::enable_if<std::is_base_of<SensorValueBase, T>::value>::type>
+ void
+ _copyTo(std::unique_ptr<T>& target) const
{
_copyTo(target.get());
}
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
- SensorValueHasGetClassMemberInfo,
- GetClassMemberInfo, SensorValueInfo<T>(*)(void));
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(SensorValueHasGetClassMemberInfo,
+ GetClassMemberInfo,
+ SensorValueInfo<T> (*)(void));
ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(SensorValueBase)
};
-}
-
-#define DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION \
- ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
- using SensorValueBase = ::armarx::SensorValueBase; \
- using VariantBasePtr = ::armarx::VariantBasePtr; \
- std::string getSensorValueType(bool withoutNamespaceSpecifier = false) const override \
- { \
- return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
- } \
- void _check_for_static_GetClassMemberInfo_overload() \
- { \
- static_assert(SensorValueHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
- "This class has to implement GetClassMemberInfo() returning " \
- "an instance of SensorValueInfo<THIS_CLASS_TYPE>"); \
- } \
- std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp,this); \
- } \
- std::size_t getNumberOfDataFields() const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
- } \
- void getDataFieldAs (std::size_t i, bool& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Byte& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Short& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Int& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Long& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Float& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, Ice::Double& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- void getDataFieldAs (std::size_t i, std::string& out) const override \
- { \
- SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
- } \
- const std::type_info& getDataFieldType(std::size_t i) const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
- } \
- std::vector<std::string> getDataFieldNames() const override \
- { \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
+} // namespace armarx
+
+#define DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION \
+ ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
+ using SensorValueBase = ::armarx::SensorValueBase; \
+ using VariantBasePtr = ::armarx::VariantBasePtr; \
+ std::string getSensorValueType(bool withoutNamespaceSpecifier = false) const override \
+ { \
+ return armarx::GetTypeString(*this, withoutNamespaceSpecifier); \
+ } \
+ void _check_for_static_GetClassMemberInfo_overload() \
+ { \
+ static_assert(SensorValueHasGetClassMemberInfo<std::decay<decltype(*this)>::type>::value, \
+ "This class has to implement GetClassMemberInfo() returning " \
+ "an instance of SensorValueInfo<THIS_CLASS_TYPE>"); \
+ } \
+ std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) \
+ const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::ToVariants(timestamp, this); \
+ } \
+ std::size_t getNumberOfDataFields() const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
+ } \
+ void getDataFieldAs(std::size_t i, bool& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Byte& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Short& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Int& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Long& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Float& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, Ice::Double& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ void getDataFieldAs(std::size_t i, std::string& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs(this, i, out); \
+ } \
+ const std::type_info& getDataFieldType(std::size_t i) const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
+ } \
+ std::vector<std::string> getDataFieldNames() const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldNames(); \
}
namespace armarx
@@ -191,10 +205,11 @@ namespace armarx
class SensorValueDummy : public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- static SensorValueInfo<SensorValueDummy> GetClassMemberInfo()
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION static SensorValueInfo<
+ SensorValueDummy>
+ GetClassMemberInfo()
{
- return SensorValueInfo<SensorValueDummy> {};
+ return SensorValueInfo<SensorValueDummy>{};
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
index 68f41cb91bc10b7744706c452d71de9eb1093014..776bb731d7ab5115953fb6cb044c0f385ce6e7a2 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
@@ -21,31 +21,36 @@
*/
#pragma once
+#include <RobotAPI/libraries/core/Pose.h>
+
#include "Eigen/Core"
#include "SensorValueBase.h"
-#include <RobotAPI/libraries/core/Pose.h>
-
namespace armarx
{
class SensorValueForceTorque : public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- Eigen::Vector3f torque;
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f torque;
Eigen::Vector3f force;
Eigen::Vector3f gravityCompensatedTorque;
Eigen::Vector3f gravityCompensatedForce;
- static SensorValueInfo<SensorValueForceTorque> GetClassMemberInfo()
+
+ static SensorValueInfo<SensorValueForceTorque>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueForceTorque> svi;
- svi.addMemberVariable(&SensorValueForceTorque::torque, "torque").setFieldNames({"tx", "ty", "tz"});
- svi.addMemberVariable(&SensorValueForceTorque::force, "force").setFieldNames({"fx", "fy", "fz"});
- svi.addMemberVariable(&SensorValueForceTorque::gravityCompensatedTorque, "gravityCompensatedTorque")
- .setFieldNames({"gravCompTx", "gravCompTy", "gravCompTz"});
- svi.addMemberVariable(&SensorValueForceTorque::gravityCompensatedForce, "gravityCompensatedForce")
- .setFieldNames({"gravCompFx", "gravCompFy", "gravCompFz"});
+ svi.addMemberVariable(&SensorValueForceTorque::torque, "torque")
+ .setFieldNames({"tx", "ty", "tz"});
+ svi.addMemberVariable(&SensorValueForceTorque::force, "force")
+ .setFieldNames({"fx", "fy", "fz"});
+ svi.addMemberVariable(&SensorValueForceTorque::gravityCompensatedTorque,
+ "gravityCompensatedTorque")
+ .setFieldNames({"gravCompTx", "gravCompTy", "gravCompTz"});
+ svi.addMemberVariable(&SensorValueForceTorque::gravityCompensatedForce,
+ "gravityCompensatedForce")
+ .setFieldNames({"gravCompFx", "gravCompFy", "gravCompFz"});
return svi;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
index b7318e72fe1ae0cc7811389a0eccaa95f80a5ef5..53d921fccbd6ef14dd113f7c442be549a6a6e053 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
@@ -21,25 +21,27 @@
*/
#pragma once
-#include "SensorValueBase.h"
-
#include <ArmarXCore/core/util/algorithm.h>
+#include "SensorValueBase.h"
+
namespace armarx
{
class SensorValueHolonomicPlatformVelocity : virtual public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float velocityX;
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float velocityX;
float velocityY;
float velocityRotation;
- static SensorValueInfo<SensorValueHolonomicPlatformVelocity> GetClassMemberInfo()
+
+ static SensorValueInfo<SensorValueHolonomicPlatformVelocity>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueHolonomicPlatformVelocity> svi;
svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityX, "velocityX");
svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityY, "velocityY");
- svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityRotation, "velocityRotation");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformVelocity::velocityRotation,
+ "velocityRotation");
return svi;
}
};
@@ -47,22 +49,28 @@ namespace armarx
class SensorValueHolonomicPlatformAcceleration : virtual public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float accelerationX;
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION float accelerationX;
float accelerationY;
float accelerationRotation;
- void deriveAccelerationFromVelocityDelta(float dvx, float dvy, float dvrot, float dt)
+
+ void
+ deriveAccelerationFromVelocityDelta(float dvx, float dvy, float dvrot, float dt)
{
accelerationX = dvx / dt;
accelerationY = dvy / dt;
accelerationRotation = dvrot / dt;
}
- static SensorValueInfo<SensorValueHolonomicPlatformAcceleration> GetClassMemberInfo()
+
+ static SensorValueInfo<SensorValueHolonomicPlatformAcceleration>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueHolonomicPlatformAcceleration> svi;
- svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationX, "accelerationX");
- svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationY, "accelerationY");
- svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationRotation, "accelerationRotation");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationX,
+ "accelerationX");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationY,
+ "accelerationY");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformAcceleration::accelerationRotation,
+ "accelerationRotation");
return svi;
}
};
@@ -77,22 +85,26 @@ namespace armarx
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float relativePositionX = 0;
+ float relativePositionX = 0;
float relativePositionY = 0;
float relativePositionRotation = 0;
- static SensorValueInfo<SensorValueHolonomicPlatformRelativePosition> GetClassMemberInfo()
+ static SensorValueInfo<SensorValueHolonomicPlatformRelativePosition>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueHolonomicPlatformRelativePosition> svi;
- svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionX, "relativePositionX");
- svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionY, "relativePositionY");
- svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionRotation, "relativePositionRotation");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionX,
+ "relativePositionX");
+ svi.addMemberVariable(&SensorValueHolonomicPlatformRelativePosition::relativePositionY,
+ "relativePositionY");
+ svi.addMemberVariable(
+ &SensorValueHolonomicPlatformRelativePosition::relativePositionRotation,
+ "relativePositionRotation");
return svi;
}
};
-
- // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
+ // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
// Therefore, SensorValueHolonomicPlatformRelativePosition + SensorValueHolonomicPlatformGlobalPositionCorrection should be used instead.
// class SensorValueHolonomicPlatformAbsolutePosition : virtual public SensorValueBase
@@ -112,7 +124,6 @@ namespace armarx
// }
// };
-
class SensorValueHolonomicPlatform :
virtual public SensorValueHolonomicPlatformVelocity,
@@ -122,14 +133,18 @@ namespace armarx
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- void setVelocitiesAndDeriveAcceleration(float vx, float vy, float vrot, float dt)
+ void
+ setVelocitiesAndDeriveAcceleration(float vx, float vy, float vrot, float dt)
{
- deriveAccelerationFromVelocityDelta(vx - velocityX, vy - velocityY, vrot - velocityRotation, dt);
+ deriveAccelerationFromVelocityDelta(
+ vx - velocityX, vy - velocityY, vrot - velocityRotation, dt);
velocityX = vx;
velocityY = vy;
velocityRotation = vrot;
}
- static SensorValueInfo<SensorValueHolonomicPlatform> GetClassMemberInfo()
+
+ static SensorValueInfo<SensorValueHolonomicPlatform>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueHolonomicPlatform> svi;
svi.addBaseClass<SensorValueHolonomicPlatformVelocity>();
@@ -139,7 +154,7 @@ namespace armarx
}
};
- // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
+ // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
// Therefore, SensorValueHolonomicPlatformRelativePosition + SensorValueHolonomicPlatformGlobalPositionCorrection should be used instead.
// class SensorValueHolonomicPlatformWithAbsolutePosition : virtual public SensorValueHolonomicPlatform, virtual public SensorValueHolonomicPlatformAbsolutePosition
@@ -154,4 +169,4 @@ namespace armarx
// return svi;
// }
// };
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h
index d8e596482f17f042f7a8d89355915565891cc88c..51080678e35f25dfaa69c619193a05b7a18efb37 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h
@@ -24,23 +24,24 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include "SensorValueBase.h"
-
#include <ArmarXCore/core/util/algorithm.h>
+
#include <RobotAPI/libraries/core/Pose.h>
+#include "SensorValueBase.h"
+
namespace armarx
{
- class SensorValueIMU :
- virtual public SensorValueBase
+ class SensorValueIMU : virtual public SensorValueBase
{
public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- Eigen::Vector3f angularVelocity = Eigen::Vector3f::Zero();
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION Eigen::Vector3f angularVelocity =
+ Eigen::Vector3f::Zero();
Eigen::Vector3f linearAcceleration = Eigen::Vector3f::Zero();
Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
- static SensorValueInfo<SensorValueIMU> GetClassMemberInfo()
+ static SensorValueInfo<SensorValueIMU>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueIMU> svi;
svi.addMemberVariable(&SensorValueIMU::angularVelocity, "angularVelocity");
@@ -49,4 +50,4 @@ namespace armarx
return svi;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h
index ece2aa54cdd1681f3f1f4f1887b624cb4040cf3b..2f13e7cc6a665999dbfd90ba3a0563e372fb4c6e 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h
@@ -21,12 +21,12 @@
*/
#pragma once
-#include "SensorValueBase.h"
-
#include <chrono>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include "SensorValueBase.h"
+
namespace armarx
{
class SensorValueRTThreadTimings : public SensorValueBase
@@ -34,7 +34,7 @@ namespace armarx
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- IceUtil::Time rtSwitchControllerSetupDuration;
+ IceUtil::Time rtSwitchControllerSetupDuration;
IceUtil::Time rtSwitchControllerSetupRoundTripTime;
IceUtil::Time rtRunNJointControllersDuration;
@@ -61,28 +61,49 @@ namespace armarx
IceUtil::Time rtLoopDuration;
IceUtil::Time rtLoopRoundTripTime;
- static SensorValueInfo<SensorValueRTThreadTimings> GetClassMemberInfo()
+ static SensorValueInfo<SensorValueRTThreadTimings>
+ GetClassMemberInfo()
{
SensorValueInfo<SensorValueRTThreadTimings> svi;
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtSwitchControllerSetupDuration, "rtSwitchControllerSetup.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtSwitchControllerSetupRoundTripTime, "rtSwitchControllerSetup.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunNJointControllersDuration, "rtRunNJointControllers.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunNJointControllersRoundTripTime, "rtRunNJointControllers.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtHandleInvalidTargetsDuration, "rtHandleInvalidTargets.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtHandleInvalidTargetsRoundTripTime, "rtHandleInvalidTargets.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunJointControllersDuration, "rtRunJointControllers.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunJointControllersRoundTripTime, "rtRunJointControllers.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtBusSendReceiveDuration, "rtBusSendReceive.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtBusSendReceiveRoundTripTime, "rtBusSendReceive.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtReadSensorDeviceValuesDuration, "rtReadSensorDeviceValues.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtReadSensorDeviceValuesRoundTripTime, "rtReadSensorDeviceValues.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtUpdateSensorAndControlBufferDuration, "rtUpdateSensorAndControlBuffer.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtUpdateSensorAndControlBufferRoundTripTime, "rtUpdateSensorAndControlBuffer.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtResetAllTargetsDuration, "rtResetAllTargets.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtResetAllTargetsRoundTripTime, "rtResetAllTargets.RoundTripTime");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtLoopDuration, "rtLoop.Duration");
- svi.addMemberVariable(&SensorValueRTThreadTimings::rtLoopRoundTripTime, "rtLoop.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtSwitchControllerSetupDuration,
+ "rtSwitchControllerSetup.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtSwitchControllerSetupRoundTripTime,
+ "rtSwitchControllerSetup.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunNJointControllersDuration,
+ "rtRunNJointControllers.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunNJointControllersRoundTripTime,
+ "rtRunNJointControllers.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtHandleInvalidTargetsDuration,
+ "rtHandleInvalidTargets.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtHandleInvalidTargetsRoundTripTime,
+ "rtHandleInvalidTargets.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunJointControllersDuration,
+ "rtRunJointControllers.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtRunJointControllersRoundTripTime,
+ "rtRunJointControllers.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtBusSendReceiveDuration,
+ "rtBusSendReceive.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtBusSendReceiveRoundTripTime,
+ "rtBusSendReceive.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtReadSensorDeviceValuesDuration,
+ "rtReadSensorDeviceValues.Duration");
+ svi.addMemberVariable(
+ &SensorValueRTThreadTimings::rtReadSensorDeviceValuesRoundTripTime,
+ "rtReadSensorDeviceValues.RoundTripTime");
+ svi.addMemberVariable(
+ &SensorValueRTThreadTimings::rtUpdateSensorAndControlBufferDuration,
+ "rtUpdateSensorAndControlBuffer.Duration");
+ svi.addMemberVariable(
+ &SensorValueRTThreadTimings::rtUpdateSensorAndControlBufferRoundTripTime,
+ "rtUpdateSensorAndControlBuffer.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtResetAllTargetsDuration,
+ "rtResetAllTargets.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtResetAllTargetsRoundTripTime,
+ "rtResetAllTargets.RoundTripTime");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtLoopDuration, "rtLoop.Duration");
+ svi.addMemberVariable(&SensorValueRTThreadTimings::rtLoopRoundTripTime,
+ "rtLoop.RoundTripTime");
return svi;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp
index 843f1799d3a11a914e6095416c75c913a976a339..6ee5adf80f1f2e409fe19bb5d31027c096fcc3a5 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp
@@ -25,7 +25,9 @@
#include <RobotAPI/libraries/core/FramedPose.h>
-void armarx::ForceTorqueSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+void
+armarx::ForceTorqueSubUnit::update(const armarx::SensorAndControl& sc,
+ const JointAndNJointControllers&)
{
if (!getProxy())
{
@@ -47,22 +49,26 @@ void armarx::ForceTorqueSubUnit::update(const armarx::SensorAndControl& sc, cons
listenerPrx->reportSensorValues(
dev.deviceName,
new armarx::FramedDirection(sensorVal.force, dev.frame, agentName),
- new armarx::FramedDirection(sensorVal.torque, dev.frame, agentName)
- );
+ new armarx::FramedDirection(sensorVal.torque, dev.frame, agentName));
}
}
-void armarx::ForceTorqueSubUnit::setOffset(const armarx::FramedDirectionBasePtr&, const armarx::FramedDirectionBasePtr&, const Ice::Current&)
+void
+armarx::ForceTorqueSubUnit::setOffset(const armarx::FramedDirectionBasePtr&,
+ const armarx::FramedDirectionBasePtr&,
+ const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-void armarx::ForceTorqueSubUnit::setToNull(const Ice::Current&)
+void
+armarx::ForceTorqueSubUnit::setToNull(const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-void armarx::ForceTorqueSubUnit::onInitForceTorqueUnit()
+void
+armarx::ForceTorqueSubUnit::onInitForceTorqueUnit()
{
agentName = getProperty<std::string>("AgentName").getValue();
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h
index eb11819df743b3c30176ca046afdd8b500b28b49..6c5b294d31d9c3c7f595d1509031c5dfa4212cdd 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h
@@ -22,30 +22,38 @@
#pragma once
-#include "RobotUnitSubUnit.h"
-
#include <RobotAPI/components/units/ForceTorqueUnit.h>
#include "../SensorValues/SensorValueForceTorque.h"
+#include "RobotUnitSubUnit.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(ForceTorqueSubUnit);
- class ForceTorqueSubUnit:
- virtual public RobotUnitSubUnit,
- virtual public ForceTorqueUnit
+
+ class ForceTorqueSubUnit : virtual public RobotUnitSubUnit, virtual public ForceTorqueUnit
{
public:
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
// ForceTorqueUnitInterface interface
- void setOffset(const FramedDirectionBasePtr&, const FramedDirectionBasePtr&, const Ice::Current& = Ice::emptyCurrent) override;
+ void setOffset(const FramedDirectionBasePtr&,
+ const FramedDirectionBasePtr&,
+ const Ice::Current& = Ice::emptyCurrent) override;
void setToNull(const Ice::Current& = Ice::emptyCurrent) override;
// ForceTorqueUnit interface
- void onInitForceTorqueUnit() override;
- void onStartForceTorqueUnit() override {}
- void onExitForceTorqueUnit() override {}
+ void onInitForceTorqueUnit() override;
+
+ void
+ onStartForceTorqueUnit() override
+ {
+ }
+
+ void
+ onExitForceTorqueUnit() override
+ {
+ }
struct DeviceData
{
@@ -55,7 +63,8 @@ namespace armarx
};
std::vector<DeviceData> devs;
+
private:
std::string agentName;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp
index 90f43ef47f332da6e210109d55a26064b96c3662..462f0c9206698f0b503a0bf36d27e729b6c825e8 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp
@@ -20,15 +20,18 @@
* GNU General Public License
*/
#include "InertialMeasurementSubUnit.h"
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-void armarx::InertialMeasurementSubUnit::onStartIMU()
+void
+armarx::InertialMeasurementSubUnit::onStartIMU()
{
ARMARX_IMPORTANT << "Devices with IMUs: " << devs;
}
-
-void armarx::InertialMeasurementSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+void
+armarx::InertialMeasurementSubUnit::update(const armarx::SensorAndControl& sc,
+ const JointAndNJointControllers&)
{
if (!getProxy())
{
@@ -50,12 +53,18 @@ void armarx::InertialMeasurementSubUnit::update(const armarx::SensorAndControl&
ARMARX_CHECK_EXPRESSION(sv->isA<SensorValueIMU>());
const SensorValueIMU* s = sv->asA<SensorValueIMU>();
IMUData data;
- data.acceleration = Ice::FloatSeq(s->linearAcceleration.data(), s->linearAcceleration.data() + s->linearAcceleration.rows() * s->linearAcceleration.cols());
- data.gyroscopeRotation = Ice::FloatSeq(s->angularVelocity.data(), s->angularVelocity.data() + s->angularVelocity.rows() * s->angularVelocity.cols());;
- data.orientationQuaternion = {s->orientation.w(), s->orientation.x(), s->orientation.y(), s->orientation.z()};
+ data.acceleration =
+ Ice::FloatSeq(s->linearAcceleration.data(),
+ s->linearAcceleration.data() +
+ s->linearAcceleration.rows() * s->linearAcceleration.cols());
+ data.gyroscopeRotation = Ice::FloatSeq(
+ s->angularVelocity.data(),
+ s->angularVelocity.data() + s->angularVelocity.rows() * s->angularVelocity.cols());
+ ;
+ data.orientationQuaternion = {
+ s->orientation.w(), s->orientation.x(), s->orientation.y(), s->orientation.z()};
auto frame = dev;
batchPrx->reportSensorValues(dev, frame, data, t);
}
batchPrx->ice_flushBatchRequests();
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h
index d5a961e663f69caf70b9e7e4b9de194f49c0732b..4282cb834244e20c918c76b49422a4b81abc6c8a 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h
@@ -22,18 +22,18 @@
#pragma once
-#include "RobotUnitSubUnit.h"
-
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <RobotAPI/components/units/InertialMeasurementUnit.h>
#include "../SensorValues/SensorValueIMU.h"
+#include "RobotUnitSubUnit.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(InertialMeasurementSubUnit);
- class InertialMeasurementSubUnit:
+
+ class InertialMeasurementSubUnit :
virtual public RobotUnitSubUnit,
virtual public InertialMeasurementUnit
{
@@ -42,10 +42,19 @@ namespace armarx
// InertialMeasurementUnit interface
protected:
- void onInitIMU() override {}
+ void
+ onInitIMU() override
+ {
+ }
+
void onStartIMU() override;
- void onExitIMU() override {}
+
+ void
+ onExitIMU() override
+ {
+ }
+
public:
std::map<std::string, std::size_t> devs;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
index 6c260f7936c42fe7cbeefefa01fcb818054bc03f..87ce13bc529cbc44c114171337e43192b83bd503 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
@@ -21,21 +21,20 @@
*/
#include "KinematicSubUnit.h"
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <ArmarXCore/core/util/algorithm.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
-armarx::KinematicSubUnit::KinematicSubUnit() :
- reportSkipper(20.0f)
+armarx::KinematicSubUnit::KinematicSubUnit() : reportSkipper(20.0f)
{
-
}
-void armarx::KinematicSubUnit::setupData(
+void
+armarx::KinematicSubUnit::setupData(
std::string relRobFile,
VirtualRobot::RobotPtr rob,
std::map<std::string, ActuatorData>&& newDevs,
@@ -43,7 +42,7 @@ void armarx::KinematicSubUnit::setupData(
std::set<std::string> controlDeviceHardwareControlModeGrpsMerged,
RobotUnit* newRobotUnit)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
ARMARX_CHECK_EXPRESSION(getState() == eManagedIceObjectCreated);
ARMARX_CHECK_EXPRESSION(!robot);
@@ -68,14 +67,16 @@ void armarx::KinematicSubUnit::setupData(
auto nodes = robot->getRobotNodes();
for (auto& node : nodes)
{
- if ((node->isRotationalJoint() || node->isTranslationalJoint()) && !devs.count(node->getName()))
+ if (node->isJoint() && !devs.count(node->getName()))
{
sensorLessJoints.push_back(node);
}
}
}
-void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const armarx::JointAndNJointControllers& c)
+void
+armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc,
+ const armarx::JointAndNJointControllers& c)
{
if (!getProxy())
{
@@ -88,7 +89,7 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
ARMARX_IMPORTANT << deactivateSpam(1) << "listener is not set";
return;
}
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
bool ctrlModesAValueChanged = false;
bool angAValueChanged = false;
bool velAValueChanged = false;
@@ -99,7 +100,8 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
bool statusesAvalueChanged = false;
long timestamp = sc.sensorValuesTimestamp.toMicroSeconds();
- std::set<NJointControllerBase*> nJointCtrls {c.nJointControllers.begin(), c.nJointControllers.end()};
+ std::set<NJointControllerBase*> nJointCtrls{c.nJointControllers.begin(),
+ c.nJointControllers.end()};
std::vector<std::string> actuatorsWithoutSensor;
actuatorsWithoutSensor.reserve(devs.size());
for (const auto& name2actuatorData : devs)
@@ -123,19 +125,41 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
ARMARX_CHECK_EXPRESSION(eUnknown == c);
c = eTorqueControl;
}
- ctrlModesAValueChanged = ctrlModesAValueChanged || !ctrlModes.count(name) || (ctrlModes.at(name) != c);
+ ctrlModesAValueChanged =
+ ctrlModesAValueChanged || !ctrlModes.count(name) || (ctrlModes.at(name) != c);
ctrlModes[name] = c;
}
if (actuatorData.sensorDeviceIndex < sc.sensors.size())
{
const SensorValueBase* s = sc.sensors.at(actuatorData.sensorDeviceIndex).get();
- UpdateNameValueMap<float, SensorValue1DoFActuatorPosition, &SensorValue1DoFActuatorPosition::position >(ang, name, s, angAValueChanged);
- UpdateNameValueMap<float, SensorValue1DoFActuatorVelocity, &SensorValue1DoFActuatorVelocity::velocity >(vel, name, s, velAValueChanged);
- UpdateNameValueMap<float, SensorValue1DoFActuatorAcceleration, &SensorValue1DoFActuatorAcceleration::acceleration >(acc, name, s, accAValueChanged);
- UpdateNameValueMap<float, SensorValue1DoFActuatorTorque, &SensorValue1DoFActuatorTorque::torque >(tor, name, s, torAValueChanged);
- UpdateNameValueMap<float, SensorValue1DoFActuatorCurrent, &SensorValue1DoFActuatorCurrent::motorCurrent >(motorCurrents, name, s, motorCurrentAValueChanged);
- UpdateNameValueMap<float, SensorValue1DoFActuatorMotorTemperature, &SensorValue1DoFActuatorMotorTemperature::motorTemperature>(motorTemperatures, name, s, motorTemperaturesAValueChanged);
- UpdateNameValueMap<JointStatus, SensorValue1DoFActuatorStatus, &SensorValue1DoFActuatorStatus::status >(statuses, name, s, statusesAvalueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorPosition,
+ &SensorValue1DoFActuatorPosition::position>(
+ ang, name, s, angAValueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorVelocity,
+ &SensorValue1DoFActuatorVelocity::velocity>(
+ vel, name, s, velAValueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorAcceleration,
+ &SensorValue1DoFActuatorAcceleration::acceleration>(
+ acc, name, s, accAValueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorTorque,
+ &SensorValue1DoFActuatorTorque::torque>(
+ tor, name, s, torAValueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorCurrent,
+ &SensorValue1DoFActuatorCurrent::motorCurrent>(
+ motorCurrents, name, s, motorCurrentAValueChanged);
+ UpdateNameValueMap<float,
+ SensorValue1DoFActuatorMotorTemperature,
+ &SensorValue1DoFActuatorMotorTemperature::motorTemperature>(
+ motorTemperatures, name, s, motorTemperaturesAValueChanged);
+ UpdateNameValueMap<JointStatus,
+ SensorValue1DoFActuatorStatus,
+ &SensorValue1DoFActuatorStatus::status>(
+ statuses, name, s, statusesAvalueChanged);
}
else
{
@@ -144,7 +168,8 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
}
if (!actuatorsWithoutSensor.empty())
{
- ARMARX_WARNING << deactivateSpam(5) << "these actuators have no sensor!:\n" << actuatorsWithoutSensor;
+ ARMARX_WARNING << deactivateSpam(5) << "these actuators have no sensor!:\n"
+ << actuatorsWithoutSensor;
}
// update the joint values of linked joints
@@ -160,14 +185,17 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
}
- ARMARX_DEBUG << deactivateSpam(30) << "reporting updated data:\n"
- << ctrlModes.size() << " \tcontrol modes (updated = " << ctrlModesAValueChanged << ")\n"
- << ang.size() << " \tjoint angles (updated = " << angAValueChanged << ")\n"
- << vel.size() << " \tjoint velocities (updated = " << velAValueChanged << ")\n"
- << acc.size() << " \tjoint accelerations (updated = " << accAValueChanged << ")\n"
- << tor.size() << " \tjoint torques (updated = " << torAValueChanged << ")\n"
- << motorCurrents.size() << " \tmotor currents (updated = " << motorCurrentAValueChanged << ")\n"
- << motorTemperatures.size() << " \tmotor temperatures (updated = " << motorTemperaturesAValueChanged << ")\n";
+ ARMARX_DEBUG << deactivateSpam(30) << "reporting updated data:\n"
+ << ctrlModes.size()
+ << " \tcontrol modes (updated = " << ctrlModesAValueChanged << ")\n"
+ << ang.size() << " \tjoint angles (updated = " << angAValueChanged << ")\n"
+ << vel.size() << " \tjoint velocities (updated = " << velAValueChanged << ")\n"
+ << acc.size() << " \tjoint accelerations (updated = " << accAValueChanged << ")\n"
+ << tor.size() << " \tjoint torques (updated = " << torAValueChanged << ")\n"
+ << motorCurrents.size()
+ << " \tmotor currents (updated = " << motorCurrentAValueChanged << ")\n"
+ << motorTemperatures.size()
+ << " \tmotor temperatures (updated = " << motorTemperaturesAValueChanged << ")\n";
auto prx = listenerPrx->ice_batchOneway();
prx->reportJointAngles(ang, timestamp, angAValueChanged);
prx->reportJointVelocities(vel, timestamp, velAValueChanged);
@@ -181,7 +209,8 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
}
if (!motorTemperatures.empty())
{
- prx->reportJointMotorTemperatures(motorTemperatures, timestamp, motorTemperaturesAValueChanged);
+ prx->reportJointMotorTemperatures(
+ motorTemperatures, timestamp, motorTemperaturesAValueChanged);
}
if (!statuses.empty())
{
@@ -192,19 +221,22 @@ void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const
prx->ice_flushBatchRequests();
}
-void armarx::KinematicSubUnit::requestJoints(const Ice::StringSeq&, const Ice::Current&)
+void
+armarx::KinematicSubUnit::requestJoints(const Ice::StringSeq&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-void armarx::KinematicSubUnit::releaseJoints(const Ice::StringSeq&, const Ice::Current&)
+void
+armarx::KinematicSubUnit::releaseJoints(const Ice::StringSeq&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-void armarx::KinematicSubUnit::setJointAngles(const armarx::NameValueMap& angles, const Ice::Current&)
+void
+armarx::KinematicSubUnit::setJointAngles(const armarx::NameValueMap& angles, const Ice::Current&)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
for (const auto& n2v : angles)
{
if (devs.count(n2v.first))
@@ -217,9 +249,11 @@ void armarx::KinematicSubUnit::setJointAngles(const armarx::NameValueMap& angles
}
}
-void armarx::KinematicSubUnit::setJointVelocities(const armarx::NameValueMap& velocities, const Ice::Current&)
+void
+armarx::KinematicSubUnit::setJointVelocities(const armarx::NameValueMap& velocities,
+ const Ice::Current&)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
for (const auto& n2v : velocities)
{
if (devs.count(n2v.first))
@@ -232,9 +266,10 @@ void armarx::KinematicSubUnit::setJointVelocities(const armarx::NameValueMap& ve
}
}
-void armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torques, const Ice::Current&)
+void
+armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torques, const Ice::Current&)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
for (const auto& n2v : torques)
{
if (devs.count(n2v.first))
@@ -247,26 +282,33 @@ void armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torqu
}
}
-void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMap& ncm, const Ice::Current&)
+void
+armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMap& ncm,
+ const Ice::Current&)
{
std::map<std::string, NJointControllerBasePtr> toActivate;
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
for (const auto& n2c : ncm)
{
const std::string& name = n2c.first;
ControlMode mode = n2c.second;
if (!devs.count(name))
{
- ARMARX_WARNING << "requested mode '" << KinematicUnitHelper::ControlModeToString(mode) << "' for nonexistent device '" << name
+ ARMARX_WARNING << "requested mode '"
+ << KinematicUnitHelper::ControlModeToString(mode)
+ << "' for nonexistent device '" << name
<< "'. (ignoring this device)";
continue;
}
- NJointKinematicUnitPassThroughControllerPtr ctrl = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(devs.at(name).getController(mode));
+ NJointKinematicUnitPassThroughControllerPtr ctrl =
+ NJointKinematicUnitPassThroughControllerPtr::dynamicCast(
+ devs.at(name).getController(mode));
if (!ctrl)
{
- ARMARX_WARNING << "requested unsupported mode '" << KinematicUnitHelper::ControlModeToString(mode) << "' for device '" << name
- << "'. (ignoring this device)";
+ ARMARX_WARNING << "requested unsupported mode '"
+ << KinematicUnitHelper::ControlModeToString(mode) << "' for device '"
+ << name << "'. (ignoring this device)";
continue;
}
if (ctrl->isControllerActive())
@@ -283,7 +325,9 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
out << " '" << elem.first << "' -> '" << elem.second->getInstanceName() << "'\n";
}
};
- ARMARX_DEBUG << "switching control modes requests these NJointControllers (without consideration of ControlDeviceHardwareControlModeGroups):\n" << printToActivate;
+ ARMARX_DEBUG << "switching control modes requests these NJointControllers (without "
+ "consideration of ControlDeviceHardwareControlModeGroups):\n"
+ << printToActivate;
for (const auto& n2NJointCtrl : toActivate)
{
const auto& name = n2NJointCtrl.first;
@@ -304,7 +348,7 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
}
}
ARMARX_CHECK_EXPRESSION(group);
- const auto jointCtrl = nJointCtrl->getControlDevicesUsedJointController().at(name);
+ const auto jointCtrl = nJointCtrl->getControlDevicesUsedJointController().at(name);
const auto hwMode = jointCtrl->getHardwareControlMode();
//check against all other elems of the group
for (const auto& other : *group)
@@ -320,36 +364,53 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
//this device may have a controller switch
if (toActivate.count(other))
{
- const auto otherJointCtrl = toActivate.at(other)->getControlDevicesUsedJointController().at(other);
+ const auto otherJointCtrl =
+ toActivate.at(other)->getControlDevicesUsedJointController().at(other);
const auto otherHwMode = otherJointCtrl->getHardwareControlMode();
if (otherHwMode != hwMode)
{
std::stringstream strstr;
- strstr << "The hardware control mode for two control devices with requested control mode changein the same group does not match!\n"
- << "Device 1: '" << name << "' mode " << "'" << jointCtrl->getControlMode() << "' hw mode '" << hwMode << "'\n"
- << "Device 2: '" << other << "' mode " << "'" << otherJointCtrl->getControlMode() << "' hw mode '" << otherHwMode << "'\n";
+ strstr << "The hardware control mode for two control devices with "
+ "requested control mode changein the same group does not match!\n"
+ << "Device 1: '" << name << "' mode "
+ << "'" << jointCtrl->getControlMode() << "' hw mode '" << hwMode
+ << "'\n"
+ << "Device 2: '" << other << "' mode "
+ << "'" << otherJointCtrl->getControlMode() << "' hw mode '"
+ << otherHwMode << "'\n";
ARMARX_ERROR << strstr.str();
- throw InvalidArgumentException {strstr.str()};
+ throw InvalidArgumentException{strstr.str()};
}
}
else
{
//get the current active
const auto otherNJointCtrl = devs.at(other).getActiveController();
- const auto otherJointCtrl = otherNJointCtrl ? otherNJointCtrl->getControlDevicesUsedJointController().at(other) : nullptr;
- const auto otherHwMode = otherJointCtrl ? otherJointCtrl->getHardwareControlMode() : "";
+ const auto otherJointCtrl =
+ otherNJointCtrl
+ ? otherNJointCtrl->getControlDevicesUsedJointController().at(other)
+ : nullptr;
+ const auto otherHwMode =
+ otherJointCtrl ? otherJointCtrl->getHardwareControlMode() : "";
if (hwMode != otherHwMode)
{
toActivate[other] = std::move(devs.at(other).getController(ncm.at(name)));
ARMARX_CHECK_EXPRESSION(toActivate.at(other));
- ARMARX_CHECK_EXPRESSION(toActivate.at(other)->getControlDevicesUsedJointController().at(other)->getHardwareControlMode() == hwMode);
- ARMARX_VERBOSE << "activating '" << nJointCtrl->getInstanceName() << "' caused activation of '" << toActivate.at(name)->getInstanceName() << "'";
+ ARMARX_CHECK_EXPRESSION(toActivate.at(other)
+ ->getControlDevicesUsedJointController()
+ .at(other)
+ ->getHardwareControlMode() == hwMode);
+ ARMARX_VERBOSE << "activating '" << nJointCtrl->getInstanceName()
+ << "' caused activation of '"
+ << toActivate.at(name)->getInstanceName() << "'";
}
}
}
ARMARX_DEBUG << "checking group of '" << name << "'...done!";
}
- ARMARX_DEBUG << "switching control modes requests these NJointControllers (with consideration of ControlDeviceHardwareControlModeGroups):\n" << printToActivate;
+ ARMARX_DEBUG << "switching control modes requests these NJointControllers (with "
+ "consideration of ControlDeviceHardwareControlModeGroups):\n"
+ << printToActivate;
//now check if groups are satisfied
ARMARX_CHECK_EXPRESSION(robotUnit);
}
@@ -359,61 +420,65 @@ void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMa
}
}
-void armarx::KinematicSubUnit::setJointAccelerations(const armarx::NameValueMap&, const Ice::Current&)
+void
+armarx::KinematicSubUnit::setJointAccelerations(const armarx::NameValueMap&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-void armarx::KinematicSubUnit::setJointDecelerations(const armarx::NameValueMap&, const Ice::Current&)
+void
+armarx::KinematicSubUnit::setJointDecelerations(const armarx::NameValueMap&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
-armarx::NameControlModeMap armarx::KinematicSubUnit::getControlModes(const Ice::Current&)
+armarx::NameControlModeMap
+armarx::KinematicSubUnit::getControlModes(const Ice::Current&)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
return ctrlModes;
}
-armarx::NameValueMap armarx::KinematicSubUnit::getJointAngles(const Ice::Current& c) const
+armarx::NameValueMap
+armarx::KinematicSubUnit::getJointAngles(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
return ang;
}
-armarx::NameValueMap armarx::KinematicSubUnit::getJointVelocities(const Ice::Current& c) const
+armarx::NameValueMap
+armarx::KinematicSubUnit::getJointVelocities(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
return vel;
}
-Ice::StringSeq armarx::KinematicSubUnit::getJoints(const Ice::Current& c) const
+Ice::StringSeq
+armarx::KinematicSubUnit::getJoints(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
return getMapKeys(ang);
}
-armarx::DebugInfo armarx::KinematicSubUnit::getDebugInfo(const Ice::Current& c) const
+armarx::DebugInfo
+armarx::KinematicSubUnit::getDebugInfo(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {dataMutex};
-
- armarx::DebugInfo debugInfo
- {
- .jointModes = ctrlModes,
- .jointAngles = ang,
- .jointVelocities = vel,
- .jointAccelerations = acc,
- .jointTorques = tor,
- .jointCurrents = motorCurrents,
- .jointMotorTemperatures = motorTemperatures,
- .jointStatus = statuses
- };
+ std::lock_guard<std::mutex> guard{dataMutex};
+
+ armarx::DebugInfo debugInfo{.jointModes = ctrlModes,
+ .jointAngles = ang,
+ .jointVelocities = vel,
+ .jointAccelerations = acc,
+ .jointTorques = tor,
+ .jointCurrents = motorCurrents,
+ .jointMotorTemperatures = motorTemperatures,
+ .jointStatus = statuses};
return debugInfo;
}
-
-armarx::NJointControllerPtr armarx::KinematicSubUnit::ActuatorData::getController(armarx::ControlMode c) const
+armarx::NJointControllerPtr
+armarx::KinematicSubUnit::ActuatorData::getController(armarx::ControlMode c) const
{
switch (c)
{
@@ -430,7 +495,8 @@ armarx::NJointControllerPtr armarx::KinematicSubUnit::ActuatorData::getControlle
}
}
-armarx::NJointControllerPtr armarx::KinematicSubUnit::ActuatorData::getActiveController() const
+armarx::NJointControllerPtr
+armarx::KinematicSubUnit::ActuatorData::getActiveController() const
{
if (ctrlPos->isControllerActive())
{
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
index 92a6caf3d845ebd2291ecf0d0ce96db597367fac..8cecc5f7e10ed0abdd29cc7e5dfb55f1260d0daf 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
@@ -25,24 +25,22 @@
#include <mutex>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/IceReportSkipper.h>
#include <RobotAPI/components/units/KinematicUnit.h>
-#include <ArmarXCore/core/util/IceReportSkipper.h>
-
-#include "RobotUnitSubUnit.h"
-#include "../util.h"
-#include "../SensorValues/SensorValue1DoFActuator.h"
#include "../NJointControllers/NJointKinematicUnitPassThroughController.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
+#include "../util.h"
+#include "RobotUnitSubUnit.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnit);
TYPEDEF_PTRS_HANDLE(KinematicSubUnit);
- class KinematicSubUnit:
- virtual public RobotUnitSubUnit,
- virtual public KinematicUnit
+
+ class KinematicSubUnit : virtual public RobotUnitSubUnit, virtual public KinematicUnit
{
public:
struct ActuatorData
@@ -56,16 +54,15 @@ namespace armarx
NJointControllerPtr getController(ControlMode c) const;
NJointControllerPtr getActiveController() const;
};
+
KinematicSubUnit();
- void setupData(
- std::string relRobFile,
- VirtualRobot::RobotPtr rob,
- std::map<std::string, ActuatorData>&& newDevs,
- std::vector<std::set<std::string> > controlDeviceHardwareControlModeGrps,
- std::set<std::string> controlDeviceHardwareControlModeGrpsMerged,
- RobotUnit* newRobotUnit
- );
+ void setupData(std::string relRobFile,
+ VirtualRobot::RobotPtr rob,
+ std::map<std::string, ActuatorData>&& newDevs,
+ std::vector<std::set<std::string>> controlDeviceHardwareControlModeGrps,
+ std::set<std::string> controlDeviceHardwareControlModeGrpsMerged,
+ RobotUnit* newRobotUnit);
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
// KinematicUnitInterface interface
@@ -80,17 +77,27 @@ namespace armarx
void setJointAccelerations(const NameValueMap&, const Ice::Current&) override;
void setJointDecelerations(const NameValueMap&, const Ice::Current&) override;
- NameControlModeMap getControlModes(const Ice::Current&) override;
- NameValueMap getJointAngles(const Ice::Current&) const override;
- NameValueMap getJointVelocities(const Ice::Current&) const override;
- Ice::StringSeq getJoints(const Ice::Current& c) const override;
+ NameControlModeMap getControlModes(const Ice::Current&) override;
+ NameValueMap getJointAngles(const Ice::Current&) const override;
+ NameValueMap getJointVelocities(const Ice::Current&) const override;
+ Ice::StringSeq getJoints(const Ice::Current& c) const override;
DebugInfo getDebugInfo(const Ice::Current& c = Ice::emptyCurrent) const override;
+ void
+ onInitKinematicUnit() override
+ {
+ }
- void onInitKinematicUnit() override {}
- void onStartKinematicUnit() override {}
- void onExitKinematicUnit() override {}
+ void
+ onStartKinematicUnit() override
+ {
+ }
+
+ void
+ onExitKinematicUnit() override
+ {
+ }
private:
std::map<std::string, ActuatorData> devs;
@@ -110,8 +117,12 @@ namespace armarx
IceReportSkipper reportSkipper;
std::vector<VirtualRobot::RobotNodePtr> sensorLessJoints;
- template<class ValueT, class SensorValueT, ValueT SensorValueT::* Member>
- static void UpdateNameValueMap(std::map<std::string, ValueT>& nvm, const std::string& name, const SensorValueBase* sv, bool& changeState)
+ template <class ValueT, class SensorValueT, ValueT SensorValueT::*Member>
+ static void
+ UpdateNameValueMap(std::map<std::string, ValueT>& nvm,
+ const std::string& name,
+ const SensorValueBase* sv,
+ bool& changeState)
{
const SensorValueT* s = sv->asA<SensorValueT>();
if (!s)
@@ -126,4 +137,4 @@ namespace armarx
nvm[name] = v;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
index 12c3faf13ffb311a36994002f8f05ef7122dc206..debb6a1a7d61f2cc348dd8214dea4f5e512bdf1e 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
@@ -31,13 +31,13 @@
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-
namespace armarx
{
LocalizationSubUnit::~LocalizationSubUnit() = default;
-
- void LocalizationSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+ void
+ LocalizationSubUnit::update(const armarx::SensorAndControl& sc,
+ const JointAndNJointControllers&)
{
if (!getProxy())
{
@@ -47,10 +47,12 @@ namespace armarx
}
}
-
- void LocalizationSubUnit::reportGlobalRobotPoseCorrection(const TransformStamped& global_T_odom, const Ice::Current&)
+ void
+ LocalizationSubUnit::reportGlobalRobotPoseCorrection(const TransformStamped& global_T_odom,
+ const Ice::Current&)
{
ARMARX_CHECK_EXPRESSION(globalPositionCorrectionSensorDevice);
- globalPositionCorrectionSensorDevice->updateGlobalPositionCorrection(global_T_odom.transform);
+ globalPositionCorrectionSensorDevice->updateGlobalPositionCorrection(
+ global_T_odom.transform);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
index f628ecc3164f02134d1a4c8c2e9b5915295e5762..c40aa7e4460792dae1ae0e8f5519edb80766a31f 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
@@ -29,9 +29,9 @@
#include <VirtualRobot/MathTools.h>
#include <RobotAPI/components/units/SensorActorUnit.h>
-#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/units/LocalizationUnitInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include "RobotUnitSubUnit.h"
@@ -44,21 +44,29 @@ namespace armarx
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "LocalizationUnit";
}
- void onInitComponent() override {}
- void onConnectComponent() override {}
- };
+ void
+ onInitComponent() override
+ {
+ }
+ void
+ onConnectComponent() override
+ {
+ }
+ };
class GlobalRobotPoseCorrectionSensorDevice;
TYPEDEF_PTRS_HANDLE(LocalizationSubUnit);
- class LocalizationSubUnit:
+
+ class LocalizationSubUnit :
virtual public RobotUnitSubUnit,
virtual public LocalizationUnit,
virtual public LocalizationSubUnitInterface
@@ -69,7 +77,8 @@ namespace armarx
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
- void reportGlobalRobotPoseCorrection(const TransformStamped& global_T_odom, const Ice::Current& = Ice::emptyCurrent) override;
+ void reportGlobalRobotPoseCorrection(const TransformStamped& global_T_odom,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* This device partially holds the information about the robot's global pose.
@@ -80,10 +89,7 @@ namespace armarx
GlobalRobotPoseCorrectionSensorDevice* globalPositionCorrectionSensorDevice;
private:
-
std::string agentName;
std::string robotRootFrame;
-
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index 5ae63c2ac2f2c7ba30ac903feeddeb9a4fed6814..c7e7348c57092485ddc70a1e5a18487b091cd358 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -26,16 +26,17 @@
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
#include <RobotAPI/interface/core/GeometryBase.h>
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
-
namespace armarx
{
- void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+ void
+ armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc,
+ const JointAndNJointControllers&)
{
if (!getProxy())
{
@@ -50,9 +51,10 @@ namespace armarx
}
const SensorValueBase* sensorValue = sc.sensors.at(platformSensorIndex).get();
ARMARX_CHECK_EXPRESSION(sensorValue);
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
- const auto timestamp = sc.sensorValuesTimestamp.toMicroSeconds();;
+ const auto timestamp = sc.sensorValuesTimestamp.toMicroSeconds();
+ ;
// odom velocity is in local robot frame
FrameHeader odomVelocityHeader;
@@ -78,10 +80,13 @@ namespace armarx
//pos
{
- ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformRelativePosition>());
- const SensorValueHolonomicPlatformRelativePosition* s = sensorValue->asA<SensorValueHolonomicPlatformRelativePosition>();
+ ARMARX_CHECK_EXPRESSION(
+ sensorValue->isA<SensorValueHolonomicPlatformRelativePosition>());
+ const SensorValueHolonomicPlatformRelativePosition* s =
+ sensorValue->asA<SensorValueHolonomicPlatformRelativePosition>();
- const auto odom_T_robot = VirtualRobot::MathTools::posrpy2eigen4f(s->relativePositionX, s->relativePositionY, 0, 0, 0, s->relativePositionRotation);
+ const auto odom_T_robot = VirtualRobot::MathTools::posrpy2eigen4f(
+ s->relativePositionX, s->relativePositionY, 0, 0, 0, s->relativePositionRotation);
// @@@ CHECK BELOW:
// if (s->isA<SensorValueHolonomicPlatformAbsolutePosition>())
@@ -108,22 +113,20 @@ namespace armarx
// const auto global_T_robot = global_T_odom * odom_T_robot;
- const TransformStamped odomPose
- {
- .header = odomPoseHeader,
- .transform = odom_T_robot
- };
+ const TransformStamped odomPose{.header = odomPoseHeader, .transform = odom_T_robot};
odometryPrx->reportOdometryPose(odomPose);
// legacy interface
const auto odomLegacyPose = toPlatformPose(odomPose);
- listenerPrx->reportPlatformOdometryPose(odomLegacyPose.x, odomLegacyPose.y, odomLegacyPose.rotationAroundZ);
+ listenerPrx->reportPlatformOdometryPose(
+ odomLegacyPose.x, odomLegacyPose.y, odomLegacyPose.rotationAroundZ);
}
-
+
//vel
{
ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformVelocity>());
- const SensorValueHolonomicPlatformVelocity* s = sensorValue->asA<SensorValueHolonomicPlatformVelocity>();
+ const SensorValueHolonomicPlatformVelocity* s =
+ sensorValue->asA<SensorValueHolonomicPlatformVelocity>();
TwistStamped odomVelocity;
odomVelocity.header = odomVelocityHeader;
@@ -138,22 +141,27 @@ namespace armarx
}
}
- void armarx::PlatformSubUnit::move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current&)
+ void
+ armarx::PlatformSubUnit::move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current&)
{
//holding the mutex here could deadlock
if (!pt->isControllerActive())
{
pt->activateController();
}
- std::lock_guard<std::mutex> guard {dataMutex};
- pt->setVelocites(
- std::clamp(vx, -maxVLin, maxVLin),
- std::clamp(vy, -maxVLin, maxVLin),
- std::clamp(vr, -maxVAng, maxVAng)
- );
+ std::lock_guard<std::mutex> guard{dataMutex};
+ pt->setVelocites(std::clamp(vx, -maxVLin, maxVLin),
+ std::clamp(vy, -maxVLin, maxVLin),
+ std::clamp(vr, -maxVAng, maxVAng));
}
- void armarx::PlatformSubUnit::moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
+ void
+ armarx::PlatformSubUnit::moveTo(Ice::Float rx,
+ Ice::Float ry,
+ Ice::Float rr,
+ Ice::Float lac,
+ Ice::Float rac,
+ const Ice::Current&)
{
globalPosCtrl->setTarget(rx, ry, rr, lac, rac);
if (!globalPosCtrl->isControllerActive())
@@ -162,7 +170,13 @@ namespace armarx
};
}
- void armarx::PlatformSubUnit::moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
+ void
+ armarx::PlatformSubUnit::moveRelative(Ice::Float rx,
+ Ice::Float ry,
+ Ice::Float rr,
+ Ice::Float lac,
+ Ice::Float rac,
+ const Ice::Current&)
{
relativePosCtrl->setTarget(rx, ry, rr, lac, rac);
if (!relativePosCtrl->isControllerActive())
@@ -172,12 +186,14 @@ namespace armarx
//holding the mutex here could deadlock
// std::lock_guard<std::mutex> guard {dataMutex};
ARMARX_INFO << "target orientation: " << rr;
-
}
- void armarx::PlatformSubUnit::setMaxVelocities(Ice::Float mxVLin, Ice::Float mxVAng, const Ice::Current&)
+ void
+ armarx::PlatformSubUnit::setMaxVelocities(Ice::Float mxVLin,
+ Ice::Float mxVAng,
+ const Ice::Current&)
{
- std::lock_guard<std::mutex> guard {dataMutex};
+ std::lock_guard<std::mutex> guard{dataMutex};
maxVLin = std::abs(mxVLin);
maxVAng = std::abs(mxVAng);
}
@@ -195,10 +211,11 @@ namespace armarx
// return new Pose {global_T_robot};
// }
- void armarx::PlatformSubUnit::stopPlatform(const Ice::Current& c)
+ void
+ armarx::PlatformSubUnit::stopPlatform(const Ice::Current& c)
{
move(0, 0, 0);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 93ea9a009c91a8c46b2c96ac0ac4917d0fb36850..90d5443bfd71a795ba244f650ae97b24988359e2 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -29,21 +29,22 @@
#include <VirtualRobot/MathTools.h>
#include <RobotAPI/components/units/PlatformUnit.h>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h>
#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/core/Pose.h>
-#include "RobotUnitSubUnit.h"
#include "../NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h"
#include "../SensorValues/SensorValueHolonomicPlatform.h"
+#include "RobotUnitSubUnit.h"
namespace armarx
{
class GlobalRobotPoseCorrectionSensorDevice;
TYPEDEF_PTRS_HANDLE(PlatformSubUnit);
- class PlatformSubUnit:
+
+ class PlatformSubUnit :
virtual public RobotUnitSubUnit,
virtual public PlatformUnit,
virtual public PlatformSubUnitInterface
@@ -52,22 +53,44 @@ namespace armarx
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
// PlatformUnitInterface interface
- void move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current& = Ice::emptyCurrent) override;
- void moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current& = Ice::emptyCurrent) override;
- void moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current& = Ice::emptyCurrent) override;
- void setMaxVelocities(Ice::Float mxVLin, Ice::Float mxVAng, const Ice::Current& = Ice::emptyCurrent) override;
-
+ void move(Ice::Float vx,
+ Ice::Float vy,
+ Ice::Float vr,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void moveTo(Ice::Float rx,
+ Ice::Float ry,
+ Ice::Float rr,
+ Ice::Float lac,
+ Ice::Float rac,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void moveRelative(Ice::Float rx,
+ Ice::Float ry,
+ Ice::Float rr,
+ Ice::Float lac,
+ Ice::Float rac,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setMaxVelocities(Ice::Float mxVLin,
+ Ice::Float mxVAng,
+ const Ice::Current& = Ice::emptyCurrent) override;
// PlatformUnit interface
- void onInitPlatformUnit() override
+ void
+ onInitPlatformUnit() override
{
}
- void onStartPlatformUnit() override
+
+ void
+ onStartPlatformUnit() override
{
agentName = robotStateComponent->getRobotName();
robotRootFrame = robotStateComponent->getSynchronizedRobot()->getRootNode()->getName();
}
- void onExitPlatformUnit() override {}
+
+ void
+ onExitPlatformUnit() override
+ {
+ }
+
void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override;
NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr pt;
@@ -77,7 +100,6 @@ namespace armarx
std::size_t platformSensorIndex;
protected:
-
mutable std::mutex dataMutex;
Ice::Float maxVLin = std::numeric_limits<Ice::Float>::infinity();
@@ -85,10 +107,7 @@ namespace armarx
private:
-
std::string agentName;
std::string robotRootFrame;
-
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
index 8d1d4db3501edd1e6198c8bf9d4457aef9bb657c..3d4aae806be9068fdba2016b8200c343c130f4cb 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
@@ -25,15 +25,16 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include "../util.h"
-#include "../util/JointAndNJointControllers.h"
#include "../util/ControlThreadOutputBuffer.h"
+#include "../util/JointAndNJointControllers.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnitSubUnit);
- class RobotUnitSubUnit: public virtual ManagedIceObject
+
+ class RobotUnitSubUnit : public virtual ManagedIceObject
{
public:
virtual void update(const SensorAndControl& sc, const JointAndNJointControllers& c) = 0;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index 67b7eec6c4b3aa1dec80caa3efc7108adbaef84c..1f4c16dc051771c747448b2536968fda71620748 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -22,26 +22,29 @@
#include "TCPControllerSubUnit.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
using namespace armarx;
-void TCPControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
+void
+TCPControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
{
-
}
-PropertyDefinitionsPtr TCPControllerSubUnit::createPropertyDefinitions()
+PropertyDefinitionsPtr
+TCPControllerSubUnit::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new TCPControllerSubUnitPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new TCPControllerSubUnitPropertyDefinitions(getConfigIdentifier()));
}
-void TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
+void
+TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
{
ARMARX_CHECK_EXPRESSION(robot);
ARMARX_CHECK_EXPRESSION(!this->coordinateTransformationRobot);
@@ -52,15 +55,23 @@ void TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
robotUnit = rUnit;
}
-void TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
+void
+TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
{
- ARMARX_WARNING << deactivateSpam() << "Setting cycle time in RT-Controller does not have an effect";
+ ARMARX_WARNING << deactivateSpam()
+ << "Setting cycle time in RT-Controller does not have an effect";
}
-void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const FramedDirectionBasePtr& translationVelocity, const FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c)
+void
+TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName,
+ const std::string& tcpName,
+ const FramedDirectionBasePtr& translationVelocity,
+ const FramedDirectionBasePtr& orientationVelocityRPY,
+ const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
- ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName)) << "The robot does not have the node set: " + nodeSetName;
+ ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName))
+ << "The robot does not have the node set: " + nodeSetName;
std::string tcp;
if (tcpName.empty())
{
@@ -68,7 +79,8 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
}
else
{
- ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNode(tcpName)) << "The robot does not have the node: " + tcpName;
+ ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNode(tcpName))
+ << "The robot does not have the node: " + tcpName;
tcp = tcpName;
}
@@ -85,7 +97,8 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
FramedDirectionPtr globalTransVel = FramedDirectionPtr::dynamicCast(translationVelocity);
if (globalTransVel)
{
- globalTransVel->changeFrame(coordinateTransformationRobot, coordinateTransformationRobot->getRootNode()->getName());
+ globalTransVel->changeFrame(coordinateTransformationRobot,
+ coordinateTransformationRobot->getRootNode()->getName());
xVel = globalTransVel->x;
yVel = globalTransVel->y;
zVel = globalTransVel->z;
@@ -94,11 +107,11 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
FramedDirectionPtr globalOriVel = FramedDirectionPtr::dynamicCast(orientationVelocityRPY);
if (globalOriVel)
{
- globalOriVel->changeFrame(coordinateTransformationRobot, coordinateTransformationRobot->getRootNode()->getName());
+ globalOriVel->changeFrame(coordinateTransformationRobot,
+ coordinateTransformationRobot->getRootNode()->getName());
rollVel = globalOriVel->x;
pitchVel = globalOriVel->y;
yawVel = globalOriVel->z;
-
}
CartesianSelectionMode::CartesianSelection mode = CartesianSelectionMode::eAll;
@@ -120,7 +133,8 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
noMode = true;
}
ARMARX_DEBUG << "CartesianSelection-Mode: " << (int)mode;
- auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName + "_mode_" + std::to_string((int)mode);
+ auto controllerName =
+ this->getName() + "_" + tcp + "_" + nodeSetName + "_mode_" + std::to_string((int)mode);
auto NJointControllers = robotUnit->getNJointControllerNames();
NJointCartesianVelocityControllerWithRampPtr tcpController;
bool nodeSetAlreadyControlled = false;
@@ -141,7 +155,8 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
{
continue;
}
- if (tcpController->getNodeSetName() == nodeSetName && tcpController->getInstanceName() == controllerName)
+ if (tcpController->getNodeSetName() == nodeSetName &&
+ tcpController->getInstanceName() == controllerName)
{
nodeSetAlreadyControlled = true;
break;
@@ -150,22 +165,25 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
if (!nodeSetAlreadyControlled)
{
- NJointCartesianVelocityControllerWithRampConfigPtr config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, tcp, mode, 500, 1, 2, 0.3f, 2);
+ NJointCartesianVelocityControllerWithRampConfigPtr config =
+ new NJointCartesianVelocityControllerWithRampConfig(
+ nodeSetName, tcp, mode, 500, 1, 2, 0.3f, 2);
// NJointCartesianVelocityControllerConfigPtr config = new NJointCartesianVelocityControllerConfig(nodeSetName, tcp, mode);
- NJointCartesianVelocityControllerWithRampPtr ctrl = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(
- robotUnit->createNJointController(
- "NJointCartesianVelocityControllerWithRamp", controllerName,
- config, true, true
- )
- );
+ NJointCartesianVelocityControllerWithRampPtr ctrl =
+ NJointCartesianVelocityControllerWithRampPtr::dynamicCast(
+ robotUnit->createNJointController("NJointCartesianVelocityControllerWithRamp",
+ controllerName,
+ config,
+ true,
+ true));
tcpController = ctrl;
tcpControllerNameMap[nodeSetName] = controllerName;
- tcpController->setKpJointLimitAvoidance(getProperty<float>("AvoidJointLimitsKp").getValue(), c);
+ tcpController->setKpJointLimitAvoidance(getProperty<float>("AvoidJointLimitsKp").getValue(),
+ c);
}
-
// Only activate controller if at least either translationVelocity or orientaionVelocity is set
if (!noMode)
{
@@ -184,7 +202,8 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
}
}
-bool TCPControllerSubUnit::isRequested(const Ice::Current&)
+bool
+TCPControllerSubUnit::isRequested(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
for (auto& pair : tcpControllerNameMap)
@@ -198,15 +217,19 @@ bool TCPControllerSubUnit::isRequested(const Ice::Current&)
return false;
}
-void armarx::TCPControllerSubUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+void
+armarx::TCPControllerSubUnit::componentPropertiesUpdated(
+ const std::set<std::string>& changedProperties)
{
if (changedProperties.count("AvoidJointLimitsKp") && robotUnit)
{
float avoidJointLimitsKp = getProperty<float>("AvoidJointLimitsKp").getValue();
- auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
+ auto activeNJointControllers =
+ robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
for (NJointControllerBasePtr controller : activeNJointControllers)
{
- auto tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
+ auto tcpController =
+ NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
if (tcpController)
{
tcpController->setKpJointLimitAvoidance(avoidJointLimitsKp, Ice::emptyCurrent);
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
index b952748176b9940a8c5529775faaadb22ce574ab..3de0d7d81f61c7f8ba469b0aa964973d5c4d37b1 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -22,30 +22,36 @@
#pragma once
+#include <mutex>
+
+#include <ArmarXCore/core/Component.h>
+
#include <RobotAPI/interface/units/TCPControlUnit.h>
-#include "RobotUnitSubUnit.h"
#include "../NJointControllers/NJointTCPController.h"
#include "../util.h"
-
-#include <ArmarXCore/core/Component.h>
-
-#include <mutex>
+#include "RobotUnitSubUnit.h"
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnit);
+
class TCPControllerSubUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- TCPControllerSubUnitPropertyDefinitions(std::string prefix):
+ TCPControllerSubUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<float>("AvoidJointLimitsKp", 1.f, "Proportional gain value of P-Controller for joint limit avoidance", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "AvoidJointLimitsKp",
+ 1.f,
+ "Proportional gain value of P-Controller for joint limit avoidance",
+ PropertyDefinitionBase::eModifiable);
}
};
TYPEDEF_PTRS_HANDLE(TCPControllerSubUnit);
+
class TCPControllerSubUnit :
virtual public RobotUnitSubUnit,
virtual public TCPControlUnitInterface,
@@ -55,12 +61,18 @@ namespace armarx
void setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot);
// TCPControlUnitInterface interface
- void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::emptyCurrent) override;
- void setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const::armarx::FramedDirectionBasePtr& translationVelocity, const::armarx::FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setCycleTime(Ice::Int milliseconds,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void setTCPVelocity(const std::string& nodeSetName,
+ const std::string& tcpName,
+ const ::armarx::FramedDirectionBasePtr& translationVelocity,
+ const ::armarx::FramedDirectionBasePtr& orientationVelocityRPY,
+ const Ice::Current& c = Ice::emptyCurrent) override;
bool isRequested(const Ice::Current& = Ice::emptyCurrent) override;
// RobotUnitSubUnit interface
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
+
private:
mutable std::mutex dataMutex;
RobotUnit* robotUnit = nullptr;
@@ -69,48 +81,109 @@ namespace armarx
// ManagedIceObject interface
protected:
- void onInitComponent() override {}
- void onConnectComponent() override {}
- std::string getDefaultName() const override
+ void
+ onInitComponent() override
{
- return "TCPControlUnit";
}
+ void
+ onConnectComponent() override
+ {
+ }
+
+ std::string
+ getDefaultName() const override
+ {
+ return "TCPControlUnit";
+ }
PropertyDefinitionsPtr createPropertyDefinitions() override;
// UnitResourceManagementInterface interface
public:
- void request(const Ice::Current&) override
+ void
+ request(const Ice::Current&) override
{
}
- void release(const Ice::Current&) override
+
+ void
+ release(const Ice::Current&) override
{
}
// UnitExecutionManagementInterface interface
public:
- void init(const Ice::Current&) override {}
- void start(const Ice::Current&) override {}
- void stop(const Ice::Current&) override {}
- UnitExecutionState getExecutionState(const Ice::Current&) override
+ void
+ init(const Ice::Current&) override
+ {
+ }
+
+ void
+ start(const Ice::Current&) override
+ {
+ }
+
+ void
+ stop(const Ice::Current&) override
+ {
+ }
+
+ UnitExecutionState
+ getExecutionState(const Ice::Current&) override
{
return UnitExecutionState::eUndefinedUnitExecutionState;
}
// KinematicUnitListener interface
public:
- void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override {}
+ void
+ reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
// PropertyUser interface
public:
void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
index e22d0319c205a8665bb23222f692b1832b417b41..fa79ff88f5f8e52d25289868d91a7d9194f2d5f5 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
@@ -24,7 +24,8 @@
using namespace armarx;
-void TrajectoryControllerSubUnit::onInitComponent()
+void
+TrajectoryControllerSubUnit::onInitComponent()
{
kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
usingProxy(kinematicUnitName);
@@ -34,23 +35,27 @@ void TrajectoryControllerSubUnit::onInitComponent()
kp = getProperty<float>("Kp").getValue();
ki = getProperty<float>("Ki").getValue();
kd = getProperty<float>("Kd").getValue();
- maxVelocity = getProperty<float>("absMaximumVelocity").getValue() / 180 * M_PI; // config expects value in rad/sec
+ maxVelocity = getProperty<float>("absMaximumVelocity").getValue() / 180 *
+ M_PI; // config expects value in rad/sec
offeringTopic("DebugDrawerUpdates");
}
-void TrajectoryControllerSubUnit::onConnectComponent()
+void
+TrajectoryControllerSubUnit::onConnectComponent()
{
kinematicUnit = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
}
-void TrajectoryControllerSubUnit::onDisconnectComponent()
+void
+TrajectoryControllerSubUnit::onDisconnectComponent()
{
debugDrawer->clearLayer("Preview");
}
-bool TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
+bool
+TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
{
ARMARX_DEBUG << "Starting TrajectoryPlayer ...";
@@ -65,17 +70,28 @@ bool TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
{
std::string fileName = kinematicUnit->getRobotFilename();
ARMARX_INFO << "robot file name : " << fileName;
- debugDrawer->setRobotVisu("Preview", "previewRobot", fileName, fileName.substr(0, fileName.find("/")), armarx::CollisionModel);
- debugDrawer->updateRobotColor("Preview", "previewRobot", DrawColor {0, 1, 0, 0.5});
-
- previewTask = new PeriodicTask<TrajectoryControllerSubUnit>(this, &TrajectoryControllerSubUnit::previewRun, 20, false, "TrajectoryControllerSubUnitPreviewTask", false);
+ debugDrawer->setRobotVisu("Preview",
+ "previewRobot",
+ fileName,
+ fileName.substr(0, fileName.find("/")),
+ armarx::CollisionModel);
+ debugDrawer->updateRobotColor("Preview", "previewRobot", DrawColor{0, 1, 0, 0.5});
+
+ previewTask =
+ new PeriodicTask<TrajectoryControllerSubUnit>(this,
+ &TrajectoryControllerSubUnit::previewRun,
+ 20,
+ false,
+ "TrajectoryControllerSubUnitPreviewTask",
+ false);
previewTask->start();
}
return true;
}
-bool TrajectoryControllerSubUnit::pauseTrajectoryPlayer(const Ice::Current& c)
+bool
+TrajectoryControllerSubUnit::pauseTrajectoryPlayer(const Ice::Current& c)
{
if (controllerExists())
{
@@ -93,7 +109,8 @@ bool TrajectoryControllerSubUnit::pauseTrajectoryPlayer(const Ice::Current& c)
return true;
}
-bool TrajectoryControllerSubUnit::stopTrajectoryPlayer(const Ice::Current& c)
+bool
+TrajectoryControllerSubUnit::stopTrajectoryPlayer(const Ice::Current& c)
{
ARMARX_DEBUG << "Stopping TrajectoryPlayer ...";
@@ -106,20 +123,25 @@ bool TrajectoryControllerSubUnit::stopTrajectoryPlayer(const Ice::Current& c)
}
jointTrajController->deactivateController();
- while (jointTrajController->isControllerActive()) {}
+ while (jointTrajController->isControllerActive())
+ {
+ }
jointTrajController->deleteController();
}
return true;
}
-bool TrajectoryControllerSubUnit::resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current& c)
+bool
+TrajectoryControllerSubUnit::resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current& c)
{
ARMARX_DEBUG << "Resetting TrajectoryPlayer ...";
if (controllerExists() && jointTrajController->isControllerActive())
{
jointTrajController->deactivateController();
- while (jointTrajController->isControllerActive()) {}
+ while (jointTrajController->isControllerActive())
+ {
+ }
}
if (!jointTraj)
@@ -133,7 +155,7 @@ bool TrajectoryControllerSubUnit::resetTrajectoryPlayer(bool moveToFrameZeroPose
jointTrajController->setTrajectory(this->jointTraj, c);
if (moveToFrameZeroPose)
{
- std::vector<Ice::DoubleSeq > states = jointTraj->getAllStates(0.0f, 1);
+ std::vector<Ice::DoubleSeq> states = jointTraj->getAllStates(0.0f, 1);
NameValueMap frameZeroPositions;
NameControlModeMap controlModes;
@@ -154,7 +176,9 @@ bool TrajectoryControllerSubUnit::resetTrajectoryPlayer(bool moveToFrameZeroPose
return true;
}
-void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTraj, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTraj,
+ const Ice::Current& c)
{
ARMARX_DEBUG << "Loading Trajectory ...";
@@ -194,7 +218,8 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
if (usedJoints.size() != this->jointTraj->dim())
{
- ARMARX_ERROR << "Not all trajectory dimensions are named !!! (would cause problems when using kinematicUnit)";
+ ARMARX_ERROR << "Not all trajectory dimensions are named !!! (would cause problems when "
+ "using kinematicUnit)";
return;
}
@@ -208,12 +233,15 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
trajEndTime = endTime;
}
-void TrajectoryControllerSubUnit::loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj,
+ const Ice::Current& c)
{
this->basePoseTraj = TrajectoryPtr::dynamicCast(basePoseTraj);
}
-void TrajectoryControllerSubUnit::setLoopPlayback(bool loop, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::setLoopPlayback(bool loop, const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
@@ -225,17 +253,20 @@ void TrajectoryControllerSubUnit::setLoopPlayback(bool loop, const Ice::Current&
jointTrajController->setLooping(loop);
}
-double TrajectoryControllerSubUnit::getEndTime(const Ice::Current& c)
+double
+TrajectoryControllerSubUnit::getEndTime(const Ice::Current& c)
{
return endTime;
}
-double TrajectoryControllerSubUnit::getTrajEndTime(const Ice::Current& c)
+double
+TrajectoryControllerSubUnit::getTrajEndTime(const Ice::Current& c)
{
return trajEndTime;
}
-double TrajectoryControllerSubUnit::getCurrentTime(const Ice::Current& c)
+double
+TrajectoryControllerSubUnit::getCurrentTime(const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
@@ -247,7 +278,8 @@ double TrajectoryControllerSubUnit::getCurrentTime(const Ice::Current& c)
return jointTrajController->getCurrentTrajTime();
}
-void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current& c)
{
ARMARX_DEBUG << "Setting end-time ...";
@@ -262,7 +294,8 @@ void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current
if (jointTrajController->isControllerActive())
{
- ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting a new end-time.";
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting a "
+ "new end-time.";
return;
}
@@ -286,11 +319,13 @@ void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current
considerConstraintsForTrajectoryOptimization = b;
}
-void TrajectoryControllerSubUnit::setIsPreview(bool isPreview, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::setIsPreview(bool isPreview, const Ice::Current& c)
{
if (controllerExists() && jointTrajController->isControllerActive())
{
- ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting whether the controller is only for preview.";
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting "
+ "whether the controller is only for preview.";
return;
}
this->isPreview = isPreview;
@@ -302,7 +337,10 @@ void TrajectoryControllerSubUnit::setIsPreview(bool isPreview, const Ice::Curren
}
}
-bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, bool isInUse, const Ice::Current& c)
+bool
+TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName,
+ bool isInUse,
+ const Ice::Current& c)
{
ARMARX_DEBUG << "Setting joints in use ...";
@@ -310,11 +348,13 @@ bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, b
if (controllerExists())
{
- ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting joints in use.";
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting "
+ "joints in use.";
}
else
{
- if (isInUse && (std::find(usedJoints.begin(), usedJoints.end(), jointName) == usedJoints.end()))
+ if (isInUse &&
+ (std::find(usedJoints.begin(), usedJoints.end(), jointName) == usedJoints.end()))
{
usedJoints.push_back(jointName);
}
@@ -338,21 +378,24 @@ bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, b
return isInUse;
}
-void TrajectoryControllerSubUnit::enableRobotPoseUnit(bool isRobotPose, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::enableRobotPoseUnit(bool isRobotPose, const Ice::Current& c)
{
ARMARX_WARNING << "NYI : TrajectoryControllerSubUnit::enableRobotPoseUnit()";
}
-void TrajectoryControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
+void
+TrajectoryControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
{
-
}
-void TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::Current& c)
+void
+TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::Current& c)
{
if (controllerExists() && jointTrajController->isControllerActive())
{
- ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting whether constraints should be considered.";
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting "
+ "whether constraints should be considered.";
return;
}
considerConstraintsForTrajectoryOptimization = consider;
@@ -364,7 +407,9 @@ void TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::
}
}
-NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist)
+NJointTrajectoryControllerPtr
+TrajectoryControllerSubUnit::createTrajectoryController(std::vector<std::string>& jointNames,
+ bool deleteIfAlreadyExist)
{
std::unique_lock lock(controllerMutex);
@@ -396,12 +441,9 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
config->considerConstraints = considerConstraintsForTrajectoryOptimization;
config->isPreview = isPreview;
controllerName = this->getName() + "_" + "JointTrajectory" + IceUtil::generateUUID();
- trajController = NJointTrajectoryControllerPtr::dynamicCast(
- robotUnit->createNJointController(
- "NJointTrajectoryController", controllerName,
- config, true, true
- )
- );
+ trajController =
+ NJointTrajectoryControllerPtr::dynamicCast(robotUnit->createNJointController(
+ "NJointTrajectoryController", controllerName, config, true, true));
while (!getArmarXManager()->getIceManager()->isObjectReachable(controllerName))
{
@@ -411,7 +453,8 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
return trajController;
}
-void TrajectoryControllerSubUnit::assureTrajectoryController()
+void
+TrajectoryControllerSubUnit::assureTrajectoryController()
{
std::unique_lock lock(controllerMutex);
@@ -422,11 +465,13 @@ void TrajectoryControllerSubUnit::assureTrajectoryController()
ARMARX_CHECK_EXPRESSION(jointTrajController);
}
-bool TrajectoryControllerSubUnit::controllerExists()
+bool
+TrajectoryControllerSubUnit::controllerExists()
{
std::unique_lock lock(controllerMutex);
- auto allNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
+ auto allNJointControllers =
+ robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
NJointTrajectoryControllerPtr trajController;
for (const auto& controller : allNJointControllers)
{
@@ -444,13 +489,15 @@ bool TrajectoryControllerSubUnit::controllerExists()
return false;
}
-void TrajectoryControllerSubUnit::previewRun()
+void
+TrajectoryControllerSubUnit::previewRun()
{
if (controllerExists())
{
if (jointTrajController->isControllerActive())
{
- std::vector<Ice::DoubleSeq > states = jointTraj->getAllStates(jointTrajController->getCurrentTrajTime(), 1);
+ std::vector<Ice::DoubleSeq> states =
+ jointTraj->getAllStates(jointTrajController->getCurrentTrajTime(), 1);
NameValueMap targetPositionValues;
auto dimNames = jointTraj->getDimensionNames();
@@ -467,15 +514,17 @@ void TrajectoryControllerSubUnit::previewRun()
}
}
-void TrajectoryControllerSubUnit::setup(RobotUnit* rUnit)
+void
+TrajectoryControllerSubUnit::setup(RobotUnit* rUnit)
{
ARMARX_CHECK_EXPRESSION(!robotUnit);
ARMARX_CHECK_EXPRESSION(rUnit);
robotUnit = rUnit;
}
-
-void armarx::TrajectoryControllerSubUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+void
+armarx::TrajectoryControllerSubUnit::componentPropertiesUpdated(
+ const std::set<std::string>& changedProperties)
{
ARMARX_INFO << "Changning properties";
if (changedProperties.count("Kp"))
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
index 9f48b80609903888caf23163cdefeb1c801ddf74..62c8d33851642c8f462a54526469f05b1340e35e 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -22,39 +22,61 @@
#pragma once
-#include "RobotUnitSubUnit.h"
+#include <mutex>
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+
#include "../NJointControllers/NJointTrajectoryController.h"
#include "../RobotUnit.h"
-
#include "KinematicSubUnit.h"
-
-#include <mutex>
+#include "RobotUnitSubUnit.h"
namespace armarx
{
- class TrajectoryControllerSubUnitPropertyDefinitions:
+ class TrajectoryControllerSubUnitPropertyDefinitions :
public armarx::ComponentPropertyDefinitions
{
public:
- TrajectoryControllerSubUnitPropertyDefinitions(std::string prefix):
+ TrajectoryControllerSubUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit. Only needed for returning to zeroFramePose while resetting.");
- defineOptionalProperty<float>("Kp", 1.f, "Proportional gain value of PID Controller", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("Ki", 0.0f, "Integral gain value of PID Controller", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("Kd", 0.0f, "Differential gain value of PID Controller", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("absMaximumVelocity", 80.0f, "The PID will never set a velocity above this threshold (degree/s)");
- defineOptionalProperty<std::string>("CustomRootNode", "", "If this value is set, the root pose of the motion is set for this node instead of the root joint.");
-
- defineOptionalProperty<bool>("EnableRobotPoseUnit", false, "Specify whether RobotPoseUnit should be used to move the robot's position. Only useful in simulation.");
- defineOptionalProperty<std::string>("RobotPoseUnitName", "RobotPoseUnit", "Name of the RobotPoseUnit to which the robot position should be sent");
+ defineRequiredProperty<std::string>("KinematicUnitName",
+ "Name of the KinematicUnit. Only needed for "
+ "returning to zeroFramePose while resetting.");
+ defineOptionalProperty<float>("Kp",
+ 1.f,
+ "Proportional gain value of PID Controller",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("Ki",
+ 0.0f,
+ "Integral gain value of PID Controller",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("Kd",
+ 0.0f,
+ "Differential gain value of PID Controller",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "absMaximumVelocity",
+ 80.0f,
+ "The PID will never set a velocity above this threshold (degree/s)");
+ defineOptionalProperty<std::string>("CustomRootNode",
+ "",
+ "If this value is set, the root pose of the motion "
+ "is set for this node instead of the root joint.");
+
+ defineOptionalProperty<bool>("EnableRobotPoseUnit",
+ false,
+ "Specify whether RobotPoseUnit should be used to move the "
+ "robot's position. Only useful in simulation.");
+ defineOptionalProperty<std::string>(
+ "RobotPoseUnitName",
+ "RobotPoseUnit",
+ "Name of the RobotPoseUnit to which the robot position should be sent");
}
};
-
TYPEDEF_PTRS_HANDLE(TrajectoryControllerSubUnit);
+
class TrajectoryControllerSubUnit :
virtual public RobotUnitSubUnit,
virtual public TrajectoryPlayerInterface,
@@ -65,10 +87,13 @@ namespace armarx
bool startTrajectoryPlayer(const Ice::Current& = Ice::emptyCurrent) override;
bool pauseTrajectoryPlayer(const Ice::Current& = Ice::emptyCurrent) override;
bool stopTrajectoryPlayer(const Ice::Current& = Ice::emptyCurrent) override;
- bool resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current& = Ice::emptyCurrent) override;
+ bool resetTrajectoryPlayer(bool moveToFrameZeroPose,
+ const Ice::Current& = Ice::emptyCurrent) override;
- void loadJointTraj(const TrajectoryBasePtr& jointTraj, const Ice::Current& = Ice::emptyCurrent) override;
- void loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj, const Ice::Current& = Ice::emptyCurrent) override;
+ void loadJointTraj(const TrajectoryBasePtr& jointTraj,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj,
+ const Ice::Current& = Ice::emptyCurrent) override;
void setLoopPlayback(bool loop, const Ice::Current& = Ice::emptyCurrent) override;
Ice::Double getEndTime(const Ice::Current& = Ice::emptyCurrent) override;
@@ -77,10 +102,14 @@ namespace armarx
void setEndTime(Ice::Double, const Ice::Current& = Ice::emptyCurrent) override;
// Within the RobotUnit the NJointTrajectoryController is always in VelocityControl
- void setIsVelocityControl(bool, const Ice::Current& = Ice::emptyCurrent) override {}
+ void
+ setIsVelocityControl(bool, const Ice::Current& = Ice::emptyCurrent) override
+ {
+ }
void setIsPreview(bool, const Ice::Current& = Ice::emptyCurrent) override;
- bool setJointsInUse(const std::string&, bool, const Ice::Current& = Ice::emptyCurrent) override;
+ bool
+ setJointsInUse(const std::string&, bool, const Ice::Current& = Ice::emptyCurrent) override;
void enableRobotPoseUnit(bool, const Ice::Current& = Ice::emptyCurrent) override;
void considerConstraints(bool, const Ice::Current& = Ice::emptyCurrent) override;
@@ -91,39 +120,82 @@ namespace armarx
void setup(RobotUnit* rUnit);
// KinematicUnitListener interface
- void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override {}
+ void
+ reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
- void setOffset(const ::Eigen::Matrix4f&, const ::Ice::Current& = ::Ice::emptyCurrent) override {}
+ void
+ reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ setOffset(const ::Eigen::Matrix4f&, const ::Ice::Current& = ::Ice::emptyCurrent) override
+ {
+ }
// ManagedIceObject interface
protected:
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
- std::string getDefaultName() const override
+
+ std::string
+ getDefaultName() const override
{
return "TrajectoryPlayer";
}
+
/**
* @see PropertyUser::createPropertyDefinitions()
*/
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return armarx::PropertyDefinitionsPtr
- {
- new TrajectoryControllerSubUnitPropertyDefinitions{getConfigIdentifier()}
- };
+ return armarx::PropertyDefinitionsPtr{
+ new TrajectoryControllerSubUnitPropertyDefinitions{getConfigIdentifier()}};
}
private:
- NJointTrajectoryControllerPtr createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist);
+ NJointTrajectoryControllerPtr
+ createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist);
void assureTrajectoryController();
bool controllerExists();
void previewRun();
@@ -163,6 +235,7 @@ namespace armarx
// Component interface
public:
- virtual void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
+ virtual void
+ componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
index 23101df79925c5bcca01d070d043f91bcf2e1a43..f02cd9172e2d368cf3877d92e60cdb736000ba3a 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
@@ -23,15 +23,19 @@
#define BOOST_TEST_MODULE RobotAPI::BasicControllers::Test
#define ARMARX_BOOST_TEST
#define CREATE_LOGFILES
-#include <random>
-#include <iostream>
#include <filesystem>
-#include "../util/CtrlUtil.h"
+#include <iostream>
+#include <random>
+
#include <boost/algorithm/clamp.hpp>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/algorithm.h>
+
#include <RobotAPI/Test.h>
+
#include "../BasicControllers.h"
+#include "../util/CtrlUtil.h"
using namespace armarx;
//params for random tests
const std::size_t tries = 1;
@@ -41,17 +45,16 @@ const std::size_t ticks = 5000; // each tick is 0.75 to 1.25 ms
#define LOG_CONTROLLER_DATA_WRITE_TO(name) change_logging_file(name)
#define LOG_CONTROLLER_DATA(...) f << __VA_ARGS__ << "\n"
-#include <boost/filesystem.hpp>
#include <fstream>
-static const std::filesystem::path fpath
-{
- "controller_logfiles/"
-};
+#include <boost/filesystem.hpp>
+
+static const std::filesystem::path fpath{"controller_logfiles/"};
static std::ofstream f;
static bool isSetup = false;
-void change_logging_file(const std::string& name)
+void
+change_logging_file(const std::string& name)
{
if (f.is_open())
{
@@ -73,19 +76,26 @@ void change_logging_file(const std::string& name)
}
#else
-#define LOG_CONTROLLER_DATA_WRITE_TO(name) do{}while(0)
-#define LOG_CONTROLLER_DATA(...) do{}while(0)
+#define LOG_CONTROLLER_DATA_WRITE_TO(name) \
+ do \
+ { \
+ } while (0)
+#define LOG_CONTROLLER_DATA(...) \
+ do \
+ { \
+ } while (0)
#endif
-unsigned int getSeed()
+unsigned int
+getSeed()
{
- static const auto seed = std::random_device {}();
+ static const auto seed = std::random_device{}();
std::cout << "seed = " << seed << std::endl;
return seed;
}
-static std::mt19937 gen {getSeed()};
+static std::mt19937 gen{getSeed()};
struct Simulation
{
@@ -122,7 +132,8 @@ struct Simulation
std::uniform_real_distribution<double> pDist;
std::uniform_real_distribution<double> tDist;
- void reset()
+ void
+ reset()
{
time = 0;
@@ -143,22 +154,24 @@ struct Simulation
brakingDist = 0;
posAfterBraking = 0;
- vDist = std::uniform_real_distribution<double> { -maxvel, maxvel};
- aDist = std::uniform_real_distribution<double> {maxvel / 4, maxvel * 4 };
- tDist = std::uniform_real_distribution<double> {maxDt * 0.75f, maxDt * 1.25f};
- pDist = std::uniform_real_distribution<double> {posLoHard, posHiHard};
+ vDist = std::uniform_real_distribution<double>{-maxvel, maxvel};
+ aDist = std::uniform_real_distribution<double>{maxvel / 4, maxvel * 4};
+ tDist = std::uniform_real_distribution<double>{maxDt * 0.75f, maxDt * 1.25f};
+ pDist = std::uniform_real_distribution<double>{posLoHard, posHiHard};
}
//updating
- template<class FNC>
- void tick(FNC& callee)
+ template <class FNC>
+ void
+ tick(FNC& callee)
{
- dt = maxDt;//tDist(gen);
+ dt = maxDt; //tDist(gen);
callee();
log();
}
- void updateVel(double newvel)
+ void
+ updateVel(double newvel)
{
BOOST_CHECK(std::isfinite(newvel));
//save old
@@ -169,14 +182,16 @@ struct Simulation
//update
curvel = newvel;
curacc = (curvel - oldvel) / dt;
- jerk = (curacc - oldacc) / dt;
- curpos += ctrlutil::s(dt, 0, curvel, curacc, 0/*math::MathUtils::Sign(curacc-oldacc) * jerk*/);
+ jerk = (curacc - oldacc) / dt;
+ curpos +=
+ ctrlutil::s(dt, 0, curvel, curacc, 0 /*math::MathUtils::Sign(curacc-oldacc) * jerk*/);
time += dt;
brakingDist = brakingDistance(curvel, dec);
posAfterBraking = curpos + brakingDist;
}
- void updatePos(double newPos)
+ void
+ updatePos(double newPos)
{
BOOST_CHECK(std::isfinite(newPos));
//save old
@@ -187,7 +202,7 @@ struct Simulation
//update
curvel = (newPos - oldpos) / dt;
curacc = (curvel - oldvel) / dt;
- jerk = (curacc - oldacc) / dt;
+ jerk = (curacc - oldacc) / dt;
curpos = newPos;
time += dt;
brakingDist = brakingDistance(curvel, dec);
@@ -195,20 +210,23 @@ struct Simulation
}
//checks
- void checkVel()
+ void
+ checkVel()
{
//check v
BOOST_CHECK_LE(curvel, maxvel * 1.01);
BOOST_CHECK_LE(-maxvel * 1.01, curvel);
}
- void checkPos()
+ void
+ checkPos()
{
BOOST_CHECK_LE(curpos, posHiHard);
BOOST_CHECK_LE(posLoHard, curpos);
}
- void checkAcc()
+ void
+ checkAcc()
{
if (sign(curvel) != sign(oldvel))
{
@@ -221,8 +239,10 @@ struct Simulation
//we decelerated
if (!(curacc < dec * 1.01))
{
- std::cout << "Time[" << time << "] violated deceleration bound! vold " << oldvel << " / vnew " << curvel << " / dv " << std::abs(oldvel - curvel) << " / dt " << dt << " / dec " << dec
- << " / (targetv " << targvel << ")\n";
+ std::cout << "Time[" << time << "] violated deceleration bound! vold " << oldvel
+ << " / vnew " << curvel << " / dv " << std::abs(oldvel - curvel)
+ << " / dt " << dt << " / dec " << dec << " / (targetv " << targvel
+ << ")\n";
}
BOOST_CHECK_LE(curacc, dec * 1.01);
}
@@ -231,21 +251,27 @@ struct Simulation
//we accellerated
if (!(curacc < acc * 1.01))
{
- std::cout << "Time[" << time << "] violated deceleration bound! vold " << oldvel << " / vnew " << curvel << " / dv " << std::abs(oldvel - curvel) << " / dt " << dt << " / acc " << acc
- << " / (targetv " << targvel << ")\n";
+ std::cout << "Time[" << time << "] violated deceleration bound! vold " << oldvel
+ << " / vnew " << curvel << " / dv " << std::abs(oldvel - curvel)
+ << " / dt " << dt << " / acc " << acc << " / (targetv " << targvel
+ << ")\n";
}
BOOST_CHECK_LE(curacc, acc * 1.01);
}
}
//logging
- void writeHeader(const std::string& name)
+ void
+ writeHeader(const std::string& name)
{
LOG_CONTROLLER_DATA_WRITE_TO(name);
- LOG_CONTROLLER_DATA("time curpos targpos posHiHard posLoHard posHi posLo curvel targvel maxv curacc acc dec brakingDist posAfterBraking");
+ LOG_CONTROLLER_DATA("time curpos targpos posHiHard posLoHard posHi posLo curvel targvel "
+ "maxv curacc acc dec brakingDist posAfterBraking");
reset();
}
- void log()
+
+ void
+ log()
{
// //output a neg val for dec and a pos val for acc
// double outputacc;
@@ -263,11 +289,10 @@ struct Simulation
// outputacc *= -1;
// }
// }
- LOG_CONTROLLER_DATA(time << " " <<
- curpos << " " << targpos << " " << posHiHard << " " << posLoHard << " " << posHi << " " << posLo << " " <<
- curvel << " " << targvel << " " << maxvel << " " <<
- curacc << " " << acc << " " << -dec << " " <<
- brakingDist << " " << posAfterBraking);
+ LOG_CONTROLLER_DATA(time << " " << curpos << " " << targpos << " " << posHiHard << " "
+ << posLoHard << " " << posHi << " " << posLo << " " << curvel
+ << " " << targvel << " " << maxvel << " " << curacc << " " << acc
+ << " " << -dec << " " << brakingDist << " " << posAfterBraking);
}
};
@@ -279,8 +304,8 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationAndPositionBoundsTe
// s.posHi = 0;
// s.posLo = 0;
- auto distPosLo = std::uniform_real_distribution<double> { -M_PI, -M_PI_4};
- auto distPosHi = std::uniform_real_distribution<double> { M_PI_4, M_PI};
+ auto distPosLo = std::uniform_real_distribution<double>{-M_PI, -M_PI_4};
+ auto distPosHi = std::uniform_real_distribution<double>{M_PI_4, M_PI};
s.posLo = distPosLo(gen);
s.posHi = distPosHi(gen);
// double p = 20.5;
@@ -323,19 +348,20 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationAndPositionBoundsTe
};
std::cout << "random tests\n";
- for (std::size_t try_ = 0; try_ < tries; ++ try_)
+ for (std::size_t try_ = 0; try_ < tries; ++try_)
{
ctrl = VelocityControllerWithRampedAccelerationAndPositionBounds();
s.writeHeader("RampedAccPositionLimitedVelCtrl+acc_random_" + to_string(try_));
- s.maxvel = 5;//std::abs(s.vDist(gen)) + 1;
- s.curvel = 5;//armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
+ s.maxvel = 5; //std::abs(s.vDist(gen)) + 1;
+ s.curvel = 5; //armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
ctrl.currentV = s.curvel;
s.curpos = s.pDist(gen);
ctrl.currentAcc = s.pDist(gen);
s.curacc = ctrl.currentAcc;
- ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << VAROUT(s.targvel) << " " << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << VAROUT(s.targvel) << " " << VAROUT(s.acc)
+ << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
- ctrl.jerk = 100;//(random() % 100) + 10;
+ ctrl.jerk = 100; //(random() % 100) + 10;
s.log();
double stopTime = -1;
for (std::size_t tick = 0; tick < ticks; ++tick)
@@ -344,17 +370,23 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationAndPositionBoundsTe
// s.targpos = tick > 900 ? 3 : 5;
s.tick(testTick);
- ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos) << VAROUT(s.targvel) << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos)
+ << VAROUT(s.targvel) << VAROUT(s.curacc) << VAROUT(s.maxvel)
+ << VAROUT(s.curvel) << VAROUT(s.jerk);
if (std::abs(s.curvel - s.targvel) < 0.001 && stopTime < 0)
{
- stopTime = s.dt * (tick + 20); // let it run for some more ticks to make sure the velocity is stable
+ stopTime =
+ s.dt *
+ (tick +
+ 20); // let it run for some more ticks to make sure the velocity is stable
}
if (stopTime > 0 && s.dt * tick > stopTime)
{
break;
}
}
- BOOST_CHECK_LE(std::abs(s.curvel - s.targvel), 0.001); // allow error of 0.5729577951308232 deg
+ BOOST_CHECK_LE(std::abs(s.curvel - s.targvel),
+ 0.001); // allow error of 0.5729577951308232 deg
}
std::cout << "bounds tests\n";
std::cout << "TODO\n";
@@ -407,16 +439,17 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationTest)
};
std::cout << "random tests\n";
- for (std::size_t try_ = 0; try_ < tries; ++ try_)
+ for (std::size_t try_ = 0; try_ < tries; ++try_)
{
ctrl = VelocityControllerWithRampedAcceleration();
s.writeHeader("RampedAccVelCtrl+acc_random_" + to_string(try_));
s.maxvel = std::abs(s.vDist(gen)) + 1;
s.curvel = armarx::math::MathUtils::LimitTo(s.vDist(gen), s.maxvel);
ctrl.currentV = s.curvel;
- s.curpos = 1;//s.pDist(gen);
+ s.curpos = 1; //s.pDist(gen);
ctrl.currentAcc = s.pDist(gen);
- ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << VAROUT(s.targvel) << " " << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << VAROUT(s.targvel) << " " << VAROUT(s.acc)
+ << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
ctrl.jerk = (random() % 100) + 10;
s.log();
@@ -430,14 +463,18 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationTest)
// ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos) << VAROUT(s.targvel) << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
if (std::abs(s.curvel - s.targvel) < 0.001 && stopTime < 0)
{
- stopTime = s.dt * (tick + 20); // let it run for some more ticks to make sure the velocity is stable
+ stopTime =
+ s.dt *
+ (tick +
+ 20); // let it run for some more ticks to make sure the velocity is stable
}
if (stopTime > 0 && s.dt * tick > stopTime)
{
break;
}
}
- BOOST_CHECK_LE(std::abs(s.curvel - s.targvel), 0.001); // allow error of 0.5729577951308232 deg
+ BOOST_CHECK_LE(std::abs(s.curvel - s.targvel),
+ 0.001); // allow error of 0.5729577951308232 deg
}
std::cout << "bounds tests\n";
std::cout << "TODO\n";
@@ -446,7 +483,6 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationTest)
std::cout << "done positionThroughVelocityControlWithAccelerationBounds \n";
}
-
//BOOST_AUTO_TEST_CASE(velocityControlWithAccelerationBoundsTest)
//{
// std::cout << "starting velocityControlWithAccelerationBoundsTest \n";
@@ -696,7 +732,7 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
};
std::cout << "random tests\n";
- for (std::size_t try_ = 0; try_ < tries; ++ try_)
+ for (std::size_t try_ = 0; try_ < tries; ++try_)
{
ctrl = MinJerkPositionController();
s.writeHeader("minJerkPosCtrl+acc_random_" + to_string(try_));
@@ -706,8 +742,10 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
s.dec = s.aDist(gen);
ctrl.currentAcc = s.curacc;
ctrl.currentV = s.curvel;
- s.curpos = 1;//s.pDist(gen);
- ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << " " << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
+ s.curpos = 1; //s.pDist(gen);
+ ARMARX_INFO << VAROUT(s.dt) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << " "
+ << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel)
+ << VAROUT(s.jerk);
// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
s.log();
double stopTime = -1;
@@ -720,7 +758,10 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
// ARMARX_INFO << /*deactivateSpam(0.01) <<*/ VAROUT(s.dt * tick) << VAROUT(s.curpos) << "TargetPos: " << s.targpos << " " << VAROUT(s.curacc) << VAROUT(s.maxvel) << VAROUT(s.curvel) << VAROUT(s.jerk);
if (std::abs(s.curpos - s.targpos) < 0.01 && stopTime < 0)
{
- stopTime = s.dt * (tick + 20); // let it run for some more ticks to make sure the velocity is stable
+ stopTime =
+ s.dt *
+ (tick +
+ 20); // let it run for some more ticks to make sure the velocity is stable
ARMARX_INFO << VAROUT(s.time) << VAROUT(stopTime);
}
if (stopTime > 0 && s.dt * tick > stopTime)
@@ -728,7 +769,8 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
break;
}
}
- BOOST_CHECK_LE(std::abs(s.curpos - s.targpos), 0.01); // allow error of 0.5729577951308232 deg
+ BOOST_CHECK_LE(std::abs(s.curpos - s.targpos),
+ 0.01); // allow error of 0.5729577951308232 deg
}
std::cout << "bounds tests\n";
std::cout << "TODO\n";
@@ -737,8 +779,6 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
std::cout << "done positionThroughVelocityControlWithAccelerationBounds \n";
}
-
-
//BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
//{
// return;
@@ -947,4 +987,3 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
//// std::cout << "done positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition \n";
////}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
index ec56bf0511be737f3b46b48d2adc7c4b3a703a40..c34b618afcd0cf9b03c4fae663866a52a1a11752 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
@@ -23,14 +23,16 @@
#define BOOST_TEST_MODULE RobotAPI::BasicControllers::Test
#define ARMARX_BOOST_TEST
#define CREATE_LOGFILES
-#include <random>
#include <iostream>
+#include <random>
#include <boost/algorithm/clamp.hpp>
-#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/util/algorithm.h>
+
#include <RobotAPI/Test.h>
+
#include "../BasicControllers.h"
using namespace armarx;
//params for random tests
@@ -41,17 +43,16 @@ const std::size_t ticks = 20000; // each tick is 0.75 to 1.25 ms
#define LOG_CONTROLLER_DATA_WRITE_TO(name) change_logging_file(name)
#define LOG_CONTROLLER_DATA(...) f << __VA_ARGS__ << "\n"
-#include <boost/filesystem.hpp>
#include <fstream>
-static const boost::filesystem::path fpath
-{
- "controller_logfiles/"
-};
+#include <boost/filesystem.hpp>
+
+static const boost::filesystem::path fpath{"controller_logfiles/"};
static std::ofstream f;
static bool isSetup = false;
-void change_logging_file(const std::string& name)
+void
+change_logging_file(const std::string& name)
{
if (f.is_open())
{
@@ -170,19 +171,26 @@ void change_logging_file(const std::string& name)
}
#else
-#define LOG_CONTROLLER_DATA_WRITE_TO(name) do{}while(0)
-#define LOG_CONTROLLER_DATA(...) do{}while(0)
+#define LOG_CONTROLLER_DATA_WRITE_TO(name) \
+ do \
+ { \
+ } while (0)
+#define LOG_CONTROLLER_DATA(...) \
+ do \
+ { \
+ } while (0)
#endif
-unsigned int getSeed()
+unsigned int
+getSeed()
{
- static const auto seed = std::random_device {}();
+ static const auto seed = std::random_device{}();
std::cout << "seed = " << seed << std::endl;
return seed;
}
-static std::mt19937 gen {getSeed()};
+static std::mt19937 gen{getSeed()};
struct Simulation
{
@@ -217,7 +225,8 @@ struct Simulation
std::uniform_real_distribution<float> pDist;
std::uniform_real_distribution<float> tDist;
- void reset()
+ void
+ reset()
{
time = 0;
@@ -237,22 +246,24 @@ struct Simulation
brakingDist = 0;
posAfterBraking = 0;
- vDist = std::uniform_real_distribution<float> { -maxvel, maxvel};
- aDist = std::uniform_real_distribution<float> {maxvel, maxvel * 4 };
- tDist = std::uniform_real_distribution<float> {maxDt * 0.75f, maxDt * 1.25f};
- pDist = std::uniform_real_distribution<float> {posLoHard, posHiHard};
+ vDist = std::uniform_real_distribution<float>{-maxvel, maxvel};
+ aDist = std::uniform_real_distribution<float>{maxvel, maxvel * 4};
+ tDist = std::uniform_real_distribution<float>{maxDt * 0.75f, maxDt * 1.25f};
+ pDist = std::uniform_real_distribution<float>{posLoHard, posHiHard};
}
//updating
- template<class FNC>
- void tick(FNC& callee)
+ template <class FNC>
+ void
+ tick(FNC& callee)
{
dt = tDist(gen);
callee();
log();
}
- void updateVel(float newvel)
+ void
+ updateVel(float newvel)
{
BOOST_CHECK(std::isfinite(newvel));
//save old
@@ -270,20 +281,23 @@ struct Simulation
}
//checks
- void checkVel()
+ void
+ checkVel()
{
//check v
BOOST_CHECK_LE(curvel, maxvel * 1.01);
BOOST_CHECK_LE(-maxvel * 1.01, curvel);
}
- void checkPos()
+ void
+ checkPos()
{
BOOST_CHECK_LE(curpos, posHiHard);
BOOST_CHECK_LE(posLoHard, curpos);
}
- void checkAcc()
+ void
+ checkAcc()
{
if (sign(curvel) != sign(oldvel))
{
@@ -306,21 +320,27 @@ struct Simulation
//we accellerated
if (!(curacc < acc * 1.01))
{
- ARMARX_INFO << "Time[" << time << "] violated deceleration bound! vold " << oldvel << " / vnew " << curvel << " / dv " << std::abs(oldvel - curvel) << " / dt " << dt << " / acc " << acc
- << " / (targetv " << targvel << ")\n";
+ ARMARX_INFO << "Time[" << time << "] violated deceleration bound! vold " << oldvel
+ << " / vnew " << curvel << " / dv " << std::abs(oldvel - curvel)
+ << " / dt " << dt << " / acc " << acc << " / (targetv " << targvel
+ << ")\n";
}
BOOST_CHECK_LE(curacc, acc * 1.01);
}
}
//logging
- void writeHeader(const std::string& name)
+ void
+ writeHeader(const std::string& name)
{
LOG_CONTROLLER_DATA_WRITE_TO(name);
- LOG_CONTROLLER_DATA("time curpos targpos posHiHard posLoHard posHi posLo curvel targvel maxvel curacc acc dec brakingDist posAfterBraking");
+ LOG_CONTROLLER_DATA("time curpos targpos posHiHard posLoHard posHi posLo curvel targvel "
+ "maxvel curacc acc dec brakingDist posAfterBraking");
reset();
}
- void log()
+
+ void
+ log()
{
//output a neg val for dec and a pos val for acc
float outputacc;
@@ -338,16 +358,14 @@ struct Simulation
outputacc *= -1;
}
}
- LOG_CONTROLLER_DATA(time << " " <<
- curpos << " " << targpos << " " << posHiHard << " " << posLoHard << " " << posHi << " " << posLo << " " <<
- curvel << " " << targvel << " " << maxvel << " " <<
- outputacc << " " << acc << " " << -dec << " " <<
- brakingDist << " " << posAfterBraking);
+ LOG_CONTROLLER_DATA(time << " " << curpos << " " << targpos << " " << posHiHard << " "
+ << posLoHard << " " << posHi << " " << posLo << " " << curvel
+ << " " << targvel << " " << maxvel << " " << outputacc << " "
+ << acc << " " << -dec << " " << brakingDist << " "
+ << posAfterBraking);
}
};
-
-
BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
{
std::cout << "starting positionThroughVelocityControlWithAccelerationBounds \n";
@@ -390,7 +408,7 @@ BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
};
std::cout << "random tests\n";
- for (std::size_t try_ = 0; try_ < tries; ++ try_)
+ for (std::size_t try_ = 0; try_ < tries; ++try_)
{
s.writeHeader("posViaVelCtrl+acc_random_" + to_string(try_));
s.maxvel = std::abs(s.vDist(gen)) + 1;
@@ -401,7 +419,8 @@ BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
s.dec = s.aDist(gen);
//s.acc: 6.08172 s.dec: 7.98634 s.maxvel: 1.73312
//s.curpos: -1.90146 TargetPos: 1.2378s.acc: 5.93058 s.dec: 3.06262 s.maxvel: 2.94756
- ARMARX_INFO << VAROUT(s.curpos) << "TargetPos: " << s.targpos << VAROUT(s.acc) << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel);
+ ARMARX_INFO << VAROUT(s.curpos) << "TargetPos: " << s.targpos << VAROUT(s.acc)
+ << VAROUT(s.dec) << VAROUT(s.maxvel) << VAROUT(s.curvel);
// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
s.log();
for (std::size_t tick = 0; tick < ticks; ++tick)
@@ -440,9 +459,20 @@ BOOST_AUTO_TEST_CASE(timeEstimationTest)
float est = c.estimateTime();
ARMARX_INFO << "estimated time " << est;
float newDt = est + (float)(rand() % 100) / 10;
- findVelocityAndAccelerationForTimeAndDistance(c.targetPosition - c.currentPosition, c.currentV, c.maxV, c.deceleration,
- trapeze(c.currentV, c.acceleration, c.maxV, c.deceleration, 0, c.targetPosition - c.currentPosition),
- newDt, c.maxV, c.acceleration, c.deceleration);
+ findVelocityAndAccelerationForTimeAndDistance(c.targetPosition - c.currentPosition,
+ c.currentV,
+ c.maxV,
+ c.deceleration,
+ trapeze(c.currentV,
+ c.acceleration,
+ c.maxV,
+ c.deceleration,
+ 0,
+ c.targetPosition - c.currentPosition),
+ newDt,
+ c.maxV,
+ c.acceleration,
+ c.deceleration);
// c.maxV = 0.583;
// c.acceleration = 0.16666;
@@ -452,6 +482,4 @@ BOOST_AUTO_TEST_CASE(timeEstimationTest)
ARMARX_INFO << "desired time: " << newDt << " estimated time " << est;
BOOST_CHECK_CLOSE(est, newDt, 1);
}
-
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util.cpp b/source/RobotAPI/components/units/RobotUnit/util.cpp
index 903217aae47729f9a77a02ab1697c3bb727dd57b..ecae915c8f06bf7d0288c033cb3f18c305370ed9 100644
--- a/source/RobotAPI/components/units/RobotUnit/util.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util.cpp
@@ -20,4 +20,3 @@
* GNU General Public License
*/
#include "util.h"
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util.h b/source/RobotAPI/components/units/RobotUnit/util.h
index ef3e5c3267124949b39a931e7f1536659eda27e1..1ff7af0703435a2c67193a9dc388dce1adc98c7f 100644
--- a/source/RobotAPI/components/units/RobotUnit/util.h
+++ b/source/RobotAPI/components/units/RobotUnit/util.h
@@ -21,15 +21,17 @@
*/
#pragma once
-#include <ArmarXCore/core/util/PropagateConst.h>
+#include <type_traits>
+
#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/util/PropagateConst.h>
+#include "util/AtomicWrapper.h"
#include "util/EigenForwardDeclarations.h"
#include "util/KeyValueVector.h"
-#include "util/AtomicWrapper.h"
#include "util/Time.h"
#include "util/introspection/ClassMemberInfo.h"
-#include <type_traits>
-
-namespace armarx::DeviceTags {}
+namespace armarx::DeviceTags
+{
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h b/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h
index 639e9efb425ff7109ef25f01ff28eefa10fc5a53..c355cb162edd52b48cefab36106dd6bddbf6064b 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/AtomicWrapper.h
@@ -22,6 +22,7 @@
#pragma once
#include <atomic>
+
namespace armarx
{
template <typename T>
@@ -29,26 +30,45 @@ namespace armarx
{
std::atomic<T> atom;
- AtomicWrapper() : atom {} {}
- AtomicWrapper(const T& v) : atom {v} {}
- AtomicWrapper(const std::atomic<T>& a) : atom {a.load()} {}
- AtomicWrapper(const AtomicWrapper& other) : atom {other.atom.load()} {}
+ AtomicWrapper() : atom{}
+ {
+ }
+
+ AtomicWrapper(const T& v) : atom{v}
+ {
+ }
+
+ AtomicWrapper(const std::atomic<T>& a) : atom{a.load()}
+ {
+ }
+
+ AtomicWrapper(const AtomicWrapper& other) : atom{other.atom.load()}
+ {
+ }
+
AtomicWrapper(AtomicWrapper&&) = default;
- AtomicWrapper& operator=(const T& v)
+
+ AtomicWrapper&
+ operator=(const T& v)
{
atom.store(v);
return *this;
}
- AtomicWrapper& operator=(const std::atomic<T>& a)
+
+ AtomicWrapper&
+ operator=(const std::atomic<T>& a)
{
atom.store(a.load());
return *this;
}
- AtomicWrapper& operator=(const AtomicWrapper& other)
+
+ AtomicWrapper&
+ operator=(const AtomicWrapper& other)
{
atom.store(other.atom.load());
return *this;
}
+
AtomicWrapper& operator=(AtomicWrapper&&) = default;
operator T() const
@@ -56,4 +76,4 @@ namespace armarx
return atom.load();
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index 1327e8e353890c42fb2200c253e42ff03325be35..bedfc430f0dee9f2030ac495d2ba6dd74bfa1350 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -20,6 +20,7 @@
* GNU General Public License
*/
#include "ControlThreadOutputBuffer.h"
+
#include <memory>
#include <utility>
@@ -27,22 +28,25 @@ namespace armarx
{
detail::RtMessageLogEntryDummy detail::RtMessageLogEntryDummy::Instance;
detail::RtMessageLogEntryNull detail::RtMessageLogEntryNull::Instance;
- thread_local ControlThreadOutputBuffer* ControlThreadOutputBuffer::RtLoggingInstance {nullptr};
+ thread_local ControlThreadOutputBuffer* ControlThreadOutputBuffer::RtLoggingInstance{nullptr};
- ControlThreadOutputBuffer::Entry& ControlThreadOutputBuffer::getWriteBuffer()
+ ControlThreadOutputBuffer::Entry&
+ ControlThreadOutputBuffer::getWriteBuffer()
{
ARMARX_CHECK_EXPRESSION(isInitialized);
return entries.at(toBounds(writePosition));
}
- void ControlThreadOutputBuffer::commitWrite()
+ void
+ ControlThreadOutputBuffer::commitWrite()
{
ARMARX_CHECK_EXPRESSION(isInitialized);
getWriteBuffer().iteration = writePosition;
++writePosition;
}
- const ControlThreadOutputBuffer::Entry& ControlThreadOutputBuffer::getReadBuffer() const
+ const ControlThreadOutputBuffer::Entry&
+ ControlThreadOutputBuffer::getReadBuffer() const
{
ARMARX_CHECK_EXPRESSION(isInitialized);
if (!onePastReadPosition)
@@ -50,119 +54,156 @@ namespace armarx
//data is not initialized
return entries.back();
}
- return entries.at(toBounds(onePastReadPosition - 1));
+ return entries.at(toBounds(onePastReadPosition - 1));
}
- bool ControlThreadOutputBuffer::updateReadBuffer() const
+ bool
+ ControlThreadOutputBuffer::updateReadBuffer() const
{
ARMARX_CHECK_EXPRESSION(isInitialized);
- std::size_t writePosition = this->writePosition;
- if (onePastReadPosition == writePosition)
+ std::size_t localWritePosition = this->writePosition;
+ if (onePastReadPosition == localWritePosition)
{
//already up to date
return false;
}
- onePastReadPosition = writePosition;
+ onePastReadPosition = localWritePosition;
return true;
}
- void ControlThreadOutputBuffer::resetLoggingPosition() const
+ void
+ ControlThreadOutputBuffer::resetLoggingPosition() const
{
ARMARX_CHECK_EXPRESSION(isInitialized);
onePastLoggingReadPosition = writePosition.load();
}
- void ControlThreadOutputBuffer::foreachNewLoggingEntry(ConsumerFunctor consumer)
+ void
+ ControlThreadOutputBuffer::forEachNewLoggingEntry(ConsumerFunctor consumer)
{
ARMARX_TRACE;
- ARMARX_CHECK_EXPRESSION(isInitialized);
- if (writePosition - onePastLoggingReadPosition >= numEntries)
+ 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)
{
- ARMARX_ERROR << "There are " << writePosition - onePastLoggingReadPosition
- << " unlogged entries, but only the last " << numEntries << " are saved! "
- "There seems to be something wrong (e.g. the rt logging threw an exception, "
- "the system load is too high or the logging takes to long). "
- "The log position will be reset to the newest entry!";
+ ARMARX_VERBOSE
+ << "There are " << writePosition_local - onePastLoggingReadPosition
+ << " unlogged entries, but only the last " << numEntries
+ << " are saved! "
+ "There seems to be something wrong (e.g. the rt logging threw an exception, "
+ "the system load is too high or the logging takes to long). "
+ "The log position will be reset to the newest entry!";
resetLoggingPosition();
}
//the number of new entries
- auto numNewEntries = writePosition - onePastLoggingReadPosition;
+ auto numNewEntries = writePosition_local - onePastLoggingReadPosition;
+
if (numNewEntries >= numEntries)
{
- ARMARX_ERROR << " more new entries (" << numNewEntries << ") than space (" << numEntries << ") -> Skipping everything else";
+ ARMARX_VERBOSE << " more new entries (" << numNewEntries << ") than space ("
+ << numEntries << ") -> Skipping everything else";
return;
}
//consume all
- const std::size_t num = writePosition - onePastLoggingReadPosition;
- for (
- std::size_t offset = 0;
- onePastLoggingReadPosition < writePosition;
- ++onePastLoggingReadPosition, ++offset
- )
+ const std::size_t num = writePosition_local - onePastLoggingReadPosition;
+ ARMARX_DEBUG << num << " new entries to be treated";
+ for (std::size_t offset = 0; onePastLoggingReadPosition < writePosition_local;
+ ++onePastLoggingReadPosition, ++offset)
{
ARMARX_TRACE;
- detail::ControlThreadOutputBufferEntry& entry = entries.at(toBounds(onePastLoggingReadPosition));
+
+ detail::ControlThreadOutputBufferEntry& entry =
+ entries.at(toBounds(onePastLoggingReadPosition));
consumer(entry, offset, num);
entry.messages.reset(messageBufferSize, messageBufferEntries, entry.iteration);
//update how many
- auto newNumNewEntries = writePosition - onePastLoggingReadPosition;
+ auto newNumNewEntries = writePosition_local - onePastLoggingReadPosition;
if (newNumNewEntries * 2 > numEntries)
{
- ARMARX_WARNING << deactivateSpam(0.5)
- << "RT-Logging is slow! "
- << "The RT-Thread writes faster new data than the RT-Logging thread consumes it! "
- << " old/new/max number of entries: "
- << numNewEntries << " /" << newNumNewEntries << " / " << numEntries;
+ ARMARX_VERBOSE << deactivateSpam(5) << "RT-Logging is slow! "
+ << "The RT-Thread writes faster new data than the RT-Logging thread "
+ "consumes it! "
+ << " old/new/max number of entries: " << numNewEntries << " /"
+ << newNumNewEntries << " / " << numEntries;
}
numNewEntries = newNumNewEntries;
if (numNewEntries >= numEntries)
{
- ARMARX_ERROR << " more new entries (" << numNewEntries << ") than space (" << numEntries << ") -> Skipping everything else";
+ ARMARX_VERBOSE << " more new entries (" << numNewEntries << ") than space ("
+ << numEntries << ") -> Skipping everything else";
return;
}
}
-
}
-
- void ControlThreadOutputBuffer::forLatestLoggingEntry(ConsumerFunctor consumer)
+ void
+ ControlThreadOutputBuffer::forLatestLoggingEntry(ConsumerFunctor consumer,
+ size_t numberOfEntriesToLog)
{
ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(isInitialized);
- if (writePosition - onePastLoggingReadPosition >= numEntries)
+ size_t unloggedEntries = writePosition - onePastLoggingReadPosition;
+ if (unloggedEntries >= numEntries)
{
- ARMARX_ERROR << "There are " << writePosition - onePastLoggingReadPosition
- << " unlogged entries, but only the last " << numEntries << " are saved! "
- "There seems to be something wrong (e.g. the rt logging threw an exception, "
- "the system load is too high or the logging takes to long). "
- "The log position will be reset to the newest entry!";
+ ARMARX_VERBOSE
+ << "There are " << unloggedEntries << " unlogged entries, but only the last "
+ << numEntries
+ << " are saved! "
+ "There seems to be something wrong (e.g. the rt logging threw an exception, "
+ "the system load is too high or the logging takes to long). "
+ "The log position will be reset to the newest entry!";
resetLoggingPosition();
+ unloggedEntries = writePosition - onePastLoggingReadPosition;
}
+ if (unloggedEntries > numberOfEntriesToLog)
+ {
+ ARMARX_DEBUG << "not logging all " << unloggedEntries << ". Only the last "
+ << numberOfEntriesToLog << " will be used.";
- // skip all messages except the latest
- onePastLoggingReadPosition = writePosition.load() - 1;
+ const std::size_t onePastLoggingReadPositionLastRead =
+ onePastLoggingReadPosition.load();
- // use already existing impl.
- foreachNewLoggingEntry(std::move(consumer));
+ // skip all messages except the latest numberOfEntriesToLog
+ onePastLoggingReadPosition.store(writePosition.load() - numberOfEntriesToLog);
+
+ // reset skipped entries
+ for (std::size_t i = onePastLoggingReadPositionLastRead;
+ i < onePastLoggingReadPosition - 1;
+ i++)
+ {
+ detail::ControlThreadOutputBufferEntry& entry = entries.at(toBounds(i));
+ entry.messages.reset(messageBufferSize, messageBufferEntries, entry.iteration);
+ }
+ }
+ // use already existing impl.
+ forEachNewLoggingEntry(std::move(consumer));
}
- std::size_t ControlThreadOutputBuffer::initialize(std::size_t numEntries,
- const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
- const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
- std::size_t messageBufferSize, std::size_t messageBufferNumberEntries,
- std::size_t messageBufferMaxSize, std::size_t messageBufferMaxNumberEntries)
+ std::size_t
+ ControlThreadOutputBuffer::initialize(
+ std::size_t initialNumEntries,
+ const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
+ const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
+ std::size_t initialMessageBufferSize,
+ std::size_t messageBufferNumberEntries,
+ std::size_t messageBufferMaxSize,
+ std::size_t messageBufferMaxNumberEntries)
{
ARMARX_CHECK_EXPRESSION(!isInitialized);
- this->numEntries = numEntries;
+ numEntries = initialNumEntries;
+ messageBufferSize = initialMessageBufferSize;
//decide whether to use a triple buffer (in case no rtlogging is used)
ARMARX_CHECK_NOT_EQUAL(numEntries, 0);
entries.reserve(numEntries);
- entries.emplace_back(
- controlDevices, sensorDevices,
- messageBufferSize, messageBufferNumberEntries,
- messageBufferMaxSize, messageBufferMaxNumberEntries);
+ entries.emplace_back(controlDevices,
+ sensorDevices,
+ messageBufferSize,
+ messageBufferNumberEntries,
+ messageBufferMaxSize,
+ messageBufferMaxNumberEntries);
for (std::size_t i = 1; i < numEntries; ++i)
{
entries.emplace_back(entries.at(0));
@@ -194,67 +235,77 @@ namespace armarx
}
}
- ControlThreadOutputBuffer::RtMessageLogEntryBase& ControlThreadOutputBuffer::RtMessageLogEntryBase::setLoggingLevel(MessageTypeT lvl)
+ ControlThreadOutputBuffer::RtMessageLogEntryBase&
+ ControlThreadOutputBuffer::RtMessageLogEntryBase::setLoggingLevel(MessageTypeT lvl)
{
printMsg = true;
loggingLevel = lvl;
return *this;
}
- ControlThreadOutputBuffer::RtMessageLogEntryBase& ControlThreadOutputBuffer::RtMessageLogEntryBase::deactivateSpam(float sec)
+ ControlThreadOutputBuffer::RtMessageLogEntryBase&
+ ControlThreadOutputBuffer::RtMessageLogEntryBase::deactivateSpam(float sec)
{
printMsg = true;
deactivateSpamSec = sec;
return *this;
}
- ControlThreadOutputBuffer::RtMessageLogEntryBase& ControlThreadOutputBuffer::RtMessageLogEntryBase::deactivateSpamTag(std::uint64_t tag)
+ ControlThreadOutputBuffer::RtMessageLogEntryBase&
+ ControlThreadOutputBuffer::RtMessageLogEntryBase::deactivateSpamTag(std::uint64_t tag)
{
printMsg = true;
deactivateSpamTag_ = tag;
return *this;
}
- void detail::RtMessageLogEntryBase::print(Ice::Int controlThreadId) const
+ void
+ detail::RtMessageLogEntryBase::print(Ice::Int controlThreadId) const
{
if (printMsg)
{
- (checkLogLevel(loggingLevel)) ?
- _GlobalDummyLogSender :
- (*loghelper(file().c_str(), line(), func().c_str())
- ->setBacktrace(false)
- ->setTag({"RtLogMessages"})
- ->setThreadId(controlThreadId)
- )
- << loggingLevel
- << ::deactivateSpam(deactivateSpamSec, to_string(deactivateSpamTag_))
- << format();
+ (checkLogLevel(loggingLevel))
+ ? _GlobalDummyLogSender
+ : (*loghelper(file().c_str(), line(), func().c_str())
+ ->setBacktrace(false)
+ ->setTag({"RtLogMessages"})
+ ->setThreadId(controlThreadId))
+ << loggingLevel
+ << ::deactivateSpam(deactivateSpamSec, to_string(deactivateSpamTag_))
+ << format();
}
}
- detail::RtMessageLogBuffer::RtMessageLogBuffer(const detail::RtMessageLogBuffer& other, bool minimize):
- initialBufferSize {other.initialBufferSize * minimize},
- initialBufferEntryNumbers {other.initialBufferEntryNumbers * minimize},
- bufferMaxSize {other.bufferMaxSize * minimize},
- bufferMaxNumberEntries {other.bufferMaxNumberEntries * minimize},
- buffer(
- minimize ?
- other.buffer.size() + other.maxAlign - 1 - other.bufferSpace :
- other.buffer.size(),
- 0),
- bufferSpace {buffer.size()},
- bufferPlace {buffer.data()},
- entries(
- minimize ?
- other.entriesWritten :
- other.entries.size(),
- nullptr),
- entriesWritten {0},
- requiredAdditionalBufferSpace {other.requiredAdditionalBufferSpace},
- requiredAdditionalEntries {other.requiredAdditionalEntries},
- messagesLost {other.messagesLost},
- maxAlign {1}
+ const auto PotentiallyMinimizeMember = [](auto& member, bool minimize)
+ { return minimize ? 0 : member; };
+
+ detail::RtMessageLogBuffer::RtMessageLogBuffer(const detail::RtMessageLogBuffer& other,
+ bool minimize) :
+ initialBufferSize{PotentiallyMinimizeMember(other.initialBufferSize, minimize)},
+ initialBufferEntryNumbers{
+ PotentiallyMinimizeMember(other.initialBufferEntryNumbers, minimize)},
+ bufferMaxSize{PotentiallyMinimizeMember(other.bufferMaxSize, minimize)},
+ bufferMaxNumberEntries{PotentiallyMinimizeMember(other.bufferMaxNumberEntries, minimize)},
+ buffer(minimize ? other.buffer.size() + other.maxAlign - 1 - other.bufferSpace
+ : other.buffer.size(),
+ 0),
+ bufferSpace{buffer.size()},
+ bufferPlace{buffer.data()},
+ entries(minimize ? other.entriesWritten : other.entries.size(), nullptr),
+ entriesWritten{0},
+ requiredAdditionalBufferSpace{other.requiredAdditionalBufferSpace},
+ requiredAdditionalEntries{other.requiredAdditionalEntries},
+ messagesLost{other.messagesLost},
+ 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);
for (std::size_t idx = 0; idx < other.entries.size() && other.entries.at(idx); ++idx)
{
const RtMessageLogEntryBase* entry = other.entries.at(idx);
@@ -270,21 +321,21 @@ namespace armarx
<< "\nbuffer place = " << bufferPlace
<< "\nbuffer space = " << bufferSpace
<< "\nentry size = " << entry->_sizeof()
- << "\nentry align = " << entry->_alignof()
- << "\n"
+ << "\nentry align = " << entry->_alignof() << "\n"
<< "\nother buffer size = " << other.buffer.size()
<< "\nother buffer space = " << other.bufferSpace
- << "\nother max align = " << other.maxAlign
- << "\n"
+ << "\nother max align = " << other.maxAlign << "\n"
<< "\nthis = " << this;
};
ARMARX_CHECK_NOT_NULL(place) << hint;
- ARMARX_CHECK_LESS_EQUAL(static_cast<void*>(&buffer.front()), static_cast<void*>(place)) << hint;
- ARMARX_CHECK_LESS_EQUAL(static_cast<void*>(place), static_cast<void*>(&buffer.back())) << hint;
+ ARMARX_CHECK_LESS_EQUAL(static_cast<void*>(&buffer.front()), static_cast<void*>(place))
+ << hint;
+ ARMARX_CHECK_LESS_EQUAL(static_cast<void*>(place), static_cast<void*>(&buffer.back()))
+ << hint;
ARMARX_CHECK_LESS_EQUAL(
static_cast<void*>(static_cast<std::uint8_t*>(place) + entry->_sizeof() - 1),
- static_cast<void*>(&buffer.back())) <<
- hint;
+ static_cast<void*>(&buffer.back()))
+ << hint;
ARMARX_CHECK_LESS_EQUAL(place, &buffer.back());
ARMARX_CHECK_LESS(entriesWritten, entries.size());
ARMARX_CHECK_EXPRESSION(!entries.at(entriesWritten));
@@ -300,22 +351,29 @@ namespace armarx
}
}
- void detail::RtMessageLogBuffer::reset(std::size_t& bufferSize, std::size_t& numEntries, std::size_t iterationCount)
+ void
+ detail::RtMessageLogBuffer::reset(std::size_t& bufferSize,
+ std::size_t& numEntries,
+ std::size_t iterationCount)
{
deleteAll();
if (requiredAdditionalEntries || entries.size() < numEntries)
{
- if (requiredAdditionalEntries)
- {
- ARMARX_WARNING << "Iteration " << iterationCount << " required "
- << requiredAdditionalEntries << " additional message entries.";
- }
- const auto requiredSize = entries.size() + requiredAdditionalEntries;
+ 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"
+ << "The requested total number of entries is " << requiredSize << ". \n"
+ << "The current number of entries is " << entries.size() << ". \n"
+ << "The maximal number of entries is "
+ << getMaximalNumberOfBufferEntries();
if (requiredSize > getMaximalNumberOfBufferEntries())
{
- ARMARX_WARNING << deactivateSpam(1, to_string(requiredSize))
- << "Iteration " << iterationCount << " would require "
- << requiredSize << " message entries, but the maximal number of entries is "
+ ARMARX_WARNING << deactivateSpam(1, to_string(requiredSize)) << "Iteration "
+ << iterationCount << " would require " << requiredSize
+ << " message entries, but the maximal number of entries is "
<< getMaximalNumberOfBufferEntries();
}
numEntries = std::max(requiredSize, getMaximalNumberOfBufferEntries());
@@ -324,17 +382,17 @@ namespace armarx
}
if (requiredAdditionalBufferSpace)
{
- if (requiredAdditionalBufferSpace)
- {
- ARMARX_WARNING << "Iteration " << iterationCount << " required "
- << requiredAdditionalBufferSpace
- << " additional bytes for messages in the buffer.";
- }
const auto requiredSpace = buffer.size() + requiredAdditionalBufferSpace;
+ ARMARX_WARNING << "Iteration " << iterationCount << " required "
+ << requiredAdditionalBufferSpace
+ << " additional bytes for messages in the buffer. \n"
+ << "Therefore the new required size is " << requiredSpace << ". \n"
+ << "The current size of the buffer is " << buffer.size() << ". \n"
+ << "The maximal size of the buffer is " << getMaximalBufferSize();
if (requiredSpace > getMaximalBufferSize())
{
- ARMARX_WARNING << deactivateSpam(1, to_string(requiredSpace))
- << "Iteration " << iterationCount << " would require " << requiredSpace
+ ARMARX_WARNING << deactivateSpam(1, to_string(requiredSpace)) << "Iteration "
+ << iterationCount << " would require " << requiredSpace
<< " bytes for messages buffer, but the maximal buffer size is "
<< getMaximalBufferSize();
}
@@ -348,7 +406,8 @@ namespace armarx
maxAlign = 1;
}
- void detail::RtMessageLogBuffer::deleteAll()
+ void
+ detail::RtMessageLogBuffer::deleteAll()
{
for (std::size_t idx = 0; idx < entries.size() && entries.at(idx); ++idx)
{
@@ -363,12 +422,16 @@ namespace armarx
detail::ControlThreadOutputBufferEntry::ControlThreadOutputBufferEntry(
const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
- std::size_t messageBufferSize, std::size_t messageBufferNumberEntries,
- std::size_t messageBufferMaxSize, std::size_t messageBufferMaxNumberEntries
- ):
- sensorValuesTimestamp {IceUtil::Time::microSeconds(0)},
- timeSinceLastIteration {IceUtil::Time::microSeconds(0)},
- messages {messageBufferSize, messageBufferNumberEntries, messageBufferMaxSize, messageBufferMaxNumberEntries}
+ std::size_t messageBufferSize,
+ std::size_t messageBufferNumberEntries,
+ std::size_t messageBufferMaxSize,
+ std::size_t messageBufferMaxNumberEntries) :
+ sensorValuesTimestamp{IceUtil::Time::microSeconds(0)},
+ timeSinceLastIteration{IceUtil::Time::microSeconds(0)},
+ messages{messageBufferSize,
+ messageBufferNumberEntries,
+ messageBufferMaxSize,
+ messageBufferMaxNumberEntries}
{
//calculate size in bytes for one buffer
const std::size_t bytes = [&]
@@ -418,7 +481,8 @@ namespace armarx
for (const SensorDevicePtr& sd : sensorDevices.values())
{
const SensorValueBase* sv = sd->getSensorValue();
- sensors.emplace_back(sv->_placementConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
+ sensors.emplace_back(
+ sv->_placementConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
}
control.reserve(controlDevices.size());
@@ -431,21 +495,29 @@ namespace armarx
for (const JointController* ctrl : ctrls)
{
const ControlTargetBase* ct = ctrl->getControlTarget();
- ctargs.emplace_back(ct->_placementConstruct(getAlignedPlace(ct->_sizeof(), ct->_alignof())));
+ ctargs.emplace_back(
+ ct->_placementConstruct(getAlignedPlace(ct->_sizeof(), ct->_alignof())));
ctargs.back()->reset();
}
}
}
- detail::ControlThreadOutputBufferEntry::ControlThreadOutputBufferEntry(const detail::ControlThreadOutputBufferEntry& other, bool minimize):
- writeTimestamp {other.writeTimestamp},
- sensorValuesTimestamp {other.sensorValuesTimestamp},
- timeSinceLastIteration {other.timeSinceLastIteration},
- iteration {other.iteration},
- messages {other.messages, minimize},
+ detail::ControlThreadOutputBufferEntry::ControlThreadOutputBufferEntry(
+ const detail::ControlThreadOutputBufferEntry& other,
+ bool minimize) :
+ writeTimestamp{other.writeTimestamp},
+ sensorValuesTimestamp{other.sensorValuesTimestamp},
+ timeSinceLastIteration{other.timeSinceLastIteration},
+ iteration{other.iteration},
+ messages{other.messages, minimize},
buffer(other.buffer.size(), 0)
{
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());
void* place = buffer.data();
std::size_t space = buffer.size();
@@ -466,7 +538,8 @@ namespace armarx
for (const SensorValueBase* sv : other.sensors)
{
ARMARX_TRACE;
- sensors.emplace_back(sv->_placementCopyConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
+ sensors.emplace_back(
+ sv->_placementCopyConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
}
//copy control targets
@@ -479,8 +552,9 @@ namespace armarx
ctargs.reserve(cdctargs.size());
for (const ControlTargetBase* ct : cdctargs)
{
- ctargs.emplace_back(ct->_placementCopyConstruct(getAlignedPlace(ct->_sizeof(), ct->_alignof())));
+ ctargs.emplace_back(
+ ct->_placementCopyConstruct(getAlignedPlace(ct->_sizeof(), ct->_alignof())));
}
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index 4cbdf1e4b7fcde010ea07db88f4d8585163cf2d9..78bd7da841220341e817df9d566e4275b150eb7b 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -22,26 +22,24 @@
#pragma once
-#include "HeterogenousContinuousContainer.h"
-
-#include "../SensorValues/SensorValueBase.h"
-#include "../ControlTargets/ControlTargetBase.h"
-
-#include "../Devices/SensorDevice.h"
-#include "../Devices/ControlDevice.h"
+#include <vector>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/util/StringHelperTemplates.h>
#include <ArmarXCore/core/util/PropagateConst.h>
+#include <ArmarXCore/core/util/StringHelperTemplates.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
-#include <vector>
+#include "../ControlTargets/ControlTargetBase.h"
+#include "../Devices/ControlDevice.h"
+#include "../Devices/SensorDevice.h"
+#include "../SensorValues/SensorValueBase.h"
+#include "HeterogenousContinuousContainer.h"
namespace armarx
{
class RobotUnit;
struct ControlThreadOutputBuffer;
-}
+} // namespace armarx
namespace armarx::RobotUnitModule
{
@@ -52,9 +50,10 @@ namespace armarx::detail
{
struct RtMessageLogEntryBase
{
- RtMessageLogEntryBase():
- time {IceUtil::Time::now()}
- {}
+ RtMessageLogEntryBase() : time{IceUtil::Time::now()}
+ {
+ }
+
virtual ~RtMessageLogEntryBase() = default;
RtMessageLogEntryBase& setLoggingLevel(MessageTypeT lvl);
@@ -71,10 +70,11 @@ namespace armarx::detail
ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(RtMessageLogEntryBase)
protected:
friend struct RtMessageLogBuffer;
+
private:
- MessageTypeT loggingLevel {MessageTypeT::UNDEFINED};
- float deactivateSpamSec {0};
- bool printMsg {false};
+ MessageTypeT loggingLevel{MessageTypeT::UNDEFINED};
+ float deactivateSpamSec{0};
+ bool printMsg{false};
std::uint64_t deactivateSpamTag_;
IceUtil::Time time;
};
@@ -82,6 +82,7 @@ namespace armarx::detail
struct RtMessageLogEntryDummy : RtMessageLogEntryBase
{
static RtMessageLogEntryDummy Instance;
+
protected:
std::string format() const final override;
std::size_t line() const final override;
@@ -94,20 +95,28 @@ namespace armarx::detail
struct RtMessageLogEntryNull : RtMessageLogEntryBase
{
static RtMessageLogEntryNull Instance;
+
protected:
- std::string format() const final override
+ std::string
+ format() const final override
{
return "";
}
- std::size_t line() const final override
+
+ std::size_t
+ line() const final override
{
return 0;
}
- std::string file() const final override
+
+ std::string
+ file() const final override
{
return "";
}
- std::string func() const final override
+
+ std::string
+ func() const final override
{
return "";
}
@@ -118,9 +127,10 @@ namespace armarx::detail
struct RtMessageLogBuffer
{
RtMessageLogBuffer(const RtMessageLogBuffer& other, bool minimize = false);
- RtMessageLogBuffer(
- std::size_t bufferSize, std::size_t numEntries,
- std::size_t bufferMaxSize, std::size_t bufferMaxNumberEntries);
+ RtMessageLogBuffer(std::size_t bufferSize,
+ std::size_t numEntries,
+ std::size_t bufferMaxSize,
+ std::size_t bufferMaxNumberEntries);
~RtMessageLogBuffer();
RtMessageLogBuffer() = delete;
@@ -134,6 +144,7 @@ namespace armarx::detail
std::size_t getMaximalBufferSize() const;
std::size_t getMaximalNumberOfBufferEntries() const;
+
private:
void deleteAll();
@@ -152,27 +163,28 @@ namespace armarx::detail
void* bufferPlace;
std::vector<const RtMessageLogEntryBase*> entries;
- std::size_t entriesWritten {0};
+ std::size_t entriesWritten{0};
- std::size_t requiredAdditionalBufferSpace {0};
- std::size_t requiredAdditionalEntries {0};
- std::size_t messagesLost {0};
+ std::size_t requiredAdditionalBufferSpace{0};
+ std::size_t requiredAdditionalEntries{0};
+ std::size_t messagesLost{0};
- std::size_t maxAlign {1};
+ std::size_t maxAlign{1};
};
-
struct ControlThreadOutputBufferEntry
{
//default ctors / ops
ControlThreadOutputBufferEntry(
const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
- const KeyValueVector<std::string, SensorDevicePtr >& sensorDevices,
- std::size_t messageBufferSize, std::size_t messageBufferNumberEntries,
- std::size_t messageBufferMaxSize, std::size_t messageBufferMaxNumberEntries
- );
- ControlThreadOutputBufferEntry(const ControlThreadOutputBufferEntry& other, bool minimize = false);
+ const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
+ std::size_t messageBufferSize,
+ std::size_t messageBufferNumberEntries,
+ std::size_t messageBufferMaxSize,
+ std::size_t messageBufferMaxNumberEntries);
+ ControlThreadOutputBufferEntry(const ControlThreadOutputBufferEntry& other,
+ bool minimize = false);
ControlThreadOutputBufferEntry() = delete;
ControlThreadOutputBufferEntry(ControlThreadOutputBufferEntry&&) = delete;
@@ -188,13 +200,14 @@ namespace armarx::detail
IceUtil::Time timeSinceLastIteration;
std::vector<PropagateConst<SensorValueBase*>> sensors;
std::vector<std::vector<PropagateConst<ControlTargetBase*>>> control;
- std::size_t iteration {0};
+ std::size_t iteration{0};
RtMessageLogBuffer messages;
+
private:
std::vector<std::uint8_t> buffer;
};
-}
+} // namespace armarx::detail
namespace armarx
{
@@ -202,17 +215,19 @@ namespace armarx
{
using Entry = detail::ControlThreadOutputBufferEntry;
using RtMessageLogEntryBase = detail::RtMessageLogEntryBase;
- using ConsumerFunctor = std::function<void (const Entry&, std::size_t, std::size_t)>;
- std::size_t initialize(
- std::size_t numEntries,
- const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
- const KeyValueVector<std::string, SensorDevicePtr >& sensorDevices,
- std::size_t messageBufferSize, std::size_t messageBufferNumberEntries,
- std::size_t messageBufferMaxSize, std::size_t messageBufferMaxNumberEntries);
+ using ConsumerFunctor = std::function<void(const Entry&, std::size_t, std::size_t)>;
+ std::size_t initialize(std::size_t numEntries,
+ const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
+ const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
+ std::size_t messageBufferSize,
+ std::size_t messageBufferNumberEntries,
+ std::size_t messageBufferMaxSize,
+ std::size_t messageBufferMaxNumberEntries);
~ControlThreadOutputBuffer();
- static ControlThreadOutputBuffer* GetRtLoggingInstance()
+ static ControlThreadOutputBuffer*
+ GetRtLoggingInstance()
{
return RtLoggingInstance;
}
@@ -226,163 +241,210 @@ namespace armarx
bool updateReadBuffer() const;
//logging read
- void resetLoggingPosition() const;
- void foreachNewLoggingEntry(ConsumerFunctor consumer);
- void forLatestLoggingEntry(ConsumerFunctor consumer);
+ void resetLoggingPosition() const;
+ void forEachNewLoggingEntry(ConsumerFunctor consumer);
+ void forLatestLoggingEntry(ConsumerFunctor consumer, size_t numberOfEntriesToLog);
std::size_t getNumberOfBytes() const;
- template<class LoggingEntryT, class...Ts>
- RtMessageLogEntryBase* addMessageToLog(Ts&& ...args);
+ template <class LoggingEntryT, class... Ts>
+ RtMessageLogEntryBase* addMessageToLog(Ts&&... args);
private:
- friend class RobotUnitModule::Logging; ///TODO change code to make this unnecessary
+ friend class RobotUnitModule::Logging; ///TODO change code to make this unnecessary
/// @brief this pointer is used for rt message logging and is not null in the control thread
static thread_local ControlThreadOutputBuffer* RtLoggingInstance;
std::size_t toBounds(std::size_t idx) const;
//settings and initialization:
- bool isInitialized {false};
- std::size_t numEntries {0};
+ bool isInitialized{false};
+ std::size_t numEntries{0};
- std::atomic_size_t writePosition {0};
- mutable std::atomic_size_t onePastLoggingReadPosition {0};
- mutable std::atomic_size_t onePastReadPosition {0};
+ std::atomic_size_t writePosition{0};
+ mutable std::atomic_size_t onePastLoggingReadPosition{0};
+ mutable std::atomic_size_t onePastReadPosition{0};
std::vector<Entry> entries;
std::vector<std::uint8_t> data;
- std::size_t messageBufferSize {0};
- std::size_t messageBufferEntries {0};
+ std::size_t messageBufferSize{0};
+ std::size_t messageBufferEntries{0};
};
- using SensorAndControl = ControlThreadOutputBuffer::Entry;
-}
-
-#define ARMARX_RT_LOGF(...) (*(_detail_ARMARX_RT_LOGF(__FILE__, ARMARX_FUNCTION,__LINE__, __VA_ARGS__, true)))
-
-#define _detail_ARMARX_RT_LOGF(file_, func_, line_, FormatString, ...) \
- ([&]{ \
- using namespace ::armarx; \
- using RtMessageLogEntryBase = ControlThreadOutputBuffer::RtMessageLogEntryBase; \
- struct RtMessageLogEntry : RtMessageLogEntryBase \
- { \
- using TupleT = decltype(std::make_tuple(__VA_ARGS__)); \
- const TupleT tuple; \
- ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
- std::size_t line() const final override {return line_;} \
- std::string file() const final override {return file_;} \
- std::string func() const final override {return func_;} \
- std::string format() const final override \
- { \
- return TupleToStringF<0,std::tuple_size<TupleT>::value -1>(FormatString, tuple); \
- } \
- RtMessageLogEntry(TupleT tuple) : tuple{std::move(tuple)} {} \
- RtMessageLogEntry(const RtMessageLogEntry&) = default; \
- }; \
- if (::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance()) { \
- return ::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance() \
- ->addMessageToLog<RtMessageLogEntry>(__VA_ARGS__); \
- } else { \
- *(loghelper(file_, line_, func_)) << "Redirected RT Logging:\n" \
- << RtMessageLogEntry(std::make_tuple(__VA_ARGS__)).format(); \
- return dynamic_cast<RtMessageLogEntryBase*> \
- (&::armarx::detail::RtMessageLogEntryNull::Instance); \
- } \
- }())
-
-#define ARMARX_RT_LOGF_DEBUG(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::DEBUG)
-#define ARMARX_RT_LOGF_VERBOSE(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::VERBOSE)
-#define ARMARX_RT_LOGF_INFO(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::INFO)
-#define ARMARX_RT_LOGF_IMPORTANT(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::IMPORTANT)
-#define ARMARX_RT_LOGF_WARNING(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
-#define ARMARX_RT_LOGF_WARN(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
-#define ARMARX_RT_LOGF_ERROR(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::ERROR)
-#define ARMARX_RT_LOGF_FATAL(...) ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::FATAL)
+ using SensorAndControl = ControlThreadOutputBuffer::Entry;
+} // namespace armarx
+
+#define ARMARX_RT_LOGF(...) \
+ (*(_detail_ARMARX_RT_LOGF(__FILE__, ARMARX_FUNCTION, __LINE__, __VA_ARGS__, true)))
+
+#define _detail_ARMARX_RT_LOGF(file_, func_, line_, FormatString, ...) \
+ ( \
+ [&] \
+ { \
+ using namespace ::armarx; \
+ using RtMessageLogEntryBase = ControlThreadOutputBuffer::RtMessageLogEntryBase; \
+ struct RtMessageLogEntry : RtMessageLogEntryBase \
+ { \
+ using TupleT = decltype(std::make_tuple(__VA_ARGS__)); \
+ const TupleT tuple; \
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
+ std::size_t \
+ line() const final override \
+ { \
+ return line_; \
+ } \
+ std::string \
+ file() const final override \
+ { \
+ return file_; \
+ } \
+ std::string \
+ func() const final override \
+ { \
+ return func_; \
+ } \
+ std::string \
+ format() const final override \
+ { \
+ return TupleToStringF<0, std::tuple_size<TupleT>::value - 1>(FormatString, \
+ tuple); \
+ } \
+ RtMessageLogEntry(TupleT tuple) : tuple{std::move(tuple)} \
+ { \
+ } \
+ RtMessageLogEntry(const RtMessageLogEntry&) = default; \
+ }; \
+ if (::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance()) \
+ { \
+ return ::armarx::ControlThreadOutputBuffer::GetRtLoggingInstance() \
+ ->addMessageToLog<RtMessageLogEntry>(__VA_ARGS__); \
+ } \
+ else \
+ { \
+ *(loghelper(file_, line_, func_)) \
+ << "Redirected RT Logging:\n" \
+ << RtMessageLogEntry(std::make_tuple(__VA_ARGS__)).format(); \
+ return dynamic_cast<RtMessageLogEntryBase*>( \
+ &::armarx::detail::RtMessageLogEntryNull::Instance); \
+ } \
+ }())
+
+#define ARMARX_RT_LOGF_DEBUG(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::DEBUG)
+#define ARMARX_RT_LOGF_VERBOSE(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::VERBOSE)
+#define ARMARX_RT_LOGF_INFO(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::INFO)
+#define ARMARX_RT_LOGF_IMPORTANT(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::IMPORTANT)
+#define ARMARX_RT_LOGF_WARNING(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
+#define ARMARX_RT_LOGF_WARN(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::WARN)
+#define ARMARX_RT_LOGF_ERROR(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::ERROR)
+#define ARMARX_RT_LOGF_FATAL(...) \
+ ARMARX_RT_LOGF(__VA_ARGS__).setLoggingLevel(::armarx::MessageTypeT::FATAL)
//impl
namespace armarx::detail
{
- inline IceUtil::Time RtMessageLogEntryBase::getTime() const
+ inline IceUtil::Time
+ RtMessageLogEntryBase::getTime() const
{
return time;
}
- inline std::string RtMessageLogEntryDummy::format() const
+ inline std::string
+ RtMessageLogEntryDummy::format() const
{
- throw std::logic_error {"called 'format' of RtMessageLogEntryDummy"};
+ throw std::logic_error{"called 'format' of RtMessageLogEntryDummy"};
}
- inline std::size_t RtMessageLogEntryDummy::line() const
+ inline std::size_t
+ RtMessageLogEntryDummy::line() const
{
- throw std::logic_error {"called 'line' of RtMessageLogEntryDummy"};
+ throw std::logic_error{"called 'line' of RtMessageLogEntryDummy"};
}
- inline std::string RtMessageLogEntryDummy::file() const
+ inline std::string
+ RtMessageLogEntryDummy::file() const
{
- throw std::logic_error {"called 'file' of RtMessageLogEntryDummy"};
+ throw std::logic_error{"called 'file' of RtMessageLogEntryDummy"};
}
- inline std::string RtMessageLogEntryDummy::func() const
+ inline std::string
+ RtMessageLogEntryDummy::func() const
{
- throw std::logic_error {"called 'func' of RtMessageLogEntryDummy"};
+ throw std::logic_error{"called 'func' of RtMessageLogEntryDummy"};
}
- inline RtMessageLogBuffer::RtMessageLogBuffer(
- std::size_t bufferSize, std::size_t numEntries,
- std::size_t bufferMaxSize, std::size_t bufferMaxNumberEntries
- ):
- initialBufferSize {bufferSize},
- initialBufferEntryNumbers {numEntries},
- bufferMaxSize {bufferMaxSize},
- bufferMaxNumberEntries {bufferMaxNumberEntries},
+ inline RtMessageLogBuffer::RtMessageLogBuffer(std::size_t bufferSize,
+ std::size_t numEntries,
+ std::size_t bufferMaxSize,
+ std::size_t bufferMaxNumberEntries) :
+ initialBufferSize{bufferSize},
+ initialBufferEntryNumbers{numEntries},
+ bufferMaxSize{bufferMaxSize},
+ bufferMaxNumberEntries{bufferMaxNumberEntries},
buffer(bufferSize, 0),
- bufferSpace {buffer.size()},
- bufferPlace {buffer.data()},
+ bufferSpace{buffer.size()},
+ bufferPlace{buffer.data()},
entries(numEntries, nullptr)
- {}
+ {
+ ARMARX_DEBUG << "RtMessageLogBuffer created with bufferSize=" << bufferSize
+ << " and numEntries=" << numEntries << " and bufferMaxSize=" << bufferMaxSize
+ << " and bufferMaxNumberEntries " << bufferMaxNumberEntries;
+ }
inline RtMessageLogBuffer::~RtMessageLogBuffer()
{
deleteAll();
}
- inline const std::vector<const RtMessageLogEntryBase*>& RtMessageLogBuffer::getEntries() const
+ inline const std::vector<const RtMessageLogEntryBase*>&
+ RtMessageLogBuffer::getEntries() const
{
return entries;
}
- inline std::size_t RtMessageLogBuffer::getMaximalBufferSize() const
+ inline std::size_t
+ RtMessageLogBuffer::getMaximalBufferSize() const
{
return bufferMaxSize;
}
- inline std::size_t RtMessageLogBuffer::getMaximalNumberOfBufferEntries() const
+ inline std::size_t
+ RtMessageLogBuffer::getMaximalNumberOfBufferEntries() const
{
return bufferMaxNumberEntries;
}
- inline std::size_t ControlThreadOutputBufferEntry::getDataBufferSize() const
+ inline std::size_t
+ ControlThreadOutputBufferEntry::getDataBufferSize() const
{
return buffer.size();
}
-}
+} // namespace armarx::detail
namespace armarx
{
- inline std::size_t ControlThreadOutputBuffer::toBounds(std::size_t idx) const
+ inline std::size_t
+ ControlThreadOutputBuffer::toBounds(std::size_t idx) const
{
return idx % numEntries;
}
- inline std::size_t ControlThreadOutputBuffer::getNumberOfBytes() const
+ inline std::size_t
+ ControlThreadOutputBuffer::getNumberOfBytes() const
{
return data.size();
}
- template<class LoggingEntryT, class...Ts>
- inline ControlThreadOutputBuffer::RtMessageLogEntryBase* ControlThreadOutputBuffer::addMessageToLog(Ts&& ...args)
+ template <class LoggingEntryT, class... Ts>
+ inline ControlThreadOutputBuffer::RtMessageLogEntryBase*
+ ControlThreadOutputBuffer::addMessageToLog(Ts&&... args)
{
detail::RtMessageLogBuffer& messages = getWriteBuffer().messages;
if (messages.entries.size() <= messages.entriesWritten)
@@ -392,14 +454,19 @@ namespace armarx
return &detail::RtMessageLogEntryDummy::Instance;
}
messages.maxAlign = std::max(messages.maxAlign, alignof(LoggingEntryT));
- void* place = std::align(alignof(LoggingEntryT), sizeof(LoggingEntryT), messages.bufferPlace, messages.bufferSpace);
+ void* place = std::align(alignof(LoggingEntryT),
+ sizeof(LoggingEntryT),
+ messages.bufferPlace,
+ messages.bufferSpace);
if (!place)
{
- messages.requiredAdditionalBufferSpace += sizeof(LoggingEntryT) + alignof(LoggingEntryT) - 1;
+ messages.requiredAdditionalBufferSpace +=
+ sizeof(LoggingEntryT) + alignof(LoggingEntryT) - 1;
++messages.messagesLost;
return &detail::RtMessageLogEntryDummy::Instance;
}
- RtMessageLogEntryBase* entry = new (place) LoggingEntryT(std::make_tuple(std::forward<Ts>(args)...));
+ RtMessageLogEntryBase* entry =
+ new (place) LoggingEntryT(std::make_tuple(std::forward<Ts>(args)...));
ARMARX_CHECK_NOT_NULL(entry);
messages.bufferPlace = static_cast<std::uint8_t*>(place) + sizeof(LoggingEntryT);
@@ -409,4 +476,4 @@ namespace armarx
messages.entries.at(messages.entriesWritten++) = entry;
return entry;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h b/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
index 706f9396e80e8680dfe75e935b4789c06bf806b7..02f14407354dc70fa2b72e57531943529f5af03b 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/CtrlUtil.h
@@ -23,26 +23,32 @@
*/
//pragma once
#include <cmath>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <iostream>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
namespace armarx::ctrlutil
{
- double s(double t, double s0, double v0, double a0, double j)
+ double
+ s(double t, double s0, double v0, double a0, double j)
{
return s0 + v0 * t + 0.5 * a0 * t * t + 1.0 / 6.0 * j * t * t * t;
}
- double v(double t, double v0, double a0, double j)
+
+ double
+ v(double t, double v0, double a0, double j)
{
return v0 + a0 * t + 0.5 * j * t * t;
}
- double a(double t, double a0, double j)
+
+ double
+ a(double t, double a0, double j)
{
return a0 + j * t;
}
-
- double t_to_v(double v, double a, double j)
+ double
+ t_to_v(double v, double a, double j)
{
// time to accelerate to v with jerk j starting at acceleration a0
//return 2*math.sqrt(a0**2+2*j*(v/2))-a0/j
@@ -51,12 +57,11 @@ namespace armarx::ctrlutil
// --> p = 2a0/j; q = -2v/j
// t = - a0/j +- sqrt((a0/j)**2 +2v/j)
double tmp = a / j;
- return - a / j + std::sqrt(tmp * tmp + 2 * v / j);
+ return -a / j + std::sqrt(tmp * tmp + 2 * v / j);
}
-
-
- double t_to_v_with_wedge_acc(double v, double a0, double j)
+ double
+ t_to_v_with_wedge_acc(double v, double a0, double j)
{
// ramp up and down of acceleration (i.e. a0=at); returns t to achieve delta v
return 2 * t_to_v(v / 2.0, a0, j);
@@ -67,16 +72,25 @@ namespace armarx::ctrlutil
double s_total, v1, v2, v3, dt1, dt2, dt3;
};
- double brakingDistance(double v0, double a0, double j)
+ double
+ brakingDistance(double v0, double a0, double j)
{
auto signV = math::MathUtils::Sign(v0);
auto t = t_to_v(v0, -signV * a0, signV * j);
return s(t, 0, v0, a0, -signV * j);
}
- BrakingData brakingDistance(double goal, double s0, double v0, double a0, double v_max, double a_max, double j, double dec_max)
+ BrakingData
+ brakingDistance(double goal,
+ double s0,
+ double v0,
+ double a0,
+ double v_max,
+ double a_max,
+ double j,
+ double dec_max)
{
- double d = math::MathUtils::Sign(goal - s0); // if v0 == 0.0 else abs(v0)/a_max
+ double d = math::MathUtils::Sign(goal - s0); // if v0 == 0.0 else abs(v0)/a_max
dec_max *= -d;
// std::cout << "dec max: " << (dec_max) << " d: " << d << std::endl;
double dt1 = std::abs((dec_max - a0) / (-j * d));
@@ -84,7 +98,8 @@ namespace armarx::ctrlutil
// double a1 = a(dt1, a0, -d * j);
double v1 = v(dt1, v0, a0, -d * j);
double acc_duration = std::abs(dec_max) / j;
- double v2 = v(acc_duration, 0, 0, d * j);// # inverse of time to accelerate from 0 to max v
+ double v2 =
+ v(acc_duration, 0, 0, d * j); // # inverse of time to accelerate from 0 to max v
v2 = math::MathUtils::LimitTo(v2, v_max);
// v2 = v1 + dv2
double dt2 = d * ((v1 - v2) / dec_max);
@@ -97,7 +112,7 @@ namespace armarx::ctrlutil
double v3 = v(dt3, v2, dec_max, d * j);
double s_total = s1 + s2 + s3;
- if (dt2 < 0)// # we dont have a phase with a=a_max and need to reduce the a_max
+ if (dt2 < 0) // # we dont have a phase with a=a_max and need to reduce the a_max
{
double excess_time = t_to_v_with_wedge_acc(std::abs(v1), std::abs(dec_max), j);
double delta_a = a(excess_time / 2, 0, d * j);
@@ -115,28 +130,31 @@ namespace armarx::ctrlutil
{
double s_total, s1, s2, v1, v2, a1, a2, t, dt1, dt2;
};
- bool isPossibleToBrake(double v0, double a0, double j)
+
+ bool
+ isPossibleToBrake(double v0, double a0, double j)
{
return j * v0 - a0 * a0 / 2 > 0.0f;
}
- double jerk(double t, double s0, double v0, double a0)
+ double
+ jerk(double t, double s0, double v0, double a0)
{
return (6 * s0 - 3 * t * (a0 * t + 2 * v0)) / (t * t * t);
}
- std::tuple<double, double, double> calcAccAndJerk(double s0, double v0)
+ std::tuple<double, double, double>
+ calcAccAndJerk(double s0, double v0)
{
s0 *= -1;
- double t = - (3 * s0) / v0;
- double a0 = - (2 * v0) / t;
+ double t = -(3 * s0) / v0;
+ double a0 = -(2 * v0) / t;
double j = 2 * v0 / (t * t);
return std::make_tuple(t, a0, j);
}
-
-
- WedgeBrakingData braking_distance_with_wedge(double v0, double a0, double j)
+ WedgeBrakingData
+ braking_distance_with_wedge(double v0, double a0, double j)
{
// # v0 = v1 + v2
// # v1 = t1 * a0 + 0.5*j*t1**2
@@ -159,8 +177,7 @@ namespace armarx::ctrlutil
r.dt2 = -1;
return r;
throw std::runtime_error("unsuitable parameters! : j: " + std::to_string(j) +
- " a0: " + std::to_string(a0) +
- " v0: " + std::to_string(v0));
+ " a0: " + std::to_string(a0) + " v0: " + std::to_string(v0));
}
double t1 = std::sqrt(part) / j;
double t2 = (a0 / j) + t1;
@@ -180,4 +197,4 @@ namespace armarx::ctrlutil
double s_total = s1 + s2;
return {s_total, s1, s2, d * v1, d * v2, d * a1, d * a2, t1 + t2, t1, t2};
}
-}
+} // namespace armarx::ctrlutil
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
index bf5e5e28970861a2ebd6e0fb8c17d052962f5786..79d076f9b2aa7202f4ad649f76090b034da2f6db 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
@@ -23,22 +23,25 @@
*/
#include "DynamicsHelper.h"
-#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+
namespace armarx
{
- DynamicsHelper::DynamicsHelper(RobotUnit* robotUnit): robotUnit(robotUnit)
+ DynamicsHelper::DynamicsHelper(RobotUnit* robotUnit) : robotUnit(robotUnit)
{
ARMARX_CHECK_EXPRESSION(robotUnit);
}
- void DynamicsHelper::addRobotPart(VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies,
- rtfilters::RTFilterBasePtr velocityFilter,
- rtfilters::RTFilterBasePtr accelerationFilter)
+ void
+ DynamicsHelper::addRobotPart(VirtualRobot::RobotNodeSetPtr rnsJoints,
+ VirtualRobot::RobotNodeSetPtr rnsBodies,
+ rtfilters::RTFilterBasePtr velocityFilter,
+ rtfilters::RTFilterBasePtr accelerationFilter)
{
DynamicsData data(rnsJoints, rnsBodies);
for (size_t i = 0; i < rnsJoints->getSize(); i++)
@@ -46,9 +49,11 @@ namespace armarx
auto selectedJoint = robotUnit->getSensorDevice(rnsJoints->getNode(i)->getName());
if (selectedJoint)
{
- auto sensorValue = const_cast<SensorValue1DoFActuator*>(selectedJoint->getSensorValue()->asA<SensorValue1DoFActuator>());
+ auto sensorValue = const_cast<SensorValue1DoFActuator*>(
+ selectedJoint->getSensorValue()->asA<SensorValue1DoFActuator>());
ARMARX_CHECK_EXPRESSION(sensorValue) << rnsJoints->getNode(i)->getName();
- ARMARX_DEBUG << "will calculate inverse dynamics for robot node " << rnsJoints->getNode(i)->getName();
+ ARMARX_DEBUG << "will calculate inverse dynamics for robot node "
+ << rnsJoints->getNode(i)->getName();
data.nodeSensorVec.push_back(std::make_pair(rnsJoints->getNode(i), sensorValue));
}
else
@@ -68,7 +73,9 @@ namespace armarx
dataMap.emplace(std::make_pair(rnsJoints, data));
}
- void DynamicsHelper::update(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ DynamicsHelper::update(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
for (auto& pair : dataMap)
{
@@ -81,17 +88,21 @@ namespace armarx
{
ARMARX_CHECK_EXPRESSION(node.second) << node.first->getName();
data.q(i) = node.second->position;
- if (i < data.velocityFilter.size() && data.velocityFilter.at(i) && !std::isnan(node.second->velocity))
+ if (i < data.velocityFilter.size() && data.velocityFilter.at(i) &&
+ !std::isnan(node.second->velocity))
{
- data.qDot(i) = data.velocityFilter.at(i)->update(sensorValuesTimestamp, node.second->velocity);
+ data.qDot(i) = data.velocityFilter.at(i)->update(sensorValuesTimestamp,
+ node.second->velocity);
}
else
{
data.qDot(i) = node.second->velocity;
}
- if (i < data.accelerationFilter.size() && data.accelerationFilter.at(i) && !std::isnan(node.second->acceleration))
+ if (i < data.accelerationFilter.size() && data.accelerationFilter.at(i) &&
+ !std::isnan(node.second->acceleration))
{
- data.qDDot(i) = data.accelerationFilter.at(i)->update(sensorValuesTimestamp, node.second->acceleration);
+ data.qDDot(i) = data.accelerationFilter.at(i)->update(
+ sensorValuesTimestamp, node.second->acceleration);
}
else
{
@@ -102,10 +113,7 @@ namespace armarx
}
// calculate external torques (tau) applied to robot induced by dynamics
dynamics.getGravityMatrix(data.q, data.tauGravity);
- dynamics.getInverseDynamics(data.q,
- data.qDot,
- data.qDDot,
- data.tau);
+ dynamics.getInverseDynamics(data.q, data.qDot, data.qDDot, data.tau);
// update virtual sensor values
size_t i = 0;
for (auto& node : data.nodeSensorVec)
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
index de313e9cee23c43e52a54617cbbbf29b2168ef72..edcf445b56cc0354889cce4475147aabf1605c2b 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
@@ -22,49 +22,58 @@
* GNU General Public License
*/
#pragma once
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Dynamics/Dynamics.h>
#include <Eigen/Core>
+
+#include <VirtualRobot/Dynamics/Dynamics.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
namespace armarx::rtfilters
{
class RTFilterBase;
using RTFilterBasePtr = std::shared_ptr<RTFilterBase>;
-}
+} // namespace armarx::rtfilters
namespace armarx
{
class RobotUnit;
+
class DynamicsHelper
{
public:
struct DynamicsData
{
- DynamicsData(VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies) :
- rnsJoints(rnsJoints), rnsBodies(rnsBodies),
- dynamics(rnsJoints, rnsBodies)
+ DynamicsData(VirtualRobot::RobotNodeSetPtr rnsJoints,
+ VirtualRobot::RobotNodeSetPtr rnsBodies) :
+ rnsJoints(rnsJoints), rnsBodies(rnsBodies), dynamics(rnsJoints, rnsBodies)
{
q = qDot = qDDot = tau = tauGravity = Eigen::VectorXd::Zero(rnsJoints->getSize());
}
+
DynamicsData(const DynamicsData& r) = default;
VirtualRobot::RobotNodeSetPtr rnsJoints, rnsBodies;
VirtualRobot::Dynamics dynamics;
Eigen::VectorXd q, qDot, qDDot, tau, tauGravity;
- std::vector<std::pair<VirtualRobot::RobotNodePtr, SensorValue1DoFActuator*>> nodeSensorVec;
+ std::vector<std::pair<VirtualRobot::RobotNodePtr, SensorValue1DoFActuator*>>
+ nodeSensorVec;
std::vector<rtfilters::RTFilterBasePtr> velocityFilter;
std::vector<rtfilters::RTFilterBasePtr> accelerationFilter;
};
DynamicsHelper(RobotUnit* robotUnit);
~DynamicsHelper() = default;
- void addRobotPart(VirtualRobot::RobotNodeSetPtr rnsJoints, VirtualRobot::RobotNodeSetPtr rnsBodies,
- rtfilters::RTFilterBasePtr velocityFilter = rtfilters::RTFilterBasePtr(),
- rtfilters::RTFilterBasePtr accelerationFilter = rtfilters::RTFilterBasePtr());
- void update(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void
+ addRobotPart(VirtualRobot::RobotNodeSetPtr rnsJoints,
+ VirtualRobot::RobotNodeSetPtr rnsBodies,
+ rtfilters::RTFilterBasePtr velocityFilter = rtfilters::RTFilterBasePtr(),
+ rtfilters::RTFilterBasePtr accelerationFilter = rtfilters::RTFilterBasePtr());
+ void update(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration);
+
protected:
RobotUnit* robotUnit;
std::map<VirtualRobot::RobotNodeSetPtr, DynamicsData> dataMap;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.h b/source/RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.h
index 490f247702f1daa761c9b61c9f8959ae9e5dbadd..3d589313c9f40cd8b369db450006f7f25369e9b2 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/EigenForwardDeclarations.h
@@ -23,41 +23,41 @@
namespace Eigen
{
- template<typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+ template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
class Matrix;
-#define ax_eigen_fwd_make_matrix_and_vector(Type, TSuff, Size, SzSuff) \
- typedef Matrix<Type, Size, Size, 0, Size, Size> Matrix##SzSuff##TSuff; \
- typedef Matrix<Type, Size, 1 , 0, Size, 1 > Vector##SzSuff##TSuff;
+#define ax_eigen_fwd_make_matrix_and_vector(Type, TSuff, Size, SzSuff) \
+ typedef Matrix<Type, Size, Size, 0, Size, Size> Matrix##SzSuff##TSuff; \
+ typedef Matrix<Type, Size, 1, 0, Size, 1> Vector##SzSuff##TSuff;
-#define ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, Size) \
- typedef Matrix<Type, Size, -1,0, Size, -1> Matrix##Size##X##TypeSuffix; \
- typedef Matrix<Type, -1, Size,0, -1, Size> Matrix##X##Size##TypeSuffix;
+#define ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, Size) \
+ typedef Matrix<Type, Size, -1, 0, Size, -1> Matrix##Size##X##TypeSuffix; \
+ typedef Matrix<Type, -1, Size, 0, -1, Size> Matrix##X##Size##TypeSuffix;
-#define ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(Type, TypeSuffix) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 2, 2) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 3, 3) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 4, 4) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 5, 5) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 6, 6) \
- ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, -1, X) \
- ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 2) \
- ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 3) \
- ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 4) \
- ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 5) \
- ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 6)
+#define ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(Type, TypeSuffix) \
+ ax_eigen_fwd_make_matrix_and_vector( \
+ Type, TypeSuffix, 2, 2) ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 3, 3) \
+ ax_eigen_fwd_make_matrix_and_vector( \
+ Type, TypeSuffix, 4, 4) ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 5, 5) \
+ ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, 6, 6) \
+ ax_eigen_fwd_make_matrix_and_vector(Type, TypeSuffix, -1, X) \
+ ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 2) \
+ ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 3) \
+ ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 4) \
+ ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 5) \
+ ax_eigen_fwd_make_matrix_one_dynamic_dim(Type, TypeSuffix, 6)
- ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(int, i)
- ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(float, f)
- ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(double, d)
+ ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(int, i)
+ ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(float, f)
+ ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES(double, d)
#undef ax_eigen_fwd_make_matrix_and_vector_ALL_SIZES
#undef ax_eigen_fwd_make_matrix_and_vector
#undef ax_eigen_fwd_make_matrix_one_dynamic_dim
- template<typename _Scalar, int _Options>
- class Quaternion;
+ template <typename _Scalar, int _Options>
+ class Quaternion;
using Quaternionf = Quaternion<float, 0>;
using Quaterniond = Quaternion<double, 0>;
-}
+} // namespace Eigen
diff --git a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
index 8539b52e860fc21b785fad49e6d1a0c342c92ae1..a7d0ed6bc06bde7a1d48242070fbb76448636a68 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainer.h
@@ -25,15 +25,16 @@
#include <vector>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
#include <ArmarXCore/core/util/PropagateConst.h>
+#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
#include "HeterogenousContinuousContainerMacros.h"
-#if __GNUC__< 5 && !defined(__clang__)
+#if __GNUC__ < 5 && !defined(__clang__)
namespace std
{
- inline void* align(size_t alignment, size_t bytes, void*& bufferPlace, size_t& bufferSpace) noexcept
+ inline void*
+ align(size_t alignment, size_t bytes, void*& bufferPlace, size_t& bufferSpace) noexcept
{
const auto uiptr = reinterpret_cast<uintptr_t>(bufferPlace);
const auto alignedPlace = (uiptr - 1u + alignment) & -alignment;
@@ -48,39 +49,50 @@ namespace std
return bufferPlace = reinterpret_cast<void*>(alignedPlace);
}
}
-}
+} // namespace std
#endif
namespace armarx::detail
{
- template<class Base>
+ template <class Base>
class HeterogenousContinuousContainerBase
{
public:
- bool empty() const
+ bool
+ empty() const
{
return begin_ == current_;
}
- bool owning() const
+
+ bool
+ owning() const
{
return storage_ != nullptr;
}
- std::size_t getRemainingStorageCapacity() const
+
+ std::size_t
+ getRemainingStorageCapacity() const
{
return static_cast<const std::uint8_t*>(end_) -
static_cast<const std::uint8_t*>(current_);
}
- std::size_t getStorageCapacity() const
+
+ std::size_t
+ getStorageCapacity() const
{
return static_cast<const std::uint8_t*>(end_) -
static_cast<const std::uint8_t*>(begin_);
}
- std::size_t getUsedStorageCapacity() const
+
+ std::size_t
+ getUsedStorageCapacity() const
{
return static_cast<const std::uint8_t*>(current_) -
static_cast<const std::uint8_t*>(begin_);
}
- void assignStorage(void* begin, void* end)
+
+ void
+ assignStorage(void* begin, void* end)
{
ARMARX_CHECK_EXPRESSION(empty());
ARMARX_CHECK_LESS_EQUAL(begin, end);
@@ -89,7 +101,9 @@ namespace armarx::detail
current_ = begin;
end_ = end;
}
- void setStorageCapacity(std::size_t sz)
+
+ void
+ setStorageCapacity(std::size_t sz)
{
ARMARX_CHECK_EXPRESSION(empty());
if (!sz)
@@ -104,14 +118,15 @@ namespace armarx::detail
current_ = begin_;
end_ = static_cast<void*>(storage_.get() + sz);
}
+
protected:
- template<class Derived>
- Base* pushBack(const Derived* d)
+ template <class Derived>
+ Base*
+ pushBack(const Derived* d)
{
static_assert(
std::is_base_of<Base, Derived>::value,
- "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base"
- );
+ "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base");
std::size_t space_ = getRemainingStorageCapacity();
void* ptr = std::align(d->_alignof(), d->_sizeof(), current_, space_);
@@ -123,37 +138,42 @@ namespace armarx::detail
current_ = static_cast<void*>(static_cast<std::uint8_t*>(ptr) + d->_sizeof());
return d->_placementCopyConstruct(ptr);
}
- void clear()
+
+ void
+ clear()
{
current_ = begin_;
}
+
private:
- std::unique_ptr<std::uint8_t[]> storage_ {nullptr};
- void* begin_ {nullptr};
- void* current_ {nullptr};
- void* end_ {nullptr};
+ std::unique_ptr<std::uint8_t[]> storage_{nullptr};
+ void* begin_{nullptr};
+ void* current_{nullptr};
+ void* end_{nullptr};
};
-}
+} // namespace armarx::detail
namespace armarx
{
- template<class Base, bool UsePropagateConst = true>
+ template <class Base, bool UsePropagateConst = true>
class HeterogenousContinuousContainer : public detail::HeterogenousContinuousContainerBase<Base>
{
using BaseContainer = detail::HeterogenousContinuousContainerBase<Base>;
+
public:
- using ElementType = typename std::conditional <
- UsePropagateConst,
- PropagateConst<Base*>,
- Base *
- >::type;
+ using ElementType =
+ typename std::conditional<UsePropagateConst, PropagateConst<Base*>, Base*>::type;
HeterogenousContinuousContainer() = default;
HeterogenousContinuousContainer(HeterogenousContinuousContainer&&) = default;
- HeterogenousContinuousContainer(const HeterogenousContinuousContainer& other, bool compressElems = false)
+
+ HeterogenousContinuousContainer(const HeterogenousContinuousContainer& other,
+ bool compressElems = false)
{
- this->setStorageCapacity(compressElems ? other.getUsedStorageCapacity() : other.getStorageCapacity());
- setElementCapacity(compressElems ? other.elements().size() : other.elements().capacity());
+ this->setStorageCapacity(compressElems ? other.getUsedStorageCapacity()
+ : other.getStorageCapacity());
+ setElementCapacity(compressElems ? other.elements().size()
+ : other.elements().capacity());
for (auto& e : other.elements())
{
pushBack(e);
@@ -161,7 +181,9 @@ namespace armarx
}
HeterogenousContinuousContainer& operator=(HeterogenousContinuousContainer&&) = default;
- HeterogenousContinuousContainer& operator=(const HeterogenousContinuousContainer& other)
+
+ HeterogenousContinuousContainer&
+ operator=(const HeterogenousContinuousContainer& other)
{
clear();
setStorageCapacity(other.getStorageCapacity());
@@ -178,37 +200,43 @@ namespace armarx
clear();
}
- std::size_t getElementCount() const
+ std::size_t
+ getElementCount() const
{
return elements_.size();
}
- std::size_t getElementCapacity() const
+
+ std::size_t
+ getElementCapacity() const
{
return elements_.capacity();
}
- std::size_t getRemainingElementCapacity() const
+
+ std::size_t
+ getRemainingElementCapacity() const
{
return getElementCapacity() - getElementCount();
}
- void setElementCapacity(std::size_t cnt)
+ void
+ setElementCapacity(std::size_t cnt)
{
ARMARX_CHECK_EXPRESSION(this->empty());
if (elements_.capacity() > cnt)
{
//force the capacity to reduce
- elements_ = std::vector<ElementType> {};
+ elements_ = std::vector<ElementType>{};
}
elements_.reserve(cnt);
}
- template<class Derived>
- Base* pushBack(const Derived* d)
+ template <class Derived>
+ Base*
+ pushBack(const Derived* d)
{
static_assert(
std::is_base_of<Base, Derived>::value,
- "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base"
- );
+ "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base");
ARMARX_CHECK_NOT_NULL(d);
if (!getRemainingElementCapacity())
{
@@ -221,27 +249,35 @@ namespace armarx
}
return ptr;
}
- template<class Derived>
- Base* pushBack(const PropagateConst<Derived*>& d)
+
+ template <class Derived>
+ Base*
+ pushBack(const PropagateConst<Derived*>& d)
{
return pushBack(*d);
}
- template<class Derived>
- Base* pushBack(const Derived& d)
+
+ template <class Derived>
+ Base*
+ pushBack(const Derived& d)
{
return pushBack(&d);
}
- std::vector<ElementType>& elements()
+ std::vector<ElementType>&
+ elements()
{
return elements_;
}
- const std::vector<ElementType>& elements() const
+
+ const std::vector<ElementType>&
+ elements() const
{
return elements_;
}
- void clear()
+ void
+ clear()
{
for (auto& e : elements_)
{
@@ -250,26 +286,29 @@ namespace armarx
elements_.clear();
BaseContainer::clear();
}
+
private:
std::vector<ElementType> elements_;
};
- template<class Base, bool UsePropagateConst = true>
- class HeterogenousContinuous2DContainer : public detail::HeterogenousContinuousContainerBase<Base>
+ template <class Base, bool UsePropagateConst = true>
+ class HeterogenousContinuous2DContainer :
+ public detail::HeterogenousContinuousContainerBase<Base>
{
using BaseContainer = detail::HeterogenousContinuousContainerBase<Base>;
+
public:
- using ElementType = typename std::conditional <
- UsePropagateConst,
- PropagateConst<Base*>,
- Base *
- >::type;
+ using ElementType =
+ typename std::conditional<UsePropagateConst, PropagateConst<Base*>, Base*>::type;
HeterogenousContinuous2DContainer() = default;
HeterogenousContinuous2DContainer(HeterogenousContinuous2DContainer&&) = default;
- HeterogenousContinuous2DContainer(const HeterogenousContinuous2DContainer& other, bool compressElems = false)
+
+ HeterogenousContinuous2DContainer(const HeterogenousContinuous2DContainer& other,
+ bool compressElems = false)
{
- setStorageCapacity(compressElems ? other.getUsedStorageCapacity() : other.getStorageCapacity());
+ setStorageCapacity(compressElems ? other.getUsedStorageCapacity()
+ : other.getStorageCapacity());
std::vector<std::size_t> elemCaps;
elemCaps.reserve(other.elements().size());
for (const auto& d1 : other.elements())
@@ -287,7 +326,9 @@ namespace armarx
}
HeterogenousContinuous2DContainer& operator=(HeterogenousContinuous2DContainer&&) = default;
- HeterogenousContinuous2DContainer& operator=(const HeterogenousContinuous2DContainer& other)
+
+ HeterogenousContinuous2DContainer&
+ operator=(const HeterogenousContinuous2DContainer& other)
{
clear();
setStorageCapacity(other.getStorageCapacity());
@@ -313,20 +354,26 @@ namespace armarx
clear();
}
- std::size_t getElementCount(std::size_t d0) const
+ std::size_t
+ getElementCount(std::size_t d0) const
{
return elements_.at(d0).size();
}
- std::size_t getElementCapacity(std::size_t d0) const
+
+ std::size_t
+ getElementCapacity(std::size_t d0) const
{
return elements_.at(d0).capacity();
}
- std::size_t getRemainingElementCapacity(std::size_t d0) const
+
+ std::size_t
+ getRemainingElementCapacity(std::size_t d0) const
{
return getElementCapacity(d0) - getElementCount(d0);
}
- void setElementCapacity(const std::vector<std::size_t>& cnt)
+ void
+ setElementCapacity(const std::vector<std::size_t>& cnt)
{
ARMARX_CHECK_EXPRESSION(this->empty());
elements_.resize(cnt.size());
@@ -335,19 +382,19 @@ namespace armarx
if (elements_.at(i).capacity() > cnt)
{
//force the capacity to reduce
- elements_.at(i) = std::vector<ElementType> {};
+ elements_.at(i) = std::vector<ElementType>{};
}
elements_.at(i).reserve(cnt.at(i));
}
}
- template<class Derived>
- Base* pushBack(std::size_t d0, const Derived* d)
+ template <class Derived>
+ Base*
+ pushBack(std::size_t d0, const Derived* d)
{
static_assert(
std::is_base_of<Base, Derived>::value,
- "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base"
- );
+ "HeterogenousContinuousContainerBase::pushBack: Derived must derive Base");
ARMARX_CHECK_NOT_NULL(d);
if (!getRemainingElementCapacity(d0))
{
@@ -360,27 +407,35 @@ namespace armarx
}
return ptr;
}
- template<class Derived>
- Base* pushBack(std::size_t d0, const PropagateConst<Derived*>& d)
+
+ template <class Derived>
+ Base*
+ pushBack(std::size_t d0, const PropagateConst<Derived*>& d)
{
return pushBack(d0, *d);
}
- template<class Derived>
- Base* pushBack(std::size_t d0, const Derived& d)
+
+ template <class Derived>
+ Base*
+ pushBack(std::size_t d0, const Derived& d)
{
return pushBack(d0, &d);
}
- std::vector<std::vector<ElementType>>& elements()
+ std::vector<std::vector<ElementType>>&
+ elements()
{
return elements_;
}
- const std::vector<std::vector<ElementType>>& elements() const
+
+ const std::vector<std::vector<ElementType>>&
+ elements() const
{
return elements_;
}
- void clear()
+ void
+ clear()
{
for (auto& d1 : elements_)
{
@@ -392,7 +447,8 @@ namespace armarx
}
BaseContainer::clear();
}
+
private:
std::vector<std::vector<ElementType>> elements_;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h
index 07415ee68c3bb1b1f97d3667b90981ab64448351..5ee3b4e087522de3615600cbccf0f895ee2334bb 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h
@@ -25,64 +25,61 @@
#include <memory>
#include <type_traits>
-#define ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
- using _typeBase = CommonBaseType; \
- virtual std::size_t _alignof() const = 0; \
- virtual std::size_t _sizeof() const = 0; \
- virtual _typeBase* _placementCopyConstruct(void* place) const = 0; \
- std::size_t _accumulateSize(std::size_t offset = 0) const \
- { \
- return offset + \
- (_alignof() - (offset % _alignof())) % _alignof() + \
- _sizeof(); \
+#define ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
+ using _typeBase = CommonBaseType; \
+ virtual std::size_t _alignof() const = 0; \
+ virtual std::size_t _sizeof() const = 0; \
+ virtual _typeBase* _placementCopyConstruct(void* place) const = 0; \
+ std::size_t _accumulateSize(std::size_t offset = 0) const \
+ { \
+ return offset + (_alignof() - (offset % _alignof())) % _alignof() + _sizeof(); \
}
-#define ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
- ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
- virtual void _copyTo(_typeBase* target) const = 0; \
- virtual std::unique_ptr<_typeBase> _clone() const = 0; \
+#define ARMARX_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER_BASE(CommonBaseType) \
+ virtual void _copyTo(_typeBase* target) const = 0; \
+ virtual std::unique_ptr<_typeBase> _clone() const = 0; \
virtual _typeBase* _placementConstruct(void* place) const = 0;
-#define ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
- std::size_t _alignof() const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- return alignof(_type); \
- } \
- std::size_t _sizeof() const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- return sizeof(_type); \
- } \
- _typeBase* _placementCopyConstruct(void* place) const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- return new(place) _type(*this); \
- } \
- void _checkBaseType() \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- static_assert( \
- std::is_base_of<_typeBase, _type>::value, \
- "This class has to derive the common base class"); \
+#define ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
+ std::size_t _alignof() const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return alignof(_type); \
+ } \
+ std::size_t _sizeof() const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return sizeof(_type); \
+ } \
+ _typeBase* _placementCopyConstruct(void* place) const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return new (place) _type(*this); \
+ } \
+ void _checkBaseType() \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ static_assert(std::is_base_of<_typeBase, _type>::value, \
+ "This class has to derive the common base class"); \
}
-#define ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
- ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
- void _copyTo(_typeBase* target) const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- _type* const castedTarget = dynamic_cast<_type*>(target); \
- ARMARX_CHECK_NOT_NULL(castedTarget); \
- *castedTarget = *this; \
- } \
- std::unique_ptr<_typeBase> _clone() const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- return std::unique_ptr<_typeBase>{new _type(*this)}; \
- } \
- _typeBase* _placementConstruct(void* place) const override \
- { \
- using _type = typename std::decay<decltype(*this)>::type; \
- return new(place) _type; \
+#define ARMARX_PLACEMENT_CONSTRUCTION_HELPER \
+ ARMARX_MINIMAL_PLACEMENT_CONSTRUCTION_HELPER \
+ void _copyTo(_typeBase* target) const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ _type* const castedTarget = dynamic_cast<_type*>(target); \
+ ARMARX_CHECK_NOT_NULL(castedTarget); \
+ *castedTarget = *this; \
+ } \
+ std::unique_ptr<_typeBase> _clone() const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return std::unique_ptr<_typeBase>{new _type(*this)}; \
+ } \
+ _typeBase* _placementConstruct(void* place) const override \
+ { \
+ using _type = typename std::decay<decltype(*this)>::type; \
+ return new (place) _type; \
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h b/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
index 84809506f96ca0f0d48211fe2b7511f75b08d8f9..c3c645936a9cab1e9d61baee4ef2640e6ba4e0f4 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/JointAndNJointControllers.h
@@ -27,16 +27,19 @@ namespace armarx
{
class JointController;
class NJointControllerBase;
+
/// @brief Structure used by the RobotUnit to swap lists of Joint and NJoint controllers
struct JointAndNJointControllers
{
- JointAndNJointControllers(std::size_t n = 0)
- : jointControllers(n),
- nJointControllers(n),
- jointToNJointControllerAssignement(n, Sentinel())
- {}
+ JointAndNJointControllers(std::size_t n = 0) :
+ jointControllers(n),
+ nJointControllers(n),
+ jointToNJointControllerAssignement(n, Sentinel())
+ {
+ }
- void resetAssignement()
+ void
+ resetAssignement()
{
jointToNJointControllerAssignement.assign(jointControllers.size(), Sentinel());
}
@@ -46,9 +49,10 @@ namespace armarx
/// @brief this is set by RobotUnit::writeRequestedControllers
std::vector<std::size_t> jointToNJointControllerAssignement;
- static constexpr std::size_t Sentinel()
+ static constexpr std::size_t
+ Sentinel()
{
return std::numeric_limits<std::size_t>::max();
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h b/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h
index be82c776f26e326f76642dda211d662a2b9e13f9..01dce758fa86860eeaf1524d58433450179acaf6 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h
@@ -21,4 +21,3 @@
*/
#pragma once
#include <ArmarXCore/util/CPPUtility/KeyValueVector.h>
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
index a9feb50c732672846a87813a87c9a309bc3c1aad..243f1a119df65ab1f5a9f79a5f53d243fe8b102c 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
@@ -29,27 +29,31 @@
namespace armarx
{
- inline IceUtil::Time rtNow()
+
+ inline IceUtil::Time
+ rtNow()
{
- return IceUtil::Time::now(IceUtil::Time::Monotonic);
- // using namespace std::chrono;
- // auto epoch = time_point_cast<microseconds>(high_resolution_clock::now()).time_since_epoch();
- // return IceUtil::Time::microSeconds(duration_cast<milliseconds>(epoch).count());
+ struct timespec ts;
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ return IceUtil::Time::microSeconds(ts.tv_sec * 1e6 + ts.tv_nsec / 1000);
}
-}
+} // namespace armarx
#define RT_TIMING_START(name) auto name = armarx::rtNow();
//! \ingroup VirtualTime
//! Prints duration with comment in front of it, yet only once per second.
-#define RT_TIMING_END_COMMENT(name, comment) ARMARX_RT_LOGF_INFO("%s - duration: %.3f ms", comment, IceUtil::Time(armarx::rtNow()-name).toMilliSecondsDouble()).deactivateSpam(1);
+#define RT_TIMING_END_COMMENT(name, comment) \
+ ARMARX_RT_LOGF_INFO( \
+ "%s - duration: %.3f ms", comment, (armarx::rtNow() - name).toMilliSecondsDouble()) \
+ .deactivateSpam(1);
//! \ingroup VirtualTime
//! Prints duration
-#define RT_TIMING_END(name) RT_TIMING_END_COMMENT(name,#name)
+#define RT_TIMING_END(name) RT_TIMING_END_COMMENT(name, #name)
//! \ingroup VirtualTime
//! Prints duration with comment in front of it if it took longer than threshold
-#define RT_TIMING_CEND_COMMENT(name, comment, thresholdMs) if((armarx::rtNow()-name).toMilliSecondsDouble() >= thresholdMs) RT_TIMING_END_COMMENT(name, comment)
+#define RT_TIMING_CEND_COMMENT(name, comment, thresholdMs) \
+ if ((armarx::rtNow() - name).toMilliSeconds() >= thresholdMs) \
+ RT_TIMING_END_COMMENT(name, comment)
//! \ingroup VirtualTime
//! Prints duration if it took longer than thresholdMs
#define RT_TIMING_CEND(name, thresholdMs) RT_TIMING_CEND_COMMENT(name, #name, thresholdMs)
-
-
diff --git a/source/RobotAPI/components/units/RobotUnit/util/Time.h b/source/RobotAPI/components/units/RobotUnit/util/Time.h
index 9199a9714d09951ef6ae4df3f9b0f82f335a9845..f4aa7903585bade190632881c41d2894c9b5a33d 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/Time.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/Time.h
@@ -23,12 +23,13 @@
#pragma once
#include <chrono>
+
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
namespace armarx::detail
{
- template<class TimeT = std::chrono::microseconds>
+ template <class TimeT = std::chrono::microseconds>
struct TimerTag
{
//assume it is std::chrono
@@ -36,15 +37,18 @@ namespace armarx::detail
using ClockT = std::chrono::high_resolution_clock;
const ClockT::time_point beg;
- TimerTag() : beg {ClockT::now()} {}
+ TimerTag() : beg{ClockT::now()}
+ {
+ }
- TimeT stop()
+ TimeT
+ stop()
{
return std::chrono::duration_cast<TimeT>(ClockT::now() - beg);
}
};
- template<>
+ template <>
struct TimerTag<long>
{
//return micro seconds as long
@@ -52,34 +56,41 @@ namespace armarx::detail
using ClockT = std::chrono::high_resolution_clock;
const ClockT::time_point beg;
- TimerTag() : beg {ClockT::now()} {}
+ TimerTag() : beg{ClockT::now()}
+ {
+ }
- long stop()
+ long
+ stop()
{
- return std::chrono::duration_cast<std::chrono::microseconds>(ClockT::now() - beg).count();
+ return std::chrono::duration_cast<std::chrono::microseconds>(ClockT::now() - beg)
+ .count();
}
};
- template<>
+ template <>
struct TimerTag<TimestampVariant> : TimerTag<>
{
- TimestampVariant stop()
+ TimestampVariant
+ stop()
{
return {TimerTag<std::chrono::microseconds>::stop().count()};
}
};
- template<>
+ template <>
struct TimerTag<IceUtil::Time> : TimerTag<>
{
- TimestampVariant stop()
+ TimestampVariant
+ stop()
{
return IceUtil::Time::microSeconds(TimerTag<std::chrono::microseconds>::stop().count());
}
};
template <class Fun, class TimeT>
- TimeT operator*(TimerTag<TimeT>&& t, Fun&& fn)
+ TimeT
+ operator*(TimerTag<TimeT>&& t, Fun&& fn)
{
fn();
return t.stop();
@@ -87,38 +98,49 @@ namespace armarx::detail
//for virtual time
- template<class TimeT = IceUtil::Time>
+ template <class TimeT = IceUtil::Time>
struct VirtualTimerTag;
- template<>
+ template <>
struct VirtualTimerTag<TimestampVariant>
{
const IceUtil::Time beg;
- VirtualTimerTag() : beg {TimeUtil::GetTime()} {}
- TimestampVariant stop()
+
+ VirtualTimerTag() : beg{TimeUtil::GetTime()}
+ {
+ }
+
+ TimestampVariant
+ stop()
{
return {TimeUtil::GetTime() - beg};
}
};
- template<>
+ template <>
struct VirtualTimerTag<IceUtil::Time>
{
const IceUtil::Time beg;
- VirtualTimerTag() : beg {TimeUtil::GetTime()} {}
- TimestampVariant stop()
+
+ VirtualTimerTag() : beg{TimeUtil::GetTime()}
+ {
+ }
+
+ TimestampVariant
+ stop()
{
return TimeUtil::GetTime() - beg;
}
};
template <class Fun, class TimeT>
- TimeT operator*(VirtualTimerTag<TimeT>&& t, Fun&& fn)
+ TimeT
+ operator*(VirtualTimerTag<TimeT>&& t, Fun&& fn)
{
fn();
return t.stop();
}
-}
+} // namespace armarx::detail
-#define ARMARX_STOPWATCH(...) ::armarx::detail::TimerTag<__VA_ARGS__>{} *[&]
-#define ARMARX_VIRTUAL_STOPWATCH(...) ::armarx::detail::VirtualTimerTag<__VA_ARGS__>{} *[&]
+#define ARMARX_STOPWATCH(...) ::armarx::detail::TimerTag<__VA_ARGS__>{}* [&]
+#define ARMARX_VIRTUAL_STOPWATCH(...) ::armarx::detail::VirtualTimerTag<__VA_ARGS__>{}* [&]
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index ccb1d28518ea3e30c3cb6cde8347cd1c3fdf8978..129771f55fec581f806de5e09d8c7a7d928f2ac5 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -21,34 +21,36 @@
*/
#pragma once
-#include "ClassMemberInfoEntry.h"
-
#include <ArmarXCore/util/CPPUtility/KeyValueVector.h>
+#include "ClassMemberInfoEntry.h"
namespace armarx::introspection
{
/**
* @brief
*/
- template<class CommonBaseT, class ClassT>
+ template <class CommonBaseT, class ClassT>
struct ClassMemberInfo
{
using ClassType = ClassT;
using CommonBaseType = CommonBaseT;
- static_assert(std::is_base_of<CommonBaseType, ClassType>::value, "The class has to inherit the common base class");
+ static_assert(std::is_base_of<CommonBaseType, ClassType>::value,
+ "The class has to inherit the common base class");
using Entry = introspection::ClassMemberInfoEntry<CommonBaseType>;
- template<class T>
- using EntryConfigurator = introspection::ClassMemberInfoEntryConfigurator<CommonBaseType, T>;
+ template <class T>
+ using EntryConfigurator =
+ introspection::ClassMemberInfoEntryConfigurator<CommonBaseType, T>;
static const ClassMemberInfo<CommonBaseT, ClassT>& GetInstance();
/// @brief add a member variable of the current class
- template<class MemberType>
- EntryConfigurator<ClassType> addMemberVariable(MemberType ClassType::* ptr, const std::string& name);
+ template <class MemberType>
+ EntryConfigurator<ClassType> addMemberVariable(MemberType ClassType::*ptr,
+ const std::string& name);
/// @brief add all variables of a base class of the current class
- template<class BaseClassType>
+ template <class BaseClassType>
void addBaseClass();
/// @brief Get the name of the current class
@@ -56,35 +58,40 @@ namespace armarx::introspection
/// @brief Get all entries for member variables
static const KeyValueVector<std::string, Entry>& GetEntries();
static std::size_t GetNumberOfDataFields();
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, bool& out);
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Byte& out);
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Short& out);
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Int& out);
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Long& out);
- static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Float& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, bool& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Byte& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Short& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Int& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Long& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Float& out);
static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Double& out);
static void GetDataFieldAs(const ClassType* ptr, std::size_t i, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static std::vector<std::string> GetDataFieldNames();
- static std::map<std::string, VariantBasePtr> ToVariants(const IceUtil::Time& timestamp, const CommonBaseT* ptr);
+ static std::map<std::string, VariantBasePtr> ToVariants(const IceUtil::Time& timestamp,
+ const CommonBaseT* ptr);
+
private:
- template<class T>
- static auto MakeConverter()
+ template <class T>
+ static auto
+ MakeConverter()
{
ARMARX_TRACE;
std::vector<std::function<void(const ClassType*, T&)>> functions;
for (std::size_t idxEntr = 0; idxEntr < GetEntries().size(); ++idxEntr)
{
- for (std::size_t idxField = 0; idxField < GetEntries().at(idxEntr).getNumberOfFields(); ++idxField)
+ for (std::size_t idxField = 0;
+ idxField < GetEntries().at(idxEntr).getNumberOfFields();
+ ++idxField)
{
functions.emplace_back(
- [idxEntr, idxField](const ClassType * classptr, T & out)
- {
- const Entry& e = GetEntries().at(idxEntr);
- e.getDataFieldAs(idxField, classptr, out);
- });
+ [idxEntr, idxField](const ClassType* classptr, T& out)
+ {
+ const Entry& e = GetEntries().at(idxEntr);
+ e.getDataFieldAs(idxField, classptr, out);
+ });
}
}
ARMARX_CHECK_EQUAL(functions.size(), GetNumberOfDataFields());
@@ -95,34 +102,39 @@ namespace armarx::introspection
KeyValueVector<std::string, Entry> entries;
std::set<std::string> addedBases;
};
-}
-
+} // namespace armarx::introspection
//implementation
namespace armarx::introspection
{
- template<class CommonBaseT, class ClassT>
- template<class MemberType>
- typename ClassMemberInfo<CommonBaseT, ClassT>::template EntryConfigurator<ClassT> ClassMemberInfo<CommonBaseT, ClassT>::addMemberVariable(MemberType ClassType::* ptr, const std::string& name)
+ template <class CommonBaseT, class ClassT>
+ template <class MemberType>
+ typename ClassMemberInfo<CommonBaseT, ClassT>::template EntryConfigurator<ClassT>
+ ClassMemberInfo<CommonBaseT, ClassT>::addMemberVariable(MemberType ClassType::*ptr,
+ 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);
}
- template<class CommonBaseT, class ClassT>
- template<class BaseClassType>
- void ClassMemberInfo<CommonBaseT, ClassT>::addBaseClass()
+ template <class CommonBaseT, class ClassT>
+ template <class BaseClassType>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::addBaseClass()
{
ARMARX_TRACE;
if (std::is_same<BaseClassType, CommonBaseType>::value)
{
return;
}
- static_assert(std::is_base_of<CommonBaseType, BaseClassType>::value, "The base class has to inherit the common base class");
- static_assert(std::is_base_of<BaseClassType, ClassType>::value, "The base class has to be a base class");
- static_assert(!std::is_same<BaseClassType, ClassType>::value, "The base class has must not be the class");
+ static_assert(std::is_base_of<CommonBaseType, BaseClassType>::value,
+ "The base class has to inherit the common base class");
+ static_assert(std::is_base_of<BaseClassType, ClassType>::value,
+ "The base class has to be a base class");
+ static_assert(!std::is_same<BaseClassType, ClassType>::value,
+ "The base class has must not be the class");
std::set<std::string> basesAddedInCall;
if (addedBases.count(ClassMemberInfo<CommonBaseType, BaseClassType>::GetClassName()))
@@ -133,11 +145,11 @@ namespace armarx::introspection
{
if (!addedBases.count(e.getClassName()))
{
- ARMARX_CHECK_EXPRESSION(
- !entries.count(e.getMemberName())) <<
- "Adding the base class '" << GetTypeString<BaseClassType>()
- << "' adds an entry for the member '" << e.getMemberName()
- << "' which already was added by class '" << entries.at(e.getMemberName()).getClassName() << "'";
+ ARMARX_CHECK_EXPRESSION(!entries.count(e.getMemberName()))
+ << "Adding the base class '" << GetTypeString<BaseClassType>()
+ << "' adds an entry for the member '" << e.getMemberName()
+ << "' which already was added by class '"
+ << entries.at(e.getMemberName()).getClassName() << "'";
basesAddedInCall.emplace(e.getClassName());
entries.add(e.getMemberName(), e);
}
@@ -145,8 +157,9 @@ namespace armarx::introspection
addedBases.insert(basesAddedInCall.begin(), basesAddedInCall.end());
}
- template<class CommonBaseT, class ClassT>
- std::vector<std::string> ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldNames()
+ template <class CommonBaseT, class ClassT>
+ std::vector<std::string>
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldNames()
{
ARMARX_TRACE;
std::vector<std::string> dataFieldNames;
@@ -162,8 +175,10 @@ namespace armarx::introspection
return dataFieldNames;
}
- template<class CommonBaseT, class ClassT>
- std::map<std::string, VariantBasePtr> ClassMemberInfo<CommonBaseT, ClassT>::ToVariants(const IceUtil::Time& timestamp, const CommonBaseT* ptr)
+ template <class CommonBaseT, class ClassT>
+ std::map<std::string, VariantBasePtr>
+ ClassMemberInfo<CommonBaseT, ClassT>::ToVariants(const IceUtil::Time& timestamp,
+ const CommonBaseT* ptr)
{
ARMARX_TRACE;
std::map<std::string, VariantBasePtr> result;
@@ -174,7 +189,8 @@ namespace armarx::introspection
{
if (result.count(elem.first))
{
- throw std::invalid_argument {"mergeMaps: newMap would override values from oldMap"};
+ throw std::invalid_argument{
+ "mergeMaps: newMap would override values from oldMap"};
}
}
for (auto& elem : newMap)
@@ -186,27 +202,31 @@ namespace armarx::introspection
return result;
}
- template<class CommonBaseT, class ClassT>
- const ClassMemberInfo<CommonBaseT, ClassT>& ClassMemberInfo<CommonBaseT, ClassT>::GetInstance()
+ template <class CommonBaseT, class ClassT>
+ const ClassMemberInfo<CommonBaseT, ClassT>&
+ ClassMemberInfo<CommonBaseT, ClassT>::GetInstance()
{
static const ClassMemberInfo<CommonBaseT, ClassT> i = ClassT::GetClassMemberInfo();
return i;
}
- template<class CommonBaseT, class ClassT>
- const std::string& ClassMemberInfo<CommonBaseT, ClassT>::GetClassName()
+ template <class CommonBaseT, class ClassT>
+ const std::string&
+ ClassMemberInfo<CommonBaseT, ClassT>::GetClassName()
{
return GetTypeString<ClassType>();
}
- template<class CommonBaseT, class ClassT>
- const KeyValueVector<std::string, ClassMemberInfoEntry<CommonBaseT>>& ClassMemberInfo<CommonBaseT, ClassT>::GetEntries()
+ template <class CommonBaseT, class ClassT>
+ const KeyValueVector<std::string, ClassMemberInfoEntry<CommonBaseT>>&
+ ClassMemberInfo<CommonBaseT, ClassT>::GetEntries()
{
return GetInstance().entries;
}
- template<class CommonBaseT, class ClassT>
- std::size_t ClassMemberInfo<CommonBaseT, ClassT>::GetNumberOfDataFields()
+ template <class CommonBaseT, class ClassT>
+ std::size_t
+ ClassMemberInfo<CommonBaseT, ClassT>::GetNumberOfDataFields()
{
ARMARX_TRACE;
static const std::size_t numberOfDataFields = []
@@ -221,64 +241,95 @@ namespace armarx::introspection
return numberOfDataFields;
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, bool& out)
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ bool& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<bool>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Byte& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Byte& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Byte>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Short& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Short& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Short>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Int& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Int& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Int>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Long& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Long& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Long>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Float& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Float& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Float>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Double& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ Ice::Double& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<Ice::Double>();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, std::string& out)
+
+ template <class CommonBaseT, class ClassT>
+ void
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr,
+ std::size_t i,
+ std::string& out)
{
ARMARX_TRACE;
static const auto convert = MakeConverter<std::string>();
@@ -286,8 +337,9 @@ namespace armarx::introspection
convert.at(i)(ptr, out);
}
- template<class CommonBaseT, class ClassT>
- const std::type_info& ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldType(std::size_t i)
+ template <class CommonBaseT, class ClassT>
+ const std::type_info&
+ ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldType(std::size_t i)
{
ARMARX_TRACE;
static const auto convert = []
@@ -297,7 +349,9 @@ namespace armarx::introspection
for (std::size_t idxEntr = 0; idxEntr < GetEntries().size(); ++idxEntr)
{
const Entry& e = GetEntries().at(idxEntr);
- for (std::size_t idxField = 0; idxField < GetEntries().at(idxEntr).getNumberOfFields(); ++idxField)
+ for (std::size_t idxField = 0;
+ idxField < GetEntries().at(idxEntr).getNumberOfFields();
+ ++idxField)
{
data.emplace_back(&e.getFieldTypeId(idxField));
}
@@ -308,4 +362,4 @@ namespace armarx::introspection
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
return *convert.at(i);
}
-}
+} // namespace armarx::introspection
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
index 1ad242babb61a5f2970b641d0ac3bbb09c338ba1..4fe0b74a29f29bbeeba89fe0593144b6c34dac51 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
@@ -26,12 +26,12 @@
namespace armarx::introspection
{
- template<class CommonBaseT, class ClassT>
+ template <class CommonBaseT, class ClassT>
struct ClassMemberInfo;
- template<class CommonBaseT, class ClassT>
+ template <class CommonBaseT, class ClassT>
struct ClassMemberInfoEntryConfigurator;
- template<class CommonBaseT>
+ template <class CommonBaseT>
struct ClassMemberInfoEntry
{
public:
@@ -42,14 +42,14 @@ namespace armarx::introspection
ClassMemberInfoEntry& operator=(ClassMemberInfoEntry&&) = default;
ClassMemberInfoEntry& operator=(const ClassMemberInfoEntry&) = default;
- template<class ClassType, class MemberType>
- ClassMemberInfoEntry(const std::string& memberName, MemberType ClassType::* ptr):
- className {GetTypeString<ClassType>()},
- memberName {memberName},
- memberTypeName {GetTypeString<MemberType>()},
- numberOfFields {DataFieldsInfo<MemberType>::GetNumberOfFields()},
- fieldToType {MakeFieldToTypeFunctors<ClassType>(numberOfFields, ptr)},
- toVariants_ {MakeToVariantsFunctor<ClassType>(ptr)}
+ template <class ClassType, class MemberType>
+ ClassMemberInfoEntry(const std::string& memberName, MemberType ClassType::*ptr) :
+ className{GetTypeString<ClassType>()},
+ memberName{memberName},
+ memberTypeName{GetTypeString<MemberType>()},
+ numberOfFields{DataFieldsInfo<MemberType>::GetNumberOfFields()},
+ fieldToType{MakeFieldToTypeFunctors<ClassType>(numberOfFields, ptr)},
+ toVariants_{MakeToVariantsFunctor<ClassType>(ptr)}
{
ARMARX_CHECK_NOT_EQUAL(0, numberOfFields);
//field names
@@ -70,113 +70,140 @@ namespace armarx::introspection
}
//general
- const std::string& getClassName() const
+ const std::string&
+ getClassName() const
{
return className;
}
- const std::string& getMemberName() const
+
+ const std::string&
+ getMemberName() const
{
return memberName;
}
- const std::string& getMemberTypeName() const
+
+ const std::string&
+ getMemberTypeName() const
{
return memberTypeName;
}
+
//fields
- std::size_t getNumberOfFields() const
+ std::size_t
+ getNumberOfFields() const
{
return numberOfFields;
}
- const std::string& getFieldName(std::size_t i) const
+
+ const std::string&
+ getFieldName(std::size_t i) const
{
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldNames.at(i);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, bool& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, bool& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toBool(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Byte& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Byte& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toByte(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Short& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Short& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toShort(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Int& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Int& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toInt(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Long& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Long& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toLong(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Float& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Float& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toFloat(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Double& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Double& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toDouble(ptr, out);
}
- void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, std::string& out) const
+
+ void
+ getDataFieldAs(std::size_t i, const CommonBaseType* ptr, std::string& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldToType.at(i).toString(ptr, out);
}
- const std::type_info& getFieldTypeId(std::size_t i) const
+
+ const std::type_info&
+ getFieldTypeId(std::size_t i) const
{
ARMARX_CHECK_LESS(i, numberOfFields);
return *fieldToType.at(i).typeId;
}
//variants
- std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp, const CommonBaseType* ptr) const
+ std::map<std::string, VariantBasePtr>
+ toVariants(const IceUtil::Time& timestamp, const CommonBaseType* ptr) const
{
return toVariants_(this, timestamp, ptr);
}
private:
- using ToVariantsFunctorType = std::function <
- std::map<std::string, VariantBasePtr>(
- const ClassMemberInfoEntry* thisptr,
- const IceUtil::Time&,
- const CommonBaseType*) >;
+ using ToVariantsFunctorType =
+ std::function<std::map<std::string, VariantBasePtr>(const ClassMemberInfoEntry* thisptr,
+ const IceUtil::Time&,
+ const CommonBaseType*)>;
+
struct FieldToType
{
- template<class T>
+ template <class T>
using FieldToFunctorType = std::function<void(const CommonBaseType*, T&)>;
- FieldToFunctorType<bool> toBool;
- FieldToFunctorType<Ice::Byte> toByte;
- FieldToFunctorType<Ice::Short> toShort;
- FieldToFunctorType<Ice::Int> toInt;
- FieldToFunctorType<Ice::Long> toLong;
- FieldToFunctorType<Ice::Float> toFloat;
+ FieldToFunctorType<bool> toBool;
+ FieldToFunctorType<Ice::Byte> toByte;
+ FieldToFunctorType<Ice::Short> toShort;
+ FieldToFunctorType<Ice::Int> toInt;
+ FieldToFunctorType<Ice::Long> toLong;
+ FieldToFunctorType<Ice::Float> toFloat;
FieldToFunctorType<Ice::Double> toDouble;
FieldToFunctorType<std::string> toString;
- const std::type_info* typeId;
+ const std::type_info* typeId;
};
- template<class ClassType, class MemberType, class MemberPtrClassType>
- static std::vector<FieldToType> MakeFieldToTypeFunctors(
- std::size_t numberOfFields,
- MemberType MemberPtrClassType::* ptr)
+ template <class ClassType, class MemberType, class MemberPtrClassType>
+ static std::vector<FieldToType>
+ MakeFieldToTypeFunctors(std::size_t numberOfFields, MemberType MemberPtrClassType::*ptr)
{
std::vector<FieldToType> result;
result.reserve(numberOfFields);
@@ -185,46 +212,48 @@ namespace armarx::introspection
result.emplace_back();
auto& fieldData = result.back();
-#define make_getter(Type) [i, ptr](const CommonBaseType * ptrBase, Type & out) \
- { \
- const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase); \
- ARMARX_CHECK_NOT_NULL(cptr); \
- DataFieldsInfo<MemberType>::GetDataFieldAs(i, cptr->*ptr, out); \
+#define make_getter(Type) \
+ [i, ptr](const CommonBaseType* ptrBase, Type& out) \
+ { \
+ const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase); \
+ ARMARX_CHECK_NOT_NULL(cptr); \
+ DataFieldsInfo<MemberType>::GetDataFieldAs(i, cptr->*ptr, out); \
}
- fieldData.toBool = make_getter(bool);
- fieldData.toByte = make_getter(Ice::Byte);
- fieldData.toShort = make_getter(Ice::Short);
- fieldData.toInt = make_getter(Ice::Int);
- fieldData.toLong = make_getter(Ice::Long);
- fieldData.toFloat = make_getter(Ice::Float);
+ fieldData.toBool = make_getter(bool);
+ fieldData.toByte = make_getter(Ice::Byte);
+ fieldData.toShort = make_getter(Ice::Short);
+ fieldData.toInt = make_getter(Ice::Int);
+ fieldData.toLong = make_getter(Ice::Long);
+ fieldData.toFloat = make_getter(Ice::Float);
fieldData.toDouble = make_getter(Ice::Double);
fieldData.toString = make_getter(std::string);
#undef make_getter
- fieldData.typeId = &DataFieldsInfo<MemberType>::GetDataFieldType(i);
+ fieldData.typeId = &DataFieldsInfo<MemberType>::GetDataFieldType(i);
}
return result;
}
- template<class ClassType, class MemberType, class MemberPtrClassType>
- static ToVariantsFunctorType MakeToVariantsFunctor(MemberType MemberPtrClassType::* ptr)
+ template <class ClassType, class MemberType, class MemberPtrClassType>
+ static ToVariantsFunctorType
+ MakeToVariantsFunctor(MemberType MemberPtrClassType::*ptr)
{
- return [ptr](
- const ClassMemberInfoEntry * thisptr,
- const IceUtil::Time & timestamp,
- const CommonBaseType * ptrBase)
+ return [ptr](const ClassMemberInfoEntry* thisptr,
+ const IceUtil::Time& timestamp,
+ const CommonBaseType* ptrBase)
{
const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase);
ARMARX_CHECK_NOT_NULL(cptr);
- return DataFieldsInfo<MemberType>::ToVariants(
- cptr->*ptr, thisptr->getMemberName(),
- timestamp,
- thisptr->variantReportFrame(), thisptr->variantReportAgent());
+ return DataFieldsInfo<MemberType>::ToVariants(cptr->*ptr,
+ thisptr->getMemberName(),
+ timestamp,
+ thisptr->variantReportFrame(),
+ thisptr->variantReportAgent());
};
}
- template<class BaseType, class ClassT>
+ template <class BaseType, class ClassT>
friend class ClassMemberInfo;
- template<class BaseType, class ClassT>
+ template <class BaseType, class ClassT>
friend class ClassMemberInfoEntryConfigurator;
const std::string className;
@@ -236,46 +265,56 @@ namespace armarx::introspection
std::vector<std::string> fieldNames;
//variants
ToVariantsFunctorType toVariants_;
- bool setVariantReportFrame_ {false};
- std::function<std::string()> variantReportFrame {[]{return "";}};
- std::function<std::string()> variantReportAgent {[]{return "";}};
+ bool setVariantReportFrame_{false};
+ std::function<std::string()> variantReportFrame{[] { return ""; }};
+ std::function<std::string()> variantReportAgent{[] { return ""; }};
};
- template<class CommonBaseT, class ClassT>
+ template <class CommonBaseT, class ClassT>
struct ClassMemberInfoEntryConfigurator
{
using ClassType = ClassT;
using CommonBaseType = CommonBaseT;
using Entry = ClassMemberInfoEntry<CommonBaseType>;
- ClassMemberInfoEntryConfigurator& setFieldNames(std::vector<std::string> fieldNames)
+ ClassMemberInfoEntryConfigurator&
+ setFieldNames(std::vector<std::string> fieldNames)
{
ARMARX_CHECK_EQUAL(fieldNames.size(), e.numberOfFields);
e.fieldNames = std::move(fieldNames);
return *this;
}
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(const std::string& agent, const std::string& frame)
+
+ ClassMemberInfoEntryConfigurator&
+ setVariantReportFrame(const std::string& agent, const std::string& frame)
{
e.setVariantReportFrame_ = true;
- e.variantReportFrame = [frame] {return frame;};
- e.variantReportAgent = [agent] {return agent;};
+ e.variantReportFrame = [frame] { return frame; };
+ e.variantReportAgent = [agent] { return agent; };
return *this;
}
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(const std::string& agent, std::function<std::string()> frame)
+
+ ClassMemberInfoEntryConfigurator&
+ setVariantReportFrame(const std::string& agent, std::function<std::string()> frame)
{
e.setVariantReportFrame_ = true;
e.variantReportFrame = std::move(frame);
- e.variantReportAgent = [agent] {return agent;};
+ e.variantReportAgent = [agent] { return agent; };
return *this;
}
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(std::function<std::string()> agent, const std::string& frame)
+
+ ClassMemberInfoEntryConfigurator&
+ setVariantReportFrame(std::function<std::string()> agent, const std::string& frame)
{
e.setVariantReportFrame_ = true;
- e.variantReportFrame = [frame] {return frame;};
+ e.variantReportFrame = [frame] { return frame; };
e.variantReportAgent = std::move(agent);
return *this;
}
- ClassMemberInfoEntryConfigurator& setVariantReportFrame(std::function<std::string()> agent, std::function<std::string()> frame)
+
+ ClassMemberInfoEntryConfigurator&
+ setVariantReportFrame(std::function<std::string()> agent,
+ std::function<std::string()> frame)
{
e.setVariantReportFrame_ = true;
e.variantReportFrame = std::move(frame);
@@ -283,13 +322,14 @@ namespace armarx::introspection
return *this;
}
- ClassMemberInfoEntryConfigurator& setVariantReportFunction(
- std::function<std::map<std::string, VariantBasePtr>(const IceUtil::Time&, const ClassType*)> f)
+ ClassMemberInfoEntryConfigurator&
+ setVariantReportFunction(
+ std::function<std::map<std::string, VariantBasePtr>(const IceUtil::Time&,
+ const ClassType*)> f)
{
- e.toVariants_ = [f](
- const ClassMemberInfoEntry<CommonBaseType>* thisptr,
- const IceUtil::Time & timestamp,
- const CommonBaseType * ptrBase)
+ e.toVariants_ = [f](const ClassMemberInfoEntry<CommonBaseType>* thisptr,
+ const IceUtil::Time& timestamp,
+ const CommonBaseType* ptrBase)
{
const ClassType* ptr = dynamic_cast<const ClassType*>(ptrBase);
ARMARX_CHECK_NOT_NULL(ptr);
@@ -300,7 +340,11 @@ namespace armarx::introspection
private:
friend struct ClassMemberInfo<CommonBaseType, ClassType>;
- ClassMemberInfoEntryConfigurator(Entry& e): e(e) {}
+
+ ClassMemberInfoEntryConfigurator(Entry& e) : e(e)
+ {
+ }
+
Entry& e;
};
-}
+} // namespace armarx::introspection
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
index 08bc46984505141a1631e669b3e8b229ea2940e2..2131c04fc9e5bd7a31fbddd78222c8d4d0ad4485 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
@@ -26,45 +26,59 @@
#include <Eigen/Geometry>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/OrientedPoint.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/OrientedPoint.h>
+#include <RobotAPI/libraries/core/Pose.h>
//Eigen::Vector3f
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<Eigen::Vector3f, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<Eigen::Vector3f, void>::GetNumberOfFields()
{
return 3;
}
- void DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, std::string& out)
+
+ void
+ DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Vector3f& field,
+ std::string& out)
{
ARMARX_CHECK_LESS(i, 3);
out = to_string(field(i));
}
- void DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, float& out)
+
+ void
+ DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Vector3f& field,
+ float& out)
{
ARMARX_CHECK_LESS(i, 3);
out = field(i);
}
- const std::type_info& DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldType(std::size_t i)
+
+ const std::type_info&
+ DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldType(std::size_t i)
{
return typeid(float);
}
- const std::vector<std::string>& DataFieldsInfo<Eigen::Vector3f, void>::GetFieldNames()
+
+ const std::vector<std::string>&
+ DataFieldsInfo<Eigen::Vector3f, void>::GetFieldNames()
{
static const std::vector<std::string> result{"x", "y", "z"};
return result;
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector3f, void>::ToVariants(
- const Eigen::Vector3f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<Eigen::Vector3f, void>::ToVariants(const Eigen::Vector3f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
{
if (!frame.empty())
{
@@ -73,110 +87,143 @@ namespace armarx::introspection
ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
return {{name, {new TimedVariant(Vector3{value}, timestamp)}}};
}
-}
+} // namespace armarx::introspection
+
//Eigen::Vector##Num##Type
namespace armarx::introspection
{
-#define make_DataFieldsInfo_for_eigen_vector(Type,TypeName,Num) \
- void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out) \
- { \
- ARMARX_CHECK_LESS(i, Num); \
- out = to_string(field(i)); \
- } \
- void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out) \
- { \
- ARMARX_CHECK_LESS(i, Num); \
- out = field(i); \
- } \
- const std::type_info& DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldType(std::size_t i) \
- { \
- return typeid(Ice::TypeName); \
- } \
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
- const Eigen::Vector##Num##Type& value, \
- const std::string& name, \
- const IceUtil::Time& timestamp, \
- const std::string& frame, \
- const std::string& agent) \
- { \
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant"; \
- std::map<std::string, VariantBasePtr> result; \
- for(int i = 0; i < Num; ++i) \
- { \
- result.emplace(name + "." + to_string(i), VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
- } \
- return result; \
- } \
- const std::vector<std::string>& DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
- { \
- static const std::vector<std::string> result = [] \
- { \
- std::vector<std::string> r; \
- for(int i = 0; i < Num; ++i) \
- { \
- r.emplace_back(to_string(i)); \
- } \
- return r; \
- }(); \
- return result; \
- }
-
- make_DataFieldsInfo_for_eigen_vector(f, Float, 2)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 4)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
-
- make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
-
- make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
+#define make_DataFieldsInfo_for_eigen_vector(Type, TypeName, Num) \
+ void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs( \
+ std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out) \
+ { \
+ ARMARX_CHECK_LESS(i, Num); \
+ out = to_string(field(i)); \
+ } \
+ void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs( \
+ std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out) \
+ { \
+ ARMARX_CHECK_LESS(i, Num); \
+ out = field(i); \
+ } \
+ const std::type_info& DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldType( \
+ std::size_t i) \
+ { \
+ return typeid(Ice::TypeName); \
+ } \
+ std::map<std::string, VariantBasePtr> \
+ DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
+ const Eigen::Vector##Num##Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame, \
+ const std::string& agent) \
+ { \
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
+ << "There is no framed version of TimestampVariant"; \
+ std::map<std::string, VariantBasePtr> result; \
+ for (int i = 0; i < Num; ++i) \
+ { \
+ result.emplace(name + "." + to_string(i), \
+ VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
+ } \
+ return result; \
+ } \
+ const std::vector<std::string>& \
+ DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
+ { \
+ static const std::vector<std::string> result = [] \
+ { \
+ std::vector<std::string> r; \
+ for (int i = 0; i < Num; ++i) \
+ { \
+ r.emplace_back(to_string(i)); \
+ } \
+ return r; \
+ }(); \
+ return result; \
+ }
+
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 2) make_DataFieldsInfo_for_eigen_vector(f,
+ Float,
+ 4)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
#undef make_DataFieldsInfo_for_eigen_vector
-}
+} // namespace armarx::introspection
+
//Eigen::Matrix4f
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<Eigen::Matrix4f, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<Eigen::Matrix4f, void>::GetNumberOfFields()
{
return 16;
}
- void DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, float& out)
+
+ void
+ DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Matrix4f& field,
+ float& out)
{
ARMARX_CHECK_LESS(i, 16);
out = field(i / 4, i % 4);
}
- void DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, std::string& out)
+
+ void
+ DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Matrix4f& field,
+ std::string& out)
{
ARMARX_CHECK_LESS(i, 16);
out = to_string(field(i / 4, i % 4));
}
- const std::type_info& DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldType(std::size_t i)
+
+ const std::type_info&
+ DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldType(std::size_t i)
{
return typeid(float);
}
- const std::vector<std::string>& DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldNames()
- {
- static const std::vector<std::string> result
- {
- "00", "01", "02", "03",
- "10", "11", "12", "13",
- "20", "21", "22", "23",
- "30", "31", "32", "33"
- };
+
+ const std::vector<std::string>&
+ DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldNames()
+ {
+ static const std::vector<std::string> result{"00",
+ "01",
+ "02",
+ "03",
+ "10",
+ "11",
+ "12",
+ "13",
+ "20",
+ "21",
+ "22",
+ "23",
+ "30",
+ "31",
+ "32",
+ "33"};
return result;
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(
- const Eigen::Matrix4f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(const Eigen::Matrix4f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
{
if (!frame.empty())
{
@@ -185,15 +232,21 @@ namespace armarx::introspection
ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
return {{name, {new TimedVariant(Pose{value}, timestamp)}}};
}
-}
+} // namespace armarx::introspection
+
//Eigen::Quaternionf
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<Eigen::Quaternionf, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<Eigen::Quaternionf, void>::GetNumberOfFields()
{
return 4;
}
- void DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, float& out)
+
+ void
+ DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Quaternionf& field,
+ float& out)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
@@ -211,14 +264,14 @@ namespace armarx::introspection
out = field.z();
return;
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
-
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- void DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out)
+
+ void
+ DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i,
+ const Eigen::Quaternionf& field,
+ std::string& out)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
@@ -236,30 +289,29 @@ namespace armarx::introspection
out = to_string(field.z());
return;
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
-
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- const std::type_info& DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldType(std::size_t i)
+ const std::type_info&
+ DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldType(std::size_t i)
{
return typeid(float);
}
- const std::vector<std::string>& DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldNames()
+
+ const std::vector<std::string>&
+ DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldNames()
{
static const std::vector<std::string> result{"qw", "qx", "qy", "qz"};
return result;
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Quaternionf, void>::ToVariants(
- const Eigen::Quaternionf& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<Eigen::Quaternionf, void>::ToVariants(const Eigen::Quaternionf& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
{
if (!frame.empty())
{
@@ -268,81 +320,116 @@ namespace armarx::introspection
ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
return {{name, {new TimedVariant(Quaternion{value}, timestamp)}}};
}
-}
+} // namespace armarx::introspection
+
//std::chrono::microseconds
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<std::chrono::microseconds, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<std::chrono::microseconds, void>::GetNumberOfFields()
{
return 1;
}
- void DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out)
+
+ void
+ DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(
+ std::size_t i,
+ const std::chrono::microseconds& field,
+ long& out)
{
ARMARX_CHECK_EQUAL(i, 0);
out = field.count();
}
- void DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out)
+
+ void
+ DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(
+ std::size_t i,
+ const std::chrono::microseconds& field,
+ std::string& out)
{
ARMARX_CHECK_EQUAL(i, 0);
out = to_string(field.count());
}
- const std::type_info& GetDataFieldType(std::size_t i)
+
+ const std::type_info&
+ GetDataFieldType(std::size_t i)
{
return typeid(long);
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<std::chrono::microseconds, void>::ToVariants(
- std::chrono::microseconds value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
- {
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant";
+
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<std::chrono::microseconds, void>::ToVariants(std::chrono::microseconds value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty())
+ << "There is no framed version of TimestampVariant";
return {{name, {new TimedVariant(TimestampVariant{value.count()}, timestamp)}}};
}
-}
+} // namespace armarx::introspection
+
//IceUtil::Time
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<IceUtil::Time, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<IceUtil::Time, void>::GetNumberOfFields()
{
return 1;
}
- void DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i, const IceUtil::Time& field, long& out)
+
+ void
+ DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i,
+ const IceUtil::Time& field,
+ long& out)
{
ARMARX_CHECK_EQUAL(i, 0);
out = field.toMicroSeconds();
}
- void DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i, const IceUtil::Time& field, std::string& out)
+
+ void
+ DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i,
+ const IceUtil::Time& field,
+ std::string& out)
{
ARMARX_CHECK_EQUAL(i, 0);
out = to_string(field.toMicroSeconds());
}
- const std::type_info& DataFieldsInfo<IceUtil::Time, void>::GetDataFieldType(std::size_t i)
+
+ const std::type_info&
+ DataFieldsInfo<IceUtil::Time, void>::GetDataFieldType(std::size_t i)
{
return typeid(long);
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<IceUtil::Time, void>::ToVariants(
- IceUtil::Time value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
- {
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant";
+
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<IceUtil::Time, void>::ToVariants(IceUtil::Time value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty())
+ << "There is no framed version of TimestampVariant";
return {{name, {new TimedVariant(TimestampVariant{value.toMicroSeconds()}, timestamp)}}};
}
-}
+} // namespace armarx::introspection
+
//JointStatus
namespace armarx::introspection
{
- std::size_t DataFieldsInfo<JointStatus, void>::GetNumberOfFields()
+ std::size_t
+ DataFieldsInfo<JointStatus, void>::GetNumberOfFields()
{
return 4;
}
- void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
- std::size_t i, const JointStatus& field, std::string& out)
+
+ void
+ DataFieldsInfo<JointStatus, void>::GetDataFieldAs(std::size_t i,
+ const JointStatus& field,
+ std::string& out)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
@@ -360,14 +447,14 @@ namespace armarx::introspection
out = to_string(field.emergencyStop);
return;
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
- std::size_t i, const JointStatus& field, Ice::Int& out)
+
+ void
+ DataFieldsInfo<JointStatus, void>::GetDataFieldAs(std::size_t i,
+ const JointStatus& field,
+ Ice::Int& out)
{
ARMARX_CHECK_EXPRESSION(i == 0 || i == 1);
switch (i)
@@ -379,14 +466,14 @@ namespace armarx::introspection
out = field.emergencyStop;
return;
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
- std::size_t i, const JointStatus& field, bool& out)
+
+ void
+ DataFieldsInfo<JointStatus, void>::GetDataFieldAs(std::size_t i,
+ const JointStatus& field,
+ bool& out)
{
ARMARX_CHECK_EXPRESSION(i == 2 || i == 3);
switch (i)
@@ -398,13 +485,12 @@ namespace armarx::introspection
out = static_cast<Ice::Int>(field.operation);
return;
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- const std::type_info& DataFieldsInfo<JointStatus, void>::GetDataFieldType(std::size_t i)
+
+ const std::type_info&
+ DataFieldsInfo<JointStatus, void>::GetDataFieldType(std::size_t i)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
@@ -418,34 +504,31 @@ namespace armarx::introspection
case 3:
return typeid(bool);
}
- throw std::logic_error
- {
- __FILE__ " : " + to_string(__LINE__) +
- " : Unreachable code reached"
- };
+ throw std::logic_error{__FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"};
}
- const std::vector<std::string>& DataFieldsInfo<JointStatus, void>::GetFieldNames()
+ const std::vector<std::string>&
+ DataFieldsInfo<JointStatus, void>::GetFieldNames()
{
- static const std::vector<std::string> result{"error", "operation", "enabled", "emergencyStop"};
+ static const std::vector<std::string> result{
+ "error", "operation", "enabled", "emergencyStop"};
return result;
}
- std::map<std::string, VariantBasePtr> DataFieldsInfo<JointStatus, void>::ToVariants(
- const JointStatus& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
- {
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version for JointStatus";
+ std::map<std::string, VariantBasePtr>
+ DataFieldsInfo<JointStatus, void>::ToVariants(const JointStatus& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty())
+ << "There is no framed version for JointStatus";
static const auto fnames = GetFieldNames();
- return
- {
- {name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
- {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
- {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
- {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}
- };
+ return {{name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
+ {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
+ {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
+ {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}};
}
-}
+} // namespace armarx::introspection
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index 34f53f63744ff7939c1cfcf94526052d0dad2d71..e0e3d736b50bb906077fd20adbc8ddab7bed13cc 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -21,19 +21,21 @@
*/
#pragma once
-#include "../EigenForwardDeclarations.h"
+#include <chrono>
+#include <string>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/observers/variant/TimedVariant.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
+#include <ArmarXCore/observers/variant/TimedVariant.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <string>
-#include <chrono>
+#include "../EigenForwardDeclarations.h"
namespace armarx::introspection
{
- template<class T, class = void> struct DataFieldsInfo;
+ template <class T, class = void>
+ struct DataFieldsInfo;
+
// static std::size_t GetNumberOfFields();
// static void GetDataFieldAs(std::size_t i, T field, bool& out);
// static void GetDataFieldAs(std::size_t i, T field, Ice::Byte& out);
@@ -52,93 +54,115 @@ namespace armarx::introspection
// const std::string& frame = "",
// const std::string& agent = "")
- template<class T>
+ template <class T>
struct DataFieldsInfoBase
{
- static void GetDataFieldAs(std::size_t i, const T& field, bool& out)
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, bool& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Byte& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Byte& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Short& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Short& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Int& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Int& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Long& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Long& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Float& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Float& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, Ice::Double& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, Ice::Double& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static void GetDataFieldAs(std::size_t i, const T& field, std::string& out)
+
+ static void
+ GetDataFieldAs(std::size_t i, const T& field, std::string& out)
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
- static const std::vector<std::string>& GetFieldNames()
+
+ static const std::vector<std::string>&
+ GetFieldNames()
{
ARMARX_TRACE;
- throw std::logic_error {"This function should never be called"};
+ throw std::logic_error{"This function should never be called"};
}
};
-}
+} // namespace armarx::introspection
+
//build_in_ice_types
namespace armarx::introspection
{
-#define make_def_for_build_in_ice_type(Type) \
- template<> \
- struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
- { \
- using DataFieldsInfoBase<Type>::GetDataFieldAs; \
- static std::size_t GetNumberOfFields() \
- { \
- return 1; \
- } \
- static void GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
- { \
- ARMARX_CHECK_EQUAL(i, 0); \
- out = to_string(field); \
- } \
- static void GetDataFieldAs(std::size_t i, const Type& field, Type& out) \
- { \
- ARMARX_CHECK_EQUAL(i, 0); \
- out = field; \
- } \
- static const std::type_info& GetDataFieldType(std::size_t i) \
- { \
- return typeid(Type); \
- } \
- static std::map<std::string, VariantBasePtr> ToVariants( \
- const Type& value, \
- const std::string& name, \
- const IceUtil::Time& timestamp, \
- const std::string& frame = "", \
- const std::string& agent = "") \
- { \
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
- << "There is no framed version for build in ice types"; \
- return {{name, {new TimedVariant(value, timestamp)}}}; \
- } \
+#define make_def_for_build_in_ice_type(Type) \
+ template <> \
+ struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
+ { \
+ using DataFieldsInfoBase<Type>::GetDataFieldAs; \
+ static std::size_t \
+ GetNumberOfFields() \
+ { \
+ return 1; \
+ } \
+ static void \
+ GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = to_string(field); \
+ } \
+ static void \
+ GetDataFieldAs(std::size_t i, const Type& field, Type& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = field; \
+ } \
+ static const std::type_info& \
+ GetDataFieldType(std::size_t i) \
+ { \
+ return typeid(Type); \
+ } \
+ static std::map<std::string, VariantBasePtr> \
+ ToVariants(const Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame = "", \
+ const std::string& agent = "") \
+ { \
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
+ << "There is no framed version for build in ice types"; \
+ return {{name, {new TimedVariant(value, timestamp)}}}; \
+ } \
}
make_def_for_build_in_ice_type(bool);
make_def_for_build_in_ice_type(Ice::Byte);
@@ -148,11 +172,12 @@ namespace armarx::introspection
make_def_for_build_in_ice_type(Ice::Float);
make_def_for_build_in_ice_type(Ice::Double);
#undef make_def_for_build_in_ice_type
-}
+} // namespace armarx::introspection
+
//Eigen::Vector3f
namespace armarx::introspection
{
- template<>
+ template <>
struct DataFieldsInfo<Eigen::Vector3f, void> : DataFieldsInfoBase<Eigen::Vector3f>
{
using DataFieldsInfoBase<Eigen::Vector3f>::GetDataFieldAs;
@@ -161,60 +186,66 @@ namespace armarx::introspection
static void GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
- static std::map<std::string, VariantBasePtr> ToVariants(
- const Eigen::Vector3f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
+ static std::map<std::string, VariantBasePtr> ToVariants(const Eigen::Vector3f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
};
-}
+} // namespace armarx::introspection
+
//Eigen::Vector##Num##Type
namespace armarx::introspection
{
-#define make_DataFieldsInfo_for_eigen_vector(Type,TypeName,Num) \
- template<> \
- struct DataFieldsInfo<Eigen::Vector##Num##Type, void> : DataFieldsInfoBase<Eigen::Vector##Num##Type> \
- { \
- using DataFieldsInfoBase<Eigen::Vector##Num##Type>::GetDataFieldAs; \
- static std::size_t GetNumberOfFields() \
- { \
- return Num; \
- } \
- static void GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out); \
- static void GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out); \
- static const std::type_info& GetDataFieldType(std::size_t i); \
- static const std::vector<std::string>& GetFieldNames(); \
- static std::map<std::string, VariantBasePtr> ToVariants( \
- const Eigen::Vector##Num##Type& value, \
- const std::string& name, \
- const IceUtil::Time& timestamp, \
- const std::string& frame = "", \
- const std::string& agent = ""); \
+#define make_DataFieldsInfo_for_eigen_vector(Type, TypeName, Num) \
+ template <> \
+ struct DataFieldsInfo<Eigen::Vector##Num##Type, void> : \
+ DataFieldsInfoBase<Eigen::Vector##Num##Type> \
+ { \
+ using DataFieldsInfoBase<Eigen::Vector##Num##Type>::GetDataFieldAs; \
+ static std::size_t \
+ GetNumberOfFields() \
+ { \
+ return Num; \
+ } \
+ static void \
+ GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out); \
+ static void \
+ GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out); \
+ static const std::type_info& GetDataFieldType(std::size_t i); \
+ static const std::vector<std::string>& GetFieldNames(); \
+ static std::map<std::string, VariantBasePtr> \
+ ToVariants(const Eigen::Vector##Num##Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame = "", \
+ const std::string& agent = ""); \
};
- make_DataFieldsInfo_for_eigen_vector(f, Float, 2)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 4)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
- make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
-
- make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
- make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
-
- make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
- make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 2) make_DataFieldsInfo_for_eigen_vector(f,
+ Float,
+ 4)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
+
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
#undef make_DataFieldsInfo_for_eigen_vector
-}
+} // namespace armarx::introspection
+
//Eigen::Matrix4f
namespace armarx::introspection
{
- template<>
+ template <>
struct DataFieldsInfo<Eigen::Matrix4f, void> : DataFieldsInfoBase<Eigen::Matrix4f>
{
using DataFieldsInfoBase<Eigen::Matrix4f>::GetDataFieldAs;
@@ -223,57 +254,61 @@ namespace armarx::introspection
static void GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
- static std::map<std::string, VariantBasePtr> ToVariants(
- const Eigen::Matrix4f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
+ static std::map<std::string, VariantBasePtr> ToVariants(const Eigen::Matrix4f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
};
-}
+} // namespace armarx::introspection
+
//Eigen::Quaternionf
namespace armarx::introspection
{
- template<>
+ template <>
struct DataFieldsInfo<Eigen::Quaternionf, void> : DataFieldsInfoBase<Eigen::Quaternionf>
{
using DataFieldsInfoBase<Eigen::Quaternionf>::GetDataFieldAs;
static std::size_t GetNumberOfFields();
static void GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, float& out);
- static void GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out);
+ static void
+ GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
- static std::map<std::string, VariantBasePtr> ToVariants(
- const Eigen::Quaternionf& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
+ static std::map<std::string, VariantBasePtr> ToVariants(const Eigen::Quaternionf& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
};
-}
+} // namespace armarx::introspection
+
//std::chrono::microseconds
namespace armarx::introspection
{
- template<>
- struct DataFieldsInfo<std::chrono::microseconds, void> : DataFieldsInfoBase<std::chrono::microseconds>
+ template <>
+ struct DataFieldsInfo<std::chrono::microseconds, void> :
+ DataFieldsInfoBase<std::chrono::microseconds>
{
using DataFieldsInfoBase<std::chrono::microseconds>::GetDataFieldAs;
static std::size_t GetNumberOfFields();
- static void GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out);
- static void GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out);
+ static void
+ GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out);
+ static void
+ GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
- static std::map<std::string, VariantBasePtr> ToVariants(
- std::chrono::microseconds value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
+ static std::map<std::string, VariantBasePtr> ToVariants(std::chrono::microseconds value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
};
-}
+} // namespace armarx::introspection
+
//IceUtil::Time
namespace armarx::introspection
{
- template<>
+ template <>
struct DataFieldsInfo<IceUtil::Time, void> : DataFieldsInfoBase<IceUtil::Time>
{
using DataFieldsInfoBase<IceUtil::Time>::GetDataFieldAs;
@@ -281,22 +316,23 @@ namespace armarx::introspection
static void GetDataFieldAs(std::size_t i, const IceUtil::Time& field, long& out);
static void GetDataFieldAs(std::size_t i, const IceUtil::Time& field, std::string& out);
static const std::type_info& GetDataFieldType(std::size_t i);
- static std::map<std::string, VariantBasePtr> ToVariants(
- IceUtil::Time value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "");
+ static std::map<std::string, VariantBasePtr> ToVariants(IceUtil::Time value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "");
};
-}
+} // namespace armarx::introspection
+
//JointStatus
namespace armarx
{
struct JointStatus;
}
+
namespace armarx::introspection
{
- template<>
+ template <>
struct DataFieldsInfo<JointStatus, void> : DataFieldsInfoBase<JointStatus>
{
using DataFieldsInfoBase<JointStatus>::GetDataFieldAs;
@@ -307,67 +343,79 @@ namespace armarx::introspection
static const std::type_info& GetDataFieldType(std::size_t i);
static const std::vector<std::string>& GetFieldNames();
- static std::map<std::string, VariantBasePtr> ToVariants(
- const JointStatus& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent);
+ static std::map<std::string, VariantBasePtr> ToVariants(const JointStatus& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent);
};
-}
+} // namespace armarx::introspection
+
//basic integral types
namespace armarx::introspection
{
-#define make_def_for_type_mapped_to_long(Type) \
- template<> \
- struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
- { \
- using DataFieldsInfoBase<Type>::GetDataFieldAs; \
- static std::size_t GetNumberOfFields() \
- { \
- return 1; \
- } \
- static void GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
- { \
- ARMARX_CHECK_EQUAL(i, 0); \
- out = to_string(field); \
- } \
- static void GetDataFieldAs(std::size_t i, const Type& field, Ice::Long& out) \
- { \
- ARMARX_CHECK_EQUAL(i, 0); \
- out = field; \
- } \
- static const std::type_info& GetDataFieldType(std::size_t i) \
- { \
- return typeid(Type); \
- } \
- static std::map<std::string, VariantBasePtr> ToVariants( \
- const Type& value, \
- const std::string& name, \
- const IceUtil::Time& timestamp, \
- const std::string& frame = "", \
- const std::string& agent = "") \
- { \
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
- << "There is no framed version for build in ice types"; \
- return {{name, {new TimedVariant(value, timestamp)}}}; \
- } \
+#define make_def_for_type_mapped_to_long(Type) \
+ template <> \
+ struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
+ { \
+ using DataFieldsInfoBase<Type>::GetDataFieldAs; \
+ static std::size_t \
+ GetNumberOfFields() \
+ { \
+ return 1; \
+ } \
+ static void \
+ GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = to_string(field); \
+ } \
+ static void \
+ GetDataFieldAs(std::size_t i, const Type& field, Ice::Long& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = field; \
+ } \
+ static const std::type_info& \
+ GetDataFieldType(std::size_t i) \
+ { \
+ return typeid(Type); \
+ } \
+ static std::map<std::string, VariantBasePtr> \
+ ToVariants(const Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame = "", \
+ const std::string& agent = "") \
+ { \
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
+ << "There is no framed version for build in ice types"; \
+ return {{name, {new TimedVariant(value, timestamp)}}}; \
+ } \
}
make_def_for_type_mapped_to_long(std::uint16_t);
make_def_for_type_mapped_to_long(std::uint32_t);
#undef make_def_for_type_mapped_to_long
-}
+} // namespace armarx::introspection
+
//std::array
namespace armarx::introspection
{
- template<class T, class = void>
- struct DataFieldsInfoHasNoFieldNames : std::false_type {};
+ template <class T, class = void>
+ struct DataFieldsInfoHasNoFieldNames : std::false_type
+ {
+ };
- template<class T>
-struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>::GetFieldNames == &DataFieldsInfoBase<T>::GetFieldNames) >> : std::true_type {};
+ template <class T>
+ struct DataFieldsInfoHasNoFieldNames<
+ T,
+ std::enable_if_t<(&DataFieldsInfo<T>::GetFieldNames ==
+ &DataFieldsInfoBase<T>::GetFieldNames)>> : std::true_type
+ {
+ };
- template<class T>
+ template <class T>
static constexpr bool DataFieldsInfoHasNoFieldNamesV = DataFieldsInfoHasNoFieldNames<T>::value;
static_assert(!DataFieldsInfoHasNoFieldNamesV<Eigen::Vector3f>);
@@ -375,18 +423,21 @@ struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>
static_assert(DataFieldsInfoHasNoFieldNamesV<Ice::Int>);
static_assert(DataFieldsInfoHasNoFieldNamesV<std::uint16_t>);
- template<class T, std::size_t N>
- struct DataFieldsInfo <std::array<T, N>, void> : DataFieldsInfoBase<std::array<T, N>>
+ template <class T, std::size_t N>
+ struct DataFieldsInfo<std::array<T, N>, void> : DataFieldsInfoBase<std::array<T, N>>
{
using sub_t = DataFieldsInfo<T>;
- static std::size_t GetNumberOfFields()
+
+ static std::size_t
+ GetNumberOfFields()
{
ARMARX_TRACE;
return N * sub_t::GetNumberOfFields();
}
- template<class OT>
- static void GetDataFieldAs(std::size_t i, const std::array<T, N>& field, OT& out)
+ template <class OT>
+ static void
+ GetDataFieldAs(std::size_t i, const std::array<T, N>& field, OT& out)
{
ARMARX_TRACE;
ARMARX_CHECK_LESS(i, GetNumberOfFields());
@@ -394,23 +445,26 @@ struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>
sub_t::GetDataFieldAs(i % subN, field.at(i / subN), out);
}
- static const std::type_info& GetDataFieldType(std::size_t i)
+ static const std::type_info&
+ GetDataFieldType(std::size_t i)
{
ARMARX_TRACE;
return sub_t::GetDataFieldType(i % N);
}
- static const std::vector<std::string>& GetFieldNames()
+
+ static const std::vector<std::string>&
+ GetFieldNames()
{
ARMARX_TRACE;
static const std::vector<std::string> result = []
{
- if constexpr(!DataFieldsInfoHasNoFieldNamesV<T>)
+ if constexpr (!DataFieldsInfoHasNoFieldNamesV<T>)
{
ARMARX_TRACE;
const auto sub_names = sub_t::GetFieldNames();
std::vector<std::string> r;
r.reserve(N * sub_names.size());
- for (std::size_t i = 0; i < N; ++i)
+ for (std::size_t i = 0; i < N; ++i)
{
const std::string pre = "element_" + std::to_string(i) + '.';
for (const auto& name : sub_names)
@@ -425,7 +479,7 @@ struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>
ARMARX_TRACE;
std::vector<std::string> r;
r.reserve(N);
- for (std::size_t i = 0; i < N; ++i)
+ for (std::size_t i = 0; i < N; ++i)
{
r.emplace_back("element_" + std::to_string(i));
}
@@ -434,19 +488,21 @@ struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>
}();
return result;
}
- static std::map<std::string, VariantBasePtr> ToVariants(
- const std::array<T, N>& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "")
+
+ static std::map<std::string, VariantBasePtr>
+ ToVariants(const std::array<T, N>& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame = "",
+ const std::string& agent = "")
{
ARMARX_TRACE;
std::map<std::string, VariantBasePtr> result;
- for (std::size_t i = 0; i < N; ++i)
+ for (std::size_t i = 0; i < N; ++i)
{
const std::string pre = "element_" + std::to_string(i) + '.';
- for (const auto& [k, v] : sub_t::ToVariants(value.at(i), name, timestamp, frame, agent))
+ for (const auto& [k, v] :
+ sub_t::ToVariants(value.at(i), name, timestamp, frame, agent))
{
result[pre + k] = v;
}
@@ -454,4 +510,4 @@ struct DataFieldsInfoHasNoFieldNames < T, std::enable_if_t < (&DataFieldsInfo<T>
return result;
}
};
-}
+} // namespace armarx::introspection
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/drivers/GamepadUnit/README.md b/source/RobotAPI/drivers/GamepadUnit/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..f987b05e9a80bc59741e77b07502516f7de3d842
--- /dev/null
+++ b/source/RobotAPI/drivers/GamepadUnit/README.md
@@ -0,0 +1,17 @@
+# GamepadUnit
+
+This component is used to control the robot with a gamepad.
+
+You have to set two device names:
+ - `GamepadDeviceName`
+ - Used to read the input from the device.
+ You can find the proper name by checking various names from `/dev/input/js*` with the command line util `jstest`.
+ - `GamepadForceFeedbackName`
+ - Used to give force feedback on the controller.
+ You can find the proper name by checking various names from `/dev/input/event*` with the command line util `fftest` or `evtest`.
+
+Alternatively you can find the proper name through `/dev/input/by-id`.
+
+E.g. for a Logitech F710 gamepad the proper names are: \
+`GamepadDeviceName=/dev/input/by-id/usb-Logitech_Wireless_Gamepad_F710_582F9A6B-joystick` \
+`GamepadForceFeedbackName=/dev/input/by-id/usb-Logitech_Wireless_Gamepad_F710_582F9A6B-event-joystick`
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 52af2c8dd81679509f7aaf965caa91e3b49349eb..a89af6168a5f66f366a4df3a36b59c01906a0695 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -22,24 +22,33 @@
#include "HokuyoLaserUnit.h"
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <HokuyoLaserScannerDriver/urg_utils.h>
+#include "ArmarXCore/core/time/Clock.h"
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <HokuyoLaserScannerDriver/urg_utils.h>
using namespace armarx;
+HokuyoLaserUnit::HokuyoLaserUnit()
+{
+ addPlugin(heartbeat);
+}
-void HokuyoLaserUnit::onInitComponent()
+
+void
+HokuyoLaserUnit::onInitComponent()
{
offeringTopicFromProperty("RobotHealthTopicName");
offeringTopicFromProperty("DebugObserverName");
- topicName = getProperty<std::string>("LaserScannerTopicName").getValue();
- offeringTopic(topicName);
- ARMARX_INFO << "Going to report on topic " << topicName;
+ laserScannerListenerProxyName = getProperty<std::string>("LaserScannerListenerName").getValue();
+ usingProxyFromProperty("LaserScannerListenerName");
+ ARMARX_INFO << "Going to report to listener " << laserScannerListenerProxyName;
updatePeriod = getProperty<int>("UpdatePeriod").getValue();
angleOffset = getProperty<float>("AngleOffset").getValue();
@@ -72,19 +81,21 @@ void HokuyoLaserUnit::onInitComponent()
}
catch (std::exception const& ex)
{
- ARMARX_WARNING << "Could not convert port to integer for laser scanner device " << deviceString
- << " (port is " << deviceInfo[1] << ") : " << ex.what();
+ ARMARX_WARNING << "Could not convert port to integer for laser scanner device "
+ << deviceString << " (port is " << deviceInfo[1] << ") : " << ex.what();
continue;
}
}
}
-
-void HokuyoLaserUnit::onConnectComponent()
+void
+HokuyoLaserUnit::onConnectComponent()
{
- robotHealthTopic = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue());
- topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
- debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName").getValue());
+ ARMARX_TRACE;
+
+ getProxyFromProperty(listenerPrx, "LaserScannerListenerName");
+ debugObserver = getTopic<DebugObserverInterfacePrx>(
+ getProperty<std::string>("DebugObserverName").getValue());
connectedDevices.clear();
for (HokuyoLaserScanDevice& device : devices)
@@ -95,12 +106,21 @@ void HokuyoLaserUnit::onConnectComponent()
device.task = nullptr;
}
+ // connecting devices for first time
if (!device.reconnect())
{
- ARMARX_WARNING << "Not starting task for laser scanner with IP: " << device.ip << ", Port: " << device.port;
+ ARMARX_WARNING << "Not starting task for laser scanner with IP: " << device.ip
+ << ", Port: " << device.port;
continue;
}
+ ARMARX_CHECK_NOT_NULL(heartbeat);
+ heartbeat->signUp(device.ip,
+ armarx::core::time::Duration::MilliSeconds(500),
+ armarx::core::time::Duration::MilliSeconds(800),
+ {"LaserScanner", "Localization"},
+ "HokuyoLaserScanDevice");
+
LaserScannerInfo info;
info.device = device.ip;
info.frame = device.frame;
@@ -114,24 +134,23 @@ void HokuyoLaserUnit::onConnectComponent()
info.stepSize = (info.maxAngle - info.minAngle) / (maxStep - minStep);
device.lengthData.resize(lengthDataSize);
- device.scanTopic = topic;
- device.robotHealthTopic = robotHealthTopic;
+ device.listenerPrx = listenerPrx;
+ device.robotHealthPlugin = heartbeat;
device.debugObserver = debugObserver;
connectedDevices.push_back(info);
- ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", "
- << info.minAngle << ", " << info.maxAngle << ", " << info.stepSize;
+ ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", " << info.minAngle
+ << ", " << info.maxAngle << ", " << info.stepSize;
- device.task = new RunningTask<HokuyoLaserScanDevice>(&device, &HokuyoLaserScanDevice::run,
- "HokuyoLaserScanUpdate_" + device.ip);
+ device.task = new RunningTask<HokuyoLaserScanDevice>(
+ &device, &HokuyoLaserScanDevice::run, "HokuyoLaserScanUpdate_" + device.ip);
device.task->start();
}
-
}
-
-void HokuyoLaserUnit::onDisconnectComponent()
+void
+HokuyoLaserUnit::onDisconnectComponent()
{
for (HokuyoLaserScanDevice& device : devices)
{
@@ -148,29 +167,32 @@ void HokuyoLaserUnit::onDisconnectComponent()
}
}
-
-void HokuyoLaserUnit::onExitComponent()
+void
+HokuyoLaserUnit::onExitComponent()
{
-
}
-armarx::PropertyDefinitionsPtr HokuyoLaserUnit::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+HokuyoLaserUnit::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new HokuyoLaserUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new HokuyoLaserUnitPropertyDefinitions(getConfigIdentifier()));
}
-std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
+std::string
+HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
{
- return topicName;
+ return laserScannerListenerProxyName;
}
-LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
+LaserScannerInfoSeq
+HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
{
return connectedDevices;
}
-bool HokuyoLaserScanDevice::reconnect()
+bool
+HokuyoLaserScanDevice::reconnect()
{
if (connected)
{
@@ -183,8 +205,8 @@ bool HokuyoLaserScanDevice::reconnect()
connected = (ret == 0);
if (!connected)
{
- ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: "
- << ip << ", Port: " << port << ", Error: " << ret << ")";
+ ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: " << ip
+ << ", Port: " << port << ", Error: " << ret << ")";
return false;
}
ret = urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, 0);
@@ -196,21 +218,26 @@ bool HokuyoLaserScanDevice::reconnect()
}
else
{
- ARMARX_WARNING << "Could not start measurement for laser scanner device using URG driver (IP: "
- << ip << ", Port: " << port << ", Error: " << ret << ")";
+ ARMARX_WARNING
+ << "Could not start measurement for laser scanner device using URG driver (IP: " << ip
+ << ", Port: " << port << ", Error: " << ret << ")";
return false;
}
}
-void HokuyoLaserScanDevice::run()
+void
+HokuyoLaserScanDevice::run()
{
+ ARMARX_TRACE;
while (!task->isStopped())
{
+ ARMARX_INFO << deactivateSpam() << "Running update loop for laserscanner device " << ip;
+
IceUtil::Time time_start = TimeUtil::GetTime();
if (errorCounter > 10)
{
- ARMARX_ERROR << "Device " << ip << " has too many consecutive errors!";
+ ARMARX_ERROR << "Device " << ip << " has too many consecutive errors!";
// assume dead
reconnect();
}
@@ -219,8 +246,9 @@ void HokuyoLaserScanDevice::run()
int lengthDataSize = urg_get_distance(&urg, lengthData.data(), nullptr);
if (lengthDataSize < 0)
{
- ARMARX_WARNING << deactivateSpam(1) << "Could not get measurement for laser scanner (IP: "
- << ip << ", Port: " << port << ", Error: " << lengthDataSize << ")";
+ ARMARX_WARNING << deactivateSpam(1)
+ << "Could not get measurement for laser scanner (IP: " << ip
+ << ", Port: " << port << ", Error: " << lengthDataSize << ")";
errorCounter++;
continue;
}
@@ -236,7 +264,8 @@ void HokuyoLaserScanDevice::run()
// TODO: Extract the min and max valid value for distance into parameters?
if (distance >= 21 && distance <= 30000)
{
- step.angle = angleOffset + (float)urg_index2rad(&urg, stepIndex); // Convert steps to rad
+ step.angle =
+ angleOffset + (float)urg_index2rad(&urg, stepIndex); // Convert steps to rad
step.distance = (float)distance; // Data is already in mm
scan.push_back(step);
}
@@ -246,26 +275,25 @@ void HokuyoLaserScanDevice::run()
errorCounter = 0;
- if (scanTopic)
+ if (listenerPrx)
{
- scanTopic->reportSensorValues(ip, frame, scan, now);
+
+ listenerPrx->reportSensorValues(ip, frame, scan, now);
}
else
{
- ARMARX_WARNING << "No scan topic available: IP: " << ip << ", Port: " << port;
+ ARMARX_WARNING << "No proxy available: IP: " << ip << ", Port: " << port;
}
- IceUtil::Time time_topicSensor = TimeUtil::GetTime();
+ IceUtil::Time time_proxyReport = TimeUtil::GetTime();
- RobotHealthHeartbeatArgs args;
- args.maximumCycleTimeWarningMS = 500;
- args.maximumCycleTimeErrorMS = 800;
- if (robotHealthTopic)
+ if (robotHealthPlugin != nullptr)
{
- robotHealthTopic->heartbeat(componentName + "_" + ip, args);
+ robotHealthPlugin->heartbeatOnChannel(ip);
}
else
{
- ARMARX_WARNING << "No robot health topic available: IP: " << ip << ", Port: " << port;
+ ARMARX_WARNING << "No robot health topic available: IP: " << ip
+ << ", Port: " << port;
}
IceUtil::Time time_topicHeartbeat = TimeUtil::GetTime();
@@ -274,20 +302,30 @@ void HokuyoLaserScanDevice::run()
StringVariantBaseMap durations;
durations["total_ms"] = new Variant(duration.toMilliSecondsDouble());
- durations["measure_ms"] = new Variant((time_measure - time_start).toMilliSecondsDouble());
- durations["update_ms"] = new Variant((time_update - time_measure).toMilliSecondsDouble());
- durations["topic_sensor_ms"] = new Variant((time_topicSensor - time_update).toMilliSecondsDouble());
- durations["topic_health_ms"] = new Variant((time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble());
- debugObserver->setDebugChannel("LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
+ durations["measure_ms"] =
+ new Variant((time_measure - time_start).toMilliSecondsDouble());
+ durations["update_ms"] =
+ new Variant((time_update - time_measure).toMilliSecondsDouble());
+ durations["proxy_report_ms"] =
+ new Variant((time_proxyReport - time_update).toMilliSecondsDouble());
+ durations["topic_health_ms"] =
+ new Variant((time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble());
+ debugObserver->setDebugChannel(
+ "LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
if (duration.toSecondsDouble() > 0.1)
{
ARMARX_WARNING << "Laserscanner reports are slow"
<< "Total time: " << duration.toMilliSecondsDouble() << "ms\n"
- << "Measure: " << (time_measure - time_start).toMilliSecondsDouble() << "ms \n"
- << "Update: " << (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
- << "TopicSensor: " << (time_topicSensor - time_update).toMilliSecondsDouble() << "ms\n"
- << "TopicHeart: " << (time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble() << "ms\n";
+ << "Measure: "
+ << (time_measure - time_start).toMilliSecondsDouble() << "ms \n"
+ << "Update: "
+ << (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
+ << "TopicSensor: "
+ << (time_proxyReport - time_update).toMilliSecondsDouble() << "ms\n"
+ << "TopicHeart: "
+ << (time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble()
+ << "ms\n";
}
}
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
index e9afa8e5b927857a03b171dc615722275d30986f..98da02fce21736694803272e4e76e046080d2cd4 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -22,14 +22,17 @@
#pragma once
+#include <vector>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
+
#include <RobotAPI/components/units/SensorActorUnit.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <HokuyoLaserScannerDriver/urg_sensor.h>
-#include <vector>
namespace armarx
{
@@ -37,19 +40,30 @@ namespace armarx
* @class HokuyoLaserUnitPropertyDefinitions
* @brief
*/
- class HokuyoLaserUnitPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class HokuyoLaserUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- HokuyoLaserUnitPropertyDefinitions(std::string prefix):
+ HokuyoLaserUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
+ defineOptionalProperty<std::string>(
+ "LaserScannerListenerName", "CartographerMappingAndLocalization", "Name of the laser scan listener.");
defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans");
- defineOptionalProperty<float>("AngleOffset", -2.3561944902, "Offset is applied the raw angles before reporting them");
- defineOptionalProperty<std::string>("Devices", "", "List of devices in form of 'IP1,port1,frame1;IP2,port2,frame2;...'");
- defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
- defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
+ defineOptionalProperty<float>("AngleOffset",
+ -2.3561944902,
+ "Offset is applied the raw angles before reporting them");
+ defineOptionalProperty<std::string>(
+ "Devices",
+ "",
+ "List of devices in form of 'IP1,port1,frame1;IP2,port2,frame2;...'");
+
+ defineOptionalProperty<std::string>(
+ "RobotHealthComponentName", "RobotHealth", "Name of the RobotHealth component");
+ defineOptionalProperty<std::string>(
+ "RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
+ defineOptionalProperty<std::string>("DebugObserverName",
+ "DebugObserver",
+ "Name of the topic the DebugObserver listens on");
}
};
@@ -66,14 +80,15 @@ namespace armarx
bool reconnect();
+
void run();
RunningTask<HokuyoLaserScanDevice>::pointer_type task;
LaserScan scan;
std::string componentName;
- LaserScannerUnitListenerPrx scanTopic;
- RobotHealthInterfacePrx robotHealthTopic;
+ LaserScannerUnitListenerPrx listenerPrx;
+ armarx::plugins::HeartbeatComponentPlugin* robotHealthPlugin;
DebugObserverInterfacePrx debugObserver;
};
@@ -93,10 +108,13 @@ namespace armarx
virtual public armarx::SensorActorUnit
{
public:
+ HokuyoLaserUnit();
+
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "HokuyoLaserUnit";
}
@@ -132,14 +150,14 @@ namespace armarx
LaserScannerInfoSeq getConnectedDevices(const Ice::Current& c) const override;
private:
- std::string topicName;
- LaserScannerUnitListenerPrx topic;
+ std::string laserScannerListenerProxyName;
+ LaserScannerUnitListenerPrx listenerPrx;
int updatePeriod = 25;
float angleOffset = 0.0f;
std::vector<HokuyoLaserScanDevice> devices;
LaserScannerInfoSeq connectedDevices;
- RobotHealthInterfacePrx robotHealthTopic;
DebugObserverInterfacePrx debugObserver;
- };
-}
+ plugins::HeartbeatComponentPlugin* heartbeat = nullptr;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidget.ui b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidget.ui
index 3c7001134bb60d82eb5017ee2b3d6f86a7bc57ee..2140458dc4fb92ddc0f065178ed4487733d053fa 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidget.ui
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidget.ui
@@ -1,55 +1,42 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
- <class>GuiHealthClientWidget</class>
- <widget class="QWidget" name="GuiHealthClientWidget">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>500</width>
- <height>71</height>
- </rect>
- </property>
- <property name="windowTitle">
- <string>GuiHealthClientWidget</string>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <item row="0" column="1">
- <widget class="QPushButton" name="pushButtonToggleOwnHeartbeats">
- <property name="text">
- <string>Toggle</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="labelState">
- <property name="text">
- <string>TextLabel</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <spacer name="horizontalSpacer">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="1" column="0" colspan="3">
- <widget class="QLabel" name="labelHealthState">
- <property name="text">
- <string>TextLabel</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <resources/>
- <connections/>
+ <class>GuiHealthClientWidget</class>
+ <widget class="QWidget" name="GuiHealthClientWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>500</width>
+ <height>71</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>GuiHealthClientWidget</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+
+ <item row="0" column="0" colspan="2">
+ <widget class="QLabel" name="labelTags">
+ <property name="text">
+ <string>...</string>
+ </property>
+ </widget>
+ </item>
+
+ <item row="0" column="2">
+ <widget class="QPushButton" name="pushButtonUnrequireAll">
+ <property name="text">
+ <string>Reset required tags</string>
+ </property>
+ </widget>
+ </item>
+
+ <item row="1" column="0" colspan="3">
+ <widget class="QTableWidget" name="tableHealthState">
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources />
+ <connections />
</ui>
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
index ca95dd07836e035a04019bd2fd1d176c4ca01256..4868ebda8519f9d571c0a62a17ccbecd66e5a18d 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
@@ -22,67 +22,115 @@
#include "GuiHealthClientWidgetController.h"
-#include <QLabel>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <qcolor.h>
+#include <qnamespace.h>
+#include <qrgb.h>
+#include <qtablewidget.h>
#include <string>
+#include <iomanip>
+
+#include <QLabel>
#include <QPushButton>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
namespace armarx
{
+ std::string serializeList(const std::vector<std::string>& l)
+ {
+ std::string s;
+ for(std::size_t i = 0; i < l.size(); i++)
+ {
+ const auto& tag = l.at(i);
+ s += tag;
+
+ if(i != l.size() - 1)
+ {
+ s += ", ";
+ }
+ }
+
+ return s;
+ }
+
GuiHealthClientWidgetController::GuiHealthClientWidgetController()
{
widget.setupUi(getWidget());
- //ARMARX_IMPORTANT << "ctor start";
- healthTimer = new QTimer(this);
- healthTimer->setInterval(10);
- connect(healthTimer, SIGNAL(timeout()), this, SLOT(healthTimerClb()));
- connect(widget.pushButtonToggleOwnHeartbeats, SIGNAL(clicked()), this, SLOT(toggleSendOwnHeartbeats()));
- connect(this, SIGNAL(invokeConnectComponentQt()), this, SLOT(onConnectComponentQt()), Qt::QueuedConnection);
- connect(this, SIGNAL(invokeDisconnectComponentQt()), this, SLOT(onDisconnectComponentQt()), Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(invokeConnectComponentQt()),
+ this,
+ SLOT(onConnectComponentQt()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(invokeDisconnectComponentQt()),
+ this,
+ SLOT(onDisconnectComponentQt()),
+ Qt::QueuedConnection);
+
+ connect(widget.pushButtonUnrequireAll,
+ SIGNAL(clicked()),
+ this,
+ SLOT(unrequireAll()));
updateSummaryTimer = new QTimer(this);
updateSummaryTimer->setInterval(100);
connect(updateSummaryTimer, SIGNAL(timeout()), this, SLOT(updateSummaryTimerClb()));
-
- widget.labelState->setText("idle.");
- widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
-
- //ARMARX_IMPORTANT << "ctor end";
}
-
GuiHealthClientWidgetController::~GuiHealthClientWidgetController()
{
-
}
-
- void GuiHealthClientWidgetController::loadSettings(QSettings* settings)
+ void
+ GuiHealthClientWidgetController::loadSettings(QSettings* settings)
{
-
}
- void GuiHealthClientWidgetController::saveSettings(QSettings* settings)
+ void
+ GuiHealthClientWidgetController::saveSettings(QSettings* settings)
{
-
}
-
- void GuiHealthClientWidgetController::onInitComponent()
+ void
+ GuiHealthClientWidgetController::onInitComponent()
{
//ARMARX_IMPORTANT << "onInitComponent";
usingProxy("RobotHealth");
offeringTopic("RobotHealthTopic");
}
- void GuiHealthClientWidgetController::healthTimerClb()
+ void
+ GuiHealthClientWidgetController::healthTimerClb()
{
- RobotHealthHeartbeatArgs rhha;
- rhha.maximumCycleTimeWarningMS = 250;
- rhha.maximumCycleTimeErrorMS = 500;
- robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
+ armarx::core::time::dto::DateTime now;
+ armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+ robotHealthTopicPrx->heartbeat(getName(), now);
}
- void GuiHealthClientWidgetController::updateSummaryTimerClb()
+
+
+ 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()
{
//ARMARX_IMPORTANT << "updateSummaryTimerClb";
if (robotHealthComponentPrx)
@@ -90,59 +138,151 @@ namespace armarx
//ARMARX_IMPORTANT << "has robotHealthComponentPrx";
try
{
- //ARMARX_IMPORTANT << "before set text";
- widget.labelHealthState->setText(QString::fromUtf8(robotHealthComponentPrx->getSummary().c_str()));
- //ARMARX_IMPORTANT << "after set text";
+ auto summary = robotHealthComponentPrx->getSummary();
+
+ const std::size_t nCols = 10;
+
+ auto& tableWidget = widget.tableHealthState;
+ tableWidget->setRowCount(summary.entries.size());
+ tableWidget->setColumnCount(nCols);
+
+ const auto summaryVals = simox::alg::get_values(summary.entries);
+
+ tableWidget->setHorizontalHeaderLabels({
+ "identifier", "required", "keeps frequency", "tags", "time since\nlast arrival [ms]", "time to\nreference [ms]", "time sync \n+ Ice [ms]", "warning\nthreshold [ms]", "error\nthreshold [ms]", "hostname"
+ });
+
+ tableWidget->setColumnWidth(0, 250);
+ tableWidget->setColumnWidth(1, 80);
+ tableWidget->setColumnWidth(2, 120);
+ tableWidget->setColumnWidth(3, 180);
+ tableWidget->setColumnWidth(4, 120);
+ tableWidget->setColumnWidth(5, 120);
+ tableWidget->setColumnWidth(6, 120);
+ tableWidget->setColumnWidth(7, 120);
+
+ for(std::size_t i = 0; i < summary.entries.size(); i++)
+ {
+ const auto& entry = summaryVals.at(i);
+
+ std::string stateRepr;
+ QColor stateColor;
+ switch(entry.state)
+ {
+ case HealthOK:
+ stateRepr = "yes";
+ stateColor.setRgb(0, 255, 0); // green
+ break;
+ case HealthWarning:
+ stateRepr = "yes";
+ stateColor.setRgb(255, 165, 0); // orange
+ break;
+ case HealthError:
+ stateRepr = "no";
+ stateColor.setRgb(255, 0, 0); // red
+ break;
+ }
+
+ const std::string hostname = entry.lastReferenceTimestamp.hostname;
+
+ 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 float syncErrorMilliSeconds = std::abs(entry.timeSinceLastArrival.microSeconds - entry.timeSinceLastUpdateReference.microSeconds) / 1000.0;
+
+ tableWidget->setItem(i, 0, new QTableWidgetItem(QString::fromStdString(entry.identifier)));
+ 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, make_item(timeSinceLastArrivalRepr, Qt::AlignRight));
+ tableWidget->setItem(i, 5, make_item(timeToLastReferenceRepr, Qt::AlignRight));
+
+ {
+ auto* item = make_item(to_string_rounded(syncErrorMilliSeconds), Qt::AlignRight);
+
+ QColor 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, 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)));
+ }
+
+ std::string tagsText = "Active tags: [";
+ {
+ std::sort(summary.activeTags.begin(), summary.activeTags.end());
+
+ tagsText += serializeList(summary.activeTags);
+
+ tagsText += "]";
+ }
+
+ widget.labelTags->setText(QString::fromStdString(tagsText));
}
catch (...)
- {}
+ {
+ }
}
}
- void GuiHealthClientWidgetController::toggleSendOwnHeartbeats()
+ void
+ GuiHealthClientWidgetController::unrequireAll()
{
- if (ownHeartbeatsActive)
- {
- ownHeartbeatsActive = false;
- healthTimer->stop();
- robotHealthTopicPrx->unregister(getName());
- widget.labelState->setText("idle.");
- //widget.pushButtonToggleOwnHeartbeats->setDisabled(true);
- widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
- }
- else
+ ARMARX_INFO << "UnrequireAll.";
+ if (robotHealthComponentPrx)
{
- ownHeartbeatsActive = true;
- healthTimer->start();
- widget.labelState->setText("sending heartbeats...");
- widget.pushButtonToggleOwnHeartbeats->setText("unregister self");
+ try
+ {
+ robotHealthComponentPrx->resetRequiredTags();
+ }
+ catch(...){}
}
-
}
-
- void GuiHealthClientWidgetController::onConnectComponent()
+ void
+ GuiHealthClientWidgetController::onConnectComponent()
{
//ARMARX_IMPORTANT << "onConnectComponent";
robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>("RobotHealthTopic");
robotHealthComponentPrx = getProxy<RobotHealthComponentInterfacePrx>("RobotHealth");
invokeConnectComponentQt();
}
- void GuiHealthClientWidgetController::onConnectComponentQt()
+
+ void
+ GuiHealthClientWidgetController::onConnectComponentQt()
{
//healthTimer->start();
- //ARMARX_IMPORTANT << "updateSummaryTimer->start";
+ //ARMARX_IMPORTANT <<ame "updateSummaryTimer->start";
updateSummaryTimer->start();
}
-
- void GuiHealthClientWidgetController::onDisconnectComponent()
+ void
+ GuiHealthClientWidgetController::onDisconnectComponent()
{
invokeDisconnectComponentQt();
}
- void GuiHealthClientWidgetController::onDisconnectComponentQt()
+
+ void
+ GuiHealthClientWidgetController::onDisconnectComponentQt()
{
- healthTimer->stop();
updateSummaryTimer->stop();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
index 0fed7c1ae22ad51714624a49388e8fec87a2967d..f5bc6ff28a593ba1d5e58bf1a02aabf42cd29bae 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
@@ -110,7 +110,7 @@ namespace armarx
void onDisconnectComponentQt();
void healthTimerClb();
void updateSummaryTimerClb();
- void toggleSendOwnHeartbeats();
+ void unrequireAll();
signals:
/* QT signal declarations */
@@ -122,13 +122,9 @@ namespace armarx
* Widget Form
*/
Ui::GuiHealthClientWidget widget;
- bool ownHeartbeatsActive = false;
RobotHealthInterfacePrx robotHealthTopicPrx;
RobotHealthComponentInterfacePrx robotHealthComponentPrx;
- QTimer* healthTimer;
QTimer* updateSummaryTimer;
};
}
-
-
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/ObjectPoseGui/ObjectPoseGuiWidget.ui b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
index e201caac70e9cce63d98578373084921c80f14e0..03b15fc4513ab5765b393c82007a7a6cc413714f 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidget.ui
@@ -43,6 +43,29 @@
</property>
</spacer>
</item>
+ <item>
+ <widget class="QPushButton" name="detchAllButton">
+ <property name="text">
+ <string>Detach All Objects</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="detchAllCommitAttachedCheckBox">
+ <property name="toolTip">
+ <string><html><head/><body><p>If checked, the object's pose according to the attachment will be committed as a new snapshot when the object is detached.</p><p>If unchecked, no new snapshot will be created, and the object's pose will return to where it was before the attachment. </p><p>In both cases, new snapshots to the object ID will overwrite the previous/committed snapshot.</p></body></html></string>
+ </property>
+ <property name="text">
+ <string>Commit Attached Pose</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="tristate">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
</layout>
</item>
<item>
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index 705f0f7584a84aff9354cf2986505436491f98ff..c337382c781ffe407403468e48b66a9c36c65054 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -24,17 +24,15 @@
#include <string>
-#include <QTimer>
#include <QMenu>
+#include <QTimer>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
-
namespace armarx
{
@@ -56,101 +54,126 @@ namespace armarx
connect(widget.updateButton, &QPushButton::pressed, this, &This::updateTab);
connect(widget.tabWidget, &QTabWidget::currentChanged, this, &This::updateTab);
+ connect(widget.detchAllButton,
+ &QPushButton::pressed,
+ [this]() {
+ this->detachAllObjectsFromRobotNodes(
+ widget.detchAllCommitAttachedCheckBox->isChecked());
+ });
- connect(widget.objectsTree, &QTreeWidget::customContextMenuRequested,
- this, &This::prepareObjectContextMenu);
+ connect(widget.objectsTree,
+ &QTreeWidget::customContextMenuRequested,
+ this,
+ &This::prepareObjectContextMenu);
connect(widget.requestButton, &QPushButton::pressed, this, &This::requestSelectedObjects);
QTimer* timer = new QTimer(this);
timer->setInterval(500);
connect(timer, &QTimer::timeout, this, &This::updateTab);
- connect(widget.autoUpdateCheckBox, &QCheckBox::toggled, this, [timer](bool checked)
- {
- if (checked)
- {
- timer->start();
- }
- else
- {
- timer->stop();
- }
- });
+ connect(widget.autoUpdateCheckBox,
+ &QCheckBox::toggled,
+ this,
+ [timer](bool checked)
+ {
+ if (checked)
+ {
+ timer->start();
+ }
+ else
+ {
+ timer->stop();
+ }
+ });
}
-
ObjectPoseGuiWidgetController::~ObjectPoseGuiWidgetController()
{
}
-
- void ObjectPoseGuiWidgetController::loadSettings(QSettings* settings)
+ void
+ ObjectPoseGuiWidgetController::loadSettings(QSettings* settings)
{
- (void) settings;
+ (void)settings;
}
- void ObjectPoseGuiWidgetController::saveSettings(QSettings* settings)
+ void
+ ObjectPoseGuiWidgetController::saveSettings(QSettings* settings)
{
- (void) settings;
+ (void)settings;
}
- QString ObjectPoseGuiWidgetController::GetWidgetName()
+ QString
+ ObjectPoseGuiWidgetController::GetWidgetName()
{
- return "MemoryX.ObjectPoseGui";
+ return "ArMem.ObjectPoseGui";
}
- static const std::string CONFIG_KEY_OBJECT_POSE_OBSERVER = "ObjectPoseStorage";
+ static const std::string CONFIG_KEY_OBJECT_POSE_STORAGE = "ObjectPoseStorage";
- QPointer<QDialog> ObjectPoseGuiWidgetController::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ ObjectPoseGuiWidgetController::getConfigDialog(QWidget* parent)
{
if (!configDialog)
{
configDialog = new SimpleConfigDialog(parent);
- configDialog->addProxyFinder<armarx::objpose::ObjectPoseStorageInterfacePrx>({CONFIG_KEY_OBJECT_POSE_OBSERVER, "Object pose observer.", "*"});
+ configDialog->addProxyFinder<armarx::objpose::ObjectPoseStorageInterfacePrx>(
+ {CONFIG_KEY_OBJECT_POSE_STORAGE, "Object pose storage", "Object*"});
}
return qobject_cast<QDialog*>(configDialog);
}
- void ObjectPoseGuiWidgetController::configured()
+ void
+ ObjectPoseGuiWidgetController::configured()
{
if (configDialog)
{
- ObjectPoseStorageName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_OBSERVER);
+ objectPoseStorageName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_STORAGE);
+ if (objectPoseStorageName.empty())
+ {
+ objectPoseStorageName = "ObjectMemory";
+ }
}
}
- void ObjectPoseGuiWidgetController::onInitComponent()
+ void
+ ObjectPoseGuiWidgetController::onInitComponent()
{
- if (!ObjectPoseStorageName.empty())
+ if (!objectPoseStorageName.empty())
{
- usingProxy(ObjectPoseStorageName);
+ usingProxy(objectPoseStorageName);
}
}
- void ObjectPoseGuiWidgetController::onConnectComponent()
+ void
+ ObjectPoseGuiWidgetController::onConnectComponent()
{
- if (!ObjectPoseStorageName.empty())
+ if (!objectPoseStorageName.empty())
{
- getProxy(ObjectPoseStorage, ObjectPoseStorageName);
+ getProxy(ObjectPoseStorage, objectPoseStorageName);
}
this->attachableFrames = ObjectPoseStorage->getAttachableFrames();
- std::sort(attachableFrames.begin(), attachableFrames.end(), [](const auto & lhs, const auto & rhs)
- {
- return lhs.agent < rhs.agent;
- });
+ std::sort(attachableFrames.begin(),
+ attachableFrames.end(),
+ [](const auto& lhs, const auto& rhs) { return lhs.agent < rhs.agent; });
for (objpose::AgentFrames& frames : attachableFrames)
{
std::sort(frames.frames.begin(), frames.frames.end());
}
+
+ // Update once.
+ updateTab();
}
- void ObjectPoseGuiWidgetController::onDisconnectComponent()
+ void
+ ObjectPoseGuiWidgetController::onDisconnectComponent()
{
ObjectPoseStorage = nullptr;
}
- void ObjectPoseGuiWidgetController::updateTab()
+ void
+ ObjectPoseGuiWidgetController::updateTab()
{
if (widget.tabWidget->currentWidget() == widget.tabObjects)
{
@@ -162,14 +185,13 @@ namespace armarx
}
}
-
-
- void ObjectPoseGuiWidgetController::updateObjectsTab()
+ void
+ ObjectPoseGuiWidgetController::updateObjectsTab()
{
if (!ObjectPoseStorage)
{
// Probably disconnected.
- ARMARX_VERBOSE << "No object pose observer.";
+ ARMARX_VERBOSE << "No object pose storage.";
return;
}
@@ -177,7 +199,8 @@ namespace armarx
ARMARX_VERBOSE << "Getting object poses...";
const objpose::data::ObjectPoseSeq objectPosesIce = ObjectPoseStorage->getObjectPoses();
ARMARX_VERBOSE << "Got " << objectPosesIce.size() << " object poses. "
- << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
+ << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble()
+ << " ms.)";
const objpose::ObjectPoseSeq objectPoses = objpose::fromIce(objectPosesIce);
@@ -192,87 +215,97 @@ namespace armarx
QTreeWidget* tree = widget.objectsTree;
MapTreeWidgetBuilder builder(objectPosesByProvider);
- builder.setMakeItemFn([](const std::string & provider, const objpose::ObjectPoseSeq&)
- {
- QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(provider)});
- return item;
- });
- builder.setUpdateItemFn([](const std::string&, const objpose::ObjectPoseSeq & objectPoses, QTreeWidgetItem * item)
- {
- bool expand = item->childCount() == 0;
-
- TreeWidgetBuilder<objpose::ObjectPose> builder;
- builder.setNameFn([](const objpose::ObjectPose & pose)
+ builder.setMakeItemFn(
+ [](const std::string& provider, const objpose::ObjectPoseSeq&)
{
- return pose.objectID.str();
- });
- builder.setMakeItemFn([](const objpose::ObjectPose&)
- {
- QTreeWidgetItem* item = new QTreeWidgetItem(QStringList{});
+ QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(provider)});
return item;
});
- builder.setUpdateItemFn([](const objpose::ObjectPose & pose, QTreeWidgetItem * item)
+ builder.setUpdateItemFn(
+ [](const std::string&, const objpose::ObjectPoseSeq& objectPoses, QTreeWidgetItem* item)
{
- int col = 0;
- item->setText(col++, QString::fromStdString(pose.objectID.str()));
- item->setText(col++, QString::fromStdString(pose.providerName));
- item->setText(col++, QString::fromStdString(objpose::ObjectTypeNames.to_name(pose.objectType)));
+ bool expand = item->childCount() == 0;
- {
- std::stringstream ss;
- if (pose.localOOBB)
+ TreeWidgetBuilder<objpose::ObjectPose> builder;
+ builder.setNameFn([](const objpose::ObjectPose& pose)
+ { return pose.objectID.str(); });
+ builder.setMakeItemFn(
+ [](const objpose::ObjectPose&)
{
- static const Eigen::IOFormat iof(5, 0, "", " x ", "", "", "", "");
- ss << pose.localOOBB->dimensions().format(iof);
- }
- else
+ QTreeWidgetItem* item = new QTreeWidgetItem(QStringList{});
+ return item;
+ });
+ builder.setUpdateItemFn(
+ [](const objpose::ObjectPose& pose, QTreeWidgetItem* item)
{
- ss << "None";
- }
- item->setText(col++, QString::fromStdString(ss.str()));
- }
- item->setText(col++, QString::number(double(pose.confidence), 'g', 2));
- item->setText(col++, QString::fromStdString(pose.timestamp.toDateTimeString()));
-
+ int col = 0;
+ item->setText(col++, QString::fromStdString(pose.objectID.str()));
+ item->setText(col++, QString::fromStdString(pose.providerName));
+ item->setText(col++,
+ QString::fromStdString(
+ objpose::ObjectTypeNames.to_name(pose.objectType)));
+
+ {
+ std::stringstream ss;
+ if (pose.localOOBB)
+ {
+ static const Eigen::IOFormat iof(5, 0, "", " x ", "", "", "", "");
+ ss << pose.localOOBB->dimensions().format(iof);
+ }
+ else
+ {
+ ss << "None";
+ }
+ item->setText(col++, QString::fromStdString(ss.str()));
+ }
+ item->setText(col++, QString::number(double(pose.confidence), 'g', 2));
+ item->setText(col++,
+ QString::fromStdString(pose.timestamp.toDateTimeString()));
+
+ {
+ std::stringstream ss;
+ if (pose.attachment)
+ {
+ ss << pose.attachment->frameName << " ("
+ << pose.attachment->agentName << ")";
+ }
+ item->setText(col++, QString::fromStdString(ss.str()));
+ }
+
+ return true;
+ });
+ builder.updateTreeWithContainer(item, objectPoses);
+
+ if (expand)
{
- std::stringstream ss;
- if (pose.attachment)
- {
- ss << pose.attachment->frameName << " (" << pose.attachment->agentName << ")";
- }
- item->setText(col++, QString::fromStdString(ss.str()));
+ item->setExpanded(true);
}
return true;
});
- builder.updateTreeWithContainer(item, objectPoses);
-
- if (expand)
- {
- item->setExpanded(true);
- }
-
- return true;
- });
builder.updateTree(tree, objectPosesByProvider);
- ARMARX_VERBOSE << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
+ ARMARX_VERBOSE << "Gui update took "
+ << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
}
-
- void ObjectPoseGuiWidgetController::updateRequestTab()
+ void
+ ObjectPoseGuiWidgetController::updateRequestTab()
{
if (!ObjectPoseStorage)
{
// Probably disconnected.
- ARMARX_VERBOSE << "No object pose observer.";
+ ARMARX_VERBOSE << "No object pose storage.";
return;
}
IceUtil::Time start = IceUtil::Time::now();
- objpose::ProviderInfoMap availableProvidersInfo = ObjectPoseStorage->getAvailableProvidersInfo();
- ARMARX_VERBOSE << "Got infos of " << availableProvidersInfo.size() << " object pose providers. "
- << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
+ objpose::ProviderInfoMap availableProvidersInfo =
+ ObjectPoseStorage->getAvailableProvidersInfo();
+ ARMARX_VERBOSE << "Got infos of " << availableProvidersInfo.size()
+ << " object pose providers. "
+ << "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble()
+ << " ms.)";
// Restructure data.
@@ -290,50 +323,62 @@ namespace armarx
QTreeWidget* tree = widget.requestTree;
MapTreeWidgetBuilder builder(data);
- builder.setMakeItemFn([](const std::string & dataset, const auto&)
- {
- QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(dataset)});
- return item;
- });
- builder.setUpdateItemFn([tree](const std::string & dataset, const auto & datasetData, QTreeWidgetItem * datasetItem)
- {
- (void) dataset;
-
- TreeWidgetBuilder<std::pair<std::string, std::string>> builder;
- builder.setCompareFn([](const std::pair<std::string, std::string>& lhs, QTreeWidgetItem * item)
- {
- auto rhs = std::make_pair(item->text(0).toStdString(), item->text(1).toStdString());
- if (lhs < rhs)
- {
- return -1;
- }
- return lhs == rhs ? 0 : 1;
- });
- builder.setMakeItemFn([](const std::pair<std::string, std::string>& element)
+ builder.setMakeItemFn(
+ [](const std::string& dataset, const auto&)
{
- QTreeWidgetItem* item = new QTreeWidgetItem({ QString::fromStdString(element.first), QString::fromStdString(element.second)});
+ QTreeWidgetItem* item = new QTreeWidgetItem({QString::fromStdString(dataset)});
return item;
});
- builder.setUpdateItemFn([tree](const std::pair<std::string, std::string>& element, QTreeWidgetItem * item)
+ builder.setUpdateItemFn(
+ [tree](
+ const std::string& dataset, const auto& datasetData, QTreeWidgetItem* datasetItem)
{
- (void) element;
- if (!tree->itemWidget(item, 2))
- {
- QCheckBox* requestCheckBox = new QCheckBox();
- tree->setItemWidget(item, 2, requestCheckBox);
- }
+ (void)dataset;
+
+ TreeWidgetBuilder<std::pair<std::string, std::string>> builder;
+ builder.setCompareFn(
+ [](const std::pair<std::string, std::string>& lhs, QTreeWidgetItem* item)
+ {
+ auto rhs = std::make_pair(item->text(0).toStdString(),
+ item->text(1).toStdString());
+ if (lhs < rhs)
+ {
+ return -1;
+ }
+ return lhs == rhs ? 0 : 1;
+ });
+ builder.setMakeItemFn(
+ [](const std::pair<std::string, std::string>& element)
+ {
+ QTreeWidgetItem* item =
+ new QTreeWidgetItem({QString::fromStdString(element.first),
+ QString::fromStdString(element.second)});
+ return item;
+ });
+ builder.setUpdateItemFn(
+ [tree](const std::pair<std::string, std::string>& element,
+ QTreeWidgetItem* item)
+ {
+ (void)element;
+ if (!tree->itemWidget(item, 2))
+ {
+ QCheckBox* requestCheckBox = new QCheckBox();
+ tree->setItemWidget(item, 2, requestCheckBox);
+ }
+ return true;
+ });
+ builder.updateTreeWithContainer(datasetItem, datasetData);
+
return true;
});
- builder.updateTreeWithContainer(datasetItem, datasetData);
-
- return true;
- });
builder.updateTree(tree, data);
- ARMARX_VERBOSE << "Gui update took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
+ ARMARX_VERBOSE << "Gui update took "
+ << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.";
}
- void ObjectPoseGuiWidgetController::prepareObjectContextMenu(const QPoint& pos)
+ void
+ ObjectPoseGuiWidgetController::prepareObjectContextMenu(const QPoint& pos)
{
QTreeWidget* tree = widget.objectsTree;
QTreeWidgetItem* item = tree->itemAt(pos);
@@ -356,11 +401,12 @@ namespace armarx
{
QAction* attachAgentAction = new QAction(QString::fromStdString(frame), tree);
// attachAgentAction->setStatusTip(tr("Attach object rigidly to a robot node"));
- connect(attachAgentAction, &QAction::triggered,
- [ =, this ]()
- {
- this->attachObjectToRobotNode(providerName, objectID, agentFrames.agent, frame);
- });
+ connect(attachAgentAction,
+ &QAction::triggered,
+ [=, this]() {
+ this->attachObjectToRobotNode(
+ providerName, objectID, agentFrames.agent, frame);
+ });
agentMenu->addAction(attachAgentAction);
}
attachMenu->addMenu(agentMenu);
@@ -368,10 +414,9 @@ namespace armarx
QAction* detachAction = new QAction(tr("Detach from to robot node"), tree);
detachAction->setEnabled(!item->text(OBJECTS_COLUMN_ATTACHMENT).isEmpty());
- connect(detachAction, &QAction::triggered, [ =, this ]()
- {
- this->detachObjectFromRobotNode(providerName, objectID);
- });
+ connect(detachAction,
+ &QAction::triggered,
+ [=, this]() { this->detachObjectFromRobotNode(providerName, objectID); });
QMenu menu(tree);
menu.addMenu(attachMenu);
@@ -380,9 +425,11 @@ namespace armarx
menu.exec(tree->mapToGlobal(pos));
}
- void ObjectPoseGuiWidgetController::attachObjectToRobotNode(
- QString providerName, QString objectID,
- const std::string& agentName, const std::string& frameName)
+ void
+ ObjectPoseGuiWidgetController::attachObjectToRobotNode(QString providerName,
+ QString objectID,
+ const std::string& agentName,
+ const std::string& frameName)
{
ARMARX_VERBOSE << "Attaching " << objectID << " by '" << providerName << "' to robot node '"
<< frameName << "' of agent '" << agentName << "'.";
@@ -395,7 +442,8 @@ namespace armarx
try
{
- objpose::AttachObjectToRobotNodeOutput output = ObjectPoseStorage->attachObjectToRobotNode(input);
+ objpose::AttachObjectToRobotNodeOutput output =
+ ObjectPoseStorage->attachObjectToRobotNode(input);
ARMARX_VERBOSE << "Success of attaching: " << output.success;
}
catch (const IceUtil::Exception& e)
@@ -406,9 +454,11 @@ namespace armarx
}
}
- void ObjectPoseGuiWidgetController::detachObjectFromRobotNode(QString providerName, QString objectID)
+ void
+ ObjectPoseGuiWidgetController::detachObjectFromRobotNode(QString providerName, QString objectID)
{
- ARMARX_VERBOSE << "Detaching " << objectID << " by '" << providerName << "' from robot node.";
+ ARMARX_VERBOSE << "Detaching " << objectID << " by '" << providerName
+ << "' from robot node.";
objpose::DetachObjectFromRobotNodeInput input;
input.providerName = providerName.toStdString();
@@ -416,17 +466,39 @@ namespace armarx
try
{
- objpose::DetachObjectFromRobotNodeOutput output = ObjectPoseStorage->detachObjectFromRobotNode(input);
+ objpose::DetachObjectFromRobotNodeOutput output =
+ ObjectPoseStorage->detachObjectFromRobotNode(input);
ARMARX_VERBOSE << "Was attached: " << output.wasAttached;
}
catch (const IceUtil::Exception& e)
{
- ARMARX_WARNING << "Failed to detach object '" << input.objectID << "' from a robot node."
+ ARMARX_WARNING << "Failed to detach object '" << input.objectID
+ << "' from a robot node."
<< "\nReason: " << e.what();
}
}
- void ObjectPoseGuiWidgetController::requestSelectedObjects()
+ void
+ ObjectPoseGuiWidgetController::detachAllObjectsFromRobotNodes(bool commitAttachedPose)
+ {
+
+ objpose::DetachAllObjectsFromRobotNodesInput input;
+ input.commitAttachedPose = commitAttachedPose;
+
+ try
+ {
+ objpose::DetachAllObjectsFromRobotNodesOutput output =
+ ObjectPoseStorage->detachAllObjectsFromRobotNodes(input);
+ ARMARX_VERBOSE << "Detached " << output.numDetached << " objects from robot nodes.";
+ }
+ catch (const IceUtil::Exception& e)
+ {
+ ARMARX_WARNING << "Failed to detach all objects from robot nodes.";
+ }
+ }
+
+ void
+ ObjectPoseGuiWidgetController::requestSelectedObjects()
{
std::map<std::string, objpose::observer::RequestObjectsInput> requestsPerProvider;
@@ -442,7 +514,8 @@ namespace armarx
if (selected->isChecked())
{
std::string providerName = classItem->text(1).toStdString();
- objpose::observer::RequestObjectsInput& requests = requestsPerProvider[providerName];
+ objpose::observer::RequestObjectsInput& requests =
+ requestsPerProvider[providerName];
data::ObjectID& id = requests.request.objectIDs.emplace_back();
id.dataset = datasetItem->text(0).toStdString();
id.className = classItem->text(0).toStdString();
@@ -463,14 +536,16 @@ namespace armarx
ARMARX_INFO << "Requesting " << request.request.objectIDs.size() << " objects for "
<< request.request.relativeTimeoutMS << " ms.";
- objpose::observer::RequestObjectsOutput output = ObjectPoseStorage->requestObjects(request);
+ objpose::observer::RequestObjectsOutput output =
+ ObjectPoseStorage->requestObjects(request);
int successful = 0;
for (const auto& [id, result] : output.results)
{
successful += int(!result.providerName.empty() && result.result.success);
}
- ARMARX_INFO << successful << " of " << request.request.objectIDs.size() << " object request successful.";
+ ARMARX_INFO << successful << " of " << request.request.objectIDs.size()
+ << " object request successful.";
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
index 5e9910bd8185cb13adbd54435758ee871dbd8460..462b1b0485eefe270e7bdcad5b2060bd2f3077dd 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
@@ -21,17 +21,15 @@
*/
#pragma once
-#include <RobotAPI/gui-plugins/ObjectPoseGui/ui_ObjectPoseGuiWidget.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
+#include <RobotAPI/gui-plugins/ObjectPoseGui/ui_ObjectPoseGuiWidget.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
-
namespace armarx
{
/**
@@ -52,9 +50,8 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- ObjectPoseGuiWidgetController :
- public armarx::ArmarXComponentWidgetControllerTemplate < ObjectPoseGuiWidgetController >
+ class ARMARXCOMPONENT_IMPORT_EXPORT ObjectPoseGuiWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<ObjectPoseGuiWidgetController>
{
Q_OBJECT
@@ -98,9 +95,12 @@ namespace armarx
void updateRequestTab();
void prepareObjectContextMenu(const QPoint& pos);
- void attachObjectToRobotNode(QString providerName, QString objectID,
- const std::string& agentName, const std::string& frameName);
+ void attachObjectToRobotNode(QString providerName,
+ QString objectID,
+ const std::string& agentName,
+ const std::string& frameName);
void detachObjectFromRobotNode(QString providerName, QString objectID);
+ void detachAllObjectsFromRobotNodes(bool commitAttachedPose);
void requestSelectedObjects();
@@ -109,18 +109,14 @@ namespace armarx
/* QT signal declarations */
private:
-
/// Widget Form
Ui::ObjectPoseGuiWidget widget;
QPointer<SimpleConfigDialog> configDialog;
- std::string ObjectPoseStorageName;
+ std::string objectPoseStorageName;
armarx::objpose::ObjectPoseStorageInterfacePrx ObjectPoseStorage;
objpose::AgentFramesSeq attachableFrames;
-
};
-}
-
-
+} // 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 1e80202efb18b2aca77e8ba45b87827e362ae233..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,14 +142,7 @@
<string>Manager</string>
</property>
<layout class="QGridLayout" name="gridLayout">
- <item row="2" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Update Frequency:</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0" colspan="3">
+ <item row="5" column="0" colspan="3">
<widget class="QTreeWidget" name="treeWidgetSkills">
<column>
<property name="text">
@@ -94,22 +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="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
+ <item row="4" column="1">
+ <widget class="QPushButton" name="pushButtonSearch">
+ <property name="text">
+ <string>Search</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLineEdit" name="lineEditSearch">
+ <property name="text">
+ <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>
@@ -120,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>
@@ -145,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">
@@ -185,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 0fba47d76eb851d528c306f46d24e17c24661223..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 / 5); // Keep this stable.
+ updateTimerFrequency();
refreshSkillsResultTimer->start();
connect(widget.doubleSpinBoxUpdateFreq,
@@ -99,7 +123,7 @@ namespace armarx
connect(refreshSkillsResultTimer,
&QTimer::timeout,
this,
- &SkillManagerMonitorWidgetController::refreshSkills);
+ &SkillManagerMonitorWidgetController::refreshSkillsAndExecutions);
connect(widget.pushButtonCopy,
&QPushButton::clicked,
@@ -114,28 +138,24 @@ 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);
- }
- SkillManagerMonitorWidgetController::~SkillManagerMonitorWidgetController()
- {
+ connect(widget.pushButtonRefreshNow,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::refreshSkills);
}
- void
- SkillManagerMonitorWidgetController::reconnectToSkillManager()
+ SkillManagerMonitorWidgetController::~SkillManagerMonitorWidgetController()
{
- if (connected)
- {
- getProxy(manager, observerName, 1000);
- }
}
void
@@ -152,6 +172,7 @@ namespace armarx
QAbstractItemView::EditTrigger::NoEditTriggers);
widget.treeWidgetSkillDetails->setColumnHidden(3, true);
+ getProxy(memory, observerName, 1000);
connected = true;
}
@@ -159,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
@@ -177,166 +198,255 @@ namespace armarx
refreshSkillsResultTimer->setInterval(f);
}
+ void
+ SkillManagerMonitorWidgetController::refreshSkillsAndExecutions()
+ {
+ 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)
{
- reconnectToSkillManager();
+ // check if null
return;
}
/* CHECK OWN SKILLS LIST */
// remove non-existing ones
- auto managerSkills = manager->getSkillDescriptions();
- std::vector<std::string> removedProviders;
- for (auto it = skills.begin(); it != skills.end();)
+ try
{
- // TODO: iterate over skills, not just over providers!
- std::string providerName = it->first;
- if (managerSkills.find(providerName) == managerSkills.end())
- {
- removedProviders.push_back(providerName);
- it = skills.erase(it);
- }
- else
- {
- it++;
- }
- }
+ std::scoped_lock l(updateMutex);
- // add new ones
- std::vector<std::string> newProviders;
- for (const auto& [providerName, providerSkills] : managerSkills)
- {
- if (skills.find(providerName) == skills.end())
- {
- skills.insert(std::make_pair(providerName, providerSkills));
- newProviders.push_back(providerName);
- }
- }
+ auto managerSkills = memory->getSkillDescriptions();
- /* CHECK TREE VIEW */
- // remove providers from tree
- int i = 0;
- while (i < widget.treeWidgetSkills->topLevelItemCount())
- {
- QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
- if (std::find(removedProviders.begin(),
- removedProviders.end(),
- item->text(0).toStdString()) != removedProviders.end())
+ // completely recreate internal skills map
+ skills.clear();
+ for (const auto& [sid, desc] : managerSkills)
{
- delete widget.treeWidgetSkills->takeTopLevelItem(i);
+ 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});
}
- else
+
+ // update tree view. Remove non-existing elements
+ int i = 0;
+ while (i < widget.treeWidgetSkills->topLevelItemCount())
{
+ auto* providerItem = widget.treeWidgetSkills->topLevelItem(i);
+ auto providerName = providerItem->text(0).toStdString();
+ skills::ProviderID providerId{.providerName = providerName};
+
+ if (skills.find(providerId) == skills.end())
+ {
+ providerItem = nullptr; // reset
+ auto remove = widget.treeWidgetSkills->takeTopLevelItem(i);
+ delete remove;
+ continue;
+ }
+
++i;
- }
- }
+ auto& providedSkills = skills.at(providerId);
- // add new providers
- for (const auto& [providerName, providerSkills] : skills)
- {
- if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName);
- it != newProviders.end())
- {
- auto item = new QTreeWidgetItem(widget.treeWidgetSkills);
- item->setText(0, QString::fromStdString(providerName));
- for (const auto& [name, sk] : providerSkills)
+ int j = 0;
+ while (j < providerItem->childCount())
{
- auto itsk = new QTreeWidgetItem(item);
- item->addChild(itsk);
- itsk->setText(0, QString::fromStdString(name));
+ 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;
}
}
- }
- // update status and active skills window
- std::map<skills::SkillID, std::string> activeSkillsAndPrefixes;
- auto managerStatuses = manager->getSkillExecutionStatuses();
- for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
- {
- try
+ // update tree view. Add new elements
+ for (const auto& [providerId, providedSkills] : skills)
{
- QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
- auto providerName = item->text(0).toStdString();
+ QTreeWidgetItem* providerItem = nullptr;
+ for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
+ {
+ auto el = widget.treeWidgetSkills->topLevelItem(i);
+ auto providerName = el->text(0).toStdString();
+ skills::ProviderID elProviderId{.providerName = providerName};
- auto allStatusesForProvider = managerStatuses.at(providerName);
+ if (providerId == elProviderId)
+ {
+ providerItem = el;
+ break;
+ }
+ }
- for (int j = 0; j < item->childCount(); ++j)
+ if (!providerItem)
{
- QTreeWidgetItem* skillItem = item->child(j);
- skills::SkillID currentSkillId(providerName, skillItem->text(0).toStdString());
+ providerItem = new QTreeWidgetItem(widget.treeWidgetSkills);
+ providerItem->setText(0, QString::fromStdString(providerId.providerName));
+ }
- auto statusForSkill = allStatusesForProvider.at(currentSkillId.skillName);
- skillItem->setText(2,
- QString::fromStdString(ExecutionStatus2String.at(
- statusForSkill.header.status)));
+ for (const auto& [skillId, skill] : providedSkills)
+ {
+ QTreeWidgetItem* skillItem = nullptr;
+ for (int i = 0; i < providerItem->childCount(); ++i)
+ {
+ auto el = providerItem->child(i);
+ auto skillName = el->text(0).toStdString();
+ skills::SkillID elSkillId{providerId, skillName};
+
+ if (skillId == elSkillId)
+ {
+ skillItem = el;
+ break;
+ }
+ }
- if (not statusForSkill.header.executorName
- .empty()) // it means that the skill was called by someone
+ if (!skillItem)
{
- activeSkillsAndPrefixes.insert(
- {currentSkillId, statusForSkill.header.executorName});
+ skillItem = new SkillInfoTreeWidgetItem(skill, providerItem);
+ skillItem->setText(0, QString::fromStdString(skillId.skillName));
}
}
}
- catch (const std::exception& e)
- {
- // Perhaps the skill provider died after the check at the beginning of this method
- continue;
- }
}
+ catch (...)
+ {
+ // perhaps the manager died during the method?
+ }
+ }
- // finally update the view of active skills
- widget.listWidgetActiveSkills->clear();
- for (const auto& [id, prefix] : activeSkillsAndPrefixes)
+ 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)
{
- auto prefixedStr = id.toString(prefix);
- bool longest = true;
- for (const auto& [id2, prefix2] :
- activeSkillsAndPrefixes) // check if there is a deeper skill currently executing
+ // check if null
+ return;
+ }
+
+ try
+ {
+ std::scoped_lock l(updateMutex);
+
+ auto currentManagerStatuses =
+ memory
+ ->getSkillExecutionStatuses(); // we assume that there are no more than 100 new skills..
+
+ for (const auto& [k, v] : currentManagerStatuses)
{
- auto prefixedStr2 = id.toString(prefix2);
- if (prefixedStr == prefixedStr2)
+ auto executionId = skills::SkillExecutionID::FromIce(k);
+ auto statusUpdate = skills::SkillStatusUpdate::FromIce(v);
+
+ SkillExecutionInfoTreeWidgetItem* found = nullptr;
+ for (int i = 0; i < widget.treeWidgetSkillExecutions->topLevelItemCount(); ++i)
{
- continue;
+ auto c = static_cast<SkillExecutionInfoTreeWidgetItem*>(
+ widget.treeWidgetSkillExecutions->topLevelItem(i));
+
+ found =
+ SkillExecutionInfoTreeWidgetItem::SearchRecursiveForMatch(c, executionId);
+
+ if (found)
+ {
+ // 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 (simox::alg::starts_with(prefixedStr2, prefixedStr))
+ if (!found)
{
- longest = false;
- break;
+ // 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 "));
}
}
-
- if (longest)
- {
- widget.listWidgetActiveSkills->addItem(
- QString::fromStdString(id.toString() + ": " + id.toString(prefix)));
- }
+ }
+ catch (...)
+ {
+ // perhaps the manager died during the method?
}
}
void
SkillManagerMonitorWidgetController::executeSkill()
{
- if (selectedSkill.providerName.empty() or selectedSkill.skillName.empty())
+ if (not selectedSkill.skillId.isFullySpecified())
{
return;
}
- 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;
}
@@ -344,43 +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()
{
- 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.skillId;
- ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.providerName << "/"
- << selectedSkill.skillName;
- manager->abortSkill(selectedSkill.providerName, selectedSkill.skillName);
+ // manager->abortSkill(selectedSkill.skillId);
}
void
SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current,
QTreeWidgetItem*)
{
+ std::scoped_lock l(updateMutex);
widget.groupBoxSkillDetails->setEnabled(false);
if (!current)
@@ -391,48 +502,40 @@ namespace armarx
if (!current->parent())
{
- // no parent available. Should not happen
+ // no parent available. Perhaps provider clicked?
return;
}
- SelectedSkill newSelectedSkill;
+ auto c = static_cast<SkillInfoTreeWidgetItem*>(current);
+ auto skillDescription = c->skillDescription;
- // 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);
-
- 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);
}
@@ -447,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 d9a9379bdfdea0ee56c9c5f3e601ef68a5ce75b5..1fd2e2f110fe8d34890f8d685c8faa5bc2031b70 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -38,19 +38,57 @@
#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 ARMARXCOMPONENT_IMPORT_EXPORT
- SkillManagerMonitorWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < SkillManagerMonitorWidgetController >
+ class SkillInfoTreeWidgetItem : public QTreeWidgetItem
{
- Q_OBJECT
+ 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>
+ {
+ Q_OBJECT
+
+ public:
/// Controller Constructor
explicit SkillManagerMonitorWidgetController();
/// Controller destructor
@@ -65,13 +103,12 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Skills.Manager";
}
- void reconnectToSkillManager();
-
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
@@ -84,6 +121,8 @@ namespace armarx
void updateTimerFrequency();
void refreshSkills();
+ void refreshExecutions();
+ void refreshSkillsAndExecutions();
void copyCurrentConfig();
void pasteCurrentConfig();
@@ -97,23 +136,29 @@ namespace armarx
/**
* Widget Form
*/
- Ui::SkillManagerMonitorWidget widget;
- QPointer<SimpleConfigDialog> dialog;
+ Ui::SkillManagerMonitorWidget widget;
+ 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)
- 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;
@@ -122,12 +167,7 @@ namespace armarx
// others
QTimer* refreshSkillsResultTimer;
- // skillExecutions
- std::vector<std::thread> executions;
-
// connected flag
std::atomic_bool connected = false;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/interface/aron/Aron.ice b/source/RobotAPI/interface/aron/Aron.ice
index fb412c46c2409c60ac236ba7579c64fe795dff28..aae664fa5f4d79e7a59f94bebc1e74823d06dfc5 100644
--- a/source/RobotAPI/interface/aron/Aron.ice
+++ b/source/RobotAPI/interface/aron/Aron.ice
@@ -3,28 +3,39 @@
/**
* Changelog:
* - Update 1.1.0: Allow templates in aron and anyobjects
- * - Update 1.1.1: Switch to armarx type. Also remove position, orientation and pose types as they are nothing else that matrices or quaternions
+ * - Update 1.1.1: Switch to armarx type. Also remove position, orientation and pose types as they
+ * are nothing else that matrices or quaternions
+ * - Update 2.0.0: Add support for aron defaults. Also add marshalling checks in aron variants.
+ * - Update 3.0.0: Change aron defaults to be optional (for now only for primitives, ndarray types
+ * will follow). Updated version object as str comparison is more expensive that int
+ * comparison. Further added TypeName object to unify typeAsString creation and use
+ * sizeof instead of fixed types (only for C++ builtin types for now).
**/
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#define ARON_MAJOR "1"
-#define ARON_MINOR "1"
-#define ARON_PATCH "2"
-#define ARON_VERSION "1.1.2"
+// Those macros stay defined
+#define ARON_MAJOR 3 // ice interface changes
+#define ARON_MINOR 0 // aron header changes
+#define ARON_PATCH 0 // aron cpp changes
module armarx
{
module aron
{
-
- const string VERSION = ARON_VERSION;
-
+ // also make version number available to other languages without a preprocessor
+ struct Version
+ {
+ int MAJOR = ARON_MAJOR;
+ int MINOR = ARON_MINOR;
+ int PATCH = ARON_PATCH;
+ };
/*************************
* Aron Types ************
************************/
module type
+
{
enum Maybe
{
@@ -48,6 +59,13 @@ module armarx
FLOAT32,
FLOAT64
};
+
+ module default_value
+ {
+ const string ONES = "1";
+ const string ZEROS = "0";
+ const string DEFAULT = "";
+ }
};
module image
@@ -57,6 +75,14 @@ module armarx
RGB24,
DEPTH32
};
+
+ module default_value
+ {
+ const string IDENTITY = "identity";
+ const string ONES = "1";
+ const string ZEROS = "0";
+ const string DEFAULT = "";
+ }
};
module pointcloud
@@ -71,6 +97,14 @@ module armarx
POINT_XYZRGBA,
POINT_XYZHSV
};
+
+ module default_value
+ {
+ const string IDENTITY = "identity";
+ const string ONES = "1";
+ const string ZEROS = "0";
+ const string DEFAULT = "";
+ }
};
module matrix
@@ -83,6 +117,14 @@ module armarx
FLOAT32,
FLOAT64
};
+
+ module default_value
+ {
+ const string IDENTITY = "identity";
+ const string ONES = "1";
+ const string ZEROS = "0";
+ const string DEFAULT = "";
+ }
};
module quaternion
@@ -92,46 +134,155 @@ module armarx
FLOAT32,
FLOAT64
};
+
+ module default_value
+ {
+ const string ONES = "1";
+ const string ZEROS = "0";
+ const string DEFAULT = "";
+ }
};
+ module aron_enum
+ {
+ module default_value
+ {
+ const string DEFAULT = "";
+ }
+ }
+
module dto
{
- class GenericType {
- string VERSION = ARON_VERSION;
+ class GenericType
+ {
+ Version VERSION;
Maybe maybe = Maybe::NONE;
- }
+ };
+
sequence<GenericType> GenericTypeSeq;
+
dictionary<string, GenericType> GenericTypeDict;
/* ***** Container types ***** */
- class List extends GenericType { GenericType acceptedType; }
- class Tuple extends GenericType { GenericTypeSeq elementTypes; }
- class Pair extends GenericType { GenericType acceptedType1; GenericType acceptedType2; }
- class AronObject extends GenericType { AronObject parent; Ice::StringSeq templates; string objectName; Ice::StringSeq templateInstantiations; GenericTypeDict elementTypes; }
- class Dict extends GenericType { GenericType acceptedType; }
+ class List extends GenericType
+ {
+ GenericType acceptedType;
+ };
+
+ class Tuple extends GenericType
+
+ {
+ GenericTypeSeq elementTypes;
+ };
+
+ class Pair extends GenericType
+ {
+ GenericType acceptedType1;
+ GenericType acceptedType2;
+ };
+
+ class AronObject extends GenericType
+ {
+ Ice::StringSeq templates;
+ string objectName;
+ AronObject parent;
+ Ice::StringSeq templateInstantiations;
+ GenericTypeDict elementTypes;
+ };
+
+ class Dict extends GenericType
+ {
+ GenericType acceptedType;
+ }
/* ***** Complex Types (serialize to ndarray) ***** */
- class NDArray extends GenericType { int ndimensions; ndarray::ElementType elementType; }
- class Matrix extends GenericType { int rows; int cols; matrix::ElementType elementType; }
- class Quaternion extends GenericType { quaternion::ElementType elementType; }
- class Image extends GenericType { image::PixelType pixelType; }
- class PointCloud extends GenericType { pointcloud::VoxelType voxelType; }
+ class NDArray extends GenericType
+
+ {
+ int ndimensions;
+ ndarray::ElementType elementType;
+ string defaultValue = type::ndarray::default_value::DEFAULT;
+ // ONES_DEFAULT_VALUE | ZEROS_DEFAULT_VALUE | <value> | ""
+ };
+
+ class Matrix extends GenericType
+ {
+ int rows;
+ int cols;
+ matrix::ElementType elementType;
+ string defaultValue = type::matrix::default_value::DEFAULT;
+ // IDENTITY_DEFAULT_VALUE (if quadratic) | ONES_DEFAULT_VALUE | ZEROS_DEFAULT_VALUE | <value> | ""
+ };
+
+ class Quaternion extends GenericType
+ {
+ quaternion::ElementType elementType;
+ string defaultValue = type::quaternion::default_value::DEFAULT;
+ // ONES_DEFAULT_VALUE | ZEROS_DEFAULT_VALUE | <value sequence> | ""
+ };
+
+ class Image extends GenericType
+ {
+ image::PixelType pixelType;
+ string defaultValue = type::image::default_value::DEFAULT;
+ // IDENTITY_DEFAULT_VALUE (if quadratic) | ONES_DEFAULT_VALUE | ZEROS_DEFAULT_VALUE | <value> | ""
+ };
+
+ class PointCloud extends GenericType
+ {
+ pointcloud::VoxelType voxelType;
+ string defaultValue = type::pointcloud::default_value::DEFAULT;
+ // IDENTITY_DEFAULT_VALUE (if quadratic) | ONES_DEFAULT_VALUE | ZEROS_DEFAULT_VALUE | <value> | ""
+ };
/* ***** Enum types ***** */
- class IntEnum extends GenericType { string enumName; StringIntDict acceptedValues; }
+ class IntEnum extends GenericType
+ {
+ string enumName;
+ StringIntDict acceptedValues;
+ string defaultValue = aron_enum::default_value::DEFAULT;
+ // in acceptedValues.keys | ""
+ };
+
//class FloatEnum extends GenericType { string enumName; StringFloatDict acceptedValues; }
//class StringEnum extends GenericType { string enumName; StringStringDict acceptedValues; }
/* ***** Any Types ***** */
- class AnyObject extends GenericType { };
+ class AnyObject extends GenericType
+ {
+ };
/* ***** Primitive Types ***** */
- class AronInt extends GenericType { };
- class AronLong extends GenericType { };
- class AronDouble extends GenericType { };
- class AronFloat extends GenericType { };
- class AronString extends GenericType { };
- class AronBool extends GenericType { };
+ class AronInt extends GenericType
+ {
+ optional(0) int defaultValue; // <value>
+ };
+
+ class AronLong extends GenericType
+ {
+
+ optional(1) long defaultValue; // <value>
+ };
+
+ class AronDouble extends GenericType
+ {
+ optional(2) double defaultValue; // <value>
+ };
+
+ class AronFloat extends GenericType
+ {
+ optional(3) float defaultValue; // <value>
+ };
+
+ class AronString extends GenericType
+ {
+ optional(4) string defaultValue; // <value>
+ };
+
+ class AronBool extends GenericType
+ {
+ optional(4) bool defaultValue; // <value>
+ };
};
};
@@ -143,30 +294,67 @@ module armarx
{
module dto
{
- class GenericData {
- string VERSION = ARON_VERSION;
+ class GenericData
+ {
+ Version VERSION;
};
+
sequence<GenericData> GenericDataSeq;
dictionary<string, GenericData> GenericDataDict;
/* ***** Container Data ***** */
- class List extends GenericData { GenericDataSeq elements; };
- class Dict extends GenericData { GenericDataDict elements; };
+ class List extends GenericData
+ {
+ GenericDataSeq elements;
+ };
+
+ class Dict extends GenericData
+ {
+ GenericDataDict elements;
+ };
/* ***** Complex Data ***** */
// The NDArray contains more or less the same information as an AronType, but there is no other way to do it
// Especially, note the difference between the type's typeName (e.g. "GRAY_SCALE_IMAGE" => language dependent) and the data's type ("0")
// Further, note the difference between the type's dimensions (e.g. 128x128) and the data's dimensions (128x128x3 for RGB)
- class NDArray extends GenericData { Ice::IntSeq shape; string type; Ice::ByteSeq data; }
-
+ class NDArray extends GenericData
+ {
+ Ice::IntSeq shape;
+ string type;
+ Ice::ByteSeq data;
+ };
/* ***** Primitive Data ***** */
- class AronInt extends GenericData { int value; };
- class AronLong extends GenericData { long value; };
- class AronDouble extends GenericData { double value; };
- class AronFloat extends GenericData { float value; };
- class AronString extends GenericData { string value; };
- class AronBool extends GenericData { bool value; };
+ // They must be prefixed with 'Aron' as e.g. Int cannot be used as class name
+ class AronInt extends GenericData
+ {
+ int value;
+ };
+
+ class AronLong extends GenericData
+ {
+ long value;
+ };
+
+ class AronDouble extends GenericData
+ {
+ double value;
+ };
+
+ class AronFloat extends GenericData
+ {
+ float value;
+ };
+
+ class AronString extends GenericData
+ {
+ string value;
+ };
+
+ class AronBool extends GenericData
+ {
+ bool value;
+ };
// useful for memory ice_conversions
sequence<Dict> AronDictSeq;
diff --git a/source/RobotAPI/interface/components/RobotHealthInterface.ice b/source/RobotAPI/interface/components/RobotHealthInterface.ice
index bbad059d5a7468428a5a351b07b95e64bc27532b..0832146d934a45cf35dd2d42591a4b4949f8d15d 100644
--- a/source/RobotAPI/interface/components/RobotHealthInterface.ice
+++ b/source/RobotAPI/interface/components/RobotHealthInterface.ice
@@ -22,8 +22,12 @@
#pragma once
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/time.ice>
+
module armarx
{
+
enum RobotHealthState
{
HealthOK, HealthWarning, HealthError
@@ -31,24 +35,74 @@ module armarx
struct RobotHealthHeartbeatArgs
{
- int maximumCycleTimeWarningMS = 100;
- int maximumCycleTimeErrorMS = 200;
- string message;
+ string identifier;
+ // bool requiredByDefault;
+ string description;
+ Ice::StringSeq tags;
+
+ armarx::core::time::dto::Duration maximumCycleTimeWarning;
+ armarx::core::time::dto::Duration maximumCycleTimeError;
};
interface RobotHealthInterface
{
- void heartbeat(string componentName, RobotHealthHeartbeatArgs args);
- void unregister(string componentName);
+ void signUp(RobotHealthHeartbeatArgs args);
+ void unregister(string identifier);
+
+ void addRequiredTags(string requesterIdentifier, Ice::StringSeq tags);
+ void removeRequiredTags(string requesterIdentifier, Ice::StringSeq tags);
+ void resetRequiredTags();
+
+ void heartbeat(string identifier, armarx::core::time::dto::DateTime referenceTime);
+
+ string getTopicName();
};
+ // Used by the RobotHealth to send an overall status update ehich e.g. the emergency stop listens to.
interface AggregatedRobotHealthInterface
{
void aggregatedHeartbeat(RobotHealthState overallHealthState);
};
+ struct RobotHealthInfoEntry
+ {
+ string identifier;
+ RobotHealthState state;
+
+ // string message;
+ armarx::core::time::dto::Duration minDelta;
+ armarx::core::time::dto::Duration maxDelta;
+
+ armarx::core::time::dto::DateTime lastReferenceTimestamp;
+
+ //< Time delta to now() when arrived at heart beat component
+ armarx::core::time::dto::Duration timeSinceLastArrival;
+
+ //< Time delta to reference timestamp sent by component
+ armarx::core::time::dto::Duration timeSinceLastUpdateReference;
+
+ armarx::core::time::dto::Duration timeSyncDelayAndIce;
+
+ bool required; //
+ bool enabled;
+
+ armarx::core::time::dto::Duration maximumCycleTimeWarning;
+ armarx::core::time::dto::Duration maximumCycleTimeError;
+
+ Ice::StringSeq tags;
+ };
+
+ dictionary<string, RobotHealthInfoEntry> RobotHealthEntries;
+
+ struct RobotHealthInfo
+ {
+ Ice::StringSeq activeTags;
+ RobotHealthEntries entries;
+ };
+
+
interface RobotHealthComponentInterface extends RobotHealthInterface
{
- string getSummary();
+ RobotHealthInfo getSummary();
};
};
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/interface/units/LaserScannerUnit.ice b/source/RobotAPI/interface/units/LaserScannerUnit.ice
index 631974396e0de5c50dcd67e73c47da2ad78fa99f..cf3a48562253065449dcdceb30030573f1a85e87 100644
--- a/source/RobotAPI/interface/units/LaserScannerUnit.ice
+++ b/source/RobotAPI/interface/units/LaserScannerUnit.ice
@@ -46,18 +46,18 @@ module armarx
**/
struct LaserScanStep
{
- float angle;
- float distance;
-// float intensity;
+ float angle;
+ float distance;
+ float intensity;
};
struct LaserScannerInfo
{
- string device;
- string frame;
- float minAngle;
- float maxAngle;
- float stepSize;
+ string device;
+ string frame;
+ float minAngle;
+ float maxAngle;
+ float stepSize;
};
sequence<LaserScanStep> LaserScan;
@@ -82,4 +82,3 @@ module armarx
};
};
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index f5516353eecf459e4d2cde12db3737b396908972..8b61c578cca4070197d4b57bcc8a353ff8ceee2c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -220,6 +220,8 @@ namespace armarx
if (fs::is_directory(datasetDir / dir))
{
ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
+ object.setLogError(logObjectDiscoveryError);
+
if (!checkPaths || object.checkPaths())
{
objects.push_back(object);
@@ -371,6 +373,11 @@ namespace armarx
return info.loadSpokenNames();
});
}
+
+ void ObjectFinder::setLogObjectDiscoveryError(bool logEnabled)
+ {
+ logObjectDiscoveryError = logEnabled;
+ }
ObjectFinder::path ObjectFinder::_rootDirAbs() const
{
@@ -388,4 +395,3 @@ namespace armarx
}
}
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index 708608528cd8d506d80f229a6c9c96896d18207a..5c461033d3d75b4d097ec2763928a9e94aab6389 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -94,6 +94,8 @@ namespace armarx
*/
std::vector<std::string> loadSpokenNames(const ObjectID& objectID, bool includeClassName = false) const;
+ void setLogObjectDiscoveryError(bool logEnabled);
+
private:
@@ -120,5 +122,9 @@ namespace armarx
/// Path to the directory containing objects in the package's data directory.
path relObjectsDir;
+
+ // see: ObjectInfo::checkPaths()
+ bool logObjectDiscoveryError = true;
+
};
}
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/aron/ObjectPose.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
index c316b4f9a694d803f8d8ef49b8340ce64bbb182d..90a1ef3d546b8914075c1f0171adeb2786dddb9c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
@@ -78,13 +78,13 @@ ARON DTO of armarx::objpose::ObjectPose.
<!-- The object's joint values if it is articulated. -->
<ObjectChild key='objectJointValues'>
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key='robotConfig'>
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
@@ -104,7 +104,7 @@ ARON DTO of armarx::objpose::ObjectPose.
</ObjectChild>
<ObjectChild key='confidence'>
- <float />
+ <float32 />
</ObjectChild>
<ObjectChild key='timestamp'>
@@ -123,4 +123,3 @@ ARON DTO of armarx::objpose::ObjectPose.
</GenerateTypes>
</AronTypeDefinition>
-
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 af59f5fa837a3d80e199350ee9da562bc5745703..0eecc8e79de97cb3e164b195dd43cedc57b789cf 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -1,25 +1,25 @@
#include "GraspCandidateReader.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error/mns.h>
-#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/libraries/armem/util/util.h>
namespace armarx::armem
{
- GraspCandidateReader::GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem)
- : memoryNameSystem(memoryNameSystem)
+ GraspCandidateReader::GraspCandidateReader()
{
}
-
- void GraspCandidateReader::connect(bool use)
+ void
+ 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 << "' ...";
+ ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '" << properties.memoryName
+ << "' ...";
try
{
memoryReader = use ? memoryNameSystem.useReader(properties.memoryName)
@@ -34,8 +34,8 @@ namespace armarx::armem
}
}
-
- armarx::grasping::GraspCandidate asGraspCandidate(const armem::wm::EntityInstance& instance)
+ armarx::grasping::GraspCandidate
+ asGraspCandidate(const armem::wm::EntityInstance& instance)
{
armarx::grasping::GraspCandidate candidate;
@@ -48,7 +48,8 @@ namespace armarx::armem
return candidate;
}
- armarx::grasping::BimanualGraspCandidate asBimanualGraspCandidate(const armem::wm::EntityInstance& instance)
+ armarx::grasping::BimanualGraspCandidate
+ asBimanualGraspCandidate(const armem::wm::EntityInstance& instance)
{
armarx::grasping::BimanualGraspCandidate candidate;
@@ -60,8 +61,8 @@ namespace armarx::armem
return candidate;
}
-
- grasping::GraspCandidatePtr GraspCandidateReader::queryGraspCandidateInstanceByID(const armem::MemoryID& id) const
+ grasping::GraspCandidatePtr
+ GraspCandidateReader::queryGraspCandidateInstanceByID(const armem::MemoryID& id) const
{
auto dict = queryGraspCandidateInstancesByID({id});
if (auto it = dict.find(id.str()); it != dict.end())
@@ -74,8 +75,8 @@ namespace armarx::armem
}
}
-
- grasping::GraspCandidateDict GraspCandidateReader::queryGraspCandidateInstancesByID(const std::vector<MemoryID>& ids) const
+ grasping::GraspCandidateDict
+ GraspCandidateReader::queryGraspCandidateInstancesByID(const std::vector<MemoryID>& ids) const
{
armem::client::query::Builder qb;
@@ -83,8 +84,7 @@ namespace armarx::armem
qb.multipleEntitySnapshots(ids);
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
if (!qResult.success)
{
@@ -104,9 +104,8 @@ namespace armarx::armem
return candidates;
}
-
- grasping::BimanualGraspCandidatePtr GraspCandidateReader::queryBimanualGraspCandidateInstanceByID(
- const armem::MemoryID& id) const
+ grasping::BimanualGraspCandidatePtr
+ GraspCandidateReader::queryBimanualGraspCandidateInstanceByID(const armem::MemoryID& id) const
{
armem::client::query::Builder qb;
@@ -114,8 +113,7 @@ namespace armarx::armem
qb.singleEntitySnapshot(id.getEntitySnapshotID());
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
if (!qResult.success)
{
@@ -127,11 +125,12 @@ namespace armarx::armem
armarx::grasping::BimanualGraspCandidatePtr candidate;
armem::wm::FunctionalVisitor visitor;
- visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const & instance)
+ visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const& instance)
{
if (instance.id() == id)
{
- candidate = new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
+ candidate =
+ new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
}
return true;
};
@@ -141,46 +140,52 @@ namespace armarx::armem
return candidate;
}
- grasping::GraspCandidateDict GraspCandidateReader::queryLatestGraspCandidateEntity(
- const std::string& provider, const std::string& entity) const
+ grasping::GraspCandidateDict
+ GraspCandidateReader::queryLatestGraspCandidateEntity(const std::string& provider,
+ const std::string& entity) const
{
armem::client::query::Builder qb;
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
- qb
- .coreSegments().withName(properties.graspCandidateMemoryName)
- .providerSegments().withName(provider)
- .entities().withName(entity)
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.graspCandidateMemoryName)
+ .providerSegments()
+ .withName(provider)
+ .entities()
+ .withName(entity)
+ .snapshots()
+ .latest();
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getGraspCandidatesFromResultSet(qResult);
}
- std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::queryLatestBimanualGraspCandidateEntity(
- const std::string& provider, const std::string& entity) const
+ std::map<std::string, grasping::BimanualGraspCandidatePtr>
+ GraspCandidateReader::queryLatestBimanualGraspCandidateEntity(const std::string& provider,
+ const std::string& entity) const
{
armem::client::query::Builder qb;
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
- qb
- .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
- .providerSegments().withName(provider)
- .entities().withName(entity)
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments()
+ .withName(provider)
+ .entities()
+ .withName(entity)
+ .snapshots()
+ .latest();
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getBimanualGraspCandidatesFromResultSet(qResult);
}
- grasping::GraspCandidateDict GraspCandidateReader::queryLatestGraspCandidates(
- const std::string& provider) const
+ grasping::GraspCandidateDict
+ GraspCandidateReader::queryLatestGraspCandidates(const std::string& provider) const
{
armem::client::query::Builder qb;
@@ -190,29 +195,34 @@ namespace armarx::armem
if (!provider.empty())
{
- qb
- .coreSegments().withName(properties.graspCandidateMemoryName)
- .providerSegments().withName(provider)
- .entities().all()
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.graspCandidateMemoryName)
+ .providerSegments()
+ .withName(provider)
+ .entities()
+ .all()
+ .snapshots()
+ .latest();
}
else
{
- qb
- .coreSegments().withName(properties.graspCandidateMemoryName)
- .providerSegments().all()
- .entities().all()
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.graspCandidateMemoryName)
+ .providerSegments()
+ .all()
+ .entities()
+ .all()
+ .snapshots()
+ .latest();
}
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getGraspCandidatesFromResultSet(qResult);
}
- std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::queryLatestBimanualGraspCandidates(
- const std::string& provider) const
+ std::map<std::string, grasping::BimanualGraspCandidatePtr>
+ GraspCandidateReader::queryLatestBimanualGraspCandidates(const std::string& provider) const
{
armem::client::query::Builder qb;
@@ -221,42 +231,49 @@ namespace armarx::armem
if (!provider.empty())
{
- qb
- .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
- .providerSegments().withName(provider)
- .entities().all()
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments()
+ .withName(provider)
+ .entities()
+ .all()
+ .snapshots()
+ .latest();
}
else
{
- qb
- .coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
- .providerSegments().all()
- .entities().all()
- .snapshots().latest();
+ qb.coreSegments()
+ .withName(properties.bimanualGraspCandidateMemoryName)
+ .providerSegments()
+ .all()
+ .entities()
+ .all()
+ .snapshots()
+ .latest();
}
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
return getBimanualGraspCandidatesFromResultSet(qResult);
}
-
- void GraspCandidateReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ GraspCandidateReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "GraspCandidateReader: registerPropertyDefinitions";
const std::string prefix = propertyPrefix;
- def->optional(properties.graspCandidateMemoryName, prefix + "GraspCandidateMemoryName",
+ def->optional(properties.graspCandidateMemoryName,
+ prefix + "GraspCandidateMemoryName",
"Name of the grasping memory core segment to use.");
def->optional(properties.memoryName, prefix + "MemoryName");
}
- grasping::GraspCandidateDict GraspCandidateReader::getGraspCandidatesFromResultSet(
- const armem::client::QueryResult& qResult) const
+ grasping::GraspCandidateDict
+ GraspCandidateReader::getGraspCandidatesFromResultSet(
+ const armem::client::QueryResult& qResult) const
{
if (!qResult.success)
{
@@ -267,9 +284,10 @@ namespace armarx::armem
std::map<std::string, armarx::grasping::GraspCandidatePtr> candidates;
armem::wm::FunctionalVisitor visitor;
- visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const & instance)
+ visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const& instance)
{
- candidates[instance.id().str()] = new grasping::GraspCandidate(asGraspCandidate(instance));
+ candidates[instance.id().str()] =
+ new grasping::GraspCandidate(asGraspCandidate(instance));
return true;
};
@@ -279,8 +297,9 @@ namespace armarx::armem
return candidates;
}
- std::map<std::string, grasping::BimanualGraspCandidatePtr> GraspCandidateReader::getBimanualGraspCandidatesFromResultSet(
- const armem::client::QueryResult& qResult) const
+ std::map<std::string, grasping::BimanualGraspCandidatePtr>
+ GraspCandidateReader::getBimanualGraspCandidatesFromResultSet(
+ const armem::client::QueryResult& qResult) const
{
if (!qResult.success)
{
@@ -291,9 +310,10 @@ namespace armarx::armem
std::map<std::string, armarx::grasping::BimanualGraspCandidatePtr> candidates;
armem::wm::FunctionalVisitor visitor;
- visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const & instance)
+ visitor.instanceConstFn = [&candidates](armem::wm::EntityInstance const& instance)
{
- candidates[instance.id().str()] = new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
+ candidates[instance.id().str()] =
+ new grasping::BimanualGraspCandidate(asBimanualGraspCandidate(instance));
return true;
};
@@ -303,5 +323,41 @@ namespace armarx::armem
return candidates;
}
+ grasping::GraspCandidateDict
+ GraspCandidateReader::queryGraspCandidatesNewerThan(const std::string& provider,
+ const DateTime& timestamp) const
+ {
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
+
+
+ if (!provider.empty())
+ {
+ qb.coreSegments()
+ .withName(properties.graspCandidateMemoryName)
+ .providerSegments()
+ .withName(provider)
+ .entities()
+ .all()
+ .snapshots()
+ .timeRange(timestamp, armarx::core::time::DateTime::Now());
+ }
+ else
+ {
+ qb.coreSegments()
+ .withName(properties.graspCandidateMemoryName)
+ .providerSegments()
+ .all()
+ .entities()
+ .all()
+ .snapshots()
+ .timeRange(timestamp, armarx::core::time::DateTime::Now());
+ }
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ return getGraspCandidatesFromResultSet(qResult);
+ }
+
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
index 5de5c25e335e5544170f18f838df2e5e23d213fa..4b7bba78baae8c78c7c37fdc77ebdaff8e6fd154 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
@@ -2,10 +2,9 @@
#include <vector>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
-
+#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
namespace armarx::armem
{
@@ -13,39 +12,47 @@ 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 queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
- grasping::GraspCandidateDict queryGraspCandidateInstancesByID(std::vector<armem::MemoryID> const& ids) const;
+ ::armarx::grasping::GraspCandidatePtr
+ queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
- grasping::BimanualGraspCandidatePtr queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
+ ::armarx::grasping::GraspCandidateDict
+ queryGraspCandidateInstancesByID(std::vector<armem::MemoryID> const& ids) const;
- grasping::GraspCandidateDict queryLatestGraspCandidateEntity(
- std::string const& provider, std::string const& entity) const;
+ ::armarx::grasping::BimanualGraspCandidatePtr
+ queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
- std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidateEntity(
- std::string const& provider, std::string const& entity) const;
+ ::armarx::grasping::GraspCandidateDict
+ queryLatestGraspCandidateEntity(std::string const& provider,
+ std::string const& entity) const;
- grasping::GraspCandidateDict queryLatestGraspCandidates(
- std::string const& provider = "") const;
+ std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
+ queryLatestBimanualGraspCandidateEntity(std::string const& provider,
+ std::string const& entity) const;
- std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidates(
- std::string const& provider = "") const;
+ ::armarx::grasping::GraspCandidateDict
+ queryLatestGraspCandidates(std::string const& provider = "") const;
+ ::armarx::grasping::GraspCandidateDict queryGraspCandidatesNewerThan(
+ std::string const& provider = "",
+ const armarx::DateTime& timestamp = armarx::DateTime::Now()) const;
+
+ std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
+ queryLatestBimanualGraspCandidates(std::string const& provider = "") const;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
private:
+ ::armarx::grasping::GraspCandidateDict
+ getGraspCandidatesFromResultSet(armem::client::QueryResult const& qResult) const;
- grasping::GraspCandidateDict getGraspCandidatesFromResultSet(
- armem::client::QueryResult const& qResult) const;
-
- std::map<std::string, grasping::BimanualGraspCandidatePtr> getBimanualGraspCandidatesFromResultSet(
- armem::client::QueryResult const& qResult) const;
+ std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
+ getBimanualGraspCandidatesFromResultSet(armem::client::QueryResult const& qResult) const;
armem::client::Reader memoryReader;
@@ -57,11 +64,7 @@ namespace armarx::armem
std::string bimanualGraspCandidateMemoryName = "BimanualGraspCandidate";
} 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/GraspingUtility/aron/GraspCandidate.xml b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
index 46655bf4366d829d1a846350c39af7273004837d..d924b40fc03e933f56e0355f8cb62f5d32d88a28 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
+++ b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
@@ -46,7 +46,7 @@
<string />
</ObjectChild>
<ObjectChild key='segmentationLabelID'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='bbox'>
<armarx::grasping::arondto::BoundingBox optional="true" />
@@ -70,18 +70,18 @@
<bool />
</ObjectChild>
<ObjectChild key='minimumJointLimitMargin'>
- <float />
+ <float32 />
</ObjectChild>
<ObjectChild key='jointLimitMargins'>
<List>
- <Float />
+ <float32 />
</List>
</ObjectChild>
<ObjectChild key='maxPosError'>
- <float />
+ <float32 />
</ObjectChild>
<ObjectChild key='maxOriError'>
- <float />
+ <float32 />
</ObjectChild>
</Object>
@@ -116,7 +116,7 @@
</ObjectChild>
<ObjectChild key='graspSuccessProbability'>
- <float optional="true" />
+ <float32 optional="true" />
</ObjectChild>
<ObjectChild key='objectType'>
@@ -124,7 +124,7 @@
</ObjectChild>
<ObjectChild key='groupNr'>
- <int optional="true" />
+ <int32 optional="true" />
</ObjectChild>
<ObjectChild key='providerName'>
@@ -196,7 +196,7 @@
</ObjectChild>
<ObjectChild key='groupNr'>
- <int optional="true" />
+ <int32 optional="true" />
</ObjectChild>
<ObjectChild key='providerName'>
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
index e5021bdda3da1683f65f81fe61253fdfdfa3dc62..d81a3fcd2e5006741ff650c2ea2fc433d7a6783f 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
@@ -1,20 +1,24 @@
#include "aron_conversions.h"
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/core/Pose.h>
-void armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto, armarx::grasping::BoundingBox& bo)
+void
+armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto,
+ armarx::grasping::BoundingBox& bo)
{
ARMARX_TRACE;
bo = BoundingBox(toIce(dto.center), toIce(dto.ha1), toIce(dto.ha2), toIce(dto.ha3));
}
-void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto, const armarx::grasping::BoundingBox& bo)
+void
+armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto,
+ const armarx::grasping::BoundingBox& bo)
{
ARMARX_TRACE;
dto.center = fromIce(bo.center);
@@ -23,25 +27,31 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto, const
dto.ha3 = fromIce(bo.ha3);
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
- armarx::grasping::GraspCandidateSourceInfo& bo)
+void
+armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
+ armarx::grasping::GraspCandidateSourceInfo& bo)
{
ARMARX_TRACE;
bo.bbox = new BoundingBox();
if (dto.bbox)
{
fromAron(dto.bbox.value(), *bo.bbox);
- } else bo.bbox = nullptr;
+ }
+ else
+ bo.bbox = nullptr;
bo.referenceObjectName = dto.referenceObjectName;
if (dto.referenceObjectPose)
{
bo.referenceObjectPose = toIce(dto.referenceObjectPose.value());
- } else bo.referenceObjectPose = nullptr;
+ }
+ else
+ bo.referenceObjectPose = nullptr;
bo.segmentationLabelID = dto.segmentationLabelID;
}
-void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
- const armarx::grasping::GraspCandidateSourceInfo& bo)
+void
+armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
+ const armarx::grasping::GraspCandidateSourceInfo& bo)
{
ARMARX_TRACE;
if (bo.bbox)
@@ -57,16 +67,21 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInf
dto.segmentationLabelID = bo.segmentationLabelID;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
- armarx::grasping::GraspCandidateReachabilityInfo& bo)
+void
+armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
+ armarx::grasping::GraspCandidateReachabilityInfo& bo)
{
ARMARX_TRACE;
- bo = GraspCandidateReachabilityInfo(dto.reachable, dto.minimumJointLimitMargin, dto.jointLimitMargins,
- dto.maxPosError, dto.maxOriError);
+ bo = GraspCandidateReachabilityInfo(dto.reachable,
+ dto.minimumJointLimitMargin,
+ dto.jointLimitMargins,
+ dto.maxPosError,
+ dto.maxOriError);
}
-void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
- const armarx::grasping::GraspCandidateReachabilityInfo& bo)
+void
+armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
+ const armarx::grasping::GraspCandidateReachabilityInfo& bo)
{
ARMARX_TRACE;
dto.jointLimitMargins = bo.jointLimitMargins;
@@ -77,7 +92,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabil
}
void
-armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto, armarx::grasping::GraspCandidate& bo)
+armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
+ armarx::grasping::GraspCandidate& bo)
{
ARMARX_TRACE;
bo = GraspCandidate();
@@ -88,13 +104,15 @@ armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
bo.sourceFrame = dto.sourceFrame;
bo.targetFrame = dto.targetFrame;
bo.side = dto.side;
- bo.graspSuccessProbability = dto.graspSuccessProbability ? dto.graspSuccessProbability.value() : -1.0;
+ bo.graspSuccessProbability =
+ dto.graspSuccessProbability ? dto.graspSuccessProbability.value() : -1.0;
fromAron(dto.objectType, bo.objectType);
if (dto.executionHints)
{
bo.executionHints = new GraspCandidateExecutionHints();
fromAron(dto.executionHints.value(), *bo.executionHints);
- } else
+ }
+ else
{
bo.executionHints = nullptr;
}
@@ -104,7 +122,8 @@ armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
{
bo.reachabilityInfo = new GraspCandidateReachabilityInfo();
fromAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
- } else
+ }
+ else
{
bo.reachabilityInfo = nullptr;
}
@@ -112,14 +131,16 @@ armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
{
bo.sourceInfo = new GraspCandidateSourceInfo();
fromAron(dto.sourceInfo.value(), *bo.sourceInfo);
- } else
+ }
+ else
{
bo.sourceInfo = nullptr;
}
}
void
-armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const armarx::grasping::GraspCandidate& bo)
+armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto,
+ const armarx::grasping::GraspCandidate& bo)
{
ARMARX_TRACE;
if (bo.approachVector)
@@ -130,7 +151,8 @@ armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const a
{
dto.executionHints = arondto::GraspCandidateExecutionHints();
toAron(dto.executionHints.value(), *bo.executionHints);
- } else
+ }
+ else
{
dto.executionHints = std::nullopt;
}
@@ -138,14 +160,16 @@ armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const a
if (bo.graspSuccessProbability < 0 || bo.graspSuccessProbability > 1.0)
{
dto.graspSuccessProbability = std::nullopt;
- } else
+ }
+ else
{
dto.graspSuccessProbability = bo.graspSuccessProbability;
}
if (bo.groupNr < 0)
{
dto.groupNr = std::nullopt;
- } else
+ }
+ else
{
dto.groupNr = bo.groupNr;
}
@@ -155,7 +179,8 @@ armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const a
{
dto.reachabilityInfo = arondto::GraspCandidateReachabilityInfo();
toAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
- } else
+ }
+ else
{
dto.reachabilityInfo = std::nullopt;
}
@@ -171,25 +196,31 @@ armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const a
{
dto.sourceInfo = arondto::GraspCandidateSourceInfo();
toAron(dto.sourceInfo.value(), *bo.sourceInfo);
- } else
+ }
+ else
{
dto.sourceInfo = std::nullopt;
}
dto.targetFrame = bo.targetFrame;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCandidate& dto,
- armarx::grasping::BimanualGraspCandidate& bo)
+void
+armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCandidate& dto,
+ armarx::grasping::BimanualGraspCandidate& bo)
{
ARMARX_TRACE;
bo = BimanualGraspCandidate();
bo.graspPoseRight = toIce(dto.graspPoseRight);
bo.graspPoseLeft = toIce(dto.graspPoseLeft);
bo.robotPose = toIce(dto.robotPose);
- bo.tcpPoseInHandRootRight = dto.tcpPoseInHandRootRight ? toIce(dto.tcpPoseInHandRootRight.value()) : nullptr;
- bo.tcpPoseInHandRootLeft = dto.tcpPoseInHandRootLeft ? toIce(dto.tcpPoseInHandRootLeft.value()) : nullptr;
- bo.approachVectorRight = dto.approachVectorRight ? toIce(dto.approachVectorRight.value()) : nullptr;
- bo.approachVectorLeft = dto.approachVectorLeft ? toIce(dto.approachVectorLeft.value()) : nullptr;
+ bo.tcpPoseInHandRootRight =
+ dto.tcpPoseInHandRootRight ? toIce(dto.tcpPoseInHandRootRight.value()) : nullptr;
+ bo.tcpPoseInHandRootLeft =
+ dto.tcpPoseInHandRootLeft ? toIce(dto.tcpPoseInHandRootLeft.value()) : nullptr;
+ bo.approachVectorRight =
+ dto.approachVectorRight ? toIce(dto.approachVectorRight.value()) : nullptr;
+ bo.approachVectorLeft =
+ dto.approachVectorLeft ? toIce(dto.approachVectorLeft.value()) : nullptr;
bo.inwardsVectorRight = toIce(dto.inwardsVectorRight);
bo.inwardsVectorLeft = toIce(dto.inwardsVectorLeft);
bo.sourceFrame = dto.sourceFrame;
@@ -199,7 +230,8 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.executionHintsRight = new GraspCandidateExecutionHints();
fromAron(dto.executionHintsRight.value(), *bo.executionHintsRight);
- } else
+ }
+ else
{
bo.executionHintsRight = nullptr;
}
@@ -207,7 +239,8 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.executionHintsLeft = new GraspCandidateExecutionHints();
fromAron(dto.executionHintsLeft.value(), *bo.executionHintsLeft);
- } else
+ }
+ else
{
bo.executionHintsLeft = nullptr;
}
@@ -217,7 +250,8 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.reachabilityInfoRight = new GraspCandidateReachabilityInfo();
fromAron(dto.reachabilityInfoRight.value(), *bo.reachabilityInfoRight);
- } else
+ }
+ else
{
bo.reachabilityInfoRight = nullptr;
}
@@ -225,7 +259,8 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.reachabilityInfoLeft = new GraspCandidateReachabilityInfo();
fromAron(dto.reachabilityInfoLeft.value(), *bo.reachabilityInfoLeft);
- } else
+ }
+ else
{
bo.reachabilityInfoLeft = nullptr;
}
@@ -233,15 +268,17 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.sourceInfo = new GraspCandidateSourceInfo();
fromAron(dto.sourceInfo.value(), *bo.sourceInfo);
- } else
+ }
+ else
{
bo.sourceInfo = nullptr;
}
bo.graspName = dto.graspName;
}
-void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate& dto,
- const armarx::grasping::BimanualGraspCandidate& bo)
+void
+armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate& dto,
+ const armarx::grasping::BimanualGraspCandidate& bo)
{
if (bo.tcpPoseInHandRootRight)
{
@@ -263,7 +300,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.executionHintsRight = arondto::GraspCandidateExecutionHints();
toAron(dto.executionHintsRight.value(), *bo.executionHintsRight);
- } else
+ }
+ else
{
dto.executionHintsRight = std::nullopt;
}
@@ -271,17 +309,18 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.executionHintsLeft = arondto::GraspCandidateExecutionHints();
toAron(dto.executionHintsLeft.value(), *bo.executionHintsLeft);
- } else
+ }
+ else
{
dto.executionHintsLeft = std::nullopt;
-
}
dto.graspPoseRight = fromIce(bo.graspPoseRight);
dto.graspPoseLeft = fromIce(bo.graspPoseLeft);
if (bo.groupNr < 0)
{
dto.groupNr = std::nullopt;
- } else
+ }
+ else
{
dto.groupNr = bo.groupNr;
}
@@ -291,7 +330,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.reachabilityInfoRight = arondto::GraspCandidateReachabilityInfo();
toAron(dto.reachabilityInfoRight.value(), *bo.reachabilityInfoRight);
- } else
+ }
+ else
{
dto.reachabilityInfoRight = std::nullopt;
}
@@ -299,7 +339,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.reachabilityInfoLeft = arondto::GraspCandidateReachabilityInfo();
toAron(dto.reachabilityInfoLeft.value(), *bo.reachabilityInfoLeft);
- } else
+ }
+ else
{
dto.reachabilityInfoLeft = std::nullopt;
}
@@ -309,7 +350,8 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.sourceInfo = arondto::GraspCandidateSourceInfo();
toAron(dto.sourceInfo.value(), *bo.sourceInfo);
- } else
+ }
+ else
{
dto.sourceInfo = std::nullopt;
}
@@ -319,8 +361,9 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
dto.graspName = bo.graspName;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
- armarx::grasping::GraspCandidateExecutionHints& bo)
+void
+armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
+ armarx::grasping::GraspCandidateExecutionHints& bo)
{
bo = GraspCandidateExecutionHints();
fromAron(dto.approach, bo.approach);
@@ -328,15 +371,18 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateE
bo.graspTrajectoryName = dto.graspTrajectoryName;
}
-void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
- const armarx::grasping::GraspCandidateExecutionHints& bo)
+void
+armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateExecutionHints& dto,
+ const armarx::grasping::GraspCandidateExecutionHints& bo)
{
toAron(dto.approach, bo.approach);
toAron(dto.preshape, bo.preshape);
dto.graspTrajectoryName = bo.graspTrajectoryName;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto, armarx::grasping::ApproachType& bo)
+void
+armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto,
+ armarx::grasping::ApproachType& bo)
{
switch (dto.value)
{
@@ -351,10 +397,11 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& d
return;
}
ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
-
}
-void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto, const armarx::grasping::ApproachType& bo)
+void
+armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto,
+ const armarx::grasping::ApproachType& bo)
{
switch (bo)
{
@@ -369,10 +416,11 @@ void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto, cons
return;
}
ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
-
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto, armarx::grasping::ApertureType& bo)
+void
+armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto,
+ armarx::grasping::ApertureType& bo)
{
switch (dto.value)
{
@@ -389,7 +437,9 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& d
ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
}
-void armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto, const armarx::grasping::ApertureType& bo)
+void
+armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto,
+ const armarx::grasping::ApertureType& bo)
{
switch (bo)
{
diff --git a/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
index 7c22f824a7161e844590073789f55b4a3e7f5ef9..03ff54827ffe615760598e6e4d76bfb0e44b7d30 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
+++ b/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
@@ -1,5 +1,12 @@
add_subdirectory(core)
+
+add_subdirectory(util/LocationLoader)
+add_subdirectory(util/AffordanceLoader)
+add_subdirectory(util/CommonPlaceLoader)
+
add_subdirectory(motions)
add_subdirectory(objects)
+add_subdirectory(robots)
+add_subdirectory(navigation_graphs)
#add_subdirectory(subjects)
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/core/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/core/CMakeLists.txt
index d9adff9e0ec67014af674e12ec170a5a465d75cc..747344cc6e21ab9a6edfe54128d9934958ed5aef 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/PriorKnowledge/core/CMakeLists.txt
@@ -5,14 +5,19 @@ armarx_set_target("Library: ${LIB_NAME}")
armarx_add_library(
LIBS
+ SimoxUtility
RobotAPI::Core
RobotAPI::Aron::Common
SOURCES
FinderBase.cpp
FinderInfoBase.cpp
+ PKFinder.cpp
+ PKDatasetFinder.cpp
HEADERS
FinderBase.h
FinderInfoBase.h
+ PKFinder.h
+ PKDatasetFinder.h
)
add_library(${PROJECT_NAME}::PriorKnowledge::Core ALIAS ${PROJECT_NAME}PriorKnowledgeCore)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h b/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
index 3dc38a1814850c68c977986ec059494dd8901a9f..a0ad9297f873496e7dbe0d1c17fb3ab0b5904ac1 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
@@ -1,19 +1,18 @@
#pragma once
// STD/STL
-#include <string>
#include <filesystem>
#include <optional>
+#include <string>
// Base Class
#include <ArmarXCore/core/logging/Logging.h>
// ArmarX
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-
namespace armarx::priorknowledge::core
{
template <class IDType, class FinderInfoType>
@@ -21,9 +20,15 @@ namespace armarx::priorknowledge::core
{
public:
FinderBase() = delete;
+
FinderBase(const std::string& packageName, const std::filesystem::path& relDir) :
- packageName(packageName),
- relPackageDataPath(relDir)
+ packageName(packageName), relPackageDataPath(relDir)
+ {
+ recalculateBasePath();
+ }
+
+ void
+ recalculateBasePath()
{
CMakePackageFinder packageFinder(packageName);
absPackageDataDir = packageFinder.getDataDir();
@@ -37,66 +42,80 @@ namespace armarx::priorknowledge::core
ARMARX_INFO << "PriorKnowledgeFinder root directory: " << absPackageDataDir;
// make sure this data path is available => e.g. for findArticulatedObjects
- armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {absPackageDataDir});
+ armarx::ArmarXDataPath::addDataPaths(std::vector<std::string>{absPackageDataDir});
}
}
- FinderBase(FinderBase&&) = default;
- FinderBase(const FinderBase&) = default;
- FinderBase& operator=(FinderBase&&) = default;
+ virtual ~FinderBase()
+ {
+ }
+
+ FinderBase(FinderBase&&) = default;
+ FinderBase(const FinderBase&) = default;
+ FinderBase& operator=(FinderBase&&) = default;
FinderBase& operator=(const FinderBase&) = default;
- void setRelativePath(const std::filesystem::path& p)
+ void
+ setRelativePath(const std::filesystem::path& p)
{
relPackageDataPath = p;
}
- std::string getPackageName() const
+ std::string
+ getPackageName() const
{
return packageName;
}
- std::filesystem::path getRelativePath() const
+ std::filesystem::path
+ getRelativePath() const
{
return relPackageDataPath;
}
- std::filesystem::path getAbsolutePackagePath() const
+ std::filesystem::path
+ getAbsolutePackagePath() const
{
checkAbsolutePathIsValid();
return absPackageDataDir;
}
- std::filesystem::path getFullPath() const
+ std::filesystem::path
+ getFullPath() const
{
checkAbsolutePathIsValid();
return absPackageDataDir / packageName / relPackageDataPath;
}
+ virtual bool accept(const std::filesystem::path& idPath) const = 0;
virtual bool checkAll() const = 0;
virtual bool check(const IDType& id) const = 0;
virtual std::optional<FinderInfoType> find(const IDType& id) const = 0;
virtual std::vector<FinderInfoType> findAll() const = 0;
protected:
- void checkAbsolutePathIsValid() const
+ void
+ checkAbsolutePathIsValid() const
{
if (!std::filesystem::exists(absPackageDataDir))
{
- ARMARX_ERROR << "PriorKnowledgeFinder is not initialized yet. Could not resolve absolute path for package '" << packageName << "'.";
+ ARMARX_ERROR << "PriorKnowledgeFinder is not initialized yet. Could not resolve "
+ "absolute path for package '"
+ << packageName << "'.";
}
}
protected:
+ std::string packageName;
private:
- std::string packageName;
std::filesystem::path relPackageDataPath;
std::filesystem::path absPackageDataDir;
};
-
- template <class IDType, class DatasetType, class FinderInfoType> // TODO: concept DatasetFinderInfoType
+ template <class IDType,
+ class DatasetType,
+ class FinderInfoType> // TODO: concept DatasetFinderInfoType
class DatasetFinderBase : public FinderBase<IDType, FinderInfoType>
{
@@ -105,16 +124,19 @@ namespace armarx::priorknowledge::core
DatasetFinderBase(const std::string& packageName, const std::filesystem::path& relDir) :
Base(packageName, relDir)
- {}
+ {
+ }
- std::filesystem::path getFullPath(const std::filesystem::path& relDatasetPath) const
+ std::filesystem::path
+ getFullPath(const std::filesystem::path& relPath) const
{
- return Base::getFullPath() / relDatasetPath;
+ return Base::getFullPath() / relPath;
}
virtual bool checkAll(const DatasetType& dataset) const = 0;
virtual bool check(const DatasetType& dataset, const IDType& id) const = 0;
- virtual std::optional<FinderInfoType> find(const DatasetType& dataset, const IDType& id) const = 0;
+ virtual std::optional<FinderInfoType> find(const DatasetType& dataset,
+ const IDType& id) const = 0;
virtual std::vector<FinderInfoType> findAll(const DatasetType& dataset) const = 0;
};
-}
+} // namespace armarx::priorknowledge::core
diff --git a/source/RobotAPI/libraries/PriorKnowledge/core/FinderInfoBase.h b/source/RobotAPI/libraries/PriorKnowledge/core/FinderInfoBase.h
index c28130755133bd903a5d7cdf6d3f0474d1db25f6..bc5a042705552b369de901ea4e3a6c565818b71b 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/core/FinderInfoBase.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/core/FinderInfoBase.h
@@ -1,21 +1,19 @@
#pragma once
// STD/STL
-#include <string>
#include <filesystem>
#include <optional>
+#include <string>
// Base Class
#include <ArmarXCore/core/logging/Logging.h>
// ArmarX
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
namespace armarx::priorknowledge::core
{
-
struct PackageFileLocation
{
/// Name of the ArmarX package.
@@ -28,50 +26,76 @@ namespace armarx::priorknowledge::core
std::filesystem::path absolutePath;
};
+ /**
+ * @brief The FinderInfoBase class
+ * Indicates an into to 'something' of type IDType
+ */
template <class IDType>
class FinderInfoBase
{
public:
- FinderInfoBase(const std::string& packageName, const std::filesystem::path& absPackageDataDir, const std::filesystem::path& relPackageDataPath, const IDType& id) :
+ FinderInfoBase(const std::string& packageName,
+ const std::filesystem::path& absPackageDataPath, // the path to the data dir
+ const std::filesystem::path&
+ relPathToId, // the path to 'something' from data/packageName
+ const IDType& id) :
packageName(packageName),
- absPackageDataDir(absPackageDataDir),
- relPackageDataPath(relPackageDataPath),
+ absPackageDataPath(absPackageDataPath),
+ relPathToId(relPathToId),
id(id)
- {}
+ {
+ }
- std::string getPackageName() const
+ virtual ~FinderInfoBase()
+ {
+ }
+
+ PackageFileLocation
+ toPackageFileLocation()
+ {
+ return {getPackageName(), getRelativePath(), getFullPath()};
+ }
+
+ std::string
+ getPackageName() const
{
return packageName;
}
- std::filesystem::path getRelativePath() const
+ std::filesystem::path
+ getRelativePath() const
{
- return relPackageDataPath;
+ return relPathToId;
}
- std::filesystem::path getAbsolutePackagePath() const
+ std::filesystem::path
+ getAbsolutePackageDataPath() const
{
checkAbsolutePathIsValid();
- return absPackageDataDir;
+ return absPackageDataPath;
}
- virtual std::filesystem::path getFullPath() const
+ virtual std::filesystem::path
+ getFullPath() const
{
checkAbsolutePathIsValid();
- return absPackageDataDir / packageName / relPackageDataPath;
+ return absPackageDataPath / packageName / relPathToId;
}
- IDType getID() const
+ IDType
+ getID() const
{
return id;
}
protected:
- void checkAbsolutePathIsValid() const
+ void
+ checkAbsolutePathIsValid() const
{
- if (!std::filesystem::exists(absPackageDataDir))
+ if (!std::filesystem::exists(absPackageDataPath))
{
- ARMARX_ERROR << "Could not resolve absolute path for package '" << packageName << "'.";
+ ARMARX_ERROR << "Could not resolve absolute path for package '" << packageName
+ << "'.";
}
}
@@ -81,11 +105,15 @@ namespace armarx::priorknowledge::core
private:
std::string packageName;
- std::filesystem::path absPackageDataDir;
- std::filesystem::path relPackageDataPath;
+ std::filesystem::path absPackageDataPath;
+ std::filesystem::path relPathToId;
IDType id;
};
+ /**
+ * @brief The DatasetFinderInfoBase class
+ * Indicates an into to 'something' of type IDType inside a dataset of type DatasetType
+ */
template <class IDType, class DatasetType>
class DatasetFinderInfoBase : public FinderInfoBase<IDType>
{
@@ -93,18 +121,26 @@ namespace armarx::priorknowledge::core
public:
using Base = FinderInfoBase<IDType>;
- DatasetFinderInfoBase(const std::string& packageName, const std::filesystem::path& absPackageDataDir, const std::filesystem::path& relPackageDataPath, const std::filesystem::path& relDatasetPath, const DatasetType& dataset, const IDType& id) :
- Base(packageName, absPackageDataDir, relPackageDataPath, id),
+ DatasetFinderInfoBase(
+ const std::string& packageName,
+ const std::filesystem::path& absPackageDataDir,
+ const std::filesystem::path&
+ relDatasetPath, // the path to the dataset from data/packageName
+ const std::filesystem::path& relPath, // the path to 'something' from the dataset
+ const DatasetType& dataset,
+ const IDType& id) :
+ Base(packageName, absPackageDataDir, (relDatasetPath / relPath), id),
dataset(dataset),
relDatasetPath(relDatasetPath)
- {}
+ {
+ }
- virtual std::filesystem::path getFullPath() const override
+ virtual ~DatasetFinderInfoBase()
{
- return Base::getFullPath() / relDatasetPath;
}
- DatasetType getDataset() const
+ DatasetType
+ getDataset() const
{
return dataset;
}
@@ -113,4 +149,5 @@ namespace armarx::priorknowledge::core
DatasetType dataset;
std::filesystem::path relDatasetPath;
};
-}
+
+} // namespace armarx::priorknowledge::core
diff --git a/source/RobotAPI/libraries/PriorKnowledge/core/PKDatasetFinder.cpp b/source/RobotAPI/libraries/PriorKnowledge/core/PKDatasetFinder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..249e72080b9668e0885ed3fa66986faac316c17d
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/core/PKDatasetFinder.cpp
@@ -0,0 +1,252 @@
+// Simox
+#include <SimoxUtility/algorithm/vector.hpp>
+
+// BaseClass
+#include "PKDatasetFinder.h"
+
+// ArmarX
+namespace armarx::priorknowledge::core
+{
+ void
+ PKDatasetFinder::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& defs,
+ const std::string& prefix)
+ {
+ if (not defs->hasDefinition(prefix + "packageName"))
+ {
+ defs->optional(this->packageName,
+ prefix + "packageName",
+ "The name of the prior knowledge data package.");
+ }
+ }
+
+ void
+ PKDatasetFinder::init()
+ {
+ this->recalculateBasePath();
+ }
+
+ bool
+ PKDatasetFinder::checkAll(const std::string& dataset) const
+ {
+ const std::filesystem::path absPathToDataset = this->getFullPath(dataset);
+ if (std::filesystem::is_regular_file(absPathToDataset))
+ {
+ ARMARX_WARNING << "The entered path is leading to a file!";
+ return false;
+ }
+
+ for (const auto& d : std::filesystem::directory_iterator(absPathToDataset))
+ {
+ if (!d.is_directory())
+ {
+ ARMARX_WARNING << "Found invalid path. Ignoring: " << d.path();
+ continue;
+ }
+ std::string k = d.path().filename();
+ if (simox::alg::contains(ID_BLACKLIST, k))
+ {
+ continue;
+ }
+
+ if (not this->check(dataset, k))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool
+ PKDatasetFinder::checkAll() const
+ {
+ const std::filesystem::path absPath = Base::Base::getFullPath();
+ if (std::filesystem::is_regular_file(absPath))
+ {
+ ARMARX_WARNING << "The entered path is leading to a file!";
+ return false;
+ }
+
+ for (const auto& d : std::filesystem::directory_iterator(absPath))
+ {
+ if (!d.is_directory())
+ {
+ ARMARX_WARNING << "Found invalid path: " << d.path();
+ continue;
+ }
+ std::string k = d.path().filename();
+ if (simox::alg::contains(DATASET_FOLDERS_BLACKLIST, k))
+ {
+ continue;
+ }
+
+ if (not this->checkAll(k))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool
+ PKDatasetFinder::check(const std::string& dataset, const std::string& id) const
+ {
+ if (simox::alg::contains(DATASET_FOLDERS_BLACKLIST, dataset))
+ {
+ return false;
+ }
+
+ if (simox::alg::contains(ID_BLACKLIST, id))
+ {
+ return false;
+ }
+
+ const std::filesystem::path idPath = std::filesystem::path(dataset) / id;
+ const std::filesystem::path absPathToId = this->getFullPath(idPath);
+
+ return this->accept(absPathToId);
+ }
+
+ bool
+ PKDatasetFinder::check(const std::string& id) const
+ {
+ const std::filesystem::path absPath = Base::Base::getFullPath();
+ if (std::filesystem::is_regular_file(absPath))
+ {
+ ARMARX_WARNING << "The entered path is leading to a file!";
+ return false;
+ }
+
+ for (const auto& d : std::filesystem::directory_iterator(absPath))
+ {
+ if (!d.is_directory())
+ {
+ ARMARX_WARNING << "Found invalid path: " << d.path();
+ continue;
+ }
+ std::string k = d.path().filename();
+ if (simox::alg::contains(DATASET_FOLDERS_BLACKLIST, k))
+ {
+ continue;
+ }
+
+ if (this->check(k, id))
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
+ std::optional<PKDatasetFinderInfo>
+ PKDatasetFinder::find(const std::string& dataset, const std::string& id) const
+ {
+ const std::filesystem::path idPath = std::filesystem::path(dataset) / id;
+ const std::filesystem::path absPathToId = this->getFullPath(idPath);
+
+ if (this->check(dataset, id))
+ {
+ return PKDatasetFinderInfo{getPackageName(),
+ getAbsolutePackagePath(),
+ getRelativePath() / dataset,
+ id,
+ dataset,
+ id};
+ }
+ return std::nullopt;
+ }
+
+ std::optional<PKDatasetFinderInfo>
+ PKDatasetFinder::find(const std::string& id) const
+ {
+ const std::filesystem::path absPath = Base::Base::getFullPath();
+ if (std::filesystem::is_regular_file(absPath))
+ {
+ ARMARX_WARNING << "The entered path is leading to a file!";
+ return std::nullopt;
+ }
+
+ std::vector<PKDatasetFinderInfo> ret;
+ for (const auto& d : std::filesystem::directory_iterator(absPath))
+ {
+ if (!d.is_directory())
+ {
+ ARMARX_WARNING << "Found invalid path: " << d.path();
+ continue;
+ }
+ std::string k = d.path().filename();
+ if (simox::alg::contains(DATASET_FOLDERS_BLACKLIST, k))
+ {
+ continue;
+ }
+
+ if (auto op = this->find(k, id); op.has_value())
+ {
+ return op;
+ }
+ }
+ return std::nullopt;
+ }
+
+ std::vector<PKDatasetFinderInfo>
+ PKDatasetFinder::findAll() const
+ {
+ const std::filesystem::path absPath = Base::Base::getFullPath();
+ if (std::filesystem::is_regular_file(absPath))
+ {
+ ARMARX_WARNING << "The entered path is leading to a file!";
+ return {};
+ }
+
+ std::vector<PKDatasetFinderInfo> ret;
+ for (const auto& d : std::filesystem::directory_iterator(absPath))
+ {
+ if (!d.is_directory())
+ {
+ ARMARX_WARNING << "Found invalid path: " << d.path();
+ continue;
+ }
+ std::string k = d.path().filename();
+ if (simox::alg::contains(DATASET_FOLDERS_BLACKLIST, k))
+ {
+ continue;
+ }
+
+ auto idsForDataset = this->findAll(k);
+ simox::alg::append(ret, idsForDataset);
+ }
+ return ret;
+ }
+
+ std::vector<PKDatasetFinderInfo>
+ PKDatasetFinder::findAll(const std::string& dataset) const
+ {
+ const std::filesystem::path absPathToDataset = this->getFullPath(dataset);
+ if (std::filesystem::is_regular_file(absPathToDataset))
+ {
+ ARMARX_WARNING << "The entered path is leading to a file!";
+ return {};
+ }
+
+ std::vector<PKDatasetFinderInfo> ret;
+ for (const auto& d : std::filesystem::directory_iterator(absPathToDataset))
+ {
+ if (!d.is_directory())
+ {
+ ARMARX_WARNING << "Found invalid path: " << d.path();
+ continue;
+ }
+ std::string k = d.path().filename();
+ if (simox::alg::contains(ID_BLACKLIST, k))
+ {
+ continue;
+ }
+
+ if (auto op = this->find(dataset, k); op.has_value())
+ {
+ ret.emplace_back(op.value());
+ }
+ }
+ return ret;
+ }
+
+} // namespace armarx::priorknowledge::core
diff --git a/source/RobotAPI/libraries/PriorKnowledge/core/PKDatasetFinder.h b/source/RobotAPI/libraries/PriorKnowledge/core/PKDatasetFinder.h
new file mode 100644
index 0000000000000000000000000000000000000000..554a45e70e47192988c81ac5204e9eba321c798a
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/core/PKDatasetFinder.h
@@ -0,0 +1,66 @@
+#pragma once
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/PriorKnowledge/core/FinderBase.h>
+#include <RobotAPI/libraries/PriorKnowledge/core/FinderInfoBase.h>
+
+namespace armarx::priorknowledge::core
+{
+ /**
+ * @brief The PKDatasetFinderInfo class
+ * Specialization of the DatasetFinderInfo with strings as dataset and id types
+ */
+ class PKDatasetFinderInfo : public core::DatasetFinderInfoBase<std::string, std::string>
+ {
+ using Base = core::DatasetFinderInfoBase<std::string, std::string>;
+
+ public:
+ using Base::Base;
+ };
+
+ /**
+ * @brief The PKDatasetFinder class
+ */
+ class PKDatasetFinder :
+ public core::DatasetFinderBase<std::string, std::string, PKDatasetFinderInfo>
+ {
+ using Base = core::DatasetFinderBase<std::string, std::string, PKDatasetFinderInfo>;
+
+ public:
+ static const constexpr auto DEFAULT_PACKAGE = "PriorKnowledgeData";
+
+ using Base::Base;
+
+ PKDatasetFinder(const std::filesystem::path& relDir) : Base(DEFAULT_PACKAGE, relDir)
+ {
+ }
+
+ virtual ~PKDatasetFinder()
+ {
+ }
+
+ virtual void init();
+ virtual void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& defs,
+ const std::string& prefix);
+
+ bool checkAll(const std::string& dataset) const override;
+ bool checkAll() const override;
+
+ bool check(const std::string& id) const override;
+ bool check(const std::string& dataset, const std::string& id) const override;
+
+ std::optional<PKDatasetFinderInfo> find(const std::string& dataset,
+ const std::string& id) const override;
+ std::optional<PKDatasetFinderInfo> find(const std::string& id) const override;
+
+ std::vector<PKDatasetFinderInfo> findAll() const override;
+
+ std::vector<PKDatasetFinderInfo> findAll(const std::string& dataset) const override;
+
+ private:
+ std::vector<std::string> DATASET_FOLDERS_BLACKLIST = {};
+ std::vector<std::string> ID_BLACKLIST = {"script"};
+ };
+
+} // namespace armarx::priorknowledge::core
diff --git a/source/RobotAPI/libraries/PriorKnowledge/core/PKFinder.cpp b/source/RobotAPI/libraries/PriorKnowledge/core/PKFinder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b4e67fc608068464ac22bf5ba0f23a143437fdb5
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/core/PKFinder.cpp
@@ -0,0 +1,109 @@
+// Simox
+#include <SimoxUtility/algorithm/vector.hpp>
+
+// BaseClass
+#include "PKFinder.h"
+
+// ArmarX
+
+namespace armarx::priorknowledge::core
+{
+ void
+ PKFinder::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& defs,
+ const std::string& prefix)
+ {
+ if (not defs->hasDefinition(prefix + "packageName"))
+ {
+ defs->optional(this->packageName,
+ prefix + "packageName",
+ "The name of the prior knowledge data package.");
+ }
+ }
+
+ void
+ PKFinder::init()
+ {
+ this->recalculateBasePath();
+ }
+
+ bool
+ PKFinder::checkAll() const
+ {
+ const std::filesystem::path absPath = Base::getFullPath();
+ if (std::filesystem::is_regular_file(absPath))
+ {
+ ARMARX_WARNING << "The entered path is leading to a file!";
+ return false;
+ }
+
+ for (const auto& d : std::filesystem::directory_iterator(absPath))
+ {
+ std::string k = d.path().filename();
+ if (simox::alg::contains(ID_BLACKLIST, k))
+ {
+ continue;
+ }
+
+ if (not this->check(d.path()))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool
+ PKFinder::check(const std::string& id) const
+ {
+ if (simox::alg::contains(ID_BLACKLIST, id))
+ {
+ return false;
+ }
+
+ const std::filesystem::path absPath = Base::getFullPath() / id;
+
+ return this->accept(absPath);
+ }
+
+ std::optional<PKFinderInfo>
+ PKFinder::find(const std::string& id) const
+ {
+ const std::filesystem::path absPath = Base::getFullPath() / id;
+
+ if (this->check(absPath))
+ {
+ return PKFinderInfo{
+ getPackageName(), getAbsolutePackagePath(), getRelativePath() / id, id};
+ }
+ return std::nullopt;
+ }
+
+ std::vector<PKFinderInfo>
+ PKFinder::findAll() const
+ {
+ const std::filesystem::path absPath = Base::getFullPath();
+ if (std::filesystem::is_regular_file(absPath))
+ {
+ ARMARX_WARNING << "The entered path is leading to a file!";
+ return {};
+ }
+
+ std::vector<PKFinderInfo> ret;
+ for (const auto& d : std::filesystem::directory_iterator(absPath))
+ {
+ std::string k = d.path().filename();
+ if (simox::alg::contains(ID_BLACKLIST, k))
+ {
+ continue;
+ }
+
+ auto el = this->find(k);
+ if (el.has_value())
+ {
+ ret.push_back(*el);
+ }
+ }
+ return ret;
+ }
+
+} // namespace armarx::priorknowledge::core
diff --git a/source/RobotAPI/libraries/PriorKnowledge/core/PKFinder.h b/source/RobotAPI/libraries/PriorKnowledge/core/PKFinder.h
new file mode 100644
index 0000000000000000000000000000000000000000..abb71dbb7f36a18a0007be221f59ee222bcac921
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/core/PKFinder.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/PriorKnowledge/core/FinderBase.h>
+#include <RobotAPI/libraries/PriorKnowledge/core/FinderInfoBase.h>
+
+namespace armarx::priorknowledge::core
+{
+ /**
+ * @brief The ObjectFinderInfo class
+ * Specialization of the DatasetFinderInfo with strings as dataset and id types
+ */
+ class PKFinderInfo : public core::FinderInfoBase<std::string>
+ {
+ using Base = core::FinderInfoBase<std::string>;
+
+ public:
+ using Base::Base;
+ };
+
+ /**
+ * @brief The ObjectFinder class
+ */
+ class PKFinder : public core::FinderBase<std::string, PKFinderInfo>
+ {
+ using Base = core::FinderBase<std::string, PKFinderInfo>;
+
+ public:
+ static const constexpr auto DEFAULT_PACKAGE = "PriorKnowledgeData";
+
+ using Base::Base;
+
+ PKFinder(const std::filesystem::path& relDir) : Base(DEFAULT_PACKAGE, relDir)
+ {
+ }
+
+ virtual ~PKFinder()
+ {
+ }
+
+ virtual void init();
+ virtual void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& defs,
+ const std::string& prefix);
+
+ bool checkAll() const override;
+
+ bool check(const std::string& id) const override;
+
+ std::optional<PKFinderInfo> find(const std::string& id) const override;
+
+ std::vector<PKFinderInfo> findAll() const override;
+
+ private:
+ std::vector<std::string> ID_BLACKLIST = {"script"};
+ };
+
+} // namespace armarx::priorknowledge::core
diff --git a/source/RobotAPI/libraries/PriorKnowledge/motions/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/motions/CMakeLists.txt
index 7f4b2c007e00ea4a575056598ec7f6f25ab3fe8c..717e8f7f50ab20d117d967a7ce39717f73ad2569 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/motions/CMakeLists.txt
+++ b/source/RobotAPI/libraries/PriorKnowledge/motions/CMakeLists.txt
@@ -6,6 +6,7 @@ armarx_set_target("Library: ${LIB_NAME}")
armarx_add_library(
LIBS
RobotAPI::Core
+ RobotAPI::PriorKnowledge::Core
RobotAPI::Aron::Common
SOURCES
MotionFinder.cpp
diff --git a/source/RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.cpp b/source/RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.cpp
index 8fa71a62b6bcbd49141eb355ab45f124a8943808..f2f278d903706f248a66c413d13e65216a35e038 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.cpp
@@ -8,126 +8,5 @@
namespace armarx::priorknowledge::motions
{
- bool MotionFinder::checkAll() const
- {
- return true;
- }
-
- bool MotionFinder::check(const std::string &id) const
- {
- return true;
- }
-
- std::optional<MotionFinderInfo> MotionFinder::find(const std::string &id) const
- {
- const std::filesystem::path absPath = Base::Base::getFullPath();
- if (std::filesystem::is_regular_file(absPath))
- {
- ARMARX_WARNING << "The entered path is leading to a file!";
- return std::nullopt;
- }
-
- for (const auto& d : std::filesystem::directory_iterator(absPath))
- {
- if (!d.is_directory())
- {
- ARMARX_WARNING << "Found invalid path: " << d.path();
- continue;
- }
- std::string k = d.path().filename();
- if (simox::alg::contains(DATASET_FOLDERS_BLACKLIST, k))
- {
- continue;
- }
-
- if (const auto op = this->find(k, id); op.has_value())
- {
- return op;
- }
- }
- return std::nullopt;
- }
-
- std::vector<MotionFinderInfo> MotionFinder::findAll() const
- {
- const std::filesystem::path absPath = Base::Base::getFullPath();
- if (std::filesystem::is_regular_file(absPath))
- {
- ARMARX_WARNING << "The entered path is leading to a file!";
- return {};
- }
-
- std::vector<MotionFinderInfo> ret;
- for (const auto& d : std::filesystem::directory_iterator(absPath))
- {
- if (!d.is_directory())
- {
- ARMARX_WARNING << "Found invalid path: " << d.path();
- continue;
- }
- std::string k = d.path().filename();
- if (simox::alg::contains(DATASET_FOLDERS_BLACKLIST, k))
- {
- continue;
- }
-
- auto motionsForDataset = this->findAll(k);
- simox::alg::append(ret, motionsForDataset);
- }
- return ret;
- }
-
- bool MotionFinder::checkAll(const std::string &dataset) const
- {
- return true;
- }
-
- bool MotionFinder::check(const std::string &dataset, const std::string &id) const
- {
- return true;
- }
-
- std::optional<MotionFinderInfo> MotionFinder::find(const std::string &dataset, const std::string &id) const
- {
- const std::filesystem::path absPathToMotion = this->getFullPath(dataset) / id;
- if (std::filesystem::is_regular_file(absPathToMotion))
- {
- ARMARX_WARNING << "The entered path is leading to a file!";
- return std::nullopt;
- }
-
- return std::make_optional(MotionFinderInfo(this->getPackageName(), this->getAbsolutePackagePath(), this->getRelativePath(), dataset, dataset, id));
- }
-
- std::vector<MotionFinderInfo> MotionFinder::findAll(const std::string &dataset) const
- {
- const std::filesystem::path absPathToDataset = this->getFullPath(dataset);
- if (std::filesystem::is_regular_file(absPathToDataset))
- {
- ARMARX_WARNING << "The entered path is leading to a file!";
- return {};
- }
-
- std::vector<MotionFinderInfo> ret;
- for (const auto& d : std::filesystem::directory_iterator(absPathToDataset))
- {
- if (!d.is_directory())
- {
- ARMARX_WARNING << "Found invalid path: " << d.path();
- continue;
- }
- std::string k = d.path().filename();
- if (simox::alg::contains(MOTION_ID_FOLDERS_BLACKLIST, k))
- {
- continue;
- }
-
- if(auto op = this->find(dataset, k); op.has_value())
- {
- ret.emplace_back(op.value());
- }
- }
- return ret;
- }
}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h b/source/RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h
index 9315ab34581341bdd34f404643f1ddda55c3aa62..54500781f138c765bebf8b987a93f05ef759e580 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h
@@ -1,47 +1,33 @@
-#include <RobotAPI/libraries/PriorKnowledge/core/FinderBase.h>
-#include <RobotAPI/libraries/PriorKnowledge/core/FinderInfoBase.h>
+#pragma once
+#include <RobotAPI/libraries/PriorKnowledge/core/PKDatasetFinder.h>
namespace armarx::priorknowledge::motions
{
- class MotionFinderInfo : public core::DatasetFinderInfoBase<std::string, std::string>
+ class MotionFinder : public core::PKDatasetFinder
{
- using Base = core::DatasetFinderInfoBase<std::string, std::string>;
public:
- MotionFinderInfo(const std::string& packageName,
- const std::filesystem::path& absPackageDataDir,
- const std::filesystem::path& relPackageDataPath,
- const std::filesystem::path& relDatasetPath,
- const std::string& dataset,
- const std::string& id) :
- Base(packageName, absPackageDataDir, relPackageDataPath, relDatasetPath, dataset, id)
+ using core::PKDatasetFinder::PKDatasetFinder;
+ static const constexpr auto DEFAULT_DIR_TO_DATASETS = "motions";
+
+ MotionFinder(const std::filesystem::path& relDir = DEFAULT_DIR_TO_DATASETS) :
+ core::PKDatasetFinder(relDir)
{
}
- };
-
- class MotionFinder : public core::DatasetFinderBase<std::string, std::string, MotionFinderInfo>
- {
- using Base = core::DatasetFinderBase<std::string, std::string, MotionFinderInfo>;
-
- public:
- MotionFinder(const std::string& packageName, const std::filesystem::path& relDir) :
- Base(packageName, relDir)
- {}
- bool checkAll() const;
- bool check(const std::string& id) const;
- std::optional<MotionFinderInfo> find(const std::string& id) const;
- std::vector<MotionFinderInfo> findAll() const;
-
- bool checkAll(const std::string& dataset) const;
- bool check(const std::string& dataset, const std::string& id) const;
- std::optional<MotionFinderInfo> find(const std::string& dataset, const std::string& id) const;
- std::vector<MotionFinderInfo> findAll(const std::string& dataset) const;
+ bool
+ accept(const std::filesystem::path& idPath) const final
+ {
+ return std::filesystem::is_directory(idPath);
+ }
- private:
- std::vector<std::string> DATASET_FOLDERS_BLACKLIST = {};
- std::vector<std::string> MOTION_ID_FOLDERS_BLACKLIST = {"script"};
+ void
+ registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& defs,
+ const std::string& prefix = "pk.finder.motions.") final
+ {
+ core::PKDatasetFinder::registerPropertyDefinitions(defs, prefix);
+ }
};
-}
+} // namespace armarx::priorknowledge::motions
diff --git a/source/RobotAPI/libraries/PriorKnowledge/navigation_graphs/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/navigation_graphs/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cd7bb753f13e5a1718d38cb4c6090b329a5fe767
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/navigation_graphs/CMakeLists.txt
@@ -0,0 +1,20 @@
+set(LIB_NAME ${PROJECT_NAME}PriorKnowledgeNavigationGraphs)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ RobotAPI::Core
+ RobotAPI::PriorKnowledge::Core
+ RobotAPI::Aron::Common
+ SOURCES
+ NavigationGraphFinder.cpp
+ HEADERS
+ NavigationGraphFinder.h
+)
+
+add_library(${PROJECT_NAME}::PriorKnowledge::NavigationGraphs ALIAS ${PROJECT_NAME}PriorKnowledgeNavigationGraphs)
+
+# add unit tests
+#add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/navigation_graphs/NavigationGraphFinder.cpp b/source/RobotAPI/libraries/PriorKnowledge/navigation_graphs/NavigationGraphFinder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0c4026324b9e085b1a80323684debcd230e8fba
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/navigation_graphs/NavigationGraphFinder.cpp
@@ -0,0 +1,12 @@
+// Simox
+#include <SimoxUtility/algorithm/vector.hpp>
+
+// BaseClass
+#include "NavigationGraphFinder.h"
+
+// ArmarX
+
+namespace armarx::priorknowledge::navigation_graphs
+{
+
+} // namespace armarx::priorknowledge::navigation_graphs
diff --git a/source/RobotAPI/libraries/PriorKnowledge/navigation_graphs/NavigationGraphFinder.h b/source/RobotAPI/libraries/PriorKnowledge/navigation_graphs/NavigationGraphFinder.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb2ce50bf37feab3b00d2d70e83ca3e516c0f5c2
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/navigation_graphs/NavigationGraphFinder.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <RobotAPI/libraries/PriorKnowledge/core/PKFinder.h>
+
+namespace armarx::priorknowledge::navigation_graphs
+{
+ class NavigationGraphFinder : public core::PKFinder
+ {
+
+ public:
+ using core::PKFinder::PKFinder;
+ static const constexpr auto DEFAULT_DIR_TO_IDS = "navigation-graphs";
+
+ NavigationGraphFinder(const std::filesystem::path& relDir = DEFAULT_DIR_TO_IDS) :
+ core::PKFinder(relDir)
+ {
+ }
+
+ bool
+ accept(const std::filesystem::path& idPath) const final
+ {
+ return std::filesystem::is_directory(idPath);
+ }
+
+ void
+ registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& defs,
+ const std::string& prefix = "pk.finder.navigation.") final
+ {
+ core::PKFinder::registerPropertyDefinitions(defs, prefix);
+ }
+ };
+
+} // namespace armarx::priorknowledge::navigation_graphs
diff --git a/source/RobotAPI/libraries/PriorKnowledge/objects/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/objects/CMakeLists.txt
index cdf52fa7d1aa8dff4eda392dcb0cebdd77ec8f00..91bd3e9e8f279da786c2c4c35ae4748cd048d13f 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/PriorKnowledge/objects/CMakeLists.txt
@@ -6,6 +6,7 @@ armarx_set_target("Library: ${LIB_NAME}")
armarx_add_library(
LIBS
RobotAPI::Core
+ RobotAPI::PriorKnowledge::Core
RobotAPI::Aron::Common
SOURCES
ObjectFinder.cpp
diff --git a/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.cpp b/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.cpp
index ed1b6768e170f9416db52ba6f27fe44ac18b8adc..0a8004e6f5cea06ad30d9f06754524aec55a3739 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.cpp
@@ -8,64 +8,5 @@
namespace armarx::priorknowledge::objects
{
- std::vector<ObjectFinderInfo> ObjectFinder::findAll() const
- {
- const std::filesystem::path absPath = Base::Base::getFullPath();
- if (std::filesystem::is_regular_file(absPath))
- {
- ARMARX_WARNING << "The entered path is leading to a file!";
- return {};
- }
- std::vector<ObjectFinderInfo> ret;
- for (const auto& d : std::filesystem::directory_iterator(absPath))
- {
- if (!d.is_directory())
- {
- ARMARX_WARNING << "Found invalid path: " << d.path();
- continue;
- }
- std::string k = d.path().filename();
- if (simox::alg::contains(DATASET_FOLDERS_BLACKLIST, k))
- {
- continue;
- }
-
- auto motionsForDataset = this->findAll(k);
- simox::alg::append(ret, motionsForDataset);
- }
- return ret;
- }
-
- std::vector<ObjectFinderInfo> ObjectFinder::findAll(const std::string &dataset) const
- {
- const std::filesystem::path absPathToDataset = this->getFullPath(dataset);
- if (std::filesystem::is_regular_file(absPathToDataset))
- {
- ARMARX_WARNING << "The entered path is leading to a file!";
- return {};
- }
-
- std::vector<ObjectFinderInfo> ret;
- for (const auto& d : std::filesystem::directory_iterator(absPathToDataset))
- {
- if (!d.is_directory())
- {
- ARMARX_WARNING << "Found invalid path: " << d.path();
- continue;
- }
- std::string k = d.path().filename();
- if (simox::alg::contains(ID_FOLDERS_BLACKLIST, k))
- {
- continue;
- }
-
- if(auto op = this->find(dataset, k); op.has_value())
- {
- ret.emplace_back(op.value());
- }
- }
- return ret;
- }
-
-}
+} // namespace armarx::priorknowledge::objects
diff --git a/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.h b/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.h
index a0e5b1820af5a9cafa561273aafabcad099820b3..9afd034e4f6076a5e9df3d5258d4dca70806d82b 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/objects/ObjectFinder.h
@@ -1,41 +1,36 @@
-#include <RobotAPI/libraries/PriorKnowledge/core/FinderBase.h>
-#include <RobotAPI/libraries/PriorKnowledge/core/FinderInfoBase.h>
+#pragma once
+
+#include <RobotAPI/libraries/PriorKnowledge/core/PKDatasetFinder.h>
-// TODO!!!
namespace armarx::priorknowledge::objects
{
- class ObjectFinderInfo : public core::DatasetFinderInfoBase<std::string, std::string>
+ /**
+ * @brief The ObjectFinder class
+ * TODO (fabian.peller): Merge with ArmarXObjects object finder!
+ */
+ class ObjectFinder : public core::PKDatasetFinder
{
- using Base = core::DatasetFinderInfoBase<std::string, std::string>;
-
public:
- ObjectFinderInfo(const std::string& packageName,
- const std::filesystem::path& absPackageDataDir,
- const std::filesystem::path& relPackageDataPath,
- const std::filesystem::path& relDatasetPath,
- const std::string& dataset,
- const std::string& id) :
- Base(packageName, absPackageDataDir, relPackageDataPath, relDatasetPath, dataset, id)
+ using core::PKDatasetFinder::PKDatasetFinder;
+ static const constexpr auto DEFAULT_DIR_TO_DATASETS = "objects";
+
+ ObjectFinder(const std::filesystem::path& relDir = DEFAULT_DIR_TO_DATASETS) :
+ core::PKDatasetFinder(relDir)
{
}
- };
- class ObjectFinder : public core::DatasetFinderBase<std::string, std::string, ObjectFinderInfo>
- {
- using Base = core::DatasetFinderBase<std::string, std::string, ObjectFinderInfo>;
-
- public:
- ObjectFinder(const std::string& packageName, const std::filesystem::path& relDir) :
- Base(packageName, relDir)
- {}
-
- std::vector<ObjectFinderInfo> findAll() const;
-
- std::vector<ObjectFinderInfo> findAll(const std::string& dataset) const;
+ bool
+ accept(const std::filesystem::path& idPath) const final
+ {
+ return std::filesystem::is_directory(idPath);
+ }
- private:
- std::vector<std::string> DATASET_FOLDERS_BLACKLIST = {};
- std::vector<std::string> ID_FOLDERS_BLACKLIST = {"script"};
+ void
+ registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& defs,
+ const std::string& prefix = "pk.finder.objects.") final
+ {
+ core::PKDatasetFinder::registerPropertyDefinitions(defs, prefix);
+ }
};
-}
+} // namespace armarx::priorknowledge::objects
diff --git a/source/RobotAPI/libraries/PriorKnowledge/robots/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/robots/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5e2439dadfa73af8c5f5d575387755df5dd37b99
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/robots/CMakeLists.txt
@@ -0,0 +1,20 @@
+set(LIB_NAME ${PROJECT_NAME}PriorKnowledgeRobots)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ RobotAPI::Core
+ RobotAPI::PriorKnowledge::Core
+ RobotAPI::Aron::Common
+ SOURCES
+ RobotFinder.cpp
+ HEADERS
+ RobotFinder.h
+)
+
+add_library(${PROJECT_NAME}::PriorKnowledge::Robots ALIAS ${PROJECT_NAME}PriorKnowledgeRobots)
+
+# add unit tests
+#add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/robots/RobotFinder.cpp b/source/RobotAPI/libraries/PriorKnowledge/robots/RobotFinder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6123bd7c16209594bc18b1ba3c653d68b7b74ac8
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/robots/RobotFinder.cpp
@@ -0,0 +1,12 @@
+// Simox
+#include <SimoxUtility/algorithm/vector.hpp>
+
+// BaseClass
+#include "RobotFinder.h"
+
+// ArmarX
+
+namespace armarx::priorknowledge::robots
+{
+
+} // namespace armarx::priorknowledge::robots
diff --git a/source/RobotAPI/libraries/PriorKnowledge/robots/RobotFinder.h b/source/RobotAPI/libraries/PriorKnowledge/robots/RobotFinder.h
new file mode 100644
index 0000000000000000000000000000000000000000..0570e1ece57d9844789131c9aecd7a21113cfb67
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/robots/RobotFinder.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <RobotAPI/libraries/PriorKnowledge/core/PKFinder.h>
+
+namespace armarx::priorknowledge::robots
+{
+ class RobotFinder : public core::PKFinder
+ {
+
+ public:
+ using core::PKFinder::PKFinder;
+ static const constexpr auto DEFAULT_DIR_TO_IDS = "robots";
+
+ RobotFinder(const std::filesystem::path& relDir = DEFAULT_DIR_TO_IDS) :
+ core::PKFinder(relDir)
+ {
+ }
+
+ bool
+ accept(const std::filesystem::path& idPath) const final
+ {
+ return std::filesystem::is_directory(idPath);
+ }
+
+ void
+ registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& defs,
+ const std::string& prefix = "pk.finder.robots.") final
+ {
+ core::PKFinder::registerPropertyDefinitions(defs, prefix);
+ }
+ };
+
+} // namespace armarx::priorknowledge::robots
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2e9db269c52a2e418e609ed0e0f79f95734f50bf
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp
@@ -0,0 +1,49 @@
+#include "AffordanceLoader.h"
+
+#include <ArmarXCore/core/eigen/ice_conversions.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+namespace armarx::priorknowledge::util
+{
+ std::vector<StaticAffordance>
+ AffordanceLoader::LoadAffordances(const std::string& source, const nlohmann::json& js)
+ {
+ std::vector<StaticAffordance> ret;
+ if (not js.contains("affordances"))
+ {
+ ARMARX_WARNING << "The affordances file has the wrong structure. Missing key "
+ "'affordances'.";
+ return ret;
+ }
+
+ for (const auto& affordance : js["affordances"].get<std::vector<std::string>>())
+ {
+ StaticAffordance a{{source, affordance}};
+ ret.push_back(a);
+ }
+ return ret;
+ }
+
+ std::vector<LocationAffordance>
+ AffordanceLoader::LoadLocationAffordances(const std::string& source, const nlohmann::json& js)
+ {
+ std::vector<LocationAffordance> ret;
+ if (not js.contains("location_affordances"))
+ {
+ ARMARX_WARNING << "The affordances file has the wrong structure. Missing key "
+ "'location_affordances'.";
+ return ret;
+ }
+
+ for (const auto& [locationName, affordances] :
+ js["location_affordances"].get<std::map<std::string, std::vector<std::string>>>())
+ {
+ for (const auto& affordance : affordances)
+ {
+ LocationAffordance a{{source, affordance}, locationName};
+ ret.push_back(a);
+ }
+ }
+ return ret;
+ }
+} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
new file mode 100644
index 0000000000000000000000000000000000000000..b8f0b9f6e0cf7a4ec7d64eab6a650c7afb90ad1b
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <fstream>
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/json.h>
+
+#include "datatypes/Affordance.h"
+
+namespace armarx::priorknowledge::util
+{
+ class AffordanceLoader
+ {
+ 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> LoadAffordances(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/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7902de1d825ac8fc5d1a7a2e402ccc4ceb521b3a
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/CMakeLists.txt
@@ -0,0 +1,22 @@
+set(LIB_NAME ${PROJECT_NAME}PriorKnowledgeAffordanceLoaderUtil)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ SimoxUtility
+ RobotAPI::Core
+ RobotAPI::Aron::Common
+ SOURCES
+ datatypes/Affordance.cpp
+ AffordanceLoader.cpp
+ HEADERS
+ datatypes/Affordance.h
+ AffordanceLoader.h
+)
+
+add_library(${PROJECT_NAME}::PriorKnowledge::util::AffordanceLoader ALIAS ${PROJECT_NAME}PriorKnowledgeAffordanceLoaderUtil)
+
+# add unit tests
+#add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f28e60c80c3932c083feb25db5b0f93b40266148
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp
@@ -0,0 +1,15 @@
+#include "Affordance.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+namespace armarx::priorknowledge::util
+{
+ std::string
+ AffordanceId::toString() const
+ {
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..39eaedc0c86c1fe7a8f3993264c2bd9987ca1601
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <string>
+
+namespace armarx::priorknowledge::util
+{
+ struct AffordanceId
+ {
+ std::string source;
+ std::string name;
+
+ std::string toString() const;
+ };
+
+ struct StaticAffordance
+ {
+ AffordanceId id;
+ };
+
+ struct LocationAffordance
+ {
+ AffordanceId id;
+ 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/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..eeadbad241752270cdf1b67eb8ab91bce237ff39
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(LIB_NAME ${PROJECT_NAME}PriorKnowledgeLocationLoaderUtil)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ SimoxUtility
+ RobotAPI::Core
+ RobotAPI::Aron::Common
+ ArViz
+ SOURCES
+ datatypes/Location.cpp
+ LocationLoader.cpp
+ Visu.cpp
+ HEADERS
+ datatypes/Location.h
+ LocationLoader.h
+ Visu.h
+)
+
+add_library(${PROJECT_NAME}::PriorKnowledge::util::LocationLoader ALIAS ${PROJECT_NAME}PriorKnowledgeLocationLoaderUtil)
+
+# add unit tests
+#add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cfeb43db57231ff85188667c60773c6d0e30390c
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
@@ -0,0 +1,157 @@
+#include "LocationLoader.h"
+
+#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
+{
+ FramedLocationPtr
+ LocationLoader::LoadFramedLocation(const std::string& source,
+ const std::string& locationName,
+ const nlohmann::json& j)
+ {
+ ARMARX_CHECK(j.find("framedPose") != j.end());
+
+ 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 (agent.empty())
+ {
+ 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?
+ }
+
+ // Utilize ice structure of eigen
+ armarx::core::eigen::fromIce(
+ pose,
+ framedPose.at("pose").get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
+
+ FramedLocationPtr loc(new FramedLocation(
+ LocationId(source, locationName), LocationType::FRAMED_LOCATION, frame, agent, pose));
+ return loc;
+ }
+
+ 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())
+ {
+ 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?
+ }
+
+ // 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(
+ extents,
+ framedOrientedBox.at("extents").get<std::vector<float>>()); // load the 4x4 matrix
+
+ FramedBoxedLocationPtr loc(new FramedBoxedLocation(LocationId(source, locationName),
+ LocationType::FRAMED_BOXED_LOCATION,
+ frame,
+ agent,
+ pose,
+ extents));
+ return loc;
+ }
+
+ 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;
+ }
+
+ 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;
+ }
+} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h
new file mode 100644
index 0000000000000000000000000000000000000000..b480f98922f78127aa395a34439ec13f8fb2279d
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <fstream>
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/json.h>
+
+#include <RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h>
+
+namespace armarx::priorknowledge::util
+{
+ class LocationLoader
+ {
+ public:
+ static const constexpr auto DEFAULT_FILE_NAME = "locations.json";
+
+ LocationLoader() = delete;
+
+ // 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
new file mode 100644
index 0000000000000000000000000000000000000000..80dfd636de50f94003f673cb47114c86afdcb844
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
@@ -0,0 +1,84 @@
+#include "Visu.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+namespace armarx::priorknowledge::util::location
+{
+ 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
+ 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, 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
+ 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;
+ }
+} // namespace armarx::priorknowledge::util::location
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..a237c48b3ee1ae78bce651ab887f37c3aa2569df
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
@@ -0,0 +1,64 @@
+#pragma once
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
+
+#include "datatypes/Location.h"
+
+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;
+
+ viz::Layer
+ locationsToLayer(const std::string& layerName,
+ 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
+ {
+ // FramedLocation
+ viz::Color framedLocationArrowColor = viz::Color::green();
+
+ // FramedBoxedLocation
+ viz::Color framedBoxedLocationColor = viz::Color(0, 150, 130, 40);
+
+ } settings;
+
+ protected:
+ viz::Client& arviz;
+ };
+} // namespace armarx::priorknowledge::util::location
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..56617061dfc387e3bc74d6247134a00dfae799fa
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
@@ -0,0 +1,15 @@
+#include "Location.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+namespace armarx::priorknowledge::util
+{
+ std::string
+ LocationId::toString() const
+ {
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..2a1eccb4cbe72f30ac0da1582c294eb6508ef0f2
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
@@ -0,0 +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 source;
+ std::string name;
+
+ LocationId(const std::string& s, const std::string& n) : source(s), name(n)
+ {
+ }
+
+ std::string toString() const;
+ };
+
+ struct Location
+ {
+ LocationId id;
+ 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/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
index 7c63d33b767f10ace65279b9be98e9426e5d8644..0bc460c8bc27fea0f3b2dcdfe9ba46921680291a 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.cpp
@@ -2,7 +2,6 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx::plugins
{
static const std::string ARVIZ_TOPIC_PROPERTY_NAME = "ArVizTopicName";
@@ -11,28 +10,31 @@ namespace armarx::plugins
static const std::string ARVIZ_STORAGE_PROPERTY_NAME = "ArVizStorageName";
static const std::string ARVIZ_STORAGE_PROPERTY_DEFAULT = "ArVizStorage";
-
- std::string ArVizComponentPlugin::getTopicName()
+ std::string
+ ArVizComponentPlugin::getTopicName()
{
- return parentDerives<Component>() ?
- parent<Component>().getProperty<std::string>(makePropertyName(ARVIZ_TOPIC_PROPERTY_NAME)) :
- ARVIZ_TOPIC_PROPERTY_DEFAULT;
+ return parentDerives<Component>() ? parent<Component>().getProperty<std::string>(
+ makePropertyName(ARVIZ_TOPIC_PROPERTY_NAME))
+ : ARVIZ_TOPIC_PROPERTY_DEFAULT;
}
- std::string ArVizComponentPlugin::getStorageName()
+ std::string
+ ArVizComponentPlugin::getStorageName()
{
- return parentDerives<Component>() ?
- parent<Component>().getProperty<std::string>(makePropertyName(ARVIZ_STORAGE_PROPERTY_NAME)) :
- ARVIZ_STORAGE_PROPERTY_DEFAULT;
+ return parentDerives<Component>() ? parent<Component>().getProperty<std::string>(
+ makePropertyName(ARVIZ_STORAGE_PROPERTY_NAME))
+ : ARVIZ_STORAGE_PROPERTY_DEFAULT;
}
- void ArVizComponentPlugin::preOnInitComponent()
+ void
+ ArVizComponentPlugin::preOnInitComponent()
{
parent().offeringTopic(getTopicName());
parent().usingProxy(getStorageName());
}
- void ArVizComponentPlugin::preOnConnectComponent()
+ void
+ ArVizComponentPlugin::preOnConnectComponent()
{
if (parentDerives<ArVizComponentPluginUser>())
{
@@ -40,7 +42,8 @@ namespace armarx::plugins
}
}
- void ArVizComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ ArVizComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(makePropertyName(ARVIZ_TOPIC_PROPERTY_NAME)))
{
@@ -58,11 +61,12 @@ namespace armarx::plugins
}
}
- viz::Client ArVizComponentPlugin::createClient()
+ viz::Client
+ ArVizComponentPlugin::createClient()
{
return viz::Client(parent(), getTopicName(), getStorageName());
}
-}
+} // namespace armarx::plugins
namespace armarx
{
@@ -71,8 +75,9 @@ namespace armarx
addPlugin(plugin);
}
- viz::Client ArVizComponentPluginUser::createArVizClient()
+ viz::Client
+ ArVizComponentPluginUser::createArVizClient()
{
return plugin->createClient();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
index 41b48bdff28d2cdbd2e601ab7e9d6dadcdbc0e2b..9c1d995dee640e413d178d900df769068f1746df 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -2,56 +2,115 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
namespace armarx::plugins
{
- void HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel,
- const RobotHealthHeartbeatArgs& args)
+
+ void
+ HeartbeatComponentPlugin::signUp(const std::string& identifier,
+ const std::vector<std::string>& tags,
+ const std::string& description)
+ {
+ RobotHealthHeartbeatArgs argsCopy = defaultHeartbeatArgs;
+ argsCopy.description = description;
+ argsCopy.identifier = identifier;
+ argsCopy.tags = tags;
+ signUp(argsCopy);
+ }
+
+ void
+ HeartbeatComponentPlugin::signUp(const std::string& identifier,
+ const armarx::core::time::Duration& warning,
+ const armarx::core::time::Duration& error,
+ const std::vector<std::string>& tags,
+ const std::string& description)
{
- channelHeartbeatConfig.emplace(channel, args);
+ RobotHealthHeartbeatArgs argsCopy = defaultHeartbeatArgs;
+ argsCopy.description = description;
+ argsCopy.tags = tags;
+ argsCopy.identifier = identifier;
+ toIce(argsCopy.maximumCycleTimeWarning, warning);
+ toIce(argsCopy.maximumCycleTimeError, error);
+
+ ARMARX_TRACE;
+ signUp(argsCopy);
}
- void HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel, const std::string& message)
+ void
+ HeartbeatComponentPlugin::signUp(const armarx::core::time::Duration& warning,
+ const armarx::core::time::Duration& error,
+ const std::vector<std::string>& tags,
+ const std::string& description)
{
- auto args = heartbeatArgs;
- args.message = message;
- configureHeartbeatChannel(channel, args);
+ RobotHealthHeartbeatArgs argsCopy = defaultHeartbeatArgs;
+ argsCopy.description = description;
+ argsCopy.tags = tags;
+ toIce(argsCopy.maximumCycleTimeWarning, warning);
+ toIce(argsCopy.maximumCycleTimeError, error);
+ // argsCopy.requiredByDefault = required;
+ signUp(argsCopy);
}
- void HeartbeatComponentPlugin::heartbeat()
+ void
+ HeartbeatComponentPlugin::signUp(const RobotHealthHeartbeatArgs& args)
{
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(robotHealthComponentPrx);
- if (robotHealthTopic)
+ if (args.identifier.empty())
{
- robotHealthTopic->heartbeat(componentName, heartbeatArgs);
- } else
+ RobotHealthHeartbeatArgs argsCopy = args;
+ argsCopy.identifier = parent().getName();
+ robotHealthComponentPrx->signUp(argsCopy);
+ }
+ else
{
- ARMARX_WARNING << "No robot health topic available!";
+ // add component name prefix to identifier
+ RobotHealthHeartbeatArgs argsCopy = args;
+ argsCopy.identifier = parent().getName() + "_" + argsCopy.identifier;
+ robotHealthComponentPrx->signUp(argsCopy);
}
}
- void HeartbeatComponentPlugin::heartbeat(const std::string& channel)
+ void
+ HeartbeatComponentPlugin::heartbeat()
{
- const auto argsIt = channelHeartbeatConfig.find(channel);
- ARMARX_CHECK(argsIt != channelHeartbeatConfig.end()) << "heartbeat() called for unknown channel '" << channel
- << "'."
- << "You must register the config using configureHeartbeatChannel(channel) first!";
-
- const auto& args = argsIt->second;
+ if (robotHealthComponentPrx)
+ {
+ armarx::core::time::dto::DateTime now;
+ armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+ robotHealthComponentPrx->heartbeat(parent().getName(), now);
+ }
+ else
+ {
+ ARMARX_WARNING << "No robot health proxy available!";
+ }
+ }
- if (robotHealthTopic)
+ void
+ HeartbeatComponentPlugin::heartbeatOnChannel(const std::string& channelName)
+ {
+ if (robotHealthComponentPrx)
{
- robotHealthTopic->heartbeat(componentName + "_" + channel, args);
- } else
+ armarx::core::time::dto::DateTime now;
+ armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+ robotHealthComponentPrx->heartbeat(parent().getName() + "_" + channelName, now);
+ }
+ else
{
- ARMARX_WARNING << "No robot health topic available!";
+ ARMARX_WARNING << "No robot health proxy available!";
}
}
- void HeartbeatComponentPlugin::preOnInitComponent()
+ void
+ HeartbeatComponentPlugin::preOnInitComponent()
{
+ // defaultHeartbeatArgs.requiredByDefault = true;
+
// if (topicName.empty())
// {
// parent<Component>().getProperty(topicName, makePropertyName(topicPropertyName));
@@ -59,33 +118,42 @@ namespace armarx::plugins
// parent<Component>().offeringTopic(topicName);
}
- void HeartbeatComponentPlugin::postOnInitComponent()
+ void
+ HeartbeatComponentPlugin::postOnInitComponent()
{
+ // set default args
+ auto warn = armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeWarningMS);
+ auto err = armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeErrorMS);
+ armarx::core::time::toIce(defaultHeartbeatArgs.maximumCycleTimeWarning, warn);
+ armarx::core::time::toIce(defaultHeartbeatArgs.maximumCycleTimeError, err);
}
- void HeartbeatComponentPlugin::preOnConnectComponent()
+ void
+ HeartbeatComponentPlugin::postOnConnectComponent()
{
- // robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName);
- componentName = parent<Component>().getName();
+ ARMARX_CHECK_NOT_NULL(robotHealthComponentPrx);
}
- void HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ void
+ HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
- if (!properties->hasDefinition(makePropertyName(topicPropertyName)))
+ if (!properties->hasDefinition(makePropertyName(healthPropertyName)))
{
- properties->topic(robotHealthTopic, topicName, topicPropertyName,
- "Name of the topic the DebugObserver listens on");
+ properties->component(
+ robotHealthComponentPrx, "RobotHealth", healthPropertyName, "Name of the robot health component.");
}
if (not properties->hasDefinition(makePropertyName(maximumCycleTimeWarningMSPropertyName)))
{
- properties->required(heartbeatArgs.maximumCycleTimeWarningMS, maximumCycleTimeWarningMSPropertyName,
+ properties->optional(p.maximumCycleTimeWarningMS,
+ maximumCycleTimeWarningMSPropertyName,
"maximum cycle time before warning is emitted");
}
if (not properties->hasDefinition(makePropertyName(maximumCycleTimeErrorMSPropertyName)))
{
- properties->required(heartbeatArgs.maximumCycleTimeErrorMS, maximumCycleTimeErrorMSPropertyName,
+ properties->optional(p.maximumCycleTimeErrorMS,
+ maximumCycleTimeErrorMSPropertyName,
"maximum cycle time before error is emitted");
}
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
index bbefa079c1d54e71444d9f0e90120f4db0250297..8a7f4a42ca0e8a603945535445a0f35abf3110c6 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
@@ -22,6 +22,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/time/Duration.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
@@ -34,21 +36,33 @@ namespace armarx::plugins
using ComponentPlugin::ComponentPlugin;
/**
- * @brief Configures a heartbeat subchannel.
- *
- * @param channel Identifier of the heartbeat channel
- * @param args Configuration of this channel's heartbeat properties
+ * @brief register component to heartbeat
*/
- void configureHeartbeatChannel(const std::string& channel, const std::string& message);
+ void signUp(const std::string& channelName = "",
+ const std::vector<std::string>& aliases = {},
+ const std::string& description = "");
/**
- * @brief Configures a heartbeat subchannel.
- *
- * @param channel Identifier of the heartbeat channel
- * @param args Configuration of this channel's heartbeat properties
+ * @brief register component to heartbeat
+ */
+ void signUp(const armarx::core::time::Duration& warning,
+ const armarx::core::time::Duration& error,
+ const std::vector<std::string>& aliases = {},
+ const std::string& description = "");
+
+ /**
+ * @brief register component to heartbeat
+ */
+ void signUp(const std::string& channelName,
+ const armarx::core::time::Duration& warning,
+ const armarx::core::time::Duration& error,
+ const std::vector<std::string>& aliases = {},
+ const std::string& description = "");
+
+ /**
+ * @brief register component to heartbeat, possibly with different component name
*/
- void configureHeartbeatChannel(const std::string& channel,
- const RobotHealthHeartbeatArgs& args);
+ void signUp(const RobotHealthHeartbeatArgs& args);
/**
* @brief Sends out a heartbeat using the default config
@@ -63,35 +77,33 @@ namespace armarx::plugins
*
* @param channel Identifier of the heartbeat channel
*/
- void heartbeat(const std::string& channel);
+ void heartbeatOnChannel(const std::string& channelName);
protected:
void preOnInitComponent() override;
void postOnInitComponent() override;
- void preOnConnectComponent() override;
+ // void preOnConnectComponent() override;
+ void postOnConnectComponent() override;
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
private:
- //! heartbeat topic name (outgoing)
- std::string topicName{"RobotHealthTopic"};
-
- //! name of this component used as identifier for heartbeats
- std::string componentName;
+ RobotHealthComponentInterfacePrx robotHealthComponentPrx;
//
- static constexpr auto topicPropertyName = "heartbeat.TopicName";
+ static constexpr auto healthPropertyName = "heartbeat.ComponentName";
static constexpr auto maximumCycleTimeWarningMSPropertyName =
"heartbeat.maximumCycleTimeWarningMS";
static constexpr auto maximumCycleTimeErrorMSPropertyName =
"heartbeat.maximumCycleTimeErrorMS";
- RobotHealthInterfacePrx robotHealthTopic;
+ struct Properties
+ {
+ long maximumCycleTimeWarningMS = 100; // [ms]
+ long maximumCycleTimeErrorMS = 200; // [ms]
+ } p;
//! default config used in heartbeat(), set via properties
- RobotHealthHeartbeatArgs heartbeatArgs;
-
- //! configs used in heartbeat(channel), set by user via configureHeartbeatChannel(...)
- std::unordered_map<std::string, RobotHealthHeartbeatArgs> channelHeartbeatConfig;
+ RobotHealthHeartbeatArgs defaultHeartbeatArgs;
};
} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 8222d062291e990872750187ff6dc1d943dc5079..345efd01519fd01e43f68c5dc7e2b583ebb64d08 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -1,23 +1,21 @@
#include "RobotUnitComponentPlugin.h"
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/util/CPPUtility/Pointer.h>
-
#include <thread>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/util/CPPUtility/Pointer.h>
namespace armarx::plugins
{
RobotUnitDataStreamingReceiverPtr
- RobotUnitComponentPlugin::startDataStreaming(
- const RobotUnitDataStreaming::Config& cfg)
+ RobotUnitComponentPlugin::startDataStreaming(const RobotUnitDataStreaming::Config& cfg)
{
//ok to create smart ptr from parent, since ice handels this
- return make_shared<RobotUnitDataStreamingReceiver>(
- &parent(), getRobotUnit(), cfg);
+ return make_shared<RobotUnitDataStreamingReceiver>(&parent(), getRobotUnit(), cfg);
}
- void RobotUnitComponentPlugin::postOnDisconnectComponent()
+ void
+ RobotUnitComponentPlugin::postOnDisconnectComponent()
{
if (!_ctrls.empty())
{
@@ -28,7 +26,8 @@ namespace armarx::plugins
_robotUnit = nullptr;
}
- NJointControllerInterfacePrx RobotUnitComponentPlugin::createNJointController(
+ NJointControllerInterfacePrx
+ RobotUnitComponentPlugin::createNJointController(
const std::string& className,
const std::string& instanceName,
const armarx::NJointControllerConfigPtr& config,
@@ -42,10 +41,10 @@ namespace armarx::plugins
{
out << "and managing ";
}
- out << " controller '" << instanceName
- << "' of class '" << instanceName << "'";
+ out << " controller '" << instanceName << "' of class '" << instanceName << "'";
};
- const auto prx = _robotUnit->createOrReplaceNJointController(className, instanceName, config);
+ const auto prx =
+ _robotUnit->createOrReplaceNJointController(className, instanceName, config);
ARMARX_CHECK_NOT_NULL(prx);
if (doManageController)
{
@@ -54,18 +53,21 @@ namespace armarx::plugins
return prx;
}
- void RobotUnitComponentPlugin::manageController(const armarx::NJointControllerInterfacePrx& ctrl)
+ void
+ RobotUnitComponentPlugin::manageController(const armarx::NJointControllerInterfacePrx& ctrl)
{
ARMARX_CHECK_NOT_NULL(ctrl);
manageController(ctrl->getInstanceName());
}
- void RobotUnitComponentPlugin::manageController(const std::string& ctrl)
+ void
+ RobotUnitComponentPlugin::manageController(const std::string& ctrl)
{
_ctrls.emplace(ctrl);
}
- void RobotUnitComponentPlugin::preOnInitComponent()
+ void
+ RobotUnitComponentPlugin::preOnInitComponent()
{
if (_robotUnitName.empty())
{
@@ -78,7 +80,8 @@ namespace armarx::plugins
}
}
- void RobotUnitComponentPlugin::preOnConnectComponent()
+ void
+ RobotUnitComponentPlugin::preOnConnectComponent()
{
if (not _robotUnitName.empty())
{
@@ -86,53 +89,58 @@ namespace armarx::plugins
}
}
- void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ RobotUnitComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
if (_isRobotUnitOptionalDependency)
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "",
- "Name of the RobotUnit");
+ PROPERTY_NAME, "", "Name of the RobotUnit");
}
else
{
- properties->defineRequiredProperty<std::string>(
- PROPERTY_NAME,
- "Name of the RobotUnit");
+ properties->defineRequiredProperty<std::string>(PROPERTY_NAME,
+ "Name of the RobotUnit");
}
}
}
- RobotUnitInterfacePrx RobotUnitComponentPlugin::getRobotUnit() const
+ RobotUnitInterfacePrx
+ RobotUnitComponentPlugin::getRobotUnit() const
{
return _robotUnit;
}
- void RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
+ void
+ RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
{
ARMARX_CHECK_EMPTY(_robotUnitName);
_robotUnitName = name;
}
- const std::string& RobotUnitComponentPlugin::getRobotUnitName() const
+ const std::string&
+ RobotUnitComponentPlugin::getRobotUnitName() const
{
return _robotUnitName;
}
- bool RobotUnitComponentPlugin::hasRobotUnitName() const
+ bool
+ RobotUnitComponentPlugin::hasRobotUnitName() const
{
return not _robotUnitName.empty();
}
- void RobotUnitComponentPlugin::setRobotUnitAsOptionalDependency(bool isOptional)
+ void
+ RobotUnitComponentPlugin::setRobotUnitAsOptionalDependency(bool isOptional)
{
_isRobotUnitOptionalDependency = isOptional;
}
- void RobotUnitComponentPlugin::waitUntilRobotUnitIsRunning(const std::function<bool ()>& termCond)
+ void
+ RobotUnitComponentPlugin::waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond)
{
ARMARX_INFO << "Waiting until robot unit is running ...";
@@ -152,7 +160,8 @@ namespace armarx::plugins
ARMARX_INFO << "Robot unit is up and running.";
}
- bool RobotUnitComponentPlugin::robotUnitIsRunning() const
+ bool
+ RobotUnitComponentPlugin::robotUnitIsRunning() const
{
if (not hasRobotUnitName())
{
@@ -163,7 +172,7 @@ namespace armarx::plugins
}
-} // namespace armarx::plugins
+} // namespace armarx::plugins
namespace armarx
{
@@ -172,25 +181,30 @@ namespace armarx
addPlugin(plugin);
}
- RobotUnitInterfacePrx RobotUnitComponentPluginUser::getRobotUnit() const
+ RobotUnitInterfacePrx
+ RobotUnitComponentPluginUser::getRobotUnit() const
{
return plugin->getRobotUnit();
}
- plugins::RobotUnitComponentPlugin& RobotUnitComponentPluginUser::getRobotUnitComponentPlugin()
+ plugins::RobotUnitComponentPlugin&
+ RobotUnitComponentPluginUser::getRobotUnitComponentPlugin()
{
return *plugin;
}
- void RobotUnitComponentPluginUser::waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond) const
+ void
+ RobotUnitComponentPluginUser::waitUntilRobotUnitIsRunning(
+ const std::function<bool()>& termCond) const
{
ARMARX_INFO << "Waiting until robot unit is running ...";
- while ((not termCond()) and ((isNullptr(getRobotUnit()) or (not getRobotUnit()->isRunning()))))
+ while ((not termCond()) and
+ ((isNullptr(getRobotUnit()) or (not getRobotUnit()->isRunning()))))
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
ARMARX_INFO << "Robot unit is up and running.";
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index ec497f88670d49e3e780114b2fcc3a9e78e3cc9e..97ab8e8bede3fc44d25e204b2013a1f7e2cb4733 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -1,36 +1,34 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
+
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
-
-namespace armarx
+namespace armarx::plugins
{
- namespace plugins
- {
- class RobotUnitComponentPlugin : public ComponentPlugin
- {
- public:
- using ComponentPlugin::ComponentPlugin;
+ class RobotUnitComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
- void preOnInitComponent() override;
+ void preOnInitComponent() override;
- void preOnConnectComponent() override;
- void postOnDisconnectComponent() override;
+ void preOnConnectComponent() override;
+ void postOnDisconnectComponent() override;
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
- RobotUnitInterfacePrx getRobotUnit() const;
+ RobotUnitInterfacePrx getRobotUnit() const;
- void setRobotUnitName(const std::string& name);
- const std::string& getRobotUnitName() const;
- bool hasRobotUnitName() const;
+ void setRobotUnitName(const std::string& name);
+ const std::string& getRobotUnitName() const;
+ bool hasRobotUnitName() const;
- void setRobotUnitAsOptionalDependency(bool isOptional = true);
+ void setRobotUnitAsOptionalDependency(bool isOptional = true);
- /**
+ /**
* @brief Waits until the robot unit is running.
*
* Although the robot unit proxy might be initialized (\see getRobotUnit()), the robot unit might
@@ -40,52 +38,55 @@ namespace armarx
* If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
* for the robot unit to become available.
*/
- void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;});
-
- bool robotUnitIsRunning() const;
-
-
-
- //controllers
- public:
- template<class PrxT>
- PrxT createNJointController(const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- bool doManageController = true)
- {
- return PrxT::checkedCast(createNJointController(className, instanceName, config, doManageController));
- }
- template<class PrxT>
- void createNJointController(PrxT& prx,
- const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- bool doManageController = true)
- {
- prx = PrxT::checkedCast(createNJointController(className, instanceName, config, doManageController));
- }
- NJointControllerInterfacePrx createNJointController(const std::string& className,
- const std::string& instanceName,
- const NJointControllerConfigPtr& config,
- bool doManageController = true);
- void manageController(const NJointControllerInterfacePrx& ctrl);
- void manageController(const std::string& ctrl);
-
- //datastreaming
- public:
- RobotUnitDataStreamingReceiverPtr startDataStreaming(const RobotUnitDataStreaming::Config& cfg);
-
- private:
- static constexpr const char* PROPERTY_NAME = "RobotUnitName";
- RobotUnitInterfacePrx _robotUnit;
- std::string _robotUnitName;
- bool _isRobotUnitOptionalDependency = false;
- std::set<std::string> _ctrls;
- };
- }
-}
+ void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = []
+ { return false; });
+
+ bool robotUnitIsRunning() const;
+
+ //controllers
+ template <class PrxT>
+ PrxT
+ createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool doManageController = true)
+ {
+ return PrxT::checkedCast(
+ createNJointController(className, instanceName, config, doManageController));
+ }
+ template <class PrxT>
+ void
+ createNJointController(PrxT& prx,
+ const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool doManageController = true)
+ {
+ prx = PrxT::checkedCast(
+ createNJointController(className, instanceName, config, doManageController));
+ }
+
+ NJointControllerInterfacePrx createNJointController(const std::string& className,
+ const std::string& instanceName,
+ const NJointControllerConfigPtr& config,
+ bool doManageController = true);
+ void manageController(const NJointControllerInterfacePrx& ctrl);
+ void manageController(const std::string& ctrl);
+
+ //datastreaming
+
+ RobotUnitDataStreamingReceiverPtr
+ startDataStreaming(const RobotUnitDataStreaming::Config& cfg);
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "RobotUnitName";
+ RobotUnitInterfacePrx _robotUnit;
+ std::string _robotUnitName;
+ bool _isRobotUnitOptionalDependency = false;
+ std::set<std::string> _ctrls;
+ };
+} // namespace armarx::plugins
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -101,7 +102,8 @@ namespace armarx
RobotUnitInterfacePrx getRobotUnit() const;
- void setRobotUnitAsOptionalDependency(bool isOptional = true)
+ void
+ setRobotUnitAsOptionalDependency(bool isOptional = true)
{
plugin->setRobotUnitAsOptionalDependency(isOptional);
}
@@ -115,21 +117,19 @@ namespace armarx
* @param termCond Termination condition. If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
* for the robot unit to become available.
*/
- void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;}) const;
+ void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = []
+ { return false; }) const;
plugins::RobotUnitComponentPlugin& getRobotUnitComponentPlugin();
+
private:
armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
-namespace armarx
+namespace armarx::plugins
{
- namespace plugins
- {
- // Legacy typedef.
- using RobotUnitComponentPluginUser = armarx::RobotUnitComponentPluginUser;
- }
-}
+ // Legacy typedef.
+ using RobotUnitComponentPluginUser = armarx::RobotUnitComponentPluginUser;
+} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
index b83206f6fb670587dc418fdd0207382390b83543..3ab4e0530b54331305cdbc8f8ad05603607b0ac9 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -20,13 +20,13 @@
* GNU General Public License
*/
+#include "RobotUnitDataStreamingReceiver.h"
+
#include <Ice/ObjectAdapter.h>
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/ArmarXManager.h>
-#include "RobotUnitDataStreamingReceiver.h"
-
namespace armarx::detail::RobotUnitDataStreamingReceiver
{
class Receiver :
@@ -34,23 +34,37 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
virtual public RobotUnitDataStreaming::Receiver
{
public:
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotUnitDataStreamingReceiver";
}
+
~Receiver()
{
std::lock_guard g{_data_mutex};
}
- void onInitComponent() override {}
- void onConnectComponent() override {}
- void onExitComponent() override {}
-
- void update_async(
- const RobotUnitDataStreaming::AMD_Receiver_updatePtr& ptr,
- const RobotUnitDataStreaming::TimeStepSeq& data,
- Ice::Long msgSequenceNbr,
- const Ice::Current&) override
+
+ void
+ onInitComponent() override
+ {
+ }
+
+ void
+ onConnectComponent() override
+ {
+ }
+
+ void
+ onExitComponent() override
+ {
+ }
+
+ void
+ update_async(const RobotUnitDataStreaming::AMD_Receiver_updatePtr& ptr,
+ const RobotUnitDataStreaming::TimeStepSeq& data,
+ Ice::Long msgSequenceNbr,
+ const Ice::Current&) override
{
ptr->ice_response();
if (_discard_data)
@@ -65,32 +79,31 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
_data[seq] = data;
}
- std::atomic_bool _discard_data = false;
- std::mutex _data_mutex;
- std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> _data;
- Ice::Identity _identity;
+ std::atomic_bool _discard_data = false;
+ std::mutex _data_mutex;
+ std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> _data;
+ Ice::Identity _identity;
};
-}
+} // namespace armarx::detail::RobotUnitDataStreamingReceiver
namespace armarx
{
RobotUnitDataStreamingReceiver::RobotUnitDataStreamingReceiver(
const ManagedIceObjectPtr& obj,
const RobotUnitInterfacePrx& ru,
- const RobotUnitDataStreaming::Config& cfg
- ) : _obj{obj}, _ru{ru}
+ const RobotUnitDataStreaming::Config& cfg) :
+ _obj{obj}, _ru{ru}
{
ARMARX_CHECK_NOT_NULL(_obj);
ARMARX_CHECK_NOT_NULL(_ru);
_receiver = make_shared<detail::RobotUnitDataStreamingReceiver::Receiver>();
- _receiver->_identity.name =
- _obj->getName() + "_RobotUnitDataStreamingReceiver_" +
- std::to_string(clock_t::now().time_since_epoch().count());
+ _receiver->_identity.name = _obj->getName() + "_RobotUnitDataStreamingReceiver_" +
+ std::to_string(clock_t::now().time_since_epoch().count());
auto adapter = _obj->getArmarXManager()->getAdapter();
_proxy = RobotUnitDataStreaming::ReceiverPrx::uncheckedCast(
- adapter->add(_receiver, _receiver->_identity));
+ adapter->add(_receiver, _receiver->_identity));
_description = _ru->startDataStreaming(_proxy, cfg);
}
@@ -120,14 +133,15 @@ namespace armarx
ARMARX_INFO << deactivateSpam() << "waiting until receiver is removed from ice";
}
}
- _proxy = nullptr;
+ _proxy = nullptr;
_receiver = nullptr;
}
- std::deque<RobotUnitDataStreaming::TimeStep>& RobotUnitDataStreamingReceiver::getDataBuffer()
+ std::deque<RobotUnitDataStreaming::TimeStep>&
+ RobotUnitDataStreamingReceiver::getDataBuffer()
{
ARMARX_CHECK_NOT_NULL(_receiver);
- std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> data;
+ std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> data;
{
std::lock_guard g{_receiver->_data_mutex};
std::swap(data, _receiver->_data);
@@ -169,14 +183,12 @@ namespace armarx
_tmp_data_buffer_seq_id = it->first;
for (auto& step : it->second)
{
- if (
- _last_iteration_id != -1 &&
- _last_iteration_id + 1 != step.iterationId
- )
+ if (_last_iteration_id != -1 && _last_iteration_id + 1 != step.iterationId)
{
- ARMARX_ERROR << "Missing Iterations or iterations out of order! "
- << "This should not happen. "
- << VAROUT(_last_iteration_id) << ", " << VAROUT(step.iterationId);
+ ARMARX_INFO << deactivateSpam(10)
+ << "Missing Iterations or iterations out of order! "
+ << "This should not happen. " << VAROUT(_last_iteration_id) << ", "
+ << VAROUT(step.iterationId);
}
_last_iteration_id = step.iterationId;
_data_buffer.emplace_back(std::move(step));
@@ -186,12 +198,14 @@ namespace armarx
return _data_buffer;
}
- const RobotUnitDataStreaming::DataStreamingDescription& RobotUnitDataStreamingReceiver::getDataDescription() const
+ const RobotUnitDataStreaming::DataStreamingDescription&
+ RobotUnitDataStreamingReceiver::getDataDescription() const
{
return _description;
}
- std::string RobotUnitDataStreamingReceiver::getDataDescriptionString() const
+ std::string
+ RobotUnitDataStreamingReceiver::getDataDescriptionString() const
{
std::stringstream str;
const auto& entr = getDataDescription().entries;
@@ -202,4 +216,4 @@ namespace armarx
}
return str.str();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem/aron/MemoryID.xml b/source/RobotAPI/libraries/armem/aron/MemoryID.xml
index 996259c63c7b66028bd8b853714fa3147fd64ed6..fad7e74d7d0120c4d9d92e7d72db3409fe54a222 100644
--- a/source/RobotAPI/libraries/armem/aron/MemoryID.xml
+++ b/source/RobotAPI/libraries/armem/aron/MemoryID.xml
@@ -18,7 +18,7 @@
<Time />
</ObjectChild>
<ObjectChild key="instanceIndex">
- <int />
+ <int32 />
</ObjectChild>
</Object>
</GenerateTypes>
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.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index f3991bc59641ac81b1efe74f781edeebb6f6f8fd..8a7200fdace884fb197530d01622d787a09b5bbc 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -17,6 +17,7 @@
#include "query/query_fns.h"
#include <mutex>
+#include <Ice/LocalException.h>
namespace armarx::armem::client
{
@@ -49,11 +50,19 @@ namespace armarx::armem::client
{
result = readingPrx->query(input);
}
+ catch (const ::Ice::ConnectionRefusedException& e)
+ {
+ // the proxy is invalid. we must perform a reconnect
+ ARMARX_INFO << "Trying to reconnect ...";
+
+ }
catch (const Ice::LocalException& e)
{
std::stringstream sstream;
sstream << "Memory query failed.\nReason: " << e.what();
result.errorMessage = sstream.str();
+
+ ARMARX_VERBOSE << result.errorMessage;
}
return result;
}
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 e567b76b8d36d8cf41c00118a49d3e041d0607bc..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;
@@ -38,42 +37,41 @@ namespace armarx::armem::client::util
class SimpleReaderBase
{
- public:
-
- SimpleReaderBase(MemoryNameSystem& memoryNameSystem);
- virtual ~SimpleReaderBase() = default;
-
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- virtual void connect();
-
-
- protected:
+ public:
// Properties
struct Properties
{
- std::string memoryName = "";
+ std::string memoryName = "";
std::string coreSegmentName = "";
};
+ SimpleReaderBase();
+ virtual ~SimpleReaderBase() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ virtual void connect(armarx::armem::client::MemoryNameSystem& mns);
+
const Properties& properties() const;
- virtual std::string propertyPrefix() const = 0;
+ void
+ setProperties(const Properties& p)
+ {
+ props = p;
+ }
+
+ protected:
+ 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..89510368848d3f29b49219ae62a5699428086627 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,23 +260,30 @@ 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())
+ 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.");
+ 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.");
}
*/
@@ -267,7 +295,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 +305,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 +327,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/server/test/ArMemMemoryTest.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp
index 70a0a2839f7f575d02d876a2721c3b3a6c73b58c..eb562e3278db9cb51aaa364e726e6fe5f71655fc 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp
@@ -24,15 +24,15 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
+#include <iostream>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <SimoxUtility/meta/type_name.h>
+
+#include <RobotAPI/Test.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/operations.h>
-
-#include <iostream>
-#include <SimoxUtility/meta/type_name.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
@@ -41,11 +41,10 @@ namespace armem = armarx::armem;
namespace aron = armarx::aron;
namespace wm = armarx::armem::wm;
-
BOOST_AUTO_TEST_CASE(test_time_to_string)
{
// 111111: seconds, 345: milliseconds, 789: microseconds
- armem::Time time { armem::Duration::MicroSeconds(111111345789) };
+ armem::Time time{armem::Duration::MicroSeconds(111111345789)};
BOOST_CHECK_EQUAL(armem::toStringMilliSeconds(time), "111111345.789 ms");
BOOST_CHECK_EQUAL(armem::toStringMilliSeconds(time, 0), "111111345 ms");
@@ -53,7 +52,10 @@ BOOST_AUTO_TEST_CASE(test_time_to_string)
BOOST_CHECK_EQUAL(armem::toStringMilliSeconds(time, 2), "111111345.78 ms");
BOOST_CHECK_EQUAL(armem::toStringMilliSeconds(time, 3), "111111345.789 ms");
- BOOST_CHECK_EQUAL(armem::toStringMicroSeconds(time), "111111345789 " "\u03BC" "s");
+ BOOST_CHECK_EQUAL(armem::toStringMicroSeconds(time),
+ "111111345789 "
+ "\u03BC"
+ "s");
BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time), "1970-01-02 07:51:51.345789");
BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time, 0), "1970-01-02 07:51:51");
@@ -63,7 +65,10 @@ BOOST_AUTO_TEST_CASE(test_time_to_string)
// 111111: seconds, 000: milliseconds, 789: microseconds
time = armem::Time(armem::Duration::MicroSeconds(111111000789));
BOOST_CHECK_EQUAL(armem::toStringMilliSeconds(time), "111111000.789 ms");
- BOOST_CHECK_EQUAL(armem::toStringMicroSeconds(time), "111111000789 " "\u03BC" "s");
+ BOOST_CHECK_EQUAL(armem::toStringMicroSeconds(time),
+ "111111000789 "
+ "\u03BC"
+ "s");
BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time), "1970-01-02 07:51:51.000789");
BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time, 0), "1970-01-02 07:51:51");
BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time, 3), "1970-01-02 07:51:51.000");
@@ -72,14 +77,16 @@ BOOST_AUTO_TEST_CASE(test_time_to_string)
// 111111: seconds, 345: milliseconds, 000: microseconds
time = armem::Time(armem::Duration::MicroSeconds(111111345000));
BOOST_CHECK_EQUAL(armem::toStringMilliSeconds(time), "111111345.000 ms");
- BOOST_CHECK_EQUAL(armem::toStringMicroSeconds(time), "111111345000 " "\u03BC" "s");
+ BOOST_CHECK_EQUAL(armem::toStringMicroSeconds(time),
+ "111111345000 "
+ "\u03BC"
+ "s");
BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time), "1970-01-02 07:51:51.345000");
BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time, 0), "1970-01-02 07:51:51");
BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time, 3), "1970-01-02 07:51:51.345");
BOOST_CHECK_EQUAL(armem::toDateTimeMilliSeconds(time, 6), "1970-01-02 07:51:51.345000");
}
-
// Public interface tests
@@ -87,17 +94,18 @@ namespace ArMemMemoryTest
{
struct APITestFixture
{
- wm::EntityInstance instance { 0 };
- wm::EntitySnapshot snapshot { armem::Time(armem::Duration::MicroSeconds(1000)) };
- wm::Entity entity { "entity" };
- wm::ProviderSegment provSeg { "provider" };
- wm::CoreSegment coreSeg { "core" };
- wm::Memory memory { "memory" };
+ wm::EntityInstance instance{0};
+ wm::EntitySnapshot snapshot{armem::Time(armem::Duration::MicroSeconds(1000))};
+ wm::Entity entity{"entity"};
+ wm::ProviderSegment provSeg{"provider"};
+ wm::CoreSegment coreSeg{"core"};
+ wm::Memory memory{"memory"};
-
- void test_add_instance(wm::EntityInstance& added, const wm::EntitySnapshot& parent)
+ void
+ test_add_instance(wm::EntityInstance& added, const wm::EntitySnapshot& parent)
{
- BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
+ BOOST_TEST_CONTEXT("Added: " << armem::print(added)
+ << "\n Parent: " << armem::print(parent))
{
const int index = 0;
BOOST_CHECK_EQUAL(added.index(), index);
@@ -106,9 +114,12 @@ namespace ArMemMemoryTest
BOOST_CHECK_EQUAL(&parent.getInstance(index), &added);
}
}
- void test_add_snapshot(wm::EntitySnapshot& added, const wm::Entity& parent)
+
+ void
+ test_add_snapshot(wm::EntitySnapshot& added, const wm::Entity& parent)
{
- BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
+ BOOST_TEST_CONTEXT("Added: " << armem::print(added)
+ << "\n Parent: " << armem::print(parent))
{
const armem::Time time = armem::Time(armem::Duration::MicroSeconds(1000));
BOOST_CHECK_EQUAL(added.time(), time);
@@ -117,9 +128,12 @@ namespace ArMemMemoryTest
BOOST_CHECK_EQUAL(&parent.getSnapshot(time), &added);
}
}
- void test_add_entity(wm::Entity& added, const wm::ProviderSegment& parent)
+
+ void
+ test_add_entity(wm::Entity& added, const wm::ProviderSegment& parent)
{
- BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
+ BOOST_TEST_CONTEXT("Added: " << armem::print(added)
+ << "\n Parent: " << armem::print(parent))
{
const std::string name = "entity";
BOOST_CHECK_EQUAL(added.name(), name);
@@ -128,9 +142,12 @@ namespace ArMemMemoryTest
BOOST_CHECK_EQUAL(&parent.getEntity(name), &added);
}
}
- void test_add_provider_segment(wm::ProviderSegment& added, const wm::CoreSegment& parent)
+
+ void
+ test_add_provider_segment(wm::ProviderSegment& added, const wm::CoreSegment& parent)
{
- BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
+ BOOST_TEST_CONTEXT("Added: " << armem::print(added)
+ << "\n Parent: " << armem::print(parent))
{
const std::string name = "provider";
BOOST_CHECK_EQUAL(added.name(), name);
@@ -139,9 +156,12 @@ namespace ArMemMemoryTest
BOOST_CHECK_EQUAL(&parent.getProviderSegment(name), &added);
}
}
- void test_add_core_segment(wm::CoreSegment& added, const wm::Memory& parent)
+
+ void
+ test_add_core_segment(wm::CoreSegment& added, const wm::Memory& parent)
{
- BOOST_TEST_CONTEXT("Added: " << armem::print(added) << "\n Parent: " << armem::print(parent))
+ BOOST_TEST_CONTEXT("Added: " << armem::print(added)
+ << "\n Parent: " << armem::print(parent))
{
const std::string name = "core";
BOOST_CHECK_EQUAL(added.name(), name);
@@ -151,21 +171,21 @@ namespace ArMemMemoryTest
}
}
};
-}
-
+} // namespace ArMemMemoryTest
BOOST_FIXTURE_TEST_SUITE(APITest, ArMemMemoryTest::APITestFixture)
-
BOOST_AUTO_TEST_CASE(test_add_instance_no_args)
{
test_add_instance(snapshot.addInstance(), snapshot);
}
+
BOOST_AUTO_TEST_CASE(test_add_instance_copy)
{
test_add_instance(snapshot.addInstance(instance), snapshot);
}
+
BOOST_AUTO_TEST_CASE(test_add_instance_move)
{
test_add_instance(snapshot.addInstance(std::move(instance)), snapshot);
@@ -175,10 +195,12 @@ BOOST_AUTO_TEST_CASE(test_add_snapshot_time)
{
test_add_snapshot(entity.addSnapshot(armem::Time(armem::Duration::MicroSeconds(1000))), entity);
}
+
BOOST_AUTO_TEST_CASE(test_add_snapshot_copy)
{
test_add_snapshot(entity.addSnapshot(snapshot), entity);
}
+
BOOST_AUTO_TEST_CASE(test_add_snapshot_move)
{
test_add_snapshot(entity.addSnapshot(std::move(snapshot)), entity);
@@ -188,10 +210,12 @@ BOOST_AUTO_TEST_CASE(test_add_entity_name)
{
test_add_entity(provSeg.addEntity("entity"), provSeg);
}
+
BOOST_AUTO_TEST_CASE(test_add_entity_copy)
{
test_add_entity(provSeg.addEntity(entity), provSeg);
}
+
BOOST_AUTO_TEST_CASE(test_add_entity_move)
{
test_add_entity(provSeg.addEntity(std::move(entity)), provSeg);
@@ -201,10 +225,12 @@ BOOST_AUTO_TEST_CASE(test_add_provider_segment_name)
{
test_add_provider_segment(coreSeg.addProviderSegment("provider"), coreSeg);
}
+
BOOST_AUTO_TEST_CASE(test_add_provider_segment_copy)
{
test_add_provider_segment(coreSeg.addProviderSegment(provSeg), coreSeg);
}
+
BOOST_AUTO_TEST_CASE(test_add_provider_segment_move)
{
test_add_provider_segment(coreSeg.addProviderSegment(std::move(provSeg)), coreSeg);
@@ -214,10 +240,12 @@ BOOST_AUTO_TEST_CASE(test_add_core_segment_name)
{
test_add_core_segment(memory.addCoreSegment("core"), memory);
}
+
BOOST_AUTO_TEST_CASE(test_add_core_segment_copy)
{
test_add_core_segment(memory.addCoreSegment(coreSeg), memory);
}
+
BOOST_AUTO_TEST_CASE(test_add_core_segment_move)
{
test_add_core_segment(memory.addCoreSegment(std::move(coreSeg)), memory);
@@ -226,10 +254,6 @@ BOOST_AUTO_TEST_CASE(test_add_core_segment_move)
BOOST_AUTO_TEST_SUITE_END()
-
-
-
-
// COPY/MOVE CTOR/OP TESTS
@@ -240,38 +264,42 @@ namespace ArMemMemoryTest
using MemoryItem::MemoryItem;
using MemoryItem::operator=;
- std::string getKeyString() const
+ std::string
+ getKeyString() const
{
return "";
}
- std::string getLevelName() const
+
+ std::string
+ getLevelName() const
{
return "";
}
};
+
struct MemoryItemCtorOpTestFixture
{
- const armem::MemoryID id {"A/B/C/123/1"};
- const armem::MemoryID moved {"////1"}; // int is not moved
- TestMemoryItem item { id };
+ const armem::MemoryID id{"A/B/C/123/1"};
+ const armem::MemoryID moved{"////1"}; // int is not moved
+ TestMemoryItem item{id};
MemoryItemCtorOpTestFixture()
{
BOOST_CHECK_EQUAL(item.id(), id);
}
};
-}
+} // namespace ArMemMemoryTest
BOOST_FIXTURE_TEST_SUITE(MemoryItemTest, ArMemMemoryTest::MemoryItemCtorOpTestFixture)
-
BOOST_AUTO_TEST_CASE(test_copy_ctor)
{
const ArMemMemoryTest::TestMemoryItem out(item);
BOOST_CHECK_EQUAL(item.id(), id);
BOOST_CHECK_EQUAL(out.id(), id);
}
+
BOOST_AUTO_TEST_CASE(test_copy_op)
{
ArMemMemoryTest::TestMemoryItem out;
@@ -279,6 +307,7 @@ BOOST_AUTO_TEST_CASE(test_copy_op)
BOOST_CHECK_EQUAL(item.id(), id);
BOOST_CHECK_EQUAL(out.id(), id);
}
+
BOOST_AUTO_TEST_CASE(test_move_ctor)
{
ArMemMemoryTest::TestMemoryItem in = item;
@@ -286,6 +315,7 @@ BOOST_AUTO_TEST_CASE(test_move_ctor)
BOOST_CHECK_EQUAL(in.id(), moved);
BOOST_CHECK_EQUAL(out.id(), id);
}
+
BOOST_AUTO_TEST_CASE(test_move_op)
{
ArMemMemoryTest::TestMemoryItem in = item;
@@ -298,34 +328,38 @@ BOOST_AUTO_TEST_CASE(test_move_op)
BOOST_AUTO_TEST_SUITE_END()
-
-
namespace ArMemMemoryTest
{
- struct TestMemoryContainer : public armem::base::detail::MemoryContainerBase<std::vector<int>, TestMemoryContainer>
+ struct TestMemoryContainer :
+ public armem::base::detail::MemoryContainerBase<std::vector<int>, TestMemoryContainer>
{
using MemoryContainerBase::MemoryContainerBase;
using MemoryContainerBase::operator=;
- std::string getKeyString() const
+ std::string
+ getKeyString() const
{
return "";
}
- std::string getLevelName() const
+
+ std::string
+ getLevelName() const
{
return "";
}
- void fill()
+ void
+ fill()
{
- _container = std::vector<int> { -1, 2, -3 };
+ _container = std::vector<int>{-1, 2, -3};
}
};
+
struct MemoryContainerCtorOpTestFixture
{
- const armem::MemoryID id {"M/C/P/E/123/1"};
- const armem::MemoryID moved {"////123/1"}; // Time and int are just copied, not moved
- TestMemoryContainer cont {id};
+ const armem::MemoryID id{"M/C/P/E/123/1"};
+ const armem::MemoryID moved{"////123/1"}; // Time and int are just copied, not moved
+ TestMemoryContainer cont{id};
MemoryContainerCtorOpTestFixture()
{
@@ -335,7 +369,7 @@ namespace ArMemMemoryTest
}
};
-}
+} // namespace ArMemMemoryTest
BOOST_FIXTURE_TEST_SUITE(MemoryContainerTest, ArMemMemoryTest::MemoryContainerCtorOpTestFixture)
@@ -347,6 +381,7 @@ BOOST_AUTO_TEST_CASE(test_copy_ctor)
BOOST_CHECK_EQUAL(out.id(), id);
BOOST_CHECK_EQUAL(out.size(), 3);
}
+
BOOST_AUTO_TEST_CASE(test_copy_op)
{
ArMemMemoryTest::TestMemoryContainer out;
@@ -356,6 +391,7 @@ BOOST_AUTO_TEST_CASE(test_copy_op)
BOOST_CHECK_EQUAL(out.id(), id);
BOOST_CHECK_EQUAL(out.size(), 3);
}
+
BOOST_AUTO_TEST_CASE(test_move_ctor)
{
ArMemMemoryTest::TestMemoryContainer in = cont;
@@ -365,6 +401,7 @@ BOOST_AUTO_TEST_CASE(test_move_ctor)
BOOST_CHECK_EQUAL(out.id(), id);
BOOST_CHECK_EQUAL(out.size(), 3);
}
+
BOOST_AUTO_TEST_CASE(test_move_op)
{
ArMemMemoryTest::TestMemoryContainer in = cont;
@@ -378,8 +415,6 @@ BOOST_AUTO_TEST_CASE(test_move_op)
BOOST_AUTO_TEST_SUITE_END()
-
-
BOOST_AUTO_TEST_CASE(test_key_ctors)
{
wm::EntityInstance instance(10);
@@ -401,29 +436,32 @@ BOOST_AUTO_TEST_CASE(test_key_ctors)
BOOST_CHECK_EQUAL(memory.name(), "memory");
}
-
-
template <class T>
struct CustomChecks
{
- static void checkEqual(const T& lhs, const T& rhs)
+ static void
+ checkEqual(const T& lhs, const T& rhs)
{
- (void) lhs, (void) rhs;
+ (void)lhs, (void)rhs;
}
- static void checkMoved(const T& moved)
+
+ static void
+ checkMoved(const T& moved)
{
- (void) moved;
+ (void)moved;
}
};
-
template <class T>
-void checkEqual_d_ltm(const T& lhs, const T& rhs)
+void
+checkEqual_d_ltm(const T& lhs, const T& rhs)
{
BOOST_CHECK_EQUAL(lhs.path, rhs.path);
}
+
template <class T>
-void checkMoved_d_ltm(const T& moved)
+void
+checkMoved_d_ltm(const T& moved)
{
BOOST_CHECK_EQUAL(moved.path, nullptr);
}
@@ -431,16 +469,20 @@ void checkMoved_d_ltm(const T& moved)
template <>
struct CustomChecks<wm::EntityInstance>
{
- static void checkEqual(const wm::EntityInstance& lhs, const wm::EntityInstance& rhs)
+ static void
+ checkEqual(const wm::EntityInstance& lhs, const wm::EntityInstance& rhs)
{
BOOST_CHECK_EQUAL(lhs.metadata(), rhs.metadata());
BOOST_CHECK_EQUAL(lhs.data(), rhs.data());
}
- static void checkMoved(const wm::EntityInstance& moved)
+
+ static void
+ checkMoved(const wm::EntityInstance& moved)
{
BOOST_CHECK_EQUAL(moved.data(), nullptr);
}
};
+
/*template <>
struct CustomChecks<armem::d_ltm::EntityInstance>
{
@@ -517,24 +559,29 @@ struct CustomChecks<armem::d_ltm::Memory>
struct CopyMoveCtorsOpsTestBase
{
- const armem::MemoryID id {"M", "C", "P", "E", armem::Time(armem::Duration::MicroSeconds(123000)), 1};
+ const armem::MemoryID id{"M",
+ "C",
+ "P",
+ "E",
+ armem::Time(armem::Duration::MicroSeconds(123000)),
+ 1};
//const armem::MemoryID idMoved {"", "", "", "", armem::Time(armem::Duration::MicroSeconds(123000), 1};
armem::MemoryID idMoved = id;
std::string typeName;
- CopyMoveCtorsOpsTestBase(const std::string& typeName) :
- typeName(typeName)
+ CopyMoveCtorsOpsTestBase(const std::string& typeName) : typeName(typeName)
{
armem::MemoryID copy = std::move(idMoved);
- (void) copy;
+ (void)copy;
}
+
virtual ~CopyMoveCtorsOpsTestBase()
{
}
-
- void test()
+ void
+ test()
{
BOOST_TEST_CONTEXT("Type " << typeName)
{
@@ -561,7 +608,8 @@ struct CopyMoveCtorsOpsTestBase
}
}
- virtual void reset()
+ virtual void
+ reset()
{
}
@@ -571,34 +619,36 @@ struct CopyMoveCtorsOpsTestBase
virtual void testMoveOp() = 0;
};
-
-
template <class T>
struct InstanceCopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
{
T in;
- InstanceCopyMoveCtorsOpsTest() :
- CopyMoveCtorsOpsTestBase(simox::meta::get_type_name<T>())
+ InstanceCopyMoveCtorsOpsTest() : CopyMoveCtorsOpsTestBase(simox::meta::get_type_name<T>())
{
}
+
virtual ~InstanceCopyMoveCtorsOpsTest() override = default;
- void reset() override
+ void
+ reset() override
{
- in = T {id};
+ in = T{id};
BOOST_CHECK_EQUAL(in.id(), id);
}
- void testCopyCtor() override
+ void
+ testCopyCtor() override
{
- T out { in };
+ T out{in};
BOOST_CHECK_EQUAL(out.id(), id);
CustomChecks<T>::checkEqual(out, in);
}
- void testCopyOp() override
+
+ void
+ testCopyOp() override
{
T out;
out = in;
@@ -607,10 +657,12 @@ struct InstanceCopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
CustomChecks<T>::checkEqual(out, in);
}
- void testMoveCtor() override
+
+ void
+ testMoveCtor() override
{
- T copy { in };
- T out { std::move(in) };
+ T copy{in};
+ T out{std::move(in)};
BOOST_CHECK_EQUAL(in.id(), idMoved);
BOOST_CHECK_EQUAL(out.id(), id);
@@ -618,9 +670,11 @@ struct InstanceCopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
CustomChecks<T>::checkEqual(out, copy);
CustomChecks<T>::checkMoved(in);
}
- void testMoveOp() override
+
+ void
+ testMoveOp() override
{
- T copy { in };
+ T copy{in};
T out;
out = std::move(in);
@@ -632,7 +686,6 @@ struct InstanceCopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
}
};
-
template <class T>
struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
{
@@ -641,18 +694,21 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
CopyMoveCtorsOpsTest() : CopyMoveCtorsOpsTestBase(simox::meta::get_type_name<T>())
{
}
+
virtual ~CopyMoveCtorsOpsTest() override = default;
- void reset() override
+ void
+ reset() override
{
- in = T {id};
- if constexpr(std::is_same_v<T, wm::Memory>)
+ in = T{id};
+ if constexpr (std::is_same_v<T, wm::Memory>)
{
in.addCoreSegment("C");
}
{
armem::EntityUpdate update;
- update.entityID = armem::MemoryID("M", "C", "P", "E", armem::Time(armem::Duration::MicroSeconds(123000)), 0);
+ update.entityID = armem::MemoryID(
+ "M", "C", "P", "E", armem::Time(armem::Duration::MicroSeconds(123000)), 0);
update.referencedTime = update.entityID.timestamp;
update.instancesData.emplace_back();
in.update(update);
@@ -661,23 +717,26 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
BOOST_CHECK_EQUAL(in.id(), id);
BOOST_CHECK_EQUAL(in.size(), 1);
- if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
+ if constexpr (std::is_base_of_v<armarx::armem::base::detail::AronTyped, T>)
{
+ auto o = std::make_shared<aron::type::Object>();
+ o->setObjectName("some_object");
+ in.aronType() = o;
- in.aronType() = std::make_shared<aron::type::Object>("some_object");
BOOST_CHECK(in.aronType());
}
}
- void testCopyCtor() override
+ void
+ testCopyCtor() override
{
- T out { in };
+ T out{in};
BOOST_CHECK_EQUAL(in.id(), id);
BOOST_CHECK_EQUAL(in.size(), 1);
BOOST_CHECK_EQUAL(out.id(), id);
BOOST_CHECK_EQUAL(out.size(), 1);
- if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
+ if constexpr (std::is_base_of_v<armarx::armem::base::detail::AronTyped, T>)
{
BOOST_CHECK(in.aronType());
BOOST_CHECK(out.aronType());
@@ -686,7 +745,9 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
CustomChecks<T>::checkEqual(out, in);
}
- void testCopyOp() override
+
+ void
+ testCopyOp() override
{
T out;
out = in;
@@ -695,7 +756,7 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
BOOST_CHECK_EQUAL(out.id(), id);
BOOST_CHECK_EQUAL(out.size(), 1);
- if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
+ if constexpr (std::is_base_of_v<armarx::armem::base::detail::AronTyped, T>)
{
BOOST_CHECK(in.aronType());
BOOST_CHECK(out.aronType());
@@ -704,16 +765,18 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
CustomChecks<T>::checkEqual(out, in);
}
- void testMoveCtor() override
+
+ void
+ testMoveCtor() override
{
- T out { std::move(in) };
+ T out{std::move(in)};
BOOST_CHECK_EQUAL(in.id(), idMoved);
BOOST_CHECK_EQUAL(in.size(), 0);
BOOST_CHECK_EQUAL(out.id(), id);
BOOST_CHECK_EQUAL(out.size(), 1);
- if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
+ if constexpr (std::is_base_of_v<armarx::armem::base::detail::AronTyped, T>)
{
BOOST_CHECK(out.aronType());
BOOST_CHECK_EQUAL(in.aronType(), nullptr);
@@ -721,7 +784,9 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
CustomChecks<T>::checkMoved(in);
}
- void testMoveOp() override
+
+ void
+ testMoveOp() override
{
T out;
out = std::move(in);
@@ -731,7 +796,7 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
BOOST_CHECK_EQUAL(out.id(), id);
BOOST_CHECK_EQUAL(out.size(), 1);
- if constexpr(std::is_base_of_v <armarx::armem::base::detail::AronTyped, T>)
+ if constexpr (std::is_base_of_v<armarx::armem::base::detail::AronTyped, T>)
{
BOOST_CHECK(out.aronType());
BOOST_CHECK_EQUAL(in.aronType(), nullptr);
@@ -741,8 +806,6 @@ struct CopyMoveCtorsOpsTest : public CopyMoveCtorsOpsTestBase
}
};
-
-
BOOST_AUTO_TEST_CASE(test_copy_move_ctors_ops)
{
{
@@ -771,8 +834,6 @@ BOOST_AUTO_TEST_CASE(test_copy_move_ctors_ops)
}*/
}
-
-
BOOST_AUTO_TEST_CASE(test_segment_setup)
{
armem::EntityUpdate update;
@@ -809,11 +870,8 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
// A successful update.
update.entityID = armem::MemoryID::fromString("Memory/ImageRGB/SomeRGBImageProvider/image");
- update.instancesData =
- {
- std::make_shared<aron::data::Dict>(),
- std::make_shared<aron::data::Dict>()
- };
+ update.instancesData = {std::make_shared<aron::data::Dict>(),
+ std::make_shared<aron::data::Dict>()};
update.referencedTime = armem::Time(armem::Duration::MilliSeconds(1000));
BOOST_CHECK_NO_THROW(providerSegment.update(update));
@@ -832,24 +890,22 @@ BOOST_AUTO_TEST_CASE(test_segment_setup)
// Another update (on memory).
- update.instancesData = { std::make_shared<aron::data::Dict>() };
+ update.instancesData = {std::make_shared<aron::data::Dict>()};
update.referencedTime = armem::Time(armem::Duration::MilliSeconds(2000));
memory.update(update);
BOOST_CHECK_EQUAL(entity.size(), 2);
BOOST_CHECK(entity.hasSnapshot(update.referencedTime));
- BOOST_CHECK_EQUAL(entity.getSnapshot(update.referencedTime).size(), update.instancesData.size());
+ BOOST_CHECK_EQUAL(entity.getSnapshot(update.referencedTime).size(),
+ update.instancesData.size());
// A third update (on entity).
- update.instancesData = { std::make_shared<aron::data::Dict>() };
+ update.instancesData = {std::make_shared<aron::data::Dict>()};
update.referencedTime = armem::Time(armem::Duration::MilliSeconds(3000));
entity.update(update);
BOOST_CHECK_EQUAL(entity.size(), 3);
-
}
-
-
BOOST_AUTO_TEST_CASE(test_history_size_in_entity)
{
armem::server::wm::Entity entity("entity");
@@ -892,7 +948,6 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_entity)
BOOST_CHECK(entity.hasSnapshot(armem::Time(armem::Duration::MilliSeconds(5000))));
}
-
BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
{
armem::server::wm::ProviderSegment providerSegment("SomeRGBImageProvider");
@@ -900,7 +955,7 @@ BOOST_AUTO_TEST_CASE(test_history_size_in_provider_segment)
armem::EntityUpdate update;
update.entityID.providerSegmentName = providerSegment.name();
- std::vector<std::string> entityNames = { "A", "B" };
+ std::vector<std::string> entityNames = {"A", "B"};
// Fill entities and histories with unlimited size.
for (const std::string& name : entityNames)
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/aron/KnownGraspCandidate.xml b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
index d2317b83f1eb16da71d6e60980d0636f10e5e1f0..fcce7ea3a3319aa4b7afeeacae9c02abb7f00482 100644
--- a/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
+++ b/source/RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.xml
@@ -15,7 +15,7 @@
</ObjectChild>
<ObjectChild key='quality'>
- <float />
+ <float32 />
</ObjectChild>
<ObjectChild key='pose'>
@@ -62,4 +62,3 @@
</GenerateTypes>
</AronTypeDefinition>
-
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/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index c03f50a293b26523cf500c9ccedf75360ced2111..65af5d76ee017656ea700023be2b34b6e51feb83 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -38,6 +38,7 @@ set(SOURCES
instance/serialize_path.cpp
instance/display_visitors/DataDisplayVisitor.cpp
+ instance/display_visitors/StringStreamMixin.cpp
instance/display_visitors/TypedDataDisplayVisitor.cpp
instance/tree_builders/DataTreeBuilder.cpp
@@ -87,6 +88,7 @@ set(HEADERS
instance/serialize_path.h
instance/display_visitors/DataDisplayVisitor.h
+ instance/display_visitors/StringStreamMixin.h
instance/display_visitors/TypedDataDisplayVisitor.h
instance/tree_builders/DataTreeBuilder.h
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 42c849af0e391fc525b98950a4cfec1f542ea225..c84f1a8b920927642dee5115d1e88ebd8f260a8e 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -146,6 +146,8 @@ namespace armarx::armem::gui
connect(this, &This::connected, this, &This::startQueries);
connect(this, &This::connected, this, &This::startPeriodicUpdateTimer);
connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::startQueries);
+ connect(
+ updateWidget, &armem::gui::PeriodicUpdateWidget::collapseAll, this, &This::collapseAll);
connect(periodicUpdateTimer, &QTimer::timeout, this, &This::updateListOfActiveMemories);
connect(periodicUpdateTimer, &QTimer::timeout, this, &This::processQueryResults);
@@ -256,6 +258,12 @@ namespace armarx::armem::gui
emit disconnected();
}
+ void
+ MemoryViewer::collapseAll()
+ {
+ memoryGroup->tree()->collapseAll();
+ }
+
void
MemoryViewer::startPeriodicUpdateTimer()
{
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index 69fb59d1c2f83b53ebd6587cfd63c148bcab9b7f..5e1b7daa15736b9a88e35193612e995a1b7c2ade 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -5,8 +5,8 @@
#include <QObject>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <RobotAPI/interface/armem/actions.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
@@ -14,10 +14,10 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h>
-#include <RobotAPI/libraries/armem_gui/lifecycle.h>
+#include <RobotAPI/libraries/armem_gui/disk/ControlWidget.h>
#include <RobotAPI/libraries/armem_gui/instance/GroupBox.h>
+#include <RobotAPI/libraries/armem_gui/lifecycle.h>
#include <RobotAPI/libraries/armem_gui/memory/GroupBox.h>
-#include <RobotAPI/libraries/armem_gui/disk/ControlWidget.h>
class QBoxLayout;
@@ -28,34 +28,29 @@ class QLayout;
class QSettings;
class QTimer;
-
namespace armarx
{
class ManagedIceObject;
class SimpleConfigDialog;
-}
+} // namespace armarx
namespace armarx::armem::gui
{
- class MemoryViewer :
- public QObject,
- public armarx::Logging,
- public armarx::gui::LifecycleClient
+ class MemoryViewer : public QObject, public armarx::Logging, public armarx::gui::LifecycleClient
{
Q_OBJECT
using This = MemoryViewer;
public:
-
- MemoryViewer(
- QBoxLayout* updateWidgetLayout,
- QGroupBox* memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
- QGroupBox* instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
- QBoxLayout* diskControlWidgetLayout,
- QLabel* statusLabel
- );
+ MemoryViewer(QBoxLayout* updateWidgetLayout,
+ QGroupBox* memoryGroupBox,
+ QLayout* memoryGroupBoxParentLayout,
+ QGroupBox* instanceGroupBox,
+ QLayout* instanceGroupBoxParentLayout,
+ QBoxLayout* diskControlWidgetLayout,
+ QLabel* statusLabel);
void setLogTag(const std::string& tag);
@@ -73,8 +68,8 @@ namespace armarx::armem::gui
void resolveMemoryID(const MemoryID& id);
- void showActionsMenu(const MemoryID& memoryID, QWidget* parent,
- const QPoint& pos, QMenu* menu);
+ void
+ showActionsMenu(const MemoryID& memoryID, QWidget* parent, const QPoint& pos, QMenu* menu);
void makePrediction(const MemoryID& entityID,
const aron::type::ObjectPtr& entityType,
@@ -92,7 +87,6 @@ namespace armarx::armem::gui
void commit();
-
signals:
void initialized();
@@ -112,6 +106,8 @@ namespace armarx::armem::gui
void updateMemoryTree();
void updateListOfActiveMemories();
+ void collapseAll();
+
signals:
@@ -119,7 +115,6 @@ namespace armarx::armem::gui
private:
-
void onInit(ManagedIceObject& component);
void onConnect(ManagedIceObject& component);
void onDisconnect(ManagedIceObject& component);
@@ -128,20 +123,16 @@ namespace armarx::armem::gui
void updateStatusLabel(int errorCount);
-
/// Start a query for each entry in `memoryReaders` which is not in `runningQueries`.
void startDueQueries();
- std::map<std::string, client::QueryResult>
- collectQueryResults();
+ std::map<std::string, client::QueryResult> collectQueryResults();
- void applyQueryResults(
- const std::map<std::string, client::QueryResult>& results,
- int* outErrorCount = nullptr);
+ void applyQueryResults(const std::map<std::string, client::QueryResult>& results,
+ int* outErrorCount = nullptr);
public:
-
std::string mnsName;
armem::client::MemoryNameSystem mns;
@@ -178,7 +169,6 @@ namespace armarx::armem::gui
// others
std::atomic_bool is_initialized = false;
std::atomic_bool is_connected = false;
-
};
-}
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp
index 9fe9db837e7cfdd3ecff3c26143fcc74093d94a1..f71a508d21cb5ba65e7f26eafb595aff6dae2b38 100644
--- a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp
@@ -1,12 +1,12 @@
#include "PeriodicUpdateWidget.h"
+#include <cmath>
+
#include <QCheckBox>
#include <QDoubleSpinBox>
+#include <QHBoxLayout>
#include <QPushButton>
#include <QTimer>
-#include <QHBoxLayout>
-
-#include <cmath>
namespace armarx::armem::gui
{
@@ -15,25 +15,30 @@ namespace armarx::armem::gui
{
setSizePolicy(QSizePolicy::Policy::Minimum, QSizePolicy::Policy::Fixed);
- QLayout* layout = new QHBoxLayout();
- this->setLayout(layout);
+ QLayout* vlayout = new QVBoxLayout();
+ QLayout* hlayout = new QHBoxLayout();
+ this->setLayout(vlayout);
const int margin = 0;
- layout->setContentsMargins(margin, margin, margin, margin);
+ hlayout->setContentsMargins(margin, margin, margin, margin);
_updateButton = new QPushButton("Update", this);
_autoCheckBox = new QCheckBox("Auto Update", this);
_frequencySpinBox = new QDoubleSpinBox(this);
+ _collapseAllButton = new QPushButton("Collapse all", this);
_frequencySpinBox->setValue(frequency);
_frequencySpinBox->setMinimum(0);
_frequencySpinBox->setMaximum(maxFrequency);
_frequencySpinBox->setSingleStep(0.5);
_frequencySpinBox->setSuffix(" Hz");
+ hlayout->addWidget(_updateButton);
+ hlayout->addWidget(_autoCheckBox);
+ hlayout->addWidget(_frequencySpinBox);
+
+ hlayout->addWidget(_collapseAllButton);
+ vlayout->addItem(hlayout);
- layout->addWidget(_updateButton);
- layout->addWidget(_autoCheckBox);
- layout->addWidget(_frequencySpinBox);
_timer = new QTimer(this);
_updateTimerFrequency();
@@ -42,10 +47,14 @@ namespace armarx::armem::gui
// Private connections.
connect(_autoCheckBox, &QCheckBox::toggled, this, &This::_toggleAutoUpdates);
- connect(_frequencySpinBox, &QDoubleSpinBox::editingFinished, this, &This::_updateTimerFrequency);
+ connect(_frequencySpinBox,
+ &QDoubleSpinBox::editingFinished,
+ this,
+ &This::_updateTimerFrequency);
// Public connections.
connect(_updateButton, &QPushButton::pressed, this, &This::updateSingle);
+ connect(_collapseAllButton, &QPushButton::pressed, this, &This::collapseAll);
connect(_timer, &QTimer::timeout, this, &This::updatePeriodic);
connect(this, &This::updateSingle, this, &This::update);
@@ -57,18 +66,26 @@ namespace armarx::armem::gui
connect(this, &This::stopTimerSignal, _timer, &QTimer::stop);
}
- QPushButton* PeriodicUpdateWidget::updateButton()
+ QPushButton*
+ PeriodicUpdateWidget::updateButton()
{
return _updateButton;
}
+ QPushButton*
+ PeriodicUpdateWidget::collapseAllButton()
+ {
+ return _collapseAllButton;
+ }
- int PeriodicUpdateWidget::getUpdateIntervalMs() const
+ int
+ PeriodicUpdateWidget::getUpdateIntervalMs() const
{
return static_cast<int>(std::round(1000 / _frequencySpinBox->value()));
}
- void PeriodicUpdateWidget::startTimerIfEnabled()
+ void
+ PeriodicUpdateWidget::startTimerIfEnabled()
{
/* A QTimer can only be started and stopped within its own thread (the thread for which
* it has the greatest affinity). Since this method can be called from any thread, we
@@ -85,18 +102,21 @@ namespace armarx::armem::gui
}
}
- void PeriodicUpdateWidget::stopTimer()
+ void
+ PeriodicUpdateWidget::stopTimer()
{
// See `startTimerIfEnabled` for the signal reasoning.
emit stopTimerSignal();
}
- void PeriodicUpdateWidget::_updateTimerFrequency()
+ void
+ PeriodicUpdateWidget::_updateTimerFrequency()
{
_timer->setInterval(getUpdateIntervalMs());
}
- void PeriodicUpdateWidget::_toggleAutoUpdates(bool enabled)
+ void
+ PeriodicUpdateWidget::_toggleAutoUpdates(bool enabled)
{
// This method is already a slot, so it doesn't need to use the timer signals.
_frequencySpinBox->setEnabled(enabled);
@@ -110,20 +130,22 @@ namespace armarx::armem::gui
}
}
- QCheckBox* PeriodicUpdateWidget::autoCheckBox()
+ QCheckBox*
+ PeriodicUpdateWidget::autoCheckBox()
{
return _autoCheckBox;
}
- QDoubleSpinBox* PeriodicUpdateWidget::frequencySpinBox()
+ QDoubleSpinBox*
+ PeriodicUpdateWidget::frequencySpinBox()
{
return _frequencySpinBox;
}
- QTimer* PeriodicUpdateWidget::timer()
+ QTimer*
+ PeriodicUpdateWidget::timer()
{
return _timer;
}
-}
-
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h
index d9438993cc765f5d7b5350f28ac85dac53ed5202..a924965a34c62b68e7b75f5c27321b32cec26a7b 100644
--- a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h
@@ -17,7 +17,6 @@ namespace armarx::armem::gui
using This = PeriodicUpdateWidget;
public:
-
PeriodicUpdateWidget(double frequency = 2.0, double maxFrequency = 60);
@@ -26,6 +25,7 @@ namespace armarx::armem::gui
QCheckBox* autoCheckBox();
QDoubleSpinBox* frequencySpinBox();
QPushButton* updateButton();
+ QPushButton* collapseAllButton();
bool isAutoEnabled() const;
double getUpdateFrequency() const;
@@ -44,6 +44,7 @@ namespace armarx::armem::gui
void updateSingle();
void updatePeriodic();
+ void collapseAll();
private slots:
@@ -56,13 +57,13 @@ namespace armarx::armem::gui
void stopTimerSignal();
private:
-
QPushButton* _updateButton;
QCheckBox* _autoCheckBox;
QDoubleSpinBox* _frequencySpinBox;
- QTimer* _timer;
+ QPushButton* _collapseAllButton;
+ QTimer* _timer;
};
-}
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
index 99f95201e220830607148af08c1a9cce6ded71d2..35fc2e930f47dd848f2745c23607bf25905a1e4d 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
@@ -5,26 +5,30 @@
namespace armarx::aron
{
- std::string DataDisplayVisitor::getValue(const data::VariantPtr& n)
+ std::string
+ DataDisplayVisitor::getValue(const data::VariantPtr& n)
{
DataDisplayVisitor v;
data::visit(v, n);
return v.value.str();
}
- void DataDisplayVisitor::visitDict(const data::VariantPtr& n)
+ void
+ DataDisplayVisitor::visitDict(const data::VariantPtr& n)
{
auto x = data::Dict::DynamicCastAndCheck(n);
value << x->childrenSize() << " items";
}
- void DataDisplayVisitor::visitList(const data::VariantPtr& n)
+ void
+ DataDisplayVisitor::visitList(const data::VariantPtr& n)
{
auto x = data::List::DynamicCastAndCheck(n);
value << x->childrenSize() << " items";
}
- void DataDisplayVisitor::visitBool(const data::VariantPtr& b)
+ void
+ DataDisplayVisitor::visitBool(const data::VariantPtr& b)
{
auto x = data::Bool::DynamicCastAndCheck(b);
if (x->getValue())
@@ -37,41 +41,101 @@ namespace armarx::aron
}
}
- void DataDisplayVisitor::visitDouble(const data::VariantPtr& n)
+ void
+ DataDisplayVisitor::visitDouble(const data::VariantPtr& n)
{
auto x = data::Double::DynamicCastAndCheck(n);
value << x->getValue();
}
- void DataDisplayVisitor::visitFloat(const data::VariantPtr& n)
+ void
+ DataDisplayVisitor::visitFloat(const data::VariantPtr& n)
{
auto x = data::Float::DynamicCastAndCheck(n);
value << x->getValue();
}
- void DataDisplayVisitor::visitInt(const data::VariantPtr& n)
+ void
+ DataDisplayVisitor::visitInt(const data::VariantPtr& n)
{
auto x = data::Int::DynamicCastAndCheck(n);
value << x->getValue();
}
- void DataDisplayVisitor::visitLong(const data::VariantPtr& n)
+ void
+ DataDisplayVisitor::visitLong(const data::VariantPtr& n)
{
auto x = data::Long::DynamicCastAndCheck(n);
value << x->getValue();
}
- void DataDisplayVisitor::visitString(const data::VariantPtr& n)
+ void
+ DataDisplayVisitor::visitString(const data::VariantPtr& n)
{
auto x = data::String::DynamicCastAndCheck(n);
value << "'" << x->getValue() << "'";
}
- void DataDisplayVisitor::visitNDArray(const data::VariantPtr& n)
+ void
+ DataDisplayVisitor::visitNDArray(const data::VariantPtr& data)
{
- auto x = data::NDArray::DynamicCastAndCheck(n);
- value << "shape " << aron::data::NDArray::DimensionsToString(x->getShape()) << ", type '" << x->getType() << "'";
+ auto d = data::NDArray::DynamicCastAndCheck(data);
+
+ if (d->getType() == "float")
+ {
+ setStreamPrecision();
+ processMatrix<float>(*d);
+ }
+ else if (d->getType() == "double")
+ {
+ setStreamPrecision();
+ processMatrix<double>(*d);
+ }
+ else
+ {
+ printShape(*d);
+ }
+ }
+
+ void
+ DataDisplayVisitor::printShape(const data::NDArray& d)
+ {
+ value << "shape " << aron::data::NDArray::DimensionsToString(d.getShape()) << ", type '"
+ << d.getType() << "'";
+ }
+
+ template <typename ScalarT>
+ void
+ DataDisplayVisitor::processMatrix(const data::NDArray& data)
+ {
+ const int rows = data.getShape().at(0);
+ const int cols = data.getShape().at(1);
+
+ const Eigen::Map<Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> m(
+ reinterpret_cast<ScalarT*>(data.getData()), rows, cols);
+
+ const int maxRows = 10;
+ const int maxCols = 10;
+
+ if (cols > maxCols)
+ {
+ printShape(data);
+ }
+ else if (rows > maxRows)
+ {
+ int shownRows = 2;
+
+ value << "(";
+ printShape(data);
+ value << ")\n"
+ << m.block(0, 0, shownRows, cols) << "\n...\n"
+ << m.block(rows - shownRows, 0, shownRows, cols);
+ }
+ else
+ {
+ value << m.format(eigenIof);
+ }
}
-}
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
index 1595e6ffb601278cb186d61f3a5f2535ff8bbcfb..26d8aae07a87dbc82bb3b44e9e8154f3cfc8315f 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
@@ -4,20 +4,18 @@
#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+#include "StringStreamMixin.h"
namespace armarx::aron
{
- class DataDisplayVisitor : public aron::data::ConstVariantVisitor
+ class DataDisplayVisitor : public aron::data::ConstVariantVisitor, public StringStreamMixin
{
public:
static std::string getValue(const data::VariantPtr& n);
public:
-
- std::stringstream value;
-
void visitDict(const data::VariantPtr& n) override;
void visitList(const data::VariantPtr& n) override;
@@ -30,6 +28,11 @@ namespace armarx::aron
void visitNDArray(const data::VariantPtr& n) override;
+ private:
+ template <typename ScalarT>
+ void processMatrix(const data::NDArray& data);
+
+ void printShape(const data::NDArray& data);
};
-}
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/StringStreamMixin.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/StringStreamMixin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..768a69b885efa7ae7aa36d8534cb52ece737dd00
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/StringStreamMixin.cpp
@@ -0,0 +1,24 @@
+#include "StringStreamMixin.h"
+
+#include <iomanip> // std::setprecision
+
+namespace armarx::aron
+{
+ StringStreamMixin::StringStreamMixin() :
+ coeffSep(" "), eigenIof(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", "")
+ {
+ }
+
+ void
+ StringStreamMixin::setStreamPrecision()
+ {
+ setStreamPrecision(value);
+ }
+
+ void
+ StringStreamMixin::setStreamPrecision(std::ostream& os)
+ {
+ os << std::setprecision(2) << std::fixed;
+ }
+
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/StringStreamMixin.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/StringStreamMixin.h
new file mode 100644
index 0000000000000000000000000000000000000000..01dfb9d91bef2208400c7a25cad3c5cfa29b8df5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/StringStreamMixin.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <sstream>
+
+#include <Eigen/Core>
+
+namespace armarx::aron
+{
+
+ class StringStreamMixin
+ {
+ public:
+ StringStreamMixin();
+
+ protected:
+ void setStreamPrecision();
+ void setStreamPrecision(std::ostream& os);
+
+ public:
+ std::stringstream value;
+
+ protected:
+ const std::string coeffSep;
+ const Eigen::IOFormat eigenIof;
+ };
+
+} // namespace armarx::aron
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 6e2e92a7cea71f53f2eb16f4ea369f53176f7079..b4c59389a918789222beb3595a29ec49320775df 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
@@ -24,8 +24,7 @@ namespace armarx::aron
return v.value.str();
}
- TypedDataDisplayVisitor::TypedDataDisplayVisitor() :
- coeffSep(" "), eigenIof(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", "")
+ TypedDataDisplayVisitor::TypedDataDisplayVisitor()
{
}
@@ -91,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);
@@ -104,7 +114,9 @@ namespace armarx::aron
TypedDataDisplayVisitor::processMatrix(const type::Matrix& type, const data::NDArray& data)
{
Eigen::Map<Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic>> m(
- reinterpret_cast<ScalarT*>(data.getData()), type.getRows(), type.getCols());
+ reinterpret_cast<ScalarT*>(data.getData()),
+ data.getShape().at(0),
+ data.getShape().at(1));
value << m.format(eigenIof);
}
@@ -158,28 +170,7 @@ namespace armarx::aron
void
TypedDataDisplayVisitor::visitMatrix(const data::VariantPtr& data, const type::VariantPtr& type)
{
- auto t = *type::Matrix::DynamicCastAndCheck(type);
- auto d = *data::NDArray::DynamicCastAndCheck(data);
-
- if (std::max(t.getRows(), t.getCols()) > 10)
- {
- // Just show the shape.
- value << DataDisplayVisitor::getValue(data);
- }
- else if (d.getType() == "float")
- {
- setStreamPrecision();
- processMatrix<float>(t, d);
- }
- else if (d.getType() == "double")
- {
- setStreamPrecision();
- processMatrix<double>(t, d);
- }
- else
- {
- value << DataDisplayVisitor::getValue(data);
- }
+ value << DataDisplayVisitor::getValue(data);
}
void
@@ -204,16 +195,4 @@ namespace armarx::aron
value << DataDisplayVisitor::getValue(data);
}
- void
- TypedDataDisplayVisitor::setStreamPrecision()
- {
- setStreamPrecision(value);
- }
-
- void
- TypedDataDisplayVisitor::setStreamPrecision(std::ostream& os)
- {
- os << std::setprecision(2) << std::fixed;
- }
-
} // namespace armarx::aron
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 9d0193e376c3ddf9d346d9f3d2114be547138fcb..c5a6fefc266f3bdda9dfde3942554334473f02ea 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
@@ -7,25 +7,22 @@
#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+#include "StringStreamMixin.h"
namespace armarx::aron
{
- class TypedDataDisplayVisitor : public aron::data::ConstTypedVariantVisitor
+ class TypedDataDisplayVisitor :
+ public aron::data::ConstTypedVariantVisitor,
+ public StringStreamMixin
{
public:
-
static std::string getValue(const type::VariantPtr& type, const data::VariantPtr& data);
public:
-
TypedDataDisplayVisitor();
-
- std::stringstream value;
-
-
void visitDict(const data::VariantPtr& data, const type::VariantPtr& type) override;
void visitObject(const data::VariantPtr& data, const type::VariantPtr& type) override;
@@ -40,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;
@@ -48,14 +47,7 @@ namespace armarx::aron
// What about NDArray? currently defaulted
- protected:
-
- std::string coeffSep;
- Eigen::IOFormat eigenIof;
-
-
private:
-
template <typename ScalarT>
void processMatrix(const type::Matrix&, const data::NDArray& data);
void processQuaternion(const data::NDArray& data);
@@ -63,10 +55,6 @@ namespace armarx::aron
void processPose(const type::Matrix&, const data::NDArray& data);
void processPosition(const type::Matrix&, const data::NDArray& data);
void processOrientation(const type::Quaternion&, const data::NDArray& data);
-
- void setStreamPrecision();
- void setStreamPrecision(std::ostream& os);
-
};
-}
+} // namespace armarx::aron
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_gui/test/ArMemGuiTest.cpp b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
index 627d1a2ea804d2aa38135d5fc670dcee44145b73..b8f3f0fe967212288b6385c99d812bf7e34e8124 100644
--- a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
+++ b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
@@ -36,14 +36,14 @@
namespace dn = armarx::aron::data;
namespace tn = armarx::aron::type;
-
namespace ArMemGuiTest
{
struct Fixture
{
};
- void test_sanitize(const std::string& in, const std::string& expected)
+ void
+ test_sanitize(const std::string& in, const std::string& expected)
{
const std::string out = armarx::armem::gui::instance::sanitizeTypeName(in);
BOOST_TEST_CONTEXT("in = '" << in << "'")
@@ -51,13 +51,11 @@ namespace ArMemGuiTest
BOOST_CHECK_EQUAL(out, expected);
}
}
-}
-
+} // namespace ArMemGuiTest
BOOST_FIXTURE_TEST_SUITE(ArMemGuiTest, ArMemGuiTest::Fixture)
-
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_int)
{
test_sanitize(tn::Int().getFullName(), "Int");
@@ -72,7 +70,8 @@ BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_float)
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_dict)
{
- tn::Dict dict(std::make_shared<tn::Float>());
+ tn::Dict dict;
+ dict.setAcceptedType(std::make_shared<tn::Float>());
test_sanitize(dict.getFullName(), "Dict<Float>");
test_sanitize(dn::Dict().getFullName(), "Dict");
@@ -80,7 +79,8 @@ BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_dict)
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_list)
{
- tn::List list(std::make_shared<tn::Long>());
+ tn::List list;
+ list.setAcceptedType(std::make_shared<tn::Long>());
test_sanitize(list.getFullName(), "List<Long>");
test_sanitize(dn::List().getFullName(), "List");
@@ -88,20 +88,24 @@ BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_list)
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_object)
{
- tn::Object obj("namespace::MyObjectName");
+ tn::Object obj;
+ obj.setObjectName("namespace::MyObjectName");
test_sanitize(obj.getFullName(), "MyObjectName (namespace)");
}
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_tuple)
{
std::vector<tn::VariantPtr> ac = {std::make_shared<tn::Int>(), std::make_shared<tn::Float>()};
- tn::Tuple type(ac);
+ tn::Tuple type;
+ type.setAcceptedTypes(ac);
test_sanitize(type.getFullName(), "Tuple<Int, Float>");
}
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_pair)
{
- tn::Pair type(std::make_shared<tn::Int>(), std::make_shared<tn::Float>());
+ tn::Pair type;
+ type.setFirstAcceptedType(std::make_shared<tn::Int>());
+ type.setSecondAcceptedType(std::make_shared<tn::Float>());
test_sanitize(type.getFullName(), "Pair<Int, Float>");
}
@@ -115,7 +119,7 @@ BOOST_AUTO_TEST_CASE(test_sanitizeTypeName_ndarry)
}
{
dn::NDArray data;
- data.setShape({ 3, 2, 1});
+ data.setShape({3, 2, 1});
test_sanitize(data.getFullName(), "NDArray<3, 2, 1, >");
}
}
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 9a7aa7e5aa8a97032bd5ab9dcbe245b693a52cc3..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
@@ -80,7 +77,7 @@ namespace armarx::armem::laser_scans::client
toAron(laserScan, timestamp, frame, agentName, aronSensorData);
auto dict = aronSensorData.toAron();
- dict->addElement("scan", toAron(laserScan));
+ dict->addElementCopy("scan", toAron(laserScan));
update.instancesData = {dict};
update.referencedTime = timestamp;
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_laser_scans/server/Visu.cpp b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
index 551e4792c2eba3b5e19d2ebf468f2d090a53af41..ff8a7c9ca7437271d6e5fdabdf6e848bb771add1 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
@@ -1,7 +1,5 @@
#include "Visu.h"
-#include <SimoxUtility/color/Color.h>
-#include <SimoxUtility/color/ColorMap.h>
#include <algorithm>
#include <exception>
#include <string>
@@ -10,6 +8,9 @@
#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/color/ColorMap.h>
+#include <SimoxUtility/color/hsv.h>
#include <SimoxUtility/math/pose.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -32,33 +33,34 @@
namespace armarx::armem::server::laser_scans
{
+
Visu::Visu()
{
Logging::setTag("Visu");
}
-
void
Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(
p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
- defs->optional(p.uniformColor, prefix + "uniformColor", "If enabled, points will be drawn in red.");
+ defs->optional(
+ p.uniformColor, prefix + "uniformColor", "If enabled, points will be drawn in red.");
defs->optional(p.maxRobotAgeMs,
prefix + "maxRobotAgeMs",
"Maximum age of robot state before a new one is retrieved in milliseconds.");
+ defs->optional(p.colorByIntensity, "colorByIntensity", "");
}
-
void
- Visu::init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader)
+ Visu::init(const wm::CoreSegment* coreSegment,
+ armem::robot_state::VirtualRobotReader* virtualRobotReader)
{
this->coreSegment = coreSegment;
this->virtualRobotReader = virtualRobotReader;
}
-
void
Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
{
@@ -79,7 +81,6 @@ namespace armarx::armem::server::laser_scans
updateTask->start();
}
-
void
Visu::visualizeRun()
{
@@ -114,9 +115,10 @@ namespace armarx::armem::server::laser_scans
}
void
- Visu::visualizeScan(const std::vector<Eigen::Vector3f>& points,
+ Visu::visualizeScan(const std::vector<ScanPoint>& points,
const std::string& sensorName,
- const std::string& agentName, const viz::Color& color)
+ const std::string& agentName,
+ const viz::Color& color)
{
viz::PointCloud pointCloud("laser_scan");
@@ -124,7 +126,30 @@ namespace armarx::armem::server::laser_scans
for (const auto& point : points)
{
- pointCloud.addPoint(point.x(), point.y(), point.z(), color);
+
+ // ARMARX_INFO << point.intensity;
+ const viz::Color specificColor = [&point, &color, this]() -> viz::Color
+ {
+ if (p.colorByIntensity)
+ {
+ Eigen::Vector3f hsv = simox::color::rgb_to_hsv(
+ Eigen::Vector3f(static_cast<float>(color.r) / 255.f,
+ static_cast<float>(color.g) / 255.f,
+ static_cast<float>(color.b) / 255.f));
+
+ // ARMARX_INFO << point.intensity;
+
+ hsv(2) = std::clamp<float>(point.intensity, 0., 1.);
+
+ const Eigen::Vector3f rgb = simox::color::hsv_to_rgb(hsv);
+
+ return viz::Color{rgb(0), rgb(1), rgb(2)};
+ }
+
+ return color;
+ }();
+
+ pointCloud.addPoint(point.point.x(), point.point.y(), point.point.z(), specificColor);
}
pointCloud.pointSizeInPixels(3);
@@ -135,22 +160,28 @@ namespace armarx::armem::server::laser_scans
arviz.commit(l);
}
- std::vector<Eigen::Vector3f>
+ std::vector<ScanPoint>
convertScanToGlobal(const armem::laser_scans::LaserScanStamped& scan,
const Eigen::Isometry3f& global_T_sensor)
{
- auto scanCartesian =
+ const auto scanCartesian =
armarx::armem::laser_scans::util::toCartesian<Eigen::Vector3f>(scan.data);
- for (auto& point : scanCartesian)
+ std::vector<ScanPoint> points;
+ points.reserve(scan.data.size());
+
+ for (std::size_t i = 0; i < scan.data.size(); i++)
{
- point = global_T_sensor * point;
+ const auto& point = scanCartesian.at(i);
+ const auto& raw = scan.data.at(i);
+
+ const Eigen::Vector3f pointGlobal = global_T_sensor * point;
+ points.push_back(ScanPoint{.point = pointGlobal, .intensity = raw.intensity});
}
- return scanCartesian;
+ return points;
}
-
// void Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
// {
// coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment & provSegment)
@@ -263,31 +294,35 @@ namespace armarx::armem::server::laser_scans
{
ARMARX_VERBOSE << "Visualizing `" << provider << "`";
- const auto global_T_sensor = [&]() -> Eigen::Isometry3f{
-
+ const auto global_T_sensor = [&]() -> Eigen::Isometry3f
+ {
const auto robot = getSynchronizedRobot(scan.header.agent, scan.header.timestamp);
- if(not robot)
+ if (not robot)
{
- ARMARX_VERBOSE << deactivateSpam(1) << "Robot `" << scan.header.agent << "`" << "not available";
+ ARMARX_VERBOSE << deactivateSpam(1) << "Robot `" << scan.header.agent << "`"
+ << "not available";
return Eigen::Isometry3f::Identity();
}
const auto sensorNode = robot->getRobotNode(scan.header.frame);
ARMARX_CHECK_NOT_NULL(sensorNode) << "No robot node `" << scan.header.frame
- << "` for robot `" << scan.header.agent << "`";
+ << "` for robot `" << scan.header.agent << "`";
- ARMARX_VERBOSE << "Sensor position for sensor `" << scan.header.frame << "` is " << sensorNode->getGlobalPosition();
+ ARMARX_VERBOSE << "Sensor position for sensor `" << scan.header.frame << "` is "
+ << sensorNode->getGlobalPosition();
return Eigen::Isometry3f{sensorNode->getGlobalPose()};
}();
- const std::vector<Eigen::Vector3f> points = convertScanToGlobal(scan, global_T_sensor);
- const auto color = [&]() -> simox::Color{
- if(p.uniformColor)
+ const std::vector<ScanPoint> points = convertScanToGlobal(scan, global_T_sensor);
+
+ const auto color = [&]() -> simox::Color
+ {
+ if (p.uniformColor)
{
return simox::Color::red();
}
-
+
return simox::color::GlasbeyLUT::at(i++);
}();
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
index d6c79dd12d9aaf7b85dfad6eacaf436e7e6f59c1..5218e6a0f5495684f2d2594d9495c2e0e31ab0fc 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
@@ -33,14 +33,18 @@
#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+#include <RobotAPI/libraries/armem_laser_scans/types.h>
#include <RobotAPI/libraries/armem_objects/types.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
-#include <RobotAPI/libraries/armem_laser_scans/types.h>
-#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
-
namespace armarx::armem::server::laser_scans
{
+ struct ScanPoint
+ {
+ Eigen::Vector3f point;
+ float intensity;
+ };
/**
* @brief Models decay of object localizations by decreasing the confidence
@@ -54,7 +58,8 @@ namespace armarx::armem::server::laser_scans
void defineProperties(armarx::PropertyDefinitionsPtr defs,
const std::string& prefix = "visu.");
- void init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader);
+ void init(const wm::CoreSegment* coreSegment,
+ armem::robot_state::VirtualRobotReader* virtualRobotReader);
void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
@@ -70,11 +75,11 @@ namespace armarx::armem::server::laser_scans
{
bool enabled = true;
bool uniformColor = false;
+ bool colorByIntensity = true;
float frequencyHz = 5;
int maxRobotAgeMs = 100;
} p;
-
SimpleRunningTask<>::pointer_type updateTask;
armem::robot_state::VirtualRobotReader* virtualRobotReader;
@@ -84,7 +89,7 @@ namespace armarx::armem::server::laser_scans
VirtualRobot::RobotPtr getSynchronizedRobot(const std::string& name,
const DateTime& timestamp);
- void visualizeScan(const std::vector<Eigen::Vector3f>& points,
+ void visualizeScan(const std::vector<ScanPoint>& points,
const std::string& sensorName,
const std::string& agentName,
const viz::Color& color);
diff --git a/source/RobotAPI/libraries/armem_locations/CMakeLists.txt b/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
index 22d418142d784080f8f4f99410a25a272d5c628f..38d927aa828a2e1d554492527093c0cb21b09035 100644
--- a/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
@@ -11,12 +11,20 @@ armarx_add_library(
RobotAPI::Core
RobotAPI::armem
+
+ armem_objects
+ armem_robot_state
+
# aronjsonconverter
SOURCES
./aron_conversions.cpp
+ ./frame_conversions.cpp
+ ./client/Reader.cpp
HEADERS
./aron_conversions.h
+ ./frame_conversions.h
+ ./client/Reader.h
)
armarx_enable_aron_file_generation_for_target(
diff --git a/source/RobotAPI/libraries/armem_locations/aron/Location.xml b/source/RobotAPI/libraries/armem_locations/aron/Location.xml
index 9a0539d5d40a3b3b91dc32ad2034077a815437ff..d53f51c2c8f75bacae2868ae9bc155aad717379e 100644
--- a/source/RobotAPI/libraries/armem_locations/aron/Location.xml
+++ b/source/RobotAPI/libraries/armem_locations/aron/Location.xml
@@ -1,41 +1,12 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- </CodeIncludes>
- <AronIncludes>
- <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true"/>
- </AronIncludes>
<GenerateTypes>
- <!--
- ToDo: Model regions. Ideas:
- - Polygon (convex, non-convex)
- -
- -->
-
-
- <Object name='armarx::navigation::location::arondto::ObjectRelativeLocation'>
-
- <ObjectChild key='objectInstanceID'>
- <armarx::armem::arondto::MemoryID />
- </ObjectChild>
-
- <ObjectChild key='relativeRobotPose'>
- <Pose />
- </ObjectChild>
-
- </Object>
-
-
<Object name='armarx::navigation::location::arondto::Location'>
- <ObjectChild key='globalRobotPose'>
- <Pose />
- </ObjectChild>
-
- <ObjectChild key='relativeToObject'>
- <armarx::navigation::location::arondto::ObjectRelativeLocation optional="true" />
+ <ObjectChild key='framedPose'>
+ <FramedPose />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..13cc8ff5a3e515e79e80838a875acad24a6186cd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
@@ -0,0 +1,101 @@
+#include "Reader.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/util/util.h>
+
+#include "../frame_conversions.h"
+
+namespace armarx::armem::locations::client
+{
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ const std::string prefix = propertyPrefix;
+
+ def->optional(p.memoryName, prefix + "MemoryName");
+ objReader.registerPropertyDefinitions(def);
+ robotReader.registerPropertyDefinitions(def);
+ }
+
+ void
+ 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 << "' ...";
+ try
+ {
+ // simply wait until memory is ready. Do nothing with reader but get prx
+ locationReader = memoryNameSystem.useReader(p.memoryName);
+
+ ARMARX_IMPORTANT << "Connected to Memory '" << p.memoryName << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+
+ objReader.connect(memoryNameSystem);
+ robotReader.connect(memoryNameSystem);
+ }
+
+ std::map<std::string, armarx::navigation::location::arondto::Location>
+ Reader::getAllLocations()
+ {
+ auto mId = armarx::armem::MemoryID{p.memoryName, "Location"};
+ auto res = locationReader.getLatestSnapshotsIn(mId);
+
+ if (res.success)
+ {
+ std::map<std::string, armarx::navigation::location::arondto::Location> ret;
+ res.memory.forEachEntity(
+ [&ret](const armarx::armem::wm::Entity& e)
+ {
+ auto i = e.findLatestInstance();
+ if (i)
+ {
+ auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
+ ret[i->id().providerSegmentName + "/" + i->id().entityName] = loc;
+ }
+ });
+ return ret;
+ }
+
+ throw error::ArMemError(res.errorMessage);
+ }
+
+ std::map<std::string, armarx::navigation::location::arondto::Location>
+ Reader::getAllLocationsInGlobalFrame()
+ {
+ auto locations = this->getAllLocations();
+ auto objects = objReader.queryLatestObjectInstances();
+ auto robotDecs = robotReader.queryDescriptions(armarx::core::time::DateTime::Now());
+
+ for (auto& [locName, location] : locations)
+ {
+ (void)locName;
+ if (location.framedPose.header.frame == armarx::GlobalFrame)
+ {
+ location.framedPose.header.agent = ""; //sanity set
+ continue;
+ }
+ // check if we have an object that matches the relative location
+ for (const auto& [objectInstanceId, object] : objects)
+ {
+ if (simox::alg::starts_with(objectInstanceId, location.framedPose.header.agent))
+ {
+ toGlobal(location, object);
+ }
+ }
+
+ // TODO: check if we have an robot that matches the relative location
+ }
+
+ return locations;
+ }
+
+
+} // namespace armarx::armem::locations::client
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.h b/source/RobotAPI/libraries/armem_locations/client/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..7a25269d181a9079ec0f770f27fd240d25d7a5af
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.h
@@ -0,0 +1,53 @@
+#pragma once
+
+#include <mutex>
+#include <optional>
+
+#include <Eigen/Core>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+
+namespace armarx::armem::locations::client
+{
+ class Reader
+ {
+ public:
+ struct Properties
+ {
+ std::string memoryName = "Navigation";
+ };
+
+ Reader() = default;
+ virtual ~Reader() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
+
+ Properties
+ getProperties()
+ {
+ return this->p;
+ }
+
+ std::map<std::string, armarx::navigation::location::arondto::Location> getAllLocations();
+ std::map<std::string, armarx::navigation::location::arondto::Location>
+ getAllLocationsInGlobalFrame();
+
+ private:
+ Properties p;
+
+ const std::string propertyPrefix = "mem.nav.location";
+
+ armarx::armem::obj::instance::Reader objReader;
+ armarx::armem::robot_state::RobotReader robotReader;
+
+ armarx::armem::client::Reader locationReader;
+ };
+
+} // namespace armarx::armem::locations::client
diff --git a/source/RobotAPI/libraries/armem_locations/frame_conversions.cpp b/source/RobotAPI/libraries/armem_locations/frame_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..50ef19ce29d8432970e47071ac3dc4358cef6745
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/frame_conversions.cpp
@@ -0,0 +1,6 @@
+#include "aron_conversions.h"
+
+namespace armarx::armem
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_locations/frame_conversions.h b/source/RobotAPI/libraries/armem_locations/frame_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..876e6f9529fb83c88c5f7575daed6c7437f4b3f3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/frame_conversions.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem_locations/aron/Location.aron.generated.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+namespace armarx::armem::locations
+{
+ void
+ toGlobal(armarx::navigation::location::arondto::Location& framedLoc,
+ const Eigen::Matrix4f& framedToGlobal)
+ {
+ if (framedLoc.framedPose.header.frame == armarx::GlobalFrame)
+ {
+ // No need to change to global
+ framedLoc.framedPose.header.agent = ""; // sanity set
+ return;
+ }
+
+ framedLoc.framedPose.pose = framedToGlobal * framedLoc.framedPose.pose;
+ framedLoc.framedPose.header.frame = armarx::GlobalFrame;
+ framedLoc.framedPose.header.agent = "";
+ }
+
+ void
+ toGlobal(armarx::navigation::location::arondto::Location& framedLoc,
+ const armarx::objpose::ObjectPose& objPose)
+ {
+ if (framedLoc.framedPose.header.frame == armarx::GlobalFrame)
+ {
+ // No need to change to global
+ framedLoc.framedPose.header.agent = ""; // sanity set
+ return;
+ }
+
+ if (simox::alg::starts_with(objPose.objectID.str(), framedLoc.framedPose.header.agent))
+ {
+ ARMARX_CHECK(framedLoc.framedPose.header.frame ==
+ "root"); // TODO: Right now we only allow root
+ toGlobal(framedLoc, objPose.objectPoseGlobal);
+ return;
+ }
+
+ throw armarx::LocalException(
+ "Could not convert a framedLocation because the used object is wrong!");
+ }
+
+} // namespace armarx::armem::locations
diff --git a/source/RobotAPI/libraries/armem_motions/aron/MDBReference.xml b/source/RobotAPI/libraries/armem_motions/aron/MDBReference.xml
index a03dc5b5d0ce7cb25e1b7a94feb41a3f04bff6d3..e89a40c946fe96ecae9afa155a6cecdaf14395b7 100644
--- a/source/RobotAPI/libraries/armem_motions/aron/MDBReference.xml
+++ b/source/RobotAPI/libraries/armem_motions/aron/MDBReference.xml
@@ -26,7 +26,7 @@
</ObjectChild>
<ObjectChild key='attachedToId'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='description'>
@@ -45,21 +45,21 @@
<Object name='armarx::motion::mdb::arondto::MDBReference'>
<ObjectChild key='id'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='associatedInstitution'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='motionDescriptions'>
<List>
- <int />
+ <int32 />
</List>
</ObjectChild>
<ObjectChild key='associatedProject'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='date'>
@@ -67,7 +67,7 @@
</ObjectChild>
<ObjectChild key='associatedProject'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='createdDate'>
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 23b84e31aef6972059feb8cdf26f75fe3d4271ab..6b3f3c184edb32070f6d7f28c853f340bcd78d32 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -31,7 +31,6 @@ namespace armarx::armem
objpose::toAron(dto.pose, bo);
}
-
/* Attachments */
void
fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo)
@@ -47,7 +46,6 @@ namespace armarx::armem
aron::toAron(dto.frame, bo.frame);
}
-
void
fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo)
{
@@ -68,7 +66,6 @@ namespace armarx::armem
// TODO aron::toAron(dto.timestamp, bo.timestamp);
}
-
void
fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
attachment::ArticulatedObjectAttachment& bo)
@@ -95,21 +92,21 @@ namespace armarx::armem
toAron(arondto::Marker& dto, const marker::Marker& bo)
{
dto.name = bo.name;
- dto.robotGlobal = bo.robotGlobal;
- dto.rgbCamera = bo.rgbCamera;
- dto.depthCamera = bo.depthCamera;
- dto.markerPose = bo.markerPose;
- dto.markerGlobal = bo.getGlobalMarkerPose();
+ armarx::toAron(dto.robotGlobal, bo.robotGlobal);
+ armarx::toAron(dto.rgbCamera, bo.rgbCamera);
+ armarx::toAron(dto.depthCamera, bo.depthCamera);
+ armarx::toAron(dto.markerPose, bo.markerPose);
+ armarx::toAron(dto.markerGlobal, bo.getGlobalMarkerPose());
}
void
fromAron(const arondto::Marker& dto, marker::Marker& bo)
{
bo.name = dto.name;
- bo.robotGlobal = dto.robotGlobal;
- bo.rgbCamera = dto.rgbCamera;
- bo.depthCamera = dto.depthCamera;
- bo.markerPose = dto.markerPose;
+ armarx::fromAron(dto.robotGlobal, bo.robotGlobal);
+ armarx::fromAron(dto.rgbCamera, bo.rgbCamera);
+ armarx::fromAron(dto.depthCamera, bo.depthCamera);
+ armarx::fromAron(dto.markerPose, bo.markerPose);
}
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index a2b0ddd6b2985997156cfa6c82998de249409e5a..093284885bc00e2b50688234c76e94c79795db21 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -11,12 +11,12 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.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_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
@@ -24,42 +24,68 @@
namespace armarx::armem::articulated_object
{
- VirtualRobot::RobotPtr ArticulatedObjectReader::getArticulatedObject(
- const std::string& name,
- const armem::Time& timestamp)
+ VirtualRobot::RobotPtr
+ ArticulatedObjectReader::getArticulatedObject(const std::string& typeName,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName,
+ const std::string& instanceName)
{
- const auto descriptions = queryDescriptions(armem::Time::Now());
+ const auto descriptions = queryDescriptions(timestamp, providerName);
ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
const auto it = std::find_if(
- descriptions.begin(),
- descriptions.end(),
- [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
- { return desc.name == name; });
+ descriptions.begin(),
+ descriptions.end(),
+ [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
+ { return desc.name == typeName; });
if (it == descriptions.end())
{
- ARMARX_WARNING << "Articulated object " << name << " not (yet) available";
+ ARMARX_WARNING << "Description for articulate object with type <" << typeName
+ << "> not (yet) available!";
return nullptr;
}
- ARMARX_DEBUG << "Object " << name << " available";
+ ARMARX_DEBUG << "Description for articulate object with type <" << typeName
+ << "> available!";
- auto obj = VirtualRobot::RobotIO::loadRobot(
- it->xml.toSystemPath(),
- VirtualRobot::RobotIO::eStructure);
+ auto obj = VirtualRobot::RobotIO::loadRobot(it->xml.toSystemPath(),
+ VirtualRobot::RobotIO::eStructure);
if (not obj)
{
- ARMARX_WARNING << "Failed to load object: " << name;
+ ARMARX_WARNING << "Failed to load description for articulated object <" << typeName
+ << ">!";
return nullptr;
}
- obj->setName("");
+ obj->setName(instanceName);
obj->setType(it->name);
return obj;
}
+ bool
+ ArticulatedObjectReader::synchronizeArticulatedObject(
+ VirtualRobot::Robot& object,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
+ {
+ const auto objectState =
+ queryState(object.getType() + "/" + object.getName(), timestamp, providerName);
+ if (not objectState)
+ {
+ ARMARX_VERBOSE << deactivateSpam(5) << "Querying object state failed for object `"
+ << object.getName() << "` "
+ << "(type `" << object.getType() << "`)!";
+ return false;
+ }
+
+ object.setJointValues(objectState->jointMap);
+ object.setGlobalPose(objectState->globalPose.matrix());
+
+ return true;
+ }
+
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
index 65e8f2fba2ba962429e040e1d0e1eefc514e2355..ab0c5aa6d30a0bc0b377981c22f65d0d3b8c3ade 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
@@ -10,8 +10,13 @@ namespace armarx::armem::articulated_object
public:
using Reader::Reader;
- VirtualRobot::RobotPtr getArticulatedObject(
- const std::string& name,
- const armem::Time& timestamp);
+ VirtualRobot::RobotPtr getArticulatedObject(const std::string& typeName,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName,
+ const std::string& instanceName = "");
+
+ bool synchronizeArticulatedObject(VirtualRobot::Robot& object,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName);
};
} // namespace armarx::armem::articulated_object
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 9a4417080bb027e4ee8d4a9cf0c2dc080fca3890..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,18 +5,21 @@
#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>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
@@ -28,38 +31,17 @@ namespace fs = ::std::filesystem;
namespace armarx::armem::articulated_object
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
- void
- Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
- {
- ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
-
- const std::string prefix = propertyPrefix;
-
- def->optional(properties.memoryName, prefix + "MemoryName");
-
- def->optional(properties.coreInstanceSegmentName,
- prefix + "CoreSegment",
- "Name of the memory core segment to use for object instances.");
- def->optional(properties.coreClassSegmentName,
- prefix + "CoreSegment",
- "Name of the memory core segment to use for object classes.");
- def->optional(properties.providerName, prefix + "read.ProviderName");
- }
-
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 '" << properties.memoryName << "' ...";
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << objects::constants::MemoryName
+ << "' ...";
try
{
- memoryReader = memoryNameSystem.useReader(properties.memoryName);
- ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName << "'";
+ memoryReader = memoryNameSystem.useReader(objects::constants::MemoryName);
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << objects::constants::MemoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -68,8 +50,8 @@ namespace armarx::armem::articulated_object
}
armem::MemoryID id = armem::MemoryID();
- id.memoryName = properties.memoryName;
- id.coreSegmentName = properties.coreClassSegmentName;
+ id.memoryName = objects::constants::MemoryName;
+ id.coreSegmentName = objects::constants::CoreClassSegmentName;
// listen to all provider segments!
memoryNameSystem.subscribe(id, this, &Reader::updateKnownObjects);
@@ -112,7 +94,9 @@ namespace armarx::armem::articulated_object
}
std::optional<ArticulatedObject>
- Reader::get(const std::string& name, const armem::Time& timestamp)
+ 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`";
@@ -120,38 +104,41 @@ namespace armarx::armem::articulated_object
const std::string className = splits.at(0) + "/" + splits.at(1); // `DATASET/NAME`
const std::string instanceName = splits.at(2);
- const auto description = queryDescription(name, timestamp);
+ const auto description = queryDescription(className, timestamp, providerName);
if (not description)
{
- ARMARX_WARNING << "Unknown object " << name;
+ ARMARX_WARNING << "Unknown object " << className;
return std::nullopt;
}
- return get(*description, timestamp, instanceName);
+ return get(*description, timestamp, instanceName, providerName);
}
ArticulatedObject
Reader::get(const ArticulatedObjectDescription& description,
const armem::Time& timestamp,
- const std::string& instanceName)
+ const std::string& instanceName,
+ const std::optional<std::string>& providerName)
{
ArticulatedObject obj{.description = description,
.instance = instanceName,
.config = {}, // will be populated by `synchronize()`
.timestamp = timestamp};
- synchronize(obj, timestamp);
+ synchronize(obj, timestamp, providerName);
return obj;
}
bool
- Reader::synchronize(ArticulatedObject& obj, const armem::Time& timestamp)
+ 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!";
- auto state = queryState(obj.name(), timestamp);
+ auto state = queryState(obj.name(), timestamp, providerName);
if (not state) /* c++20 [[unlikely]] */
{
@@ -164,14 +151,15 @@ namespace armarx::armem::articulated_object
}
std::vector<robot::RobotDescription>
- Reader::queryDescriptions(const armem::Time& timestamp)
+ Reader::queryDescriptions(const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
// Query all entities from provider.
armem::client::query::Builder qb;
// clang-format off
qb
- .coreSegments().withName(properties.coreClassSegmentName)
+ .coreSegments().withName(objects::constants::CoreClassSegmentName)
.providerSegments().all()
.entities().all()
.snapshots().latest(); // TODO beforeTime(timestamp);
@@ -189,33 +177,35 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
- std::string
- Reader::getProviderName() const
- {
- return properties.providerName;
- }
-
- void
- Reader::setProviderName(const std::string& providerName)
- {
- this->properties.providerName = providerName;
- }
-
std::optional<robot::RobotDescription>
- Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ Reader::queryDescription(const std::string& name,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName)
{
- // FIXME: why is `name` unused?
-
// Query all entities from provider.
armem::client::query::Builder qb;
- // clang-format off
- qb
- .coreSegments().withName(properties.coreClassSegmentName)
- .providerSegments().withName(properties.providerName)
- .entities().all() // withName(name)
- .snapshots().latest();
- // clang-format on
+ if (providerName.has_value()) // query single provider
+ {
+ // clang-format off
+ qb
+ .coreSegments().withName(objects::constants::CoreClassSegmentName)
+ .providerSegments().withName(providerName.value())
+ .entities().withName(name)
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+ }
+ else // query all providers
+ {
+ // clang-format off
+ qb
+ .coreSegments().withName(objects::constants::CoreClassSegmentName)
+ .providerSegments().all()
+ .entities().withName(name)
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+ }
+
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -231,7 +221,9 @@ namespace armarx::armem::articulated_object
}
std::optional<robot::RobotState>
- Reader::queryState(const std::string& instanceName, const armem::Time& timestamp)
+ 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?
@@ -240,7 +232,7 @@ namespace armarx::armem::articulated_object
// clang-format off
qb
- .coreSegments().withName(properties.coreInstanceSegmentName)
+ .coreSegments().withName(objects::constants::CoreInstanceSegmentName)
.providerSegments().all() // withName(properties.providerName) // agent
.entities().withName(instanceName)
.snapshots().latest();
@@ -258,7 +250,6 @@ namespace armarx::armem::articulated_object
return getArticulatedObjectState(qResult.memory);
}
-
std::optional<robot::RobotState>
convertToRobotState(const armem::wm::EntityInstance& instance)
{
@@ -278,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 +280,7 @@ namespace armarx::armem::articulated_object
{
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
- .getCoreSegment(properties.coreInstanceSegmentName);
+ .getCoreSegment(objects::constants::CoreInstanceSegmentName);
// clang-format on
std::optional<wm::EntityInstance> instance;
@@ -304,12 +296,11 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
-
std::optional<robot::RobotDescription>
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
- const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+ const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(objects::constants::CoreClassSegmentName);
// clang-format on
std::optional<wm::EntityInstance> instance;
@@ -324,12 +315,11 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
-
std::vector<robot::RobotDescription>
Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
{
const armem::wm::CoreSegment& coreSegment =
- memory.getCoreSegment(properties.coreClassSegmentName);
+ memory.getCoreSegment(objects::constants::CoreClassSegmentName);
std::vector<robot::RobotDescription> descriptions;
coreSegment.forEachEntity(
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 91a38432faba65f7fe4404986042d1c953c75f70..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,31 +39,44 @@ 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) override;
+ bool synchronize(ArticulatedObject& obj,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName) override;
- std::optional<ArticulatedObject> get(const std::string& name,
- const armem::Time& timestamp) override;
+ std::optional<ArticulatedObject>
+ get(const std::string& name,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName) override;
ArticulatedObject get(const ArticulatedObjectDescription& description,
- const armem::Time& timestamp, const std::string& instanceName) override;
+ const armem::Time& timestamp,
+ const std::string& instanceName,
+ const std::optional<std::string>& providerName) override;
- std::optional<robot::RobotState> queryState(const std::string &instanceName,
- const armem::Time& timestamp);
- std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp);
+ std::optional<robot::RobotState> queryState(const std::string& instanceName,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName);
+ std::optional<robot::RobotDescription>
+ queryDescription(const std::string& name,
+ const armem::Time& timestamp,
+ const std::optional<std::string>& providerName);
- std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
+ std::vector<robot::RobotDescription>
+ queryDescriptions(const armem::Time& timestamp,
+ const std::optional<std::string>& providerName);
std::string getProviderName() const;
void setProviderName(const std::string& providerName);
- // TODO(fabian.reister): register property defs
-
protected:
std::optional<robot::RobotState>
getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const;
@@ -77,18 +90,8 @@ namespace armarx::armem::articulated_object
const std::vector<armem::MemoryID>& snapshotIDs);
void updateKnownObject(const armem::MemoryID& snapshotId);
- struct Properties
- {
- std::string memoryName = "Object";
- std::string coreInstanceSegmentName = "Instance";
- std::string coreClassSegmentName = "Class";
- std::string providerName = "PriorKnowledgeData";
- } properties;
-
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 80c3168c5fc5e7c893c2813082171e5cd9c6319b..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);
@@ -231,9 +237,11 @@ namespace armarx::armem::articulated_object
cs.dataset = objectId.dataset();
objectInstance.pose.objectID = cs;
+ objectInstance.pose.providerName = properties.providerName;
+ 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);
@@ -248,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);
@@ -290,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/articulated_object/interfaces.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
index d7e74a9a02db0d58d4a24ef3f88658b3d13ca57a..6031eba4bfd1288d6567b946a0dd359018482792 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/interfaces.h
@@ -11,10 +11,10 @@ namespace armarx::armem::articulated_object
public:
virtual ~ReaderInterface() = default;
- virtual bool synchronize(ArticulatedObject& obj, const armem::Time& timestamp) = 0;
+ virtual bool synchronize(ArticulatedObject& obj, const armem::Time& timestamp, const std::optional<std::string>& providerName) = 0;
- virtual ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp, const std::string& instanceName) = 0;
- virtual std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) = 0;
+ virtual ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp, const std::string& instanceName, const std::optional<std::string>& providerName) = 0;
+ virtual std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp, const std::optional<std::string>& providerName) = 0;
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index a515fe725b51ff8302a129053996e97d6738872c..399fce56d0cc22c10de24a8b1a4ab29eb6668581 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -3,50 +3,48 @@
#include <mutex>
#include <optional>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.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
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem, const objpose::ObjectPoseProviderPrx& objPoseProvider) :
- memoryNameSystem(memoryNameSystem), objPoseProvider(objPoseProvider)
- {}
-
- void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
- ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
-
const std::string prefix = propertyPrefix;
- def->optional(properties.memoryName, prefix + "MemoryName");
-
- def->optional(properties.coreSegmentName,
- prefix + "CoreSegment",
- "Name of the memory core segment to use for object instances.");
+ def->optional(p.memoryName, prefix + "MemoryName");
}
-
- 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 << "' ...";
+ ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
try
{
- memoryReader = memoryNameSystem.useReader(properties.memoryName);
- ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName << "'";
+ // simply wait until memory is ready. Do nothing with reader but get prx
+ auto r = memoryNameSystem.useReader(p.memoryName);
+
+ // cast MemoryPrx to objPoseStoragePrx
+ this->objPoseStorage =
+ objpose::ObjectPoseStorageInterfacePrx::checkedCast(r.readingPrx);
+
+ ARMARX_IMPORTANT << "Connected to Memory and ObjectPoseStorage '" << p.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -55,97 +53,93 @@ namespace armarx::armem::obj::instance
}
}
- bool Reader::requestLocalization(const std::string& entityName, const armarx::core::time::Duration& until)
+ std::map<std::string, bool>
+ Reader::requestLocalization(const ObjectID& instanceId,
+ const armarx::core::time::Duration& until)
{
- auto entityNameParts = GetEntityNameParts(entityName);
-
- armarx::data::ObjectID requestObject({std::get<0>(entityNameParts), std::get<1>(entityNameParts), std::get<2>(entityNameParts)});
- armarx::objpose::provider::RequestObjectsOutput requestResult = objPoseProvider->requestObjects({{requestObject}, until.toMilliSeconds()});
-
- return requestResult.results.at(requestObject).success;
- }
-
- /// get the latest object from an memory and cast it to an ObjectInstance
- std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObject(const armem::wm::Memory& memory, const armem::Time& timestamp)
- {
- // clang-format off
- const armem::wm::CoreSegment& s = memory
- .getCoreSegment(properties.coreSegmentName);
- // clang-format on
-
- // What to do with timestamp?
- const armem::wm::EntityInstance* instance = nullptr;
- s.forEachInstance([&instance](const wm::EntityInstance& i)
- { instance = &i; });
- if (instance == nullptr)
+ std::map<std::string, bool> ret;
+ auto providers = objPoseStorage->getAvailableProvidersInfo();
+ for (const auto& [k, p] : providers)
{
- return std::nullopt;
+ // TODO: check supported objects?
+ ret[k] = this->requestLocalization(instanceId, k, until);
}
- return armem::arondto::ObjectInstance::FromAron(instance->data());
+ return ret;
}
- /// Query an object with full name entityName (e.g. Kitchen/green-cup/0)
- std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObjectByEntityID(const std::string& entityName, const armem::Time& timestamp)
+ bool
+ Reader::requestLocalization(const ObjectID& instanceId,
+ const std::string& provider,
+ const armarx::core::time::Duration& until)
{
- // Query all entities from all provider.
- armem::client::query::Builder qb;
- // clang-format off
- qb
- .coreSegments().withName(properties.coreSegmentName)
- .providerSegments().all()
- .entities().withName(entityName)
- .snapshots().beforeOrAtTime(timestamp);
- // clang-format on
+ armarx::data::ObjectID requestObject;
+ armarx::toIce(requestObject, instanceId);
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ armarx::objpose::observer::RequestObjectsInput req;
+ req.provider = provider;
+ req.request.objectIDs = {requestObject};
+ req.request.relativeTimeoutMS = until.toMilliSeconds();
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+ auto requestResult = objPoseStorage->requestObjects(req);
- if (not qResult.success) /* c++20 [[unlikely]] */
+ if (requestResult.results.count(requestObject))
{
- return std::nullopt;
+ return requestResult.results.at(requestObject).result.success;
}
-
- return queryObject(qResult.memory, timestamp);
+ return false;
}
- /// Query an object by objectId (e.g. Kitchen/green-cup in Entity Kitchen/green-cup/0)
- std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObjectByObjectID(const std::string& objectId, const armem::Time& timestamp)
+ std::optional<objpose::ObjectPose>
+ Reader::queryLatestObjectInstance(const ObjectID& instanceId)
{
- // Query all entities from all provider.
- armem::client::query::Builder qb;
-
- // clang-format off
- qb
- .coreSegments().withName(properties.coreSegmentName)
- .providerSegments().all()
- .entities().all()
- .snapshots().beforeOrAtTime(timestamp);
- // clang-format on
-
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ // TODO: Shall we throw an exception if no instance index is set?
- ARMARX_DEBUG << "Lookup result in reader: " << qResult;
-
- if (not qResult.success) /* c++20 [[unlikely]] */
+ auto objectPoses = objPoseStorage->getObjectPoses();
+ for (const auto& pose : objectPoses)
{
- return std::nullopt;
+ ObjectID oid;
+ fromIce(pose.objectID, oid);
+ if (oid == instanceId)
+ {
+ objpose::ObjectPose oi;
+ fromIce(pose, oi);
+ return oi;
+ }
}
+ return std::nullopt;
+ }
- // clang-format off
- const armem::wm::CoreSegment& s = qResult.memory
- .getCoreSegment(properties.coreSegmentName);
- // clang-format on
+ std::map<std::string, objpose::ObjectPose>
+ Reader::queryLatestObjectInstances()
+ {
+ std::map<std::string, objpose::ObjectPose> ret;
+ auto objectPoses = objPoseStorage->getObjectPoses();
+ for (const auto& pose : objectPoses)
+ {
+ ObjectID oid;
+ fromIce(pose.objectID, oid);
+ fromIce(pose, ret[oid.str()]);
+ }
+ return ret;
+ }
- const armem::wm::EntityInstance* instance = nullptr;
- s.forEachInstance([&instance, &objectId](const wm::EntityInstance& i)
- { if (simox::alg::starts_with(i.id().entityName, objectId)) instance = &i; });
- if (instance == nullptr)
+ std::map<std::string, objpose::ObjectPose>
+ Reader::queryLatestObjectInstances(const ObjectID& classId)
+ {
+ std::map<std::string, objpose::ObjectPose> ret;
+ auto objectPoses = objPoseStorage->getObjectPoses();
+ for (const auto& pose : objectPoses)
{
- return std::nullopt;
+ ObjectID oid;
+ fromIce(pose.objectID, oid);
+
+ if (oid.equalClass(classId))
+ {
+ fromIce(pose, ret[oid.str()]);
+ }
}
- return armem::arondto::ObjectInstance::FromAron(instance->data());
+ return ret;
}
-} // namespace armarx::armem::attachment
+} // namespace armarx::armem::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 0a517497c617db0773adc21851adb791835fe2b3..18f52a6111d96525c0b5940b2e03084b8ff29a96 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -28,9 +28,8 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.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>
// The object pose provider. As long as the provider is not connected to armem we need the second proxy
#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
@@ -41,144 +40,53 @@ namespace armarx::armem::obj::instance
class Reader
{
public:
- Reader(armem::client::MemoryNameSystem& memoryNameSystem, const objpose::ObjectPoseProviderPrx& objPoseProvider);
+ struct Properties
+ {
+ std::string memoryName = "Object";
+ };
+
+ Reader() = default;
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
- void connect();
+ void connect(armem::client::MemoryNameSystem& memoryNameSystem);
- // localization stuff
- bool requestLocalization(const std::string& entityName, const armarx::core::time::Duration& until);
+ // localization stuff. Requires an instance index to be set.
+ std::map<std::string, bool> requestLocalization(const ObjectID& instanceId,
+ const armarx::core::time::Duration& until);
+ bool requestLocalization(const ObjectID& instanceId,
+ const std::string& provider,
+ const armarx::core::time::Duration& until);
- // query existing instances from the memory
- std::optional<armem::arondto::ObjectInstance> queryObject(const armem::wm::Memory& memory, const armem::Time&);
- std::optional<armem::arondto::ObjectInstance> queryObjectByEntityID(const std::string& entityName, const armem::Time&);
- std::optional<armem::arondto::ObjectInstance> queryObjectByObjectID(const std::string& objectId, const armem::Time&);
+ // query existing instance from the memory. Requires an instance index to be set.
+ std::optional<objpose::ObjectPose> queryLatestObjectInstance(const ObjectID& instanceId);
- // return the dataset/class, e.g. Kitchen/greencup in Kitchen/greencup/0
- static std::string GetObjectId(const std::string& s)
- {
- auto split = simox::alg::split(s, "/");
- if (simox::alg::starts_with(s, "/"))
- {
- split.insert(split.begin(), ""); // sanitize
- }
-
- for (auto& e : split)
- {
- e = simox::alg::replace_all(e, "/", "");
- }
-
- if (IsEntityId(s)) return (split[0] + "/" + split[1]);
- if (IsObjectId(s)) return s;
-
- ARMARX_ERROR << "Unknown structure for object id '" << s << "'.";
- return "";
- }
+ // query the latest instances. Ignores the instance index.
+ std::map<std::string, objpose::ObjectPose>
+ queryLatestObjectInstances(const ObjectID& classId);
- // return all parts of the entity name
- static std::tuple<std::string, std::string, std::string> GetEntityNameParts(const std::string& s)
- {
- std::string dataset = "";
- std::string clazz = "";
- std::string instance = "";
-
- auto split = simox::alg::split(s, "/");
- if (simox::alg::starts_with(s, "/"))
- {
- split.insert(split.begin(), ""); // sanitize
- }
-
- for (auto& e : split)
- {
- e = simox::alg::replace_all(e, "/", "");
- }
-
- if (split.size() > 0)
- {
- dataset = split[0];
- }
- if (split.size() > 1)
- {
- clazz = split[1];
- }
- if (split.size() > 2)
- {
- instance = split[2];
- }
- return {dataset, clazz, instance};
- }
-
- // return the class, e.g. greencup in Kitchen/greencup/0
- static std::string GetObjectClassName(const std::string& s)
- {
- auto split = simox::alg::split(s, "/");
- if (simox::alg::starts_with(s, "/"))
- {
- split.insert(split.begin(), ""); // sanitize
- }
-
- for (auto& e : split)
- {
- e = simox::alg::replace_all(e, "/", "");
- }
-
- if (IsEntityId(s)) return split[1];
- if (IsObjectId(s)) return split[1];
-
- ARMARX_ERROR << "Unknown structure for object id '" << s << "'.";
- return "";
- }
+ // query all latest object instances.
+ std::map<std::string, objpose::ObjectPose> queryLatestObjectInstances();
- // check if s matches ??/??/??
- static bool IsEntityId(const std::string& s)
+ Properties
+ getProperties()
{
- auto split = simox::alg::split(s, "/");
- if (simox::alg::starts_with(s, "/"))
- {
- split.insert(split.begin(), ""); // sanitize
- }
-
- if (split.size() != 3)
- {
- return false;
- }
- return true;
+ return this->p;
}
- // check if s matches ??/??
- static bool IsObjectId(const std::string& s)
+ objpose::ObjectPoseStorageInterfacePrx
+ getObjectPoseStorage() const
{
- auto split = simox::alg::split(s, "/");
- if (simox::alg::starts_with(s, "/"))
- {
- split.insert(split.begin(), ""); // sanitize
- }
-
- if (split.size() != 2)
- {
- return false;
- }
- return true;
+ return objPoseStorage;
}
private:
-
- struct Properties
- {
- std::string memoryName = "Object";
- std::string coreSegmentName = "Instance";
- } properties;
+ Properties p;
const std::string propertyPrefix = "mem.obj.instance.";
- armem::client::MemoryNameSystem& memoryNameSystem;
objpose::ObjectPoseStorageInterfacePrx objPoseStorage;
- objpose::ObjectPoseProviderPrx objPoseProvider;
-
- armem::client::Reader memoryReader;
- mutable std::mutex memoryWriterMutex;
};
-} // namespace armarx::armem::attachment
+} // namespace armarx::armem::obj::instance
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/constants.h b/source/RobotAPI/libraries/armem_objects/constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..2417346925c1b6352c3c425f3cb44c87dc0507e7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/constants.h
@@ -0,0 +1,30 @@
+/**
+ * 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 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+namespace armarx::armem::objects::constants
+{
+ inline const std::string MemoryName = "Object";
+ inline const std::string CoreInstanceSegmentName = "Instance";
+ inline const std::string CoreClassSegmentName = "Class";
+
+} // namespace armarx::armem_objects::constants
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 03da4c2061d786b22fd1f126d9e8c2983e28faec..d6635696acd4f72d3c3342423233a459baf89af0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,59 +1,53 @@
#include "Segment.h"
-#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
-#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
-
-#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/memory_ids.h>
+#include <sstream>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem/util/util.h>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/Clock.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include "ArmarXCore/core/time/Clock.h"
-#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/time/DateTime.h>
-#include <ArmarXCore/core/time/ice_conversions.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/algorithm/string.h>
-#include <SimoxUtility/json.h>
-#include <SimoxUtility/math/pose/pose.h>
-#include <SimoxUtility/math/regression/linear.h>
-
-#include <Eigen/Geometry>
-#include <Eigen/Dense>
-
-#include <sstream>
-
-
namespace armarx::armem::server::obj::instance
{
- void Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(robotName, prefix + "robotName",
+ defs->optional(robotName,
+ prefix + "robotName",
"Name of robot whose note can be calibrated.\n"
"If not given, the 'fallbackName' is used.");
defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
@@ -66,111 +60,126 @@ namespace armarx::armem::server::obj::instance
arondto::ObjectInstance::ToAronType(),
64)
{
- oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
- {
- // Try to get OOBB from repository.
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ oobbCache.setFetchFn(
+ [this](const ObjectID& id) -> std::optional<simox::OrientedBoxf>
{
- try
+ // Try to get OOBB from repository.
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
{
- objectInfo->setLogError(false); // Don't log missing files
- return objectInfo->loadOOBB();
+ try
+ {
+ objectInfo->setLogError(false); // Don't log missing files
+ return objectInfo->loadOOBB();
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ // Give up - no OOBB information.
+ ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- "
+ << e.what();
+ return std::nullopt;
+ }
}
- catch (const std::ios_base::failure& e)
+ else
{
- // Give up - no OOBB information.
- ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
return std::nullopt;
}
- }
- else
- {
- return std::nullopt;
- }
- });
+ });
- classNameToDatasetCache.setFetchFn([this](const std::string & className)
- {
- std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
- return objectInfo ? objectInfo->dataset() : "";
- });
+ classNameToDatasetCache.setFetchFn(
+ [this](const std::string& className)
+ {
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
+ return objectInfo ? objectInfo->dataset() : "";
+ });
}
-
Segment::~Segment()
{
}
-
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
SpecializedCoreSegment::defineProperties(defs, prefix);
- defs->optional(p.discardSnapshotsWhileAttached, prefix + "DiscardSnapshotsWhileAttached",
- "If true, no new snapshots are stored while an object is attached to a robot node.\n"
- "If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
+ defs->optional(
+ p.discardSnapshotsWhileAttached,
+ prefix + "DiscardSnapshotsWhileAttached",
+ "If true, no new snapshots are stored while an object is attached to a robot node.\n"
+ "If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
- defs->optional(robots.fallbackName, prefix + "robots.FallbackName",
+ defs->optional(robots.fallbackName,
+ prefix + "robots.FallbackName",
"Robot name to use as fallback if the robot name is not specified "
"in a provided object pose.");
- defs->optional(p.sceneSnapshotsPackage, prefix + "scene.10_Package",
- "ArmarX package containing the scene snapshots.\n"
- "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
- defs->optional(p.sceneSnapshotsDirectory, prefix + "scene.11_Directory",
+ defs->optional(
+ p.sceneSnapshotsPackage,
+ prefix + "scene.10_Package",
+ "ArmarX package containing the scene snapshots.\n"
+ "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
+ defs->optional(p.sceneSnapshotsDirectory,
+ prefix + "scene.11_Directory",
"Directory in Package/data/Package/ containing the scene snapshots.");
- std::vector<std::string> sceneSnapshotToLoadDescription =
- {
+ std::vector<std::string> sceneSnapshotToLoadDescription = {
"Scene(s) to load on startup.",
"Specify multiple scenes in a ; separated list.",
"Each entry must be one of the following:",
"(1) A scene file in 'Package/scenes/' (with or without '.json' extension), "
"e.g. 'MyScene', 'MyScene.json'",
- "(2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' extension), "
+ "(2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' "
+ "extension), "
"e.g. 'path/to/MyScene', 'path/to/MyScene.json'",
"(3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'",
};
- defs->optional(p.sceneSnapshotsToLoad, prefix + "scene.12_SnapshotToLoad",
+ defs->optional(p.sceneSnapshotsToLoad,
+ prefix + "scene.12_SnapshotToLoad",
simox::alg::join(sceneSnapshotToLoadDescription, " \n"));
decay.defineProperties(defs, prefix + "decay.");
}
+ std::vector<std::string>
+ Segment::Properties::getSceneSnapshotsToLoad() const
+ {
+ if (sceneSnapshotsToLoad.empty())
+ {
+ return {};
+ }
+ bool trim = true;
+ return simox::alg::split(sceneSnapshotsToLoad, ";", trim);
+ }
- void Segment::init()
+ void
+ Segment::init()
{
SpecializedCoreSegment::init();
- if (not p.sceneSnapshotsToLoad.empty())
+ const std::vector<std::string> scenes = p.getSceneSnapshotsToLoad();
+ for (const std::string& scene : scenes)
{
- bool trim = true;
- const std::vector<std::string> scenes = simox::alg::split(p.sceneSnapshotsToLoad, ";", trim);
- for (const std::string& scene : scenes)
- {
- const bool lockMemory = false;
- commitSceneSnapshotFromFilename(scene, lockMemory);
- }
+ const bool lockMemory = false;
+ commitSceneSnapshotFromFilename(scene, lockMemory);
}
robots.setTag(Logging::tag);
}
-
- void Segment::connect(viz::Client arviz)
+ void
+ Segment::connect(viz::Client arviz)
{
- (void) arviz;
+ (void)arviz;
// ARMARX_INFO << "ArticulatedObjectVisu";
// this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
// visu->init();
}
-
- Segment::CommitStats Segment::commitObjectPoses(
- const std::string& providerName,
- const objpose::data::ProvidedObjectPoseSeq& providedPoses,
- const Calibration& calibration,
- std::optional<armem::Time> discardUpdatesUntil)
+ Segment::CommitStats
+ Segment::commitObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ const Calibration& calibration,
+ std::optional<armem::Time> discardUpdatesUntil)
{
CommitStats stats;
@@ -187,7 +196,8 @@ namespace armarx::armem::server::obj::instance
// Check whether we have an old snapshot for this object.
std::optional<objpose::ObjectPose> previousPose;
- const wm::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
+ const wm::Entity* entity =
+ findObjectEntity(armarx::fromIce(provided.objectID), providerName);
if (entity)
{
const arondto::ObjectInstance data = getLatestInstanceData(*entity);
@@ -221,11 +231,13 @@ namespace armarx::armem::server::obj::instance
// Update the entity.
stats.numUpdated++;
- VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName);
+ VirtualRobot::RobotPtr robot =
+ robots.get(provided.robotName, provided.providerName);
- if(not robot)
+ if (not robot)
{
- ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `" << provided.robotName << "`.";
+ ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
+ << provided.robotName << "`.";
}
// robot may be null!
@@ -233,10 +245,10 @@ namespace armarx::armem::server::obj::instance
// Update the robot to obtain correct local -> global transformation
if (robot and robotSyncTimestamp != timestamp)
{
- ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
- << "Failed to synchronize robot to timestamp " << timestamp
- << ". This is " << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
-
+ ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
+ << "Failed to synchronize robot to timestamp " << timestamp << ". This is "
+ << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
+
robotSyncTimestamp = timestamp;
@@ -244,7 +256,8 @@ namespace armarx::armem::server::obj::instance
{
if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
{
- VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode(calibration.robotNode);
+ VirtualRobot::RobotNodePtr robotNode =
+ robot->getRobotNode(calibration.robotNode);
float value = robotNode->getJointValue();
float newValue = value + calibration.offset;
@@ -255,7 +268,8 @@ namespace armarx::armem::server::obj::instance
* for the calibrated value.
* As this is just for perception (and not for controlling the robot), this should be fine^TM.
*/
- VirtualRobot::RobotNode::JointLimits limits = robotNode->getJointLimits();
+ VirtualRobot::RobotNode::JointLimits limits =
+ robotNode->getJointLimits();
bool limitsChanged = false;
if (newValue < limits.low)
{
@@ -282,11 +296,11 @@ namespace armarx::armem::server::obj::instance
{
objpose::data::ProvidedObjectPose copy = provided;
copy.objectPoseFrame = armarx::GlobalFrame;
- newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
+ newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
}
else
{
- newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
+ newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
}
if (previousPose && previousPose->attachment)
@@ -299,10 +313,12 @@ namespace armarx::armem::server::obj::instance
if (newPose.objectID.dataset().empty())
{
// Try to find the data set.
- const std::string dataset = classNameToDatasetCache.get(newPose.objectID.className());
+ const std::string dataset =
+ classNameToDatasetCache.get(newPose.objectID.className());
if (!dataset.empty())
{
- newPose.objectID = { dataset, newPose.objectID.className(), newPose.objectID.instanceName() };
+ newPose.objectID = {
+ dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
}
}
if (!provided.localOOBB)
@@ -318,8 +334,8 @@ namespace armarx::armem::server::obj::instance
return stats;
}
-
- void Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
+ void
+ Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
{
Time now = Time::Now();
@@ -332,7 +348,7 @@ namespace armarx::armem::server::obj::instance
EntityUpdate& update = commit.updates.emplace_back();
const MemoryID providerID = coreSegmentID.withProviderSegmentName(
- providerName.empty() ? pose.providerName : providerName);
+ providerName.empty() ? pose.providerName : providerName);
update.entityID = providerID.withEntityName(pose.objectID.str());
update.arrivedTime = now;
@@ -352,13 +368,11 @@ namespace armarx::armem::server::obj::instance
}
toAron(dto.sourceID, MemoryID());
update.instancesData.push_back(dto.toAron());
-
}
// Commit non-locking.
iceMemory.commit(commit);
}
-
objpose::ObjectPoseMap
Segment::getObjectPoses(const DateTime& now)
{
@@ -367,20 +381,16 @@ namespace armarx::armem::server::obj::instance
return filterObjectPoses(objectPoses);
}
-
-
objpose::ObjectPoseMap
- Segment::getObjectPosesByProvider(
- const std::string& providerName,
- const DateTime& now)
+ Segment::getObjectPosesByProvider(const std::string& providerName, const DateTime& now)
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
- ObjectPoseMap objectPoses = getLatestObjectPoses(segmentPtr->getProviderSegment(providerName));
+ ObjectPoseMap objectPoses =
+ getLatestObjectPoses(segmentPtr->getProviderSegment(providerName));
updateObjectPoses(objectPoses, now);
return filterObjectPoses(objectPoses);
}
-
wm::Entity*
Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
{
@@ -389,15 +399,16 @@ namespace armarx::armem::server::obj::instance
if (providerName.empty())
{
wm::Entity* result = nullptr;
- segmentPtr->forEachProviderSegment([&result, &entityID](wm::ProviderSegment & prov)
- {
- if (prov.hasEntity(entityID.entityName))
+ segmentPtr->forEachProviderSegment(
+ [&result, &entityID](wm::ProviderSegment& prov)
{
- result = &prov.getEntity(entityID);
- return false;
- }
- return true;
- });
+ if (prov.hasEntity(entityID.entityName))
+ {
+ result = &prov.getEntity(entityID);
+ return false;
+ }
+ return true;
+ });
return result;
}
else
@@ -415,24 +426,24 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now)
+ void
+ Segment::updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now)
{
bool agentSynchronized = false;
for (auto& [id, objectPose] : objectPoses)
{
- VirtualRobot::RobotPtr robot = robots.get(objectPose.robotName, objectPose.providerName);
+ VirtualRobot::RobotPtr robot =
+ robots.get(objectPose.robotName, objectPose.providerName);
updateObjectPose(objectPose, now, robot, agentSynchronized);
}
}
-
- void Segment::updateObjectPoses(
- ObjectPoseMap& objectPoses,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
+ void
+ Segment::updateObjectPoses(ObjectPoseMap& objectPoses,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
{
for (auto& [id, objectPose] : objectPoses)
{
@@ -440,12 +451,11 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::updateObjectPose(
- ObjectPose& objectPose,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
+ void
+ Segment::updateObjectPose(ObjectPose& objectPose,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
{
updateAttachement(objectPose, agent, agentSynchronized);
@@ -455,8 +465,8 @@ namespace armarx::armem::server::obj::instance
}
}
-
- Segment::ObjectPoseMap Segment::filterObjectPoses(const ObjectPoseMap& objectPoses) const
+ Segment::ObjectPoseMap
+ Segment::filterObjectPoses(const ObjectPoseMap& objectPoses) const
{
ObjectPoseMap result;
for (const auto& [id, objectPose] : objectPoses)
@@ -469,9 +479,10 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
- void Segment::updateAttachement(
- ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
+ void
+ Segment::updateAttachement(ObjectPose& objectPose,
+ VirtualRobot::RobotPtr agent,
+ bool& synchronized) const
{
if (not objectPose.attachment.has_value())
{
@@ -487,7 +498,7 @@ namespace armarx::armem::server::obj::instance
}
ARMARX_CHECK_NOT_NULL(agent);
- if (not synchronized) // Synchronize only once.
+ if (not synchronized) // Synchronize only once.
{
ARMARX_CHECK_NOT_NULL(robots.reader);
@@ -499,7 +510,6 @@ namespace armarx::armem::server::obj::instance
objectPose.updateAttached(agent);
}
-
objpose::ObjectPoseMap
Segment::getLatestObjectPoses() const
{
@@ -507,7 +517,6 @@ namespace armarx::armem::server::obj::instance
return getLatestObjectPoses(*segmentPtr);
}
-
objpose::ObjectPoseMap
Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg)
{
@@ -516,7 +525,6 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
objpose::ObjectPoseMap
Segment::getLatestObjectPoses(const wm::ProviderSegment& provSeg)
{
@@ -525,7 +533,6 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
objpose::ObjectPose
Segment::getLatestObjectPose(const wm::Entity& entity)
{
@@ -534,51 +541,51 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
- void Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
+ void
+ Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
{
- coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment & provSegment)
- {
- getLatestObjectPoses(provSegment, out);
- });
+ coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment& provSegment)
+ { getLatestObjectPoses(provSegment, out); });
}
-
- void Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
+ void
+ Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
{
- provSegment.forEachEntity([&out](const wm::Entity & entity)
- {
- if (!entity.empty())
+ provSegment.forEachEntity(
+ [&out](const wm::Entity& entity)
{
- ObjectPose pose = getLatestObjectPose(entity);
- // Try to insert. Fails and returns false if an entry already exists.
- const auto [it, success] = out.insert({pose.objectID, pose});
- if (!success)
+ if (!entity.empty())
{
- // An entry with that ID already exists. We keep the newest.
- if (it->second.timestamp < pose.timestamp)
+ ObjectPose pose = getLatestObjectPose(entity);
+ // Try to insert. Fails and returns false if an entry already exists.
+ const auto [it, success] = out.insert({pose.objectID, pose});
+ if (!success)
{
- it->second = pose;
+ // An entry with that ID already exists. We keep the newest.
+ if (it->second.timestamp < pose.timestamp)
+ {
+ it->second = pose;
+ }
}
}
- }
- });
+ });
}
-
- void Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
+ void
+ Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
{
- entity.getLatestSnapshot().forEachInstance([&out](const wm::EntityInstance & instance)
- {
- arondto::ObjectInstance dto;
- dto.fromAron(instance.data());
+ entity.getLatestSnapshot().forEachInstance(
+ [&out](const wm::EntityInstance& instance)
+ {
+ arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
- fromAron(dto, out);
- });
+ fromAron(dto, out);
+ });
}
-
- arondto::ObjectInstance Segment::getLatestInstanceData(const wm::Entity& entity)
+ arondto::ObjectInstance
+ Segment::getLatestInstanceData(const wm::Entity& entity)
{
ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
@@ -592,7 +599,6 @@ namespace armarx::armem::server::obj::instance
return data;
}
-
::armarx::armem::articulated_object::ArticulatedObjects
Segment::getArticulatedObjects()
{
@@ -601,7 +607,7 @@ namespace armarx::armem::server::obj::instance
ARMARX_INFO << "Found " << objectPoses.size() << " object poses";
::armarx::armem::articulated_object::ArticulatedObjects objects;
- for (const auto&[objectId, objectPose] : objectPoses)
+ for (const auto& [objectId, objectPose] : objectPoses)
{
armem::articulated_object::ArticulatedObject articulatedObject;
articulatedObject.config.jointMap = objectPose.objectJointValues;
@@ -625,7 +631,6 @@ namespace armarx::armem::server::obj::instance
fromAron(dto, description);
articulatedObject.description = description;
-
}
catch (...)
{
@@ -648,7 +653,6 @@ namespace armarx::armem::server::obj::instance
return objects;
}
-
std::map<DateTime, objpose::ObjectPose>
Segment::getObjectPosesInRange(const wm::Entity& entity,
const DateTime& start,
@@ -677,14 +681,14 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
- std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
+ std::optional<simox::OrientedBoxf>
+ Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
}
-
- objpose::ProviderInfo Segment::getProviderInfo(const std::string& providerName)
+ objpose::ProviderInfo
+ Segment::getProviderInfo(const std::string& providerName)
{
try
{
@@ -703,14 +707,13 @@ namespace armarx::armem::server::obj::instance
}
}
-
objpose::AttachObjectToRobotNodeOutput
Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
{
const armem::Time now = armem::Time::Now();
objpose::AttachObjectToRobotNodeOutput output;
- output.success = false; // We are not successful until proven otherwise.
+ output.success = false; // We are not successful until proven otherwise.
ObjectID objectID = armarx::fromIce(input.objectID);
@@ -718,7 +721,8 @@ namespace armarx::armem::server::obj::instance
if (not agent)
{
std::stringstream ss;
- ss << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName << "'."
+ ss << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName
+ << "'."
<< "\n(You can leave the agent name empty if there is only one agent.)\n"
<< "\nKnown agents: ";
for (const auto& [name, robot] : robots.loaded)
@@ -732,8 +736,8 @@ namespace armarx::armem::server::obj::instance
if (!agent->hasRobotNode(input.frameName))
{
- ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
- << "' of agent '" << agent->getName() << "'.";
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '"
+ << input.frameName << "' of agent '" << agent->getName() << "'.";
return output;
}
std::string frameName = input.frameName;
@@ -744,7 +748,8 @@ namespace armarx::armem::server::obj::instance
if (!objectEntity || objectEntity->empty())
{
ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
- << "' of agent '" << agent->getName() << "', but object is currently not provided.";
+ << "' of agent '" << agent->getName()
+ << "', but object is currently not provided.";
return output;
}
arondto::ObjectInstance data = getLatestInstanceData(*objectEntity);
@@ -764,7 +769,8 @@ namespace armarx::armem::server::obj::instance
const auto timestamp = armarx::Clock::Now();
ARMARX_CHECK(robots.reader->synchronizeRobot(*agent, timestamp));
- armarx::FramedPose framed(data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
+ armarx::FramedPose framed(
+ data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
if (frameName == armarx::GlobalFrame)
{
info.poseInFrame = framed.toGlobalEigen(agent);
@@ -779,21 +785,23 @@ namespace armarx::armem::server::obj::instance
// Store attachment in new entity snapshot.
{
armem::Commit commit;
- armem::EntityUpdate & update = commit.add();
+ armem::EntityUpdate& update = commit.add();
update.entityID = objectEntity->id();
update.referencedTime = now;
{
arondto::ObjectInstance updated = data;
toAron(updated.pose.attachment, info);
updated.pose.attachmentValid = true;
- update.instancesData = { updated.toAron() };
+ update.instancesData = {updated.toAron()};
}
iceMemory.commit(commit);
}
- ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName << "' "
+ ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName
+ << "' "
<< "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
- << "Object pose in frame: \n" << info.poseInFrame;
+ << "Object pose in frame: \n"
+ << info.poseInFrame;
output.success = true;
output.attachment = new objpose::data::ObjectAttachmentInfo();
@@ -804,10 +812,8 @@ namespace armarx::armem::server::obj::instance
return output;
}
-
objpose::DetachObjectFromRobotNodeOutput
- Segment::detachObjectFromRobotNode(
- const objpose::DetachObjectFromRobotNodeInput& input)
+ Segment::detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input)
{
const armem::Time now = armem::Time::Now();
@@ -840,19 +846,20 @@ namespace armarx::armem::server::obj::instance
output.wasAttached = bool(attachment);
if (attachment)
{
- ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
- << attachment->frameName << "' of agent '" << attachment->agentName << "'.";
+ ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName
+ << "' from robot node '" << attachment->frameName << "' of agent '"
+ << attachment->agentName << "'.";
}
else
{
- ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName << "' "
+ ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName
+ << "' "
<< "from robot node, but it was not attached.";
}
return output;
}
-
objpose::DetachAllObjectsFromRobotNodesOutput
Segment::detachAllObjectsFromRobotNodes(
const objpose::DetachAllObjectsFromRobotNodesInput& input)
@@ -863,31 +870,31 @@ namespace armarx::armem::server::obj::instance
objpose::DetachAllObjectsFromRobotNodesOutput output;
output.numDetached = 0;
- segmentPtr->forEachEntity([this, now, &input, &output](wm::Entity & entity)
- {
- const arondto::ObjectInstance data = this->getLatestInstanceData(entity);
- if (data.pose.attachmentValid)
+ segmentPtr->forEachEntity(
+ [this, now, &input, &output](wm::Entity& entity)
{
- ++output.numDetached;
- // Store non-attached pose in new snapshot.
- this->storeDetachedSnapshot(entity, data, now, input.commitAttachedPose);
- }
- });
+ const arondto::ObjectInstance data = this->getLatestInstanceData(entity);
+ if (data.pose.attachmentValid)
+ {
+ ++output.numDetached;
+ // Store non-attached pose in new snapshot.
+ this->storeDetachedSnapshot(entity, data, now, input.commitAttachedPose);
+ }
+ });
ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
return output;
}
-
- void Segment::storeDetachedSnapshot(
- wm::Entity& entity,
- const arondto::ObjectInstance& data,
- Time now,
- bool commitAttachedPose)
+ void
+ Segment::storeDetachedSnapshot(wm::Entity& entity,
+ const arondto::ObjectInstance& data,
+ Time now,
+ bool commitAttachedPose)
{
armem::Commit commit;
- armem::EntityUpdate & update = commit.add();
+ armem::EntityUpdate& update = commit.add();
update.entityID = entity.id();
update.referencedTime = now;
{
@@ -897,7 +904,8 @@ namespace armarx::armem::server::obj::instance
ObjectPose objectPose;
fromAron(data, objectPose);
- VirtualRobot::RobotPtr robot = robots.get(objectPose.robotName, objectPose.providerName);
+ VirtualRobot::RobotPtr robot =
+ robots.get(objectPose.robotName, objectPose.providerName);
bool agentSynchronized = false;
updateAttachement(objectPose, robot, agentSynchronized);
@@ -911,34 +919,34 @@ namespace armarx::armem::server::obj::instance
toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
}
- update.instancesData = { updated.toAron() };
+ update.instancesData = {updated.toAron()};
}
iceMemory.commit(commit);
}
-
std::optional<wm::EntityInstance>
Segment::findClassInstance(const ObjectID& objectID) const
{
- const ObjectID classID = { objectID.dataset(), objectID.className() };
+ const ObjectID classID = {objectID.dataset(), objectID.className()};
try
{
std::optional<wm::EntityInstance> result;
iceMemory.workingMemory->getCoreSegment("Class").forEachProviderSegment(
- [&classID, &result](const wm::ProviderSegment & provSeg)
- {
- if (provSeg.hasEntity(classID.str()))
+ [&classID, &result](const wm::ProviderSegment& provSeg)
{
- result = provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
- return false;
- }
- return true;
- });
+ if (provSeg.hasEntity(classID.str()))
+ {
+ result =
+ provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ return false;
+ }
+ return true;
+ });
if (not result.has_value())
{
- ARMARX_WARNING << deactivateSpam(120)
- << "No provider segment for classID " << classID.str() << " found";
+ ARMARX_WARNING << deactivateSpam(120) << "No provider segment for classID "
+ << classID.str() << " found";
}
return result;
}
@@ -950,12 +958,11 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
+ void
+ Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
{
if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
{
-
ARMARX_INFO << "Storing scene snapshot at: \n" << path.value();
try
{
@@ -968,7 +975,6 @@ namespace armarx::armem::server::obj::instance
}
}
-
std::optional<armarx::objects::Scene>
Segment::loadScene(const std::string& filename)
{
@@ -982,7 +988,6 @@ namespace armarx::armem::server::obj::instance
}
}
-
std::optional<armarx::objects::Scene>
Segment::loadScene(const std::filesystem::path& path)
{
@@ -997,57 +1002,56 @@ namespace armarx::armem::server::obj::instance
}
}
-
const std::string Segment::timestampPlaceholder = "%TIMESTAMP";
-
std::optional<std::filesystem::path>
Segment::resolveSceneFilepath(const std::string& _filename)
{
- std::string filepath = _filename;
+ std::stringstream log;
+ std::filesystem::path filepath = _filename;
- filepath = simox::alg::replace_all(filepath, timestampPlaceholder,
- Time::Now().toString("%Y-%m-%d_%H-%M-%S"));
+ filepath = simox::alg::replace_all(
+ filepath, timestampPlaceholder, Time::Now().toString("%Y-%m-%d_%H-%M-%S"));
if (not simox::alg::ends_with(filepath, ".json"))
{
filepath += ".json";
}
- // Try to interprete it as relative to 'Package/scenes/'.
- if (!finder)
+ if (filepath.is_absolute())
{
- finder.reset(new CMakePackageFinder(p.sceneSnapshotsPackage));
- if (not finder->packageFound())
- {
- ARMARX_WARNING << "Could not find CMake package " << p.sceneSnapshotsPackage << ".";
- }
+ return filepath;
}
- if (finder->packageFound())
+
+ armarx::PackagePath packagePath = [&filepath, this]()
{
- std::filesystem::path absDataDir = finder->getDataDir();
- std::filesystem::path absPath = absDataDir / p.sceneSnapshotsPackage
- / p.sceneSnapshotsDirectory / filepath;
- if (std::filesystem::is_regular_file(absPath))
+ if (filepath.has_parent_path())
{
- return absPath;
+ // Interpret the first component as package name.
+ std::string packageName = *filepath.begin();
+ return PackagePath(packageName, std::filesystem::relative(filepath, packageName));
}
- }
-
- // Try to interprete it as ArmarXDataPath.
- {
- std::string resolved = ArmarXDataPath::resolvePath(filepath);
- if (resolved != filepath)
+ else
{
- return resolved;
+ // Only a filename => Interprete it as relative to 'Package/scenes/'.
+ return PackagePath(p.sceneSnapshotsPackage, p.sceneSnapshotsDirectory / filepath);
}
- }
+ }();
- // Else: Fail
- return std::nullopt;
+ try
+ {
+ std::filesystem::path systemPath = packagePath.toSystemPath();
+ return systemPath;
+ }
+ catch (const armarx::PackageNotFoundException& e)
+ {
+ ARMARX_INFO << "Could not interpret '" << packagePath.serialize().package
+ << "' as ArmarX package name (" << e.what() << ").";
+ return std::nullopt;
+ }
}
-
- armarx::objects::Scene Segment::getSceneSnapshot() const
+ armarx::objects::Scene
+ Segment::getSceneSnapshot() const
{
using armarx::objects::SceneObject;
@@ -1060,36 +1064,42 @@ namespace armarx::armem::server::obj::instance
};
std::map<ObjectID, StampedSceneObject> objects;
- segmentPtr->forEachEntity([&objects](wm::Entity & entity)
- {
- const wm::EntityInstance* entityInstance = entity.findLatestInstance();
- if (entityInstance)
+ segmentPtr->forEachEntity(
+ [&objects](wm::Entity& entity)
{
- std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(*entityInstance);
- if (objectInstance)
+ const wm::EntityInstance* entityInstance = entity.findLatestInstance();
+ if (entityInstance)
{
- const ObjectID objectID = ObjectID::FromString(objectInstance->classID.entityName);
-
- auto it = objects.find(objectID);
- if (it == objects.end() or objectInstance->pose.timestamp > it->second.timestamp)
+ std::optional<arondto::ObjectInstance> objectInstance =
+ tryCast<arondto::ObjectInstance>(*entityInstance);
+ if (objectInstance)
{
- StampedSceneObject& stamped = objects[objectID];
- stamped.timestamp = objectInstance->pose.timestamp;
+ const ObjectID objectID =
+ ObjectID::FromString(objectInstance->classID.entityName);
- SceneObject& object = stamped.object;
- object.className = objectID.getClassID().str();
- object.instanceName = objectID.instanceName();
- object.collection = "";
+ auto it = objects.find(objectID);
+ if (it == objects.end() or
+ objectInstance->pose.timestamp > it->second.timestamp)
+ {
+ StampedSceneObject& stamped = objects[objectID];
+ stamped.timestamp = objectInstance->pose.timestamp;
+
+ SceneObject& object = stamped.object;
+ object.className = objectID.getClassID().str();
+ object.instanceName = objectID.instanceName();
+ object.collection = "";
- object.position = simox::math::position(objectInstance->pose.objectPoseGlobal);
- object.orientation = simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+ object.position =
+ simox::math::position(objectInstance->pose.objectPoseGlobal);
+ object.orientation =
+ simox::math::orientation(objectInstance->pose.objectPoseGlobal);
- object.isStatic = objectInstance->pose.isStatic;
- object.jointValues = objectInstance->pose.objectJointValues;
+ object.isStatic = objectInstance->pose.isStatic;
+ object.jointValues = objectInstance->pose.objectJointValues;
+ }
}
}
- }
- });
+ });
armarx::objects::Scene scene;
for (const auto& [id, stamped] : objects)
@@ -1099,8 +1109,8 @@ namespace armarx::armem::server::obj::instance
return scene;
}
-
- void Segment::commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName)
+ void
+ Segment::commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName)
{
const Time now = Time::Now();
std::map<ObjectID, int> idCounters;
@@ -1109,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();
@@ -1117,11 +1133,9 @@ namespace armarx::armem::server::obj::instance
pose.objectType = objpose::ObjectType::KnownObject;
// If not specified, assume loaded objects are static.
pose.isStatic = object.isStatic.has_value() ? object.isStatic.value() : true;
- pose.objectID = classID.withInstanceName(
- object.instanceName.empty()
- ? std::to_string(idCounters[classID]++)
- : object.instanceName
- );
+ pose.objectID = classID.withInstanceName(object.instanceName.empty()
+ ? std::to_string(idCounters[classID]++)
+ : object.instanceName);
pose.objectPoseGlobal = simox::math::pose(object.position, object.orientation);
pose.objectPoseRobot = pose.objectPoseGlobal;
@@ -1142,8 +1156,8 @@ namespace armarx::armem::server::obj::instance
commitObjectPoses(objectPoses);
}
-
- void Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
+ void
+ Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
{
std::stringstream ss;
ss << "Loading scene '" << filename << "' ...";
@@ -1156,7 +1170,7 @@ namespace armarx::armem::server::obj::instance
auto makeSceneName = [](const std::filesystem::path& path)
{
std::filesystem::path filename = path.filename();
- filename.replace_extension(); // Removes extension
+ filename.replace_extension(); // Removes extension
return filename.string();
};
std::string sceneName = makeSceneName(path.value());
@@ -1164,10 +1178,8 @@ namespace armarx::armem::server::obj::instance
// The check seems useless?
if (lockMemory)
{
- segmentPtr->doLocked([this,&snapshot, &sceneName]()
- {
- commitSceneSnapshot(snapshot.value(), sceneName);
- });
+ segmentPtr->doLocked([this, &snapshot, &sceneName]()
+ { commitSceneSnapshot(snapshot.value(), sceneName); });
}
else
{
@@ -1181,8 +1193,8 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::RemoteGui::setup(const Segment& data)
+ void
+ Segment::RemoteGui::setup(const Segment& data)
{
using namespace armarx::RemoteGui::Client;
@@ -1191,18 +1203,33 @@ namespace armarx::armem::server::obj::instance
infiniteHistory.setValue(data.properties.maxHistorySize == -1);
discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
- storeLoadLine.setValue("Scene_" + timestampPlaceholder);
+ const std::string storeLoadPath = [&data]()
+ {
+ const std::vector<std::string> scenes = data.p.getSceneSnapshotsToLoad();
+ if (scenes.empty())
+ {
+ std::string storeLoadFilename = "Scene_" + timestampPlaceholder;
+ return data.p.sceneSnapshotsPackage + "/" + data.p.sceneSnapshotsToLoad;
+ }
+ else
+ {
+ return scenes.front();
+ }
+ }();
+
+ storeLoadLine.setValue(storeLoadPath);
loadButton.setLabel("Load Scene");
storeButton.setLabel("Store Scene");
- HBoxLayout storeLoadLineLayout(
- {
- Label(data.p.sceneSnapshotsPackage + "/" + data.p.sceneSnapshotsDirectory + "/"),
- storeLoadLine,
- Label(".json")
- });
+ HBoxLayout storeLoadLineLayout({storeLoadLine, Label(".json")});
HBoxLayout storeLoadButtonsLayout({loadButton, storeButton});
+ detachAllObjectsButton.setLabel("Detach All Objects");
+ detachAllObjectsCommitAttachedPoseCheckBox.setValue(true);
+ HBoxLayout detachAllObjectsCommitAttachedPoseLayout(
+ {Label("Commit Attached Pose"), detachAllObjectsCommitAttachedPoseCheckBox});
+
+
GridLayout grid;
int row = 0;
@@ -1215,15 +1242,19 @@ namespace armarx::armem::server::obj::instance
row++;
grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
row++;
- grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ grid.add(Label("Discard Snapshots while Attached"), {row, 0})
+ .add(discardSnapshotsWhileAttached, {row, 1});
row++;
+ grid.add(detachAllObjectsButton, {row, 0})
+ .add(detachAllObjectsCommitAttachedPoseLayout, {row, 1});
+
group.setLabel("Data");
group.addChild(grid);
}
-
- void Segment::RemoteGui::update(Segment& segment)
+ void
+ Segment::RemoteGui::update(Segment& segment)
{
if (loadButton.wasClicked())
{
@@ -1234,41 +1265,51 @@ namespace armarx::armem::server::obj::instance
if (storeButton.wasClicked())
{
armarx::objects::Scene scene;
- segment.doLocked([&scene, &segment]()
- {
- scene = segment.getSceneSnapshot();
- });
+ segment.doLocked([&scene, &segment]() { scene = segment.getSceneSnapshot(); });
segment.storeScene(storeLoadLine.getValue(), scene);
}
- if (infiniteHistory.hasValueChanged()
- || maxHistorySize.hasValueChanged()
- || discardSnapshotsWhileAttached.hasValueChanged())
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged() ||
+ discardSnapshotsWhileAttached.hasValueChanged())
{
- segment.doLocked([this, &segment]()
- {
- if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ segment.doLocked(
+ [this, &segment]()
{
- segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (segment.segmentPtr)
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- segment.segmentPtr->setMaxHistorySize(long(segment.properties.maxHistorySize));
+ segment.properties.maxHistorySize =
+ infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.segmentPtr)
+ {
+ segment.segmentPtr->setMaxHistorySize(
+ long(segment.properties.maxHistorySize));
+ }
}
- }
- segment.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- });
+ segment.p.discardSnapshotsWhileAttached =
+ discardSnapshotsWhileAttached.getValue();
+ });
}
- }
+ if (detachAllObjectsButton.wasClicked())
+ {
+ objpose::DetachAllObjectsFromRobotNodesInput input;
+ input.commitAttachedPose = detachAllObjectsCommitAttachedPoseCheckBox.getValue();
+
+ objpose::DetachAllObjectsFromRobotNodesOutput output = segment.doLocked(
+ [&segment, &input]() { return segment.detachAllObjectsFromRobotNodes(input); });
+ (void)output;
+ }
+ }
- VirtualRobot::RobotPtr Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
+ VirtualRobot::RobotPtr
+ Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
{
std::string robotName = _robotName;
if (robotName.empty())
{
- auto antiSpam = deactivateSpam(10 * 60); // 10 minutes.
+ auto antiSpam = deactivateSpam(10 * 60); // 10 minutes.
std::stringstream ss;
if (providerName.empty())
@@ -1285,8 +1326,7 @@ namespace armarx::armem::server::obj::instance
{
ss << ", and no fallback robot name was configured (e.g. via the properties).\n"
<< "For these object poses, the object instance segment is not able "
- << "to provide transformed object poses (global and in robot root frame)."
- ;
+ << "to provide transformed object poses (global and in robot root frame).";
ARMARX_INFO << antiSpam << ss.str();
return nullptr;
@@ -1312,8 +1352,7 @@ namespace armarx::armem::server::obj::instance
// Try to fetch the robot.
ARMARX_CHECK_NOT_NULL(reader);
VirtualRobot::RobotPtr robot = reader->getRobot(
- robotName, Clock::Now(),
- VirtualRobot::RobotIO::RobotDescription::eStructure);
+ robotName, Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eStructure);
if (robot)
{
@@ -1321,14 +1360,16 @@ namespace armarx::armem::server::obj::instance
if (not synchronized)
{
ARMARX_INFO << "The robot '" << robotName << "' could be loaded, but not"
- << " synchronized successfully (e.g., the global localization could be missing). "
+ << " synchronized successfully (e.g., the global localization "
+ "could be missing). "
<< "Make sure to synchronize it before use if necessary.";
}
// Store robot if valid.
loaded.emplace(robotName, robot);
}
- return robot; // valid or nullptr
+ return robot; // valid or nullptr
}
}
-}
+
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index c5e62d975ca161773c9ae4e8b7d7f5352f0f1922..b922f1211cbe769e89d9a55108b257b336c73642 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -9,24 +9,20 @@
#include <SimoxUtility/shapes/OrientedBox.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
-
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
-
#include <RobotAPI/libraries/armem/core/Prediction.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include "ArticulatedObjectVisu.h"
#include "Decay.h"
-
namespace armarx::armem::arondto
{
class ObjectInstance;
@@ -38,11 +34,11 @@ namespace armarx::armem::server::obj::instance
class Segment : public segment::SpecializedCoreSegment
{
public:
-
struct CommitStats
{
int numUpdated = 0;
};
+
using ObjectPose = objpose::ObjectPose;
using ObjectPoseSeq = objpose::ObjectPoseSeq;
using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
@@ -53,41 +49,46 @@ namespace armarx::armem::server::obj::instance
std::string robotNode = "Neck_2_Pitch";
float offset = 0.0f;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "calibration.");
};
public:
-
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
void init() override;
void connect(viz::Client arviz);
-
- CommitStats commitObjectPoses(
- const std::string& providerName,
- const objpose::data::ProvidedObjectPoseSeq& providedPoses,
- const Calibration& calibration,
- std::optional<Time> discardUpdatesUntil = std::nullopt);
- void commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName = "");
+ CommitStats commitObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ const Calibration& calibration,
+ std::optional<Time> discardUpdatesUntil = std::nullopt);
+ void commitObjectPoses(const ObjectPoseSeq& objectPoses,
+ const std::string& providerName = "");
objpose::ObjectPoseMap getObjectPoses(const DateTime& now);
- objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName, const DateTime& now);
+ objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName,
+ const DateTime& now);
- wm::Entity* findObjectEntity(const ObjectID& objectID, const std::string& providerName = "");
+ wm::Entity* findObjectEntity(const ObjectID& objectID,
+ const std::string& providerName = "");
std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
objpose::ProviderInfo getProviderInfo(const std::string& providerName);
- objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input);
- objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input);
- objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input);
+ objpose::AttachObjectToRobotNodeOutput
+ attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input);
+ objpose::DetachObjectFromRobotNodeOutput
+ detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input);
+ objpose::DetachAllObjectsFromRobotNodesOutput
+ detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input);
/**
@@ -100,7 +101,8 @@ namespace armarx::armem::server::obj::instance
*
* @param synchronized Indicates whether the agent is already synchronized to the current time.
*/
- void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
+ void updateAttachement(ObjectPose& objectPose,
+ VirtualRobot::RobotPtr agent,
bool& synchronized) const;
@@ -120,34 +122,26 @@ namespace armarx::armem::server::obj::instance
getObjectPosesInRange(const wm::Entity& entity, const DateTime& start, const DateTime& end);
private:
-
ObjectPoseMap getLatestObjectPoses() const;
- void updateObjectPoses(
- ObjectPoseMap& objectPoses,
- const DateTime& now);
- void updateObjectPoses(
- ObjectPoseMap& objectPoses,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
- void updateObjectPose(
- ObjectPose& objectPose,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
+ void updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now);
+ void updateObjectPoses(ObjectPoseMap& objectPoses,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const;
+ void updateObjectPose(ObjectPose& objectPose,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const;
ObjectPoseMap filterObjectPoses(const ObjectPoseMap& objectPoses) const;
- void storeDetachedSnapshot(
- wm::Entity& entity,
- const arondto::ObjectInstance& data,
- Time now,
- bool commitAttachedPose);
+ void storeDetachedSnapshot(wm::Entity& entity,
+ const arondto::ObjectInstance& data,
+ Time now,
+ bool commitAttachedPose);
std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
@@ -156,7 +150,6 @@ namespace armarx::armem::server::obj::instance
private:
-
void storeScene(const std::string& filename, const armarx::objects::Scene& scene);
std::optional<armarx::objects::Scene> loadScene(const std::string& filename);
std::optional<armarx::objects::Scene> loadScene(const std::filesystem::path& path);
@@ -168,7 +161,6 @@ namespace armarx::armem::server::obj::instance
public:
-
/// Loaded robot models identified by the robot name.
struct RobotsCache : public armarx::Logging
{
@@ -177,8 +169,10 @@ namespace armarx::armem::server::obj::instance
std::map<std::string, VirtualRobot::RobotPtr> loaded;
- VirtualRobot::RobotPtr get(const std::string& robotName, const std::string& providerName = "");
+ VirtualRobot::RobotPtr get(const std::string& robotName,
+ const std::string& providerName = "");
};
+
RobotsCache robots;
@@ -192,7 +186,6 @@ namespace armarx::armem::server::obj::instance
private:
-
struct Properties
{
bool discardSnapshotsWhileAttached = true;
@@ -201,7 +194,10 @@ namespace armarx::armem::server::obj::instance
std::string sceneSnapshotsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
std::string sceneSnapshotsDirectory = "scenes";
std::string sceneSnapshotsToLoad = "";
+
+ std::vector<std::string> getSceneSnapshotsToLoad() const;
};
+
Properties p;
@@ -216,7 +212,6 @@ namespace armarx::armem::server::obj::instance
static const std::string timestampPlaceholder;
public:
-
struct RemoteGui
{
armarx::RemoteGui::Client::GroupBox group;
@@ -229,15 +224,16 @@ namespace armarx::armem::server::obj::instance
armarx::RemoteGui::Client::CheckBox infiniteHistory;
armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+ armarx::RemoteGui::Client::Button detachAllObjectsButton;
+ armarx::RemoteGui::Client::CheckBox detachAllObjectsCommitAttachedPoseCheckBox;
+
void setup(const Segment& data);
void update(Segment& data);
};
private:
-
std::unique_ptr<ArticulatedObjectVisu> visu;
-
};
-}
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 514096731b83167b902dfe4198deae82725837ed..b9abf459ec14789b7e4fe3fccf42ba73db0f5eed 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -70,6 +70,11 @@ namespace armarx::armem::server::obj::instance
std::vector<viz::Layer> layers;
for (const auto& [name, poses] : objectPoses)
{
+ if(name.empty())
+ {
+ continue;
+ }
+
auto poseHistoryMap = poseHistories.find(name);
if (poseHistoryMap != poseHistories.end())
{
@@ -92,6 +97,12 @@ namespace armarx::armem::server::obj::instance
std::map<std::string, viz::Layer> stage;
for (size_t i = 0; i < objectPoses.size(); ++i)
{
+ if(objectPoses.at(i).providerName.empty())
+ {
+ ARMARX_INFO << "Object pose provider not set!";
+ continue;
+ }
+
visualizeObjectPose(getLayer(objectPoses.at(i).providerName, stage),
objectPoses.at(i),
poseHistories.at(i),
diff --git a/source/RobotAPI/libraries/armem_objects/types.cpp b/source/RobotAPI/libraries/armem_objects/types.cpp
index 58c02ecd6d2392a4367ec2731dc0a8d8d9a1d4f7..fc79d9d5fd982777b374abc8d17731adc88c85fd 100644
--- a/source/RobotAPI/libraries/armem_objects/types.cpp
+++ b/source/RobotAPI/libraries/armem_objects/types.cpp
@@ -24,10 +24,10 @@
#include "RobotAPI/libraries/core/FramedPose.h"
#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/framed.h>
#include "aron_forward_declarations.h"
-
namespace armarx::armem::marker
{
Marker::Marker(const std::string& name,
@@ -46,13 +46,17 @@ namespace armarx::armem::marker
Marker::Marker(const armarx::armem::arondto::Marker& dto) :
name(dto.name),
- robotGlobal(dto.robotGlobal),
- rgbCamera(dto.rgbCamera),
- depthCamera(dto.depthCamera),
- markerPose(dto.markerPose)
+ robotGlobal(
+ armarx::fromAron<armarx::FramedPose, armarx::arondto::FramedPose>(dto.robotGlobal)),
+ rgbCamera(armarx::fromAron<armarx::FramedPose, armarx::arondto::FramedPose>(dto.rgbCamera)),
+ depthCamera(
+ armarx::fromAron<armarx::FramedPose, armarx::arondto::FramedPose>(dto.depthCamera)),
+ markerPose(
+ armarx::fromAron<armarx::FramedPose, armarx::arondto::FramedPose>(dto.markerPose))
{
markerGlobal = getGlobalMarkerPose();
}
+
armarx::FramedPose
Marker::getGlobalMarkerPose() const
{
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
index f3326b90e514c9a802a79d19851d158d93e2b845..325075db3994c74ee21bafa949b37a7a35f66b58 100644
--- a/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
+++ b/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml
@@ -18,7 +18,7 @@
</ObjectChild>
<!-- <ObjectChild key='scaling'>
- <float />
+ <float32 />
</ObjectChild> -->
<!--
@@ -37,4 +37,3 @@
</GenerateTypes>
</AronTypeDefinition>
-
diff --git a/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml b/source/RobotAPI/libraries/armem_robot/aron/RobotState.xml
index d5db402107325d0bdf788006bc1846a45d369b86..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">
@@ -18,13 +21,15 @@
<ObjectChild key='jointMap'>
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
+ <ObjectChild key='proprioception'>
+ <armarx::armem::arondto::Proprioception optional="true"/>
+ </ObjectChild>
</Object>
</GenerateTypes>
</AronTypeDefinition>
-
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 2bc45c144b53e3900e8c7041c54d33ee0d205f56..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
{
@@ -37,7 +37,9 @@ namespace armarx::armem::robot
struct ForceTorque
{
Eigen::Vector3f force;
+ Eigen::Vector3f gravityCompensatedForce;
Eigen::Vector3f torque;
+ Eigen::Vector3f gravityCompensatedTorque;
};
using ToFArray = Eigen::MatrixXf;
@@ -58,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/Exteroception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
index 80f72e06059a39e60e6709a5e7acb28c63ee3395..6f0f045ac7f1b10df70776093aa0541e5eca723f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
@@ -19,7 +19,7 @@
<Object name="armarx::armem::arondto::Exteroception">
<ObjectChild key="iterationID">
- <long />
+ <int64 />
</ObjectChild>
<ObjectChild key="tof">
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 c7b78a90cfa452579dfea76227833b66c5befbe4..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'>
- <Float />
- </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 d5472cbde89434e346f116e6b6f461bf4f2ca34e..db6a0a9c5898f6332df65199da67c0b4b7b16c1c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -14,87 +14,87 @@
<ObjectChild key="position">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="velocity">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="acceleration">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="relativePosition">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="filteredVelocity">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="currentTarget">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="positionTarget">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="velocityTarget">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="torque">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="inertiaTorque">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="gravityTorque">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="gravityCompensatedTorque">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="inverseDynamicsTorque">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="torqueTicks">
<Dict>
- <Int />
+ <int32 />
</Dict>
</ObjectChild>
@@ -122,7 +122,7 @@
<ObjectChild key="temperature">
<Dict>
<Dict>
- <Float />
+ <float32 />
</Dict>
</Dict>
</ObjectChild>
@@ -130,26 +130,26 @@
<ObjectChild key="motorCurrent">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="maxTargetCurrent">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="sensorBoardUpdateRate">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="I2CUpdateRate">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
@@ -168,13 +168,13 @@
<ObjectChild key="JointStatusError">
<Dict>
- <Int />
+ <int32 />
</Dict>
</ObjectChild>
<ObjectChild key="JointStatusOperation">
<Dict>
- <Int />
+ <int32 />
</Dict>
</ObjectChild>
@@ -202,7 +202,7 @@
<ObjectChild key="extra">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
@@ -229,7 +229,7 @@
<ObjectChild key="extra">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
@@ -238,7 +238,7 @@
<Object name="armarx::armem::arondto::Proprioception">
<ObjectChild key="iterationID">
- <long />
+ <int64 />
</ObjectChild>
<ObjectChild key="joints">
@@ -257,16 +257,41 @@
<ObjectChild key="extraFloats">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="extraLongs">
<Dict>
- <Long />
+ <int64 />
</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 3749c892d5e46e6ce0f7a848a86e8059ed70991c..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)
{
@@ -88,14 +70,18 @@ namespace armarx::armem
fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
{
aron::fromAron(dto.force, bo.force);
+ aron::fromAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
aron::fromAron(dto.torque, bo.torque);
+ aron::fromAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque);
}
void
toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
{
aron::toAron(dto.force, bo.force);
+ aron::toAron(dto.gravityCompensationForce, bo.gravityCompensatedForce);
aron::toAron(dto.torque, bo.torque);
+ aron::toAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque);
}
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 eaa2e9ca07b063eaba3a71887eda2d14fa839c16..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>
@@ -215,7 +219,7 @@ namespace armarx::armem::robot_state
.agent = description.name,
.timestamp = timestamp}};
- try
+ // try
{
const auto result = transformReader.lookupTransform(query);
if (not result)
@@ -224,17 +228,16 @@ namespace armarx::armem::robot_state
}
return result.transform;
}
- catch (...)
- {
- ARMARX_VERBOSE << GetHandledExceptionString();
- return std::nullopt;
- }
+ // catch (...)
+ // {
+ // ARMARX_VERBOSE << GetHandledExceptionString();
+ // return std::nullopt;
+ // }
}
-
- 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 76fa10a4c0186acecdb969ec69af75d97975708f..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)
{
@@ -117,5 +121,16 @@ namespace armarx::armem::robot_state
return nullptr;
}
+ std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+ VirtualRobotReader::queryForceTorque(const std::string& name, const Time& timestamp)
+ {
+ const auto description = queryDescription(name, timestamp);
+ if (not description.has_value())
+ {
+ return std::nullopt;
+ }
+ return RobotReader::queryForceTorque(description.value(), timestamp);
+ }
+
} // namespace armarx::armem::robot_state
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 3de8fbe8b49c453a79a7cafbcf7904713f4d756f..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,
@@ -68,6 +68,9 @@ namespace armarx::armem::robot_state
VirtualRobot::RobotIO::RobotDescription::eStructure,
bool blocking = true);
+ std::optional<std::map<RobotReader::Hand, robot::ForceTorque>>
+ queryForceTorque(const std::string& name, const armem::Time& timestamp);
+ using RobotReader::queryForceTorque;
private:
[[nodiscard]] VirtualRobot::RobotPtr
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/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index 529cd6d28a198d14779682d27826cb4d6b0f3cd4..c163ff7f68ca6fae8b4bccdf6ecfce72187f10fa 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -1,4 +1,5 @@
#include "TransformHelper.h"
+
#include <optional>
#include <string>
@@ -6,55 +7,53 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/math/pose/interpolate.h>
-#include "ArmarXCore/core/logging/Logging.h"
-#include <ArmarXCore/core/exceptions/LocalException.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/DateTime.h"
#include "ArmarXCore/core/time/Duration.h"
-#include "RobotAPI/libraries/armem/core/forward_declarations.h"
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
-
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::common::robot_state::localization
{
- template <class ...Args>
+ template <class... Args>
TransformResult
TransformHelper::_lookupTransform(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
const TransformQuery& query)
{
- const std::vector<std::string> tfChain = _buildTransformChain(localizationCoreSegment, query);
+ const std::vector<std::string> tfChain =
+ _buildTransformChain(localizationCoreSegment, query);
if (tfChain.empty())
{
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Cannot create tf lookup chain '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "' for robot `" + query.header.agent + "`."};
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" + query.header.parentFrame +
+ " -> " + query.header.frame + "' for robot `" +
+ query.header.agent + "`."};
}
const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
- localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+ localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
- const std::optional<armem::Time> sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+ const std::optional<armem::Time> sanitizedTimestamp =
+ _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
- if(not sanitizedTimestamp.has_value())
+ if (not sanitizedTimestamp.has_value())
{
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::Error,
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::Error,
.errorMessage = "Error: Issue with timestamp"};
}
@@ -66,79 +65,69 @@ namespace armarx::armem::common::robot_state::localization
// ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
header.timestamp = sanitizedTimestamp.value();
-
+
if (transforms.empty())
{
- ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
+ ARMARX_INFO << deactivateSpam(1) << "No transform available.";
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
.errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
" -> " + query.header.frame +
"'. No memory data in time range."};
}
- const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
- transforms.end(),
- Eigen::Affine3f::Identity(),
- std::multiplies<>());
+ const Eigen::Affine3f transform = std::accumulate(
+ transforms.begin(), transforms.end(), Eigen::Affine3f::Identity(), std::multiplies<>());
ARMARX_DEBUG << "Found valid transform";
return {.transform = {.header = header, .transform = transform},
- .status = TransformResult::Status::Success};
+ .status = TransformResult::Status::Success};
}
-
- template <class ...Args>
+ template <class... Args>
TransformChainResult
TransformHelper::_lookupTransformChain(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
const TransformQuery& query)
{
- const std::vector<std::string> tfChain = _buildTransformChain(localizationCoreSegment, query);
+ const std::vector<std::string> tfChain =
+ _buildTransformChain(localizationCoreSegment, query);
if (tfChain.empty())
{
- ARMARX_VERBOSE << "TF chain is empty";
- return
- {
- .header = query.header,
- .transforms = std::vector<Eigen::Affine3f>{},
- .status = TransformChainResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Cannot create tf lookup chain '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "' for robot `" + query.header.agent + "`."
- };
+ ARMARX_DEBUG << "TF chain is empty";
+ return {.header = query.header,
+ .transforms = std::vector<Eigen::Affine3f>{},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" + query.header.parentFrame +
+ " -> " + query.header.frame + "' for robot `" +
+ query.header.agent + "`."};
}
const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
- localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+ localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
if (transforms.empty())
{
- ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
+ ARMARX_INFO << deactivateSpam(1) << "No transform available.";
return
{
.header = query.header,
- .transforms = {},
- .status = TransformChainResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
- " -> " + query.header.frame +
- "'. No memory data in time range."
+ .transforms = {},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF loookup: '" + query.header.parentFrame + " -> " +
+ query.header.frame + "'. No memory data in time range."
};
}
ARMARX_DEBUG << "Found valid transform";
- return
- {
- .header = query.header,
- .transforms = transforms,
- .status = TransformChainResult::Status::Success
- };
+ return {.header = query.header,
+ .transforms = transforms,
+ .status = TransformChainResult::Status::Success};
}
-
- template <class ...Args>
+ template <class... Args>
std::vector<std::string>
TransformHelper::_buildTransformChain(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
@@ -148,14 +137,15 @@ namespace armarx::armem::common::robot_state::localization
std::vector<std::string> chain;
- const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(query.header.agent);
+ const auto& agentProviderSegment =
+ localizationCoreSegment.getProviderSegment(query.header.agent);
const std::vector<std::string> tfs = agentProviderSegment.getEntityNames();
// lookup from robot root to global
std::map<std::string, std::string> tfLookup;
- for(const std::string& tf: tfs)
+ for (const std::string& tf : tfs)
{
const auto frames = simox::alg::split(tf, ",");
ARMARX_CHECK_EQUAL(frames.size(), 2);
@@ -165,60 +155,66 @@ namespace armarx::armem::common::robot_state::localization
std::string currentFrame = query.header.parentFrame;
chain.push_back(currentFrame);
- while(tfLookup.count(currentFrame) > 0 and currentFrame != query.header.frame)
+ while (tfLookup.count(currentFrame) > 0 and currentFrame != query.header.frame)
{
currentFrame = tfLookup.at(currentFrame);
chain.push_back(currentFrame);
}
-
+
ARMARX_DEBUG << VAROUT(chain);
if (chain.empty() or chain.back() != query.header.frame)
{
- ARMARX_WARNING << deactivateSpam(60) << "Cannot create tf lookup chain '" << query.header.parentFrame
- << " -> " << query.header.frame << "' for robot `" + query.header.agent + "`.";
+ ARMARX_INFO << deactivateSpam(60) << "Cannot create tf lookup chain '"
+ << query.header.parentFrame << " -> " << query.header.frame
+ << "' for robot `" + query.header.agent + "`.";
return {};
}
std::vector<std::string> frameChain;
- for(size_t i = 0; i < (chain.size() - 1); i++)
+ for (size_t i = 0; i < (chain.size() - 1); i++)
{
- frameChain.push_back(chain.at(i) + "," + chain.at(i+1));
+ frameChain.push_back(chain.at(i) + "," + chain.at(i + 1));
}
return frameChain;
}
- template <class ...Args>
+ template <class... Args>
std::optional<armarx::core::time::DateTime>
- TransformHelper::_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp)
+ TransformHelper::_obtainTimestamp(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const armem::Time& timestamp)
{
-
+
// first we check which the newest timestamp is
std::optional<int64_t> timeSinceEpochUs = std::nullopt;
- localizationCoreSegment.forEachEntity([&timeSinceEpochUs, ×tamp](const auto& entity){
- auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
-
- if(snapshot == nullptr)
+ localizationCoreSegment.forEachEntity(
+ [&timeSinceEpochUs, ×tamp](const auto& entity)
{
- return;
- }
+ auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
- if(not snapshot->hasInstance(0))
- {
- return;
- }
+ if (snapshot == nullptr)
+ {
+ return;
+ }
+
+ if (not snapshot->hasInstance(0))
+ {
+ return;
+ }
- const armem::wm::EntityInstance& item = snapshot->getInstance(0);
- const auto tf = _convertEntityToTransform(item);
-
- const auto& dataTs = tf.header.timestamp;
+ const armem::wm::EntityInstance& item = snapshot->getInstance(0);
+ const auto tf = _convertEntityToTransform(item);
- timeSinceEpochUs = std::max(timeSinceEpochUs.value_or(0), dataTs.toMicroSecondsSinceEpoch());
- });
+ const auto& dataTs = tf.header.timestamp;
- if(not timeSinceEpochUs.has_value())
+ timeSinceEpochUs =
+ std::max(timeSinceEpochUs.value_or(0), dataTs.toMicroSecondsSinceEpoch());
+ });
+
+ if (not timeSinceEpochUs.has_value())
{
return std::nullopt;
}
@@ -226,11 +222,11 @@ namespace armarx::armem::common::robot_state::localization
// then we ensure that the timestamp is not more recent than the query timestamp
timeSinceEpochUs = std::min(timeSinceEpochUs.value(), timestamp.toMicroSecondsSinceEpoch());
- return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
+ return armarx::core::time::DateTime(
+ armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
}
-
- template <class ...Args>
+ template <class... Args>
std::vector<Eigen::Affine3f>
TransformHelper::_obtainTransforms(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
@@ -250,34 +246,32 @@ namespace armarx::armem::common::robot_state::localization
std::transform(tfChain.begin(),
tfChain.end(),
std::back_inserter(transforms),
- [&](const std::string & entityName)
- {
- return _obtainTransform(entityName, agentProviderSegment, timestamp);
- });
+ [&](const std::string& entityName) {
+ return _obtainTransform(entityName, agentProviderSegment, timestamp);
+ });
return transforms;
}
catch (const armem::error::MissingEntry& missingEntryError)
{
- ARMARX_WARNING << missingEntryError.what();
+ ARMARX_VERBOSE << missingEntryError.what();
}
catch (const ::armarx::exceptions::local::ExpressionException& ex)
{
- ARMARX_WARNING << "Local expression exception: " << ex.what();
+ ARMARX_VERBOSE << "Local expression exception: " << ex.what();
}
catch (const ::armarx::LocalException& ex)
{
- ARMARX_WARNING << "Local exception: " << ex.what();
+ ARMARX_VERBOSE << "Local exception: " << ex.what();
}
catch (...)
{
- ARMARX_WARNING << "Unexpected error: " << GetHandledExceptionString();
+ ARMARX_VERBOSE << "Unexpected error: " << GetHandledExceptionString();
}
return {};
}
-
- template <class ...Args>
+ template <class... Args>
Eigen::Affine3f
TransformHelper::_obtainTransform(
const std::string& entityName,
@@ -327,7 +321,6 @@ namespace armarx::armem::common::robot_state::localization
return transforms.front().transform;
}
-
::armarx::armem::robot_state::Transform
TransformHelper::_convertEntityToTransform(const armem::wm::EntityInstance& item)
{
@@ -344,18 +337,13 @@ namespace armarx::armem::common::robot_state::localization
findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
const armem::Time& timestamp)
{
- const auto comp = [](const armem::Time & timestamp, const auto & transform)
- {
- return transform.header.timestamp < timestamp;
- };
+ const auto comp = [](const armem::Time& timestamp, const auto& transform)
+ { return transform.header.timestamp < timestamp; };
const auto it = std::upper_bound(transforms.begin(), transforms.end(), timestamp, comp);
- auto timestampBeyond =
- [timestamp](const ::armarx::armem::robot_state::Transform & transform)
- {
- return transform.header.timestamp > timestamp;
- };
+ auto timestampBeyond = [timestamp](const ::armarx::armem::robot_state::Transform& transform)
+ { return transform.header.timestamp > timestamp; };
const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
@@ -374,7 +362,7 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_DEBUG << "Entering";
ARMARX_CHECK(not queue.empty())
- << "The queue has to contain at least two items to perform a lookup";
+ << "The queue has to contain at least two items to perform a lookup";
ARMARX_DEBUG << "Entering ... "
<< "Q front " << queue.front().header.timestamp << " "
@@ -384,11 +372,11 @@ namespace armarx::armem::common::robot_state::localization
// TODO(fabian.reister): sort queue.
ARMARX_CHECK(queue.back().header.timestamp > timestamp)
- << "Cannot perform lookup into the future!";
+ << "Cannot perform lookup into the future!";
// ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
ARMARX_CHECK(queue.front().header.timestamp < timestamp)
- << "Cannot perform lookup. Timestamp too old";
+ << "Cannot perform lookup. Timestamp too old";
// => now we know that there is an element right after and before the timestamp within our queue
ARMARX_DEBUG << "Entering 2";
@@ -402,36 +390,39 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_DEBUG << "deref";
// the time fraction [0..1] of the lookup wrt to posePre and poseNext
- const double t = (timestamp - posePreIt->header.timestamp).toMicroSecondsDouble() /
- (poseNextIt->header.timestamp - posePreIt->header.timestamp).toMicroSecondsDouble();
+ const double t =
+ (timestamp - posePreIt->header.timestamp).toMicroSecondsDouble() /
+ (poseNextIt->header.timestamp - posePreIt->header.timestamp).toMicroSecondsDouble();
ARMARX_DEBUG << "interpolate";
- return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, static_cast<float>(t));
+ return simox::math::interpolatePose(
+ posePreIt->transform, poseNextIt->transform, static_cast<float>(t));
}
-
- TransformResult TransformHelper::lookupTransform(
- const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+ TransformResult
+ TransformHelper::lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
return _lookupTransform(localizationCoreSegment, query);
}
- TransformResult TransformHelper::lookupTransform(
- const armem::server::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+
+ TransformResult
+ TransformHelper::lookupTransform(const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
return _lookupTransform(localizationCoreSegment, query);
}
-
- TransformChainResult TransformHelper::lookupTransformChain(
- const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+ TransformChainResult
+ TransformHelper::lookupTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
return _lookupTransformChain(localizationCoreSegment, query);
}
- TransformChainResult TransformHelper::lookupTransformChain(
+
+ TransformChainResult
+ TransformHelper::lookupTransformChain(
const armem::server::wm::CoreSegment& localizationCoreSegment,
const TransformQuery& query)
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index b8b3190d1bae586226c60b490bb399abe5799272..41eaf6e2ec2f2bcead697ec6ca7e86868bca44c4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -117,6 +117,24 @@ namespace armarx::armem::server::robot_state
}
}
+ void Visu::visualizeFramesIndividual(
+ viz::Layer& layerFrames,
+ const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
+ {
+
+ std::vector<std::string> FRAME_NAMES{/*world*/ "map", "odom", "robot"};
+
+ for (const auto& [robotName, robotFrames] : frames)
+ {
+ int i = 0;
+ for (const auto& frame : robotFrames)
+ {
+ layerFrames.add(viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
+ }
+ }
+ }
+
+
void Visu::visualizeRun()
{
@@ -213,6 +231,12 @@ namespace armarx::armem::server::robot_state
TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
+ {
+ viz::Layer layerFrames = arviz.layer("FramesIndividual");
+ visualizeFramesIndividual(layerFrames, frames);
+ layers.push_back(layerFrames);
+ }
+
// Commit layers.
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index 66222e0cf8dc6cf6043e6db35ebb7ba00e0cab00..99321b656be9977a23096e3b9bdd9ed4ba62a101 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -71,6 +71,11 @@ namespace armarx::armem::server::robot_state
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+ static
+ void visualizeFramesIndividual(
+ viz::Layer& layerFrames,
+ const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+
private:
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index 0c5ac1284fba2003474b9f885682de280a5b4403..e1469a80d35feecd9cb12914a62b944cabb4ba81 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -68,9 +68,8 @@ namespace armarx::armem::server::robot_state::proprioception
MemoryToIceAdapter& memory,
localization::Segment& localizationSegment)
{
- while (task and not task->isStopped())
+ while (isRunning())
{
-
// if (debugObserver)
// {
// debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", dataBuffer.size());
@@ -147,6 +146,11 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
+ bool
+ RobotStateWriter::isRunning()
+ {
+ return task and not task->isStopped();
+ }
RobotStateWriter::Update
RobotStateWriter::buildUpdate(const RobotUnitData& data)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 78863697a61ecd20c4947188a04965d5990c6f7b..27922266466b6156bb8f217dc887699a193027d9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -103,6 +103,7 @@ namespace armarx::armem::server::robot_state::proprioception
bool noOdometryDataLogged = false;
+ bool isRunning();
};
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index 53021dc7d88f9362a967f395b1381d07934f271e..34260f51e36951f7b9a0a68369cf8317e8a4dedc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -3,7 +3,6 @@
#include <filesystem>
#include <istream>
-
#include "ArmarXCore/core/time/Frequency.h"
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
@@ -11,13 +10,11 @@
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
namespace armarx::armem::server::robot_state::proprioception
{
RobotUnitReader::RobotUnitReader() = default;
-
void
RobotUnitReader::connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
@@ -48,7 +45,6 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
-
void
RobotUnitReader::run(float pollFrequency, Queue& dataBuffer)
{
@@ -56,14 +52,22 @@ namespace armarx::armem::server::robot_state::proprioception
while (task and not task->isStopped())
{
+ auto start = std::chrono::high_resolution_clock::now();
+
if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
// will lock a mutex
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t commitTimestamp [us]",
+ commit->timestamp.toMicroSecondsSinceEpoch());
+
dataBuffer.push(std::move(commit.value()));
}
-
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start);
if (debugObserver)
{
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t run [ms]",
+ duration.count() / 1000.f);
debugObserver->sendDebugObserverBatch();
}
@@ -71,13 +75,24 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
-
std::optional<RobotUnitData>
RobotUnitReader::fetchAndConvertLatestRobotUnitData()
{
ARMARX_CHECK_NOT_NULL(converterProprioception);
- const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
+
+ std::optional<RobotUnitDataStreaming::TimeStep> data;
+ {
+ auto start = std::chrono::high_resolution_clock::now();
+ data = fetchLatestData();
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start);
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t Fetch [ms]",
+ duration.count() / 1000.f);
+ }
+ }
if (not data.has_value())
{
return std::nullopt;
@@ -88,16 +103,16 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitData result;
- if(converterProprioception != nullptr)
+ if (converterProprioception != nullptr)
{
result.proprioception = converterProprioception->convert(data.value(), description);
}
- if(converterExteroception != nullptr)
+ if (converterExteroception != nullptr)
{
result.exteroception = converterExteroception->convert(data.value(), description);
}
-
+
result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
auto stop = std::chrono::high_resolution_clock::now();
@@ -114,7 +129,6 @@ namespace armarx::armem::server::robot_state::proprioception
return result;
}
-
std::optional<RobotUnitDataStreaming::TimeStep>
RobotUnitReader::fetchLatestData()
{
@@ -122,6 +136,14 @@ namespace armarx::armem::server::robot_state::proprioception
if (debugObserver)
{
debugObserver->setDebugObserverDatafield("RobotUnitReader | Buffer Size", data.size());
+ if (data.size())
+ {
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | RT Timestamp USec",
+ data.back().timestampUSec);
+ debugObserver->setDebugObserverDatafield(
+ "RobotUnitReader | RT Timestamp Since Last Iteration",
+ data.back().timesSinceLastIterationUSec);
+ }
}
if (data.empty())
{
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/Statechart.xml b/source/RobotAPI/libraries/armem_skills/aron/Statechart.xml
index 7b6721ef7dd188c231f70e8331a5c114c3f75c18..1bdbea9fe2bcb4cde0734b200fa3a2f3500564fa 100644
--- a/source/RobotAPI/libraries/armem_skills/aron/Statechart.xml
+++ b/source/RobotAPI/libraries/armem_skills/aron/Statechart.xml
@@ -19,7 +19,7 @@
<Object name='armarx::skills::arondto::Statechart::Transition'>
<ObjectChild key='processId'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key="sourceStateIdentifier">
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/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
index eed9e12ee2b2d5870ef6f0262bd2f23bda92fb9e..74a2ece78b8db2462731d6460836387d143cf514 100644
--- a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -19,16 +19,12 @@ armarx_add_library(
./aron_conversions.h
./client/occupancy_grid/Reader.h
./client/occupancy_grid/Writer.h
- ./client/laser_scanner_features/Reader.h
- ./client/laser_scanner_features/Writer.h
./OccupancyGridHelper.h
constants.h
SOURCES
./aron_conversions.cpp
./client/occupancy_grid/Reader.cpp
./client/occupancy_grid/Writer.cpp
- ./client/laser_scanner_features/Reader.cpp
- ./client/laser_scanner_features/Writer.cpp
./OccupancyGridHelper.cpp
)
@@ -36,7 +32,6 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
ARON_FILES
- aron/LaserScannerFeatures.xml
aron/OccupancyGrid.xml
)
diff --git a/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml b/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml
deleted file mode 100644
index 32600d7d19eef8f3969bde99c4a66d60635cf4c6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml
+++ /dev/null
@@ -1,67 +0,0 @@
-<!--Some fancy comment -->
-<?xml version="1.0" encoding="UTF-8" ?>
-<AronTypeDefinition>
- <CodeIncludes>
- </CodeIncludes>
- <AronIncludes>
- </AronIncludes>
-
- <GenerateTypes>
-
- <Object name='armarx::armem::vision::arondto::Ellipsoid'>
- <ObjectChild key='globalPose'>
- <Pose />
- </ObjectChild>
- <ObjectChild key='radii'>
- <Matrix rows="2" cols="1" type="float32" />
- </ObjectChild>
- </Object>
-
- <Object name='armarx::armem::vision::arondto::Circle'>
- <ObjectChild key='center'>
- <Matrix rows="2" cols="1" type="float32" />
- </ObjectChild>
- <ObjectChild key='radius'>
- <float />
- </ObjectChild>
- </Object>
-
- <Object name="armarx::armem::vision::arondto::LaserScannerFeature">
- <ObjectChild key="convexHull">
- <List>
- <Matrix rows="2" cols="1" type="float32" />
- </List>
- </ObjectChild>
- <ObjectChild key="circle">
- <armarx::armem::vision::arondto::Circle/>
- </ObjectChild>
- <ObjectChild key="ellipsoid">
- <armarx::armem::vision::arondto::Ellipsoid/>
- </ObjectChild>
- <!-- <ObjectChild key="chain">
- <armarx::armem::vision::arondto::Chain/>
- </ObjectChild> -->
- <ObjectChild key="points">
- <List>
- <Matrix rows="2" cols="1" type="float32" />
- </List>
- </ObjectChild>
- </Object>
-
- <Object name="armarx::armem::vision::arondto::LaserScannerFeatures">
- <ObjectChild key="frame">
- <String/>
- </ObjectChild>
- <ObjectChild key="frameGlobalPose">
- <Pose/>
- </ObjectChild>
- <ObjectChild key="features">
- <List>
- <armarx::armem::vision::arondto::LaserScannerFeature />
- </List>
- </ObjectChild>
- </Object>
-
-
- </GenerateTypes>
-</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
index 3f273cded23f6942d6be82f76521f929146fb21e..f3fd627a3329e164d742dceedab78fcb753db2b4 100644
--- a/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
+++ b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
@@ -10,7 +10,7 @@
<Object name='armarx::armem::vision::arondto::OccupancyGrid'>
<ObjectChild key='resolution'>
- <float />
+ <float32 />
</ObjectChild>
<ObjectChild key='frame'>
<string />
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index 86a6c62c0e047f528a5b4eb649056659886d18d8..7051c186ac290a3e71fe3cbb435d1b9ab09b997d 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -4,34 +4,18 @@
#include <cstdint>
#include <iterator>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
#include "types.h"
-
namespace armarx::armem::vision
{
/************ toAron ************/
- // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
- // {
- // aronLaserScan.scan = toAron(laserScan);
- // }
-
- int64_t
- toAron(const armem::Time& timestamp)
- {
- return timestamp.toMicroSecondsSinceEpoch();
- }
-
-
void
toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
{
@@ -50,70 +34,4 @@ namespace armarx::armem::vision
// bo.grid is NdArray -> need special handling.
}
- // LaserScannerFeatures
-
-
- void
- toAron(arondto::Circle& dto, const Circle& bo)
- {
- dto.center = bo.center;
- dto.radius = bo.radius;
- }
-
- void
- toAron(arondto::Ellipsoid& dto, const Ellipsoid& bo)
- {
- dto.globalPose = bo.pose.matrix();
- dto.radii = bo.radii;
- }
-
- void
- toAron(arondto::LaserScannerFeature& dto, const LaserScannerFeature& bo)
- {
- toAron(dto.circle, bo.circle);
- dto.convexHull = bo.convexHull;
- toAron(dto.ellipsoid, bo.ellipsoid);
- dto.points = bo.points;
- }
-
- void
- toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo)
- {
- aron::toAron(dto.frame, bo.frame);
- aron::toAron(dto.frameGlobalPose, bo.frameGlobalPose);
- aron::toAron(dto.features, bo.features);
- }
-
- void
- fromAron(const arondto::Circle& dto, Circle& bo)
- {
- bo.center = dto.center;
- bo.radius = dto.radius;
- }
- void
- fromAron(const arondto::Ellipsoid& dto, Ellipsoid& bo)
- {
- bo.pose = dto.globalPose;
- bo.radii = dto.radii;
- }
-
- void
- fromAron(const arondto::LaserScannerFeature& dto, LaserScannerFeature& bo)
- {
- bo.convexHull = dto.convexHull;
- fromAron(dto.circle, bo.circle);
- fromAron(dto.ellipsoid, bo.ellipsoid);
-
- // bo.chain = dto.chain;
- bo.points = dto.points;
- }
-
- void
- fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo)
- {
- aron::fromAron(dto.frame, bo.frame);
- aron::fromAron(dto.frameGlobalPose, bo.frameGlobalPose);
- aron::fromAron(dto.features, bo.features);
- }
-
} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
index 4d70758f166b66666b9feb22b2dd2a754082bb76..f6f1a53225698f2617559073db3f19c3351b1dd1 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -21,9 +21,7 @@
#pragma once
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
@@ -33,42 +31,6 @@
namespace armarx::armem::vision
{
- namespace arondto
- {
- struct LaserScanStamped;
- } // namespace arondto
-
- // struct LaserScan;
- struct LaserScanStamped;
-
- void fromAron(const arondto::LaserScanStamped& aronLaserScan,
- LaserScan& laserScan,
- std::int64_t& timestamp,
- std::string& frame,
- std::string& agentName);
-
- template <typename T>
- auto
- fromAron(const aron::data::NDArrayPtr& navigator)
- {
- return aron::data::converter::AronVectorConverter::ConvertToVector<T>(navigator);
- }
-
- void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
-
- void toAron(const LaserScan& laserScan,
- const armem::Time& timestamp,
- const std::string& frame,
- const std::string& agentName,
- arondto::LaserScanStamped& aronLaserScan);
-
- inline aron::data::NDArrayPtr
- toAron(const LaserScan& laserScan)
- {
- using aron::data::converter::AronVectorConverter;
- return AronVectorConverter::ConvertFromVector(laserScan);
- }
-
// OccupancyGrid
void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo);
void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo);
@@ -79,8 +41,4 @@ namespace armarx::armem::vision
return aron::data::converter::AronEigenConverter::ConvertFromArray(grid);
}
- // LaserScannerFeatures
- void toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo);
- void fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo);
-
} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
deleted file mode 100644
index 216088b410231671dd49a22a95f9fa2b6ae9362b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
+++ /dev/null
@@ -1,218 +0,0 @@
-#include "Reader.h"
-
-// STD / STL
-#include <algorithm>
-#include <cstring>
-#include <map>
-#include <optional>
-#include <ostream>
-#include <type_traits>
-#include <utility>
-#include <vector>
-
-// ICE
-#include <IceUtil/Handle.h>
-#include <IceUtil/Time.h>
-
-// Simox
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
-// ArmarXCore
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/LogSender.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-// RobotAPI Interfaces
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-
-// RobotAPI Aron
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
-
-// RobotAPI Armem
-#include <RobotAPI/libraries/armem/client/Query.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/selectors.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
-
-
-namespace armarx::armem::vision::laser_scanner_features::client
-{
-
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
- Reader::~Reader() = default;
-
-
- void
- Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
- {
- // ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- // registerPropertyDefinitions(def);
-
- const std::string prefix = propertyPrefix;
-
- def->optional(properties.coreSegmentName,
- prefix + "CoreSegment",
- "Name of the mapping memory core segment to use.");
-
- def->optional(properties.memoryName, prefix + "MemoryName");
- }
-
- void
- Reader::connect()
- {
- // Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << properties.memoryName
- << "' ...";
- try
- {
- memoryReader =
- memoryNameSystem.useReader(MemoryID().withMemoryName(properties.memoryName));
- ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName
- << "'";
- }
- catch (const armem::error::CouldNotResolveMemoryServer& e)
- {
- ARMARX_ERROR << e.what();
- return;
- }
- }
-
- armarx::armem::client::query::Builder
- Reader::buildQuery(const Query& query) const
- {
- armarx::armem::client::query::Builder qb;
-
- qb.coreSegments()
- .withName(properties.coreSegmentName)
- .providerSegments()
- .withName(query.providerName)
- .entities()
- .all()
- .snapshots()
- .beforeOrAtTime(query.timestamp);
-
- // auto entitySel = [&]()
- // {
- // if (query.name.empty())
- // {
- // ARMARX_INFO << "querying all entities";
- // return sel.entities().all();
- // }
- // return sel.entities().withName(query.name);
- // }();
-
- // entitySel.snapshots().beforeOrAtTime(query.timestamp);
-
- return qb;
- }
-
- std::vector<LaserScannerFeatures>
- asFeaturesList(const wm::ProviderSegment& providerSegment)
- {
- if (providerSegment.empty())
- {
- ARMARX_WARNING << "No entities!";
- return {};
- }
-
- // const auto convert = [](const auto& aronLaserScanStamped,
- // const wm::EntityInstance& ei) -> LaserScanStamped
- // {
- // LaserScanStamped laserScanStamped;
- // fromAron(aronLaserScanStamped, laserScanStamped);
-
- // const auto ndArrayNavigator =
- // aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
-
- // ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
-
- // laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
- // ARMARX_IMPORTANT << "4";
-
- // return laserScanStamped;
- // };
-
- std::vector<LaserScannerFeatures> laserScannerFeatures;
-
- // loop over all entities and their snapshots
- providerSegment.forEachEntity(
- [&](const wm::Entity& entity)
- {
- // If we don't need this warning, we could directly iterate over the snapshots.
- if (entity.empty())
- {
- ARMARX_WARNING << "Empty history for " << entity.id();
- }
- ARMARX_DEBUG << "History size: " << entity.size();
-
- entity.forEachInstance(
- [&](const wm::EntityInstance& entityInstance)
- {
- if (const auto o = tryCast<arondto::LaserScannerFeatures>(entityInstance))
- {
- LaserScannerFeatures& f = laserScannerFeatures.emplace_back();
- fromAron(o.value(), f);
- }
- return true;
- });
- return true;
- });
-
- return laserScannerFeatures;
- }
-
- Reader::Result
- Reader::queryData(const Query& query) const
- {
- const auto qb = buildQuery(query);
-
- ARMARX_DEBUG << "[MappingDataReader] query ... ";
-
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
-
- ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
-
- if (not qResult.success)
- {
- ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
- return {.features = {},
- .status = Result::Status::Error,
- .errorMessage = qResult.errorMessage};
- }
-
- // now create result from memory
- const wm::ProviderSegment& providerSegment =
- qResult.memory.getCoreSegment(properties.coreSegmentName)
- .getProviderSegment(query.providerName);
-
- const auto features = asFeaturesList(providerSegment);
-
- // const auto laserScans = asLaserScans(providerSegment);
- // std::vector<std::string> sensors;
- // providerSegment.forEachEntity(
- // [&sensors](const wm::Entity& entity)
- // {
- // sensors.push_back(entity.name());
- // return true;
- // });
-
- return {.features = features,
- // .sensors = sensors,
- .status = Result::Status::Success,
- .errorMessage = ""};
- }
-
-} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h
deleted file mode 100644
index 4752ca85cf3df286b3d1378c2a0526f5a89f1a31..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * 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
-
-#include <string>
-#include <vector>
-
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <ArmarXCore/core/time/DateTime.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_vision/types.h>
-
-
-namespace armarx
-{
- class ManagedIceObject;
-}
-
-namespace armarx::armem::vision::laser_scanner_features::client
-{
-
-
- /**
- * @defgroup Component-ExampleClient ExampleClient
- * @ingroup RobotAPI-Components
- * A description of the component ExampleClient.
- *
- * @class ExampleClient
- * @ingroup Component-ExampleClient
- * @brief Brief description of class ExampleClient.
- *
- * Detailed description of class ExampleClient.
- */
- class Reader
- {
- public:
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
- virtual ~Reader();
-
- void connect();
-
- struct Query
- {
- std::string providerName;
-
- std::string name; // sensor name
-
- armarx::core::time::DateTime timestamp;
-
- // std::vector<std::string> sensorList;
- };
-
- struct Result
- {
- std::vector<LaserScannerFeatures> features;
-
- // std::vector<std::string> sensors;
-
- enum Status
- {
- Error,
- Success
- } status;
-
- std::string errorMessage;
- };
-
- Result queryData(const Query& query) const;
-
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
-
-
- protected:
- armarx::armem::client::query::Builder buildQuery(const Query& query) const;
-
- private:
- armem::client::MemoryNameSystem& memoryNameSystem;
- armem::client::Reader memoryReader;
-
- // Properties
- struct Properties
- {
- std::string memoryName = "Vision";
- std::string coreSegmentName = "LaserScannerFeatures";
- } properties;
-
- const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
- };
-
-} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
deleted file mode 100644
index 8ae7ba4379288420d3578d7f30ae9bc9c58c559d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
+++ /dev/null
@@ -1,111 +0,0 @@
-#include "Writer.h"
-
-#include "RobotAPI/libraries/armem_vision/constants.h"
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
-
-
-namespace armarx::armem::vision::laser_scanner_features::client
-{
-
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
- Writer::~Writer() = default;
-
-
- void
- Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
- {
- ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
-
- const std::string prefix = propertyPrefix;
-
- // def->optional(properties.coreSegmentName,
- // prefix + "CoreSegment",
- // "Name of the mapping memory core segment to use.");
-
- // def->optional(properties.memoryName, prefix + "MemoryName");
- }
-
- void
- Writer::connect()
- {
- // Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
- << "' ...";
- try
- {
- memoryWriter =
- memoryNameSystem.useWriter(MemoryID().withMemoryName(constants::memoryName));
- ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << constants::memoryName
- << "'";
- }
- catch (const armem::error::CouldNotResolveMemoryServer& e)
- {
- ARMARX_ERROR << e.what();
- return;
- }
-
- ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << constants::memoryName;
- }
-
- bool
- Writer::store(const LaserScannerFeatures& features,
- const std::string& providerName,
- const Time& timestamp)
- {
- std::lock_guard g{memoryWriterMutex};
-
- // const auto result = memoryWriter.addSegment(constants::memoryName,
- // constants::laserScannerFeaturesCoreSegment);
-
- // if (not result.success)
- // {
- // ARMARX_ERROR << result.errorMessage;
-
- // // TODO(fabian.reister): message
- // return false;
- // }
-
- const auto entityID = armem::MemoryID()
- .withMemoryName(constants::memoryName)
- .withCoreSegmentName(constants::laserScannerFeaturesCoreSegment)
- .withProviderSegmentName(providerName)
- .withEntityName(features.frame)
- .withTimestamp(timestamp);
-
- ARMARX_VERBOSE << "Memory id is " << entityID.str();
-
- armem::EntityUpdate update;
- update.entityID = entityID;
-
- ARMARX_TRACE;
-
- arondto::LaserScannerFeatures dto;
- toAron(dto, features);
-
- ARMARX_TRACE;
-
- update.instancesData = {dto.toAron()};
- update.referencedTime = timestamp;
-
- ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
-
- ARMARX_TRACE;
- armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
-
- ARMARX_DEBUG << updateResult;
-
- if (not updateResult.success)
- {
- ARMARX_ERROR << updateResult.errorMessage;
- }
-
- return updateResult.success;
- }
-
-} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h
deleted file mode 100644
index ca58005772268d70f7eedf06189c02250c75fef6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * 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::
- * @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
-
-#include <mutex>
-
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-#include "RobotAPI/libraries/armem_vision/types.h"
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-
-
-namespace armarx::armem::vision::laser_scanner_features::client
-{
-
- /**
- * @defgroup Component-ExampleClient ExampleClient
- * @ingroup RobotAPI-Components
- * A description of the component ExampleClient.
- *
- * @class ExampleClient
- * @ingroup Component-ExampleClient
- * @brief Brief description of class ExampleClient.
- *
- * Detailed description of class ExampleClient.
- */
- class Writer
- {
- public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystem);
- virtual ~Writer();
-
-
- void connect();
-
- // MappingDataWriterInterface
- /// to be called in Component::onConnectComponent
- // void connect() override;
-
- /// to be called in Component::addPropertyDefinitions
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
-
- bool store(const LaserScannerFeatures& features,
- const std::string& providerName,
- const Time& timestamp);
-
- private:
- armem::client::MemoryNameSystem& memoryNameSystem;
- armem::client::Writer memoryWriter;
-
- // Properties
- struct Properties
- {
- // std::string memoryName = "Vision";
- // std::string coreSegmentName = "LaserScannerFeatures";
- } properties;
-
- std::mutex memoryWriterMutex;
-
- const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
- };
-
-} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/constants.h b/source/RobotAPI/libraries/armem_vision/constants.h
index 653d6fda6fc9e787d00f570af5440a6c81c6108b..3474d925b7b548068fc9059d82e5ec469b0f24b3 100644
--- a/source/RobotAPI/libraries/armem_vision/constants.h
+++ b/source/RobotAPI/libraries/armem_vision/constants.h
@@ -28,6 +28,5 @@ namespace armarx::armem::vision::constants
const inline std::string memoryName = "Vision";
// core segments
- const inline std::string laserScannerFeaturesCoreSegment = "LaserScannerFeatures";
} // namespace armarx::armem::vision::constants
diff --git a/source/RobotAPI/libraries/armem_vision/types.h b/source/RobotAPI/libraries/armem_vision/types.h
index fe584e3dc235a19333fd9b174cfab390c1a5d435..1a6b6b7d69eb52cc7857744dfb774564ac984720 100644
--- a/source/RobotAPI/libraries/armem_vision/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -47,45 +47,5 @@ namespace armarx::armem::vision
Grid grid;
};
- struct Ellipsoid
- {
- Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
-
- Eigen::Vector2f radii = Eigen::Vector2f::Zero();
- };
-
- struct Circle
- {
- Eigen::Vector2f center = Eigen::Vector2f::Zero();
- float radius = 0.F;
- };
-
- struct LaserScannerFeature
- {
- using Points = std::vector<Eigen::Vector2f>;
- using Chain = Points;
-
- Points convexHull;
-
- Circle circle;
- Ellipsoid ellipsoid;
-
- Chain chain;
-
- Points points;
- };
-
- struct LaserScannerFeatures
- {
- // TODO(fabian.reister): framed pose
- std::string frame;
- Eigen::Isometry3f frameGlobalPose;
-
- std::vector<LaserScannerFeature> features;
-
-
- // std::vector<Ellipsoid> linesAsEllipsoids(float axisLength) const;
- };
-
} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/aron/CMakeLists.txt b/source/RobotAPI/libraries/aron/CMakeLists.txt
index 692784791b779d28035abc83adb5354033a4b335..f5892473e96f66d0b3dc3f89ed8f00ccff8a724a 100644
--- a/source/RobotAPI/libraries/aron/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/CMakeLists.txt
@@ -1,7 +1,9 @@
add_subdirectory(core)
+add_subdirectory(codegeneration_util)
+add_subdirectory(codegeneration)
+
add_subdirectory(common)
add_subdirectory(converter)
add_subdirectory(filter)
-add_subdirectory(codegeneration)
add_subdirectory(similarity)
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/codegeneration/CMakeLists.txt b/source/RobotAPI/libraries/aron/codegeneration/CMakeLists.txt
index c3a800811daceb9e10dbad233aec1b071e72d83b..a89c63b19217c9e5cfafcfcbf4c709f786dbca3a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/codegeneration/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIB_NAME aroncodegenerator)
+set(LIB_NAME aroncodegeneration)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
@@ -12,6 +12,7 @@ set(LIBS
RobotAPIInterfaces
cppgen
aron
+ aroncodegenerationutil
SimoxUtility
)
@@ -115,9 +116,11 @@ armarx_add_library(
"${LIBS}"
)
-add_library(AronCodegenerator ALIAS "${LIB_NAME}")
-add_library(RobotAPI::AronCodegenerator ALIAS "${LIB_NAME}")
-add_library(RobotAPI::aroncodegenerator ALIAS "${LIB_NAME}")
+add_library(AronCodegeneration ALIAS "${LIB_NAME}")
+add_library(RobotAPI::AronCodegeneration ALIAS "${LIB_NAME}")
+add_library(RobotAPI::aroncodegeneration ALIAS "${LIB_NAME}")
+add_library(RobotAPI::aron::Codegeneration ALIAS "${LIB_NAME}")
+add_library(RobotAPI::aron::codegeneration ALIAS "${LIB_NAME}")
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
index d3c61320d0bd8d1f57fd03531edf1f79df5754f2..076d5aa61a6d0c79e4e3a56453675308e09f7bde 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
@@ -24,40 +24,41 @@
// STD/STL
#include "Generator.h"
-#include "Factory.h"
-#include <SimoxUtility/meta/type_name.h>
#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/meta/type_name.h>
+#include "Factory.h"
namespace armarx::aron::codegenerator::cpp
{
// constantes
const std::string Generator::ARON_VARIABLE_PREFIX = "aron";
- const std::string Generator::ARON_MAYBE_TYPE_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_maybeType";
+ const std::string Generator::ARON_MAYBE_TYPE_ACCESSOR =
+ Generator::ARON_VARIABLE_PREFIX + "_maybeType";
const std::string Generator::ARON_READER_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_r";
const std::string Generator::ARON_WRITER_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_w";
- const std::string Generator::ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_tmpls";
- const std::string Generator::ARON_VARIANT_RETURN_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_variant";
+ const std::string Generator::ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR =
+ Generator::ARON_VARIABLE_PREFIX + "_tmpls";
+ const std::string Generator::ARON_VARIANT_RETURN_ACCESSOR =
+ Generator::ARON_VARIABLE_PREFIX + "_variant";
const std::string Generator::ARON_PATH_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_p";
const std::string Generator::ARON_OTHER_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_o";
const SerializerFactoryPtr Generator::FACTORY = SerializerFactoryPtr(new GeneratorFactory());
// static methods
- std::string Generator::EscapeAccessor(const std::string& accessor)
+ std::string
+ Generator::EscapeAccessor(const std::string& accessor)
{
- const static std::map<std::string, std::string> ESCAPE_ACCESSORS =
- {
- {"->", "_ptr_"},
- {".", "_dot_"},
- {"[", "_lbrC_"},
- {"]", "_rbrC_"},
- {"(", "_lbrR_"},
- {")", "_rbrR_"},
- {"*", "_ast_"}
- };
+ const static std::map<std::string, std::string> ESCAPE_ACCESSORS = {{"->", "_ptr_"},
+ {".", "_dot_"},
+ {"[", "_lbrC_"},
+ {"]", "_rbrC_"},
+ {"(", "_lbrR_"},
+ {")", "_rbrR_"},
+ {"*", "_ast_"}};
std::string escaped_accessor = accessor;
for (const auto& [key, value] : ESCAPE_ACCESSORS)
@@ -67,13 +68,16 @@ namespace armarx::aron::codegenerator::cpp
return escaped_accessor;
}
- std::string Generator::ExtractCppTypename(const type::Variant& n)
+ std::string
+ Generator::ExtractCppTypename(const type::Variant& n)
{
auto cpp = Generator::FromAronType(n);
ARMARX_CHECK_NOT_NULL(cpp);
return cpp->getInstantiatedCppTypename();
}
- std::vector<std::string> Generator::ExtractCppTypenames(const std::vector<type::VariantPtr>& n)
+
+ std::vector<std::string>
+ Generator::ExtractCppTypenames(const std::vector<type::VariantPtr>& n)
{
std::vector<std::string> ret;
for (const auto& v : n)
@@ -83,29 +87,33 @@ namespace armarx::aron::codegenerator::cpp
return ret;
}
- std::unique_ptr<Generator> Generator::FromAronType(const type::Variant& n)
+ std::unique_ptr<Generator>
+ Generator::FromAronType(const type::Variant& n)
{
return FACTORY->create(n, n.getPath());
}
-
// constructors
- Generator::Generator(const std::string& instantiatedCppTypename, const std::string& classCppTypename, const std::string& aronDataTypename, const std::string& aronTypeTypename) :
+ Generator::Generator(const std::string& instantiatedCppTypename,
+ const std::string& classCppTypename,
+ const std::string& aronDataTypename,
+ const std::string& aronTypeTypename) :
instantiatedCppTypename(instantiatedCppTypename),
classCppTypename(classCppTypename),
aronDataTypename(aronDataTypename),
aronTypeTypename(aronTypeTypename)
{
-
}
// public methods
- std::string Generator::getInstantiatedCppTypename() const
+ std::string
+ Generator::getInstantiatedCppTypename() const
{
return instantiatedCppTypename;
}
- std::string Generator::getFullInstantiatedCppTypename() const
+ std::string
+ Generator::getFullInstantiatedCppTypename() const
{
switch (getType().getMaybe())
{
@@ -120,11 +128,15 @@ namespace armarx::aron::codegenerator::cpp
case type::Maybe::UNIQUE_PTR:
return "std::unique_ptr<" + getInstantiatedCppTypename() + ">";
default:
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Received unknown maybe enum", std::to_string((int) getType().getMaybe()), getType().getPath());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Received unknown maybe enum",
+ std::to_string((int)getType().getMaybe()),
+ getType().getPath());
}
}
- std::string Generator::getFullInstantiatedCppTypenameGenerator() const
+ std::string
+ Generator::getFullInstantiatedCppTypenameGenerator() const
{
switch (getType().getMaybe())
{
@@ -139,16 +151,21 @@ namespace armarx::aron::codegenerator::cpp
case type::Maybe::UNIQUE_PTR:
return "std::make_unique<" + getInstantiatedCppTypename() + ">";
default:
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Received unknown maybe enum", std::to_string((int) getType().getMaybe()), getType().getPath());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Received unknown maybe enum",
+ std::to_string((int)getType().getMaybe()),
+ getType().getPath());
}
}
- std::string Generator::getClassCppTypename() const
+ std::string
+ Generator::getClassCppTypename() const
{
return classCppTypename;
}
- std::string Generator::getFullClassCppTypename() const
+ std::string
+ Generator::getFullClassCppTypename() const
{
switch (getType().getMaybe())
{
@@ -163,11 +180,15 @@ namespace armarx::aron::codegenerator::cpp
case type::Maybe::UNIQUE_PTR:
return "std::unique_ptr<" + getClassCppTypename() + ">";
default:
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Received unknown maybe enum", std::to_string((int) getType().getMaybe()), getType().getPath());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Received unknown maybe enum",
+ std::to_string((int)getType().getMaybe()),
+ getType().getPath());
}
}
- CppCtorPtr Generator::toCtor(const std::string& name) const
+ CppCtorPtr
+ Generator::toCtor(const std::string& name) const
{
CppBlockPtr b = this->getCtorBlock("");
auto initList = this->getCtorInitializers("");
@@ -184,24 +205,28 @@ namespace armarx::aron::codegenerator::cpp
return c;
}
- CppCtorPtr Generator::toCopyCtor(const std::string& name) const
+ CppCtorPtr
+ Generator::toCopyCtor(const std::string& name) const
{
CppBlockPtr b = this->getCopyCtorBlock("");
auto initList = this->getCopyCtorInitializers("");
if (b->size() > 0 || initList.second)
{
- CppCtorPtr c = CppCtorPtr(new CppCtor(name + "(const " + name + "& " + ARON_OTHER_ACCESSOR + ")"));
+ CppCtorPtr c =
+ CppCtorPtr(new CppCtor(name + "(const " + name + "& " + ARON_OTHER_ACCESSOR + ")"));
c->addInitListEntries(initList.first);
c->setBlock(b);
return c;
}
- CppCtorPtr c = CppCtorPtr(new CppCtor(name + "(const " + name + "& " + ARON_OTHER_ACCESSOR + ") = default;"));
+ CppCtorPtr c = CppCtorPtr(
+ new CppCtor(name + "(const " + name + "& " + ARON_OTHER_ACCESSOR + ") = default;"));
return c;
}
- CppMethodPtr Generator::toDtor(const std::string& name) const
+ CppMethodPtr
+ Generator::toDtor(const std::string& name) const
{
CppBlockPtr b = this->getDtorBlock("");
if (b->size() > 0)
@@ -218,13 +243,16 @@ namespace armarx::aron::codegenerator::cpp
return m;
}
- CppMethodPtr Generator::toResetSoftMethod() const
+ CppMethodPtr
+ Generator::toResetSoftMethod() const
{
std::stringstream doc;
- doc << "@brief resetSoft() - This method resets all member variables with respect to the current parameterization. \n";
+ doc << "@brief resetSoft() - This method resets all member variables with respect to the "
+ "current parameterization. \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetSoft() override", doc.str()));
+ CppMethodPtr m =
+ CppMethodPtr(new CppMethod("virtual void resetSoft() override", doc.str()));
CppBlockPtr b = this->getResetSoftBlock("");
m->setBlock(b);
@@ -232,13 +260,16 @@ namespace armarx::aron::codegenerator::cpp
return m;
}
- CppMethodPtr Generator::toResetHardMethod() const
+ CppMethodPtr
+ Generator::toResetHardMethod() const
{
std::stringstream doc;
- doc << "@brief resetHard() - This method resets member variables according to the XML type description. \n";
+ doc << "@brief resetHard() - This method resets member variables according to the XML type "
+ "description. \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetHard() override", doc.str()));
+ CppMethodPtr m =
+ CppMethodPtr(new CppMethod("virtual void resetHard() override", doc.str()));
CppBlockPtr b = this->getResetHardBlock("");
m->setBlock(b);
@@ -246,13 +277,22 @@ namespace armarx::aron::codegenerator::cpp
return m;
}
- CppMethodPtr Generator::toWriteTypeMethod() const
+ CppMethodPtr
+ Generator::toWriteTypeMethod() const
{
std::stringstream doc;
- doc << "@brief writeType() - This method returns a new type from the class structure using a type writer implementation. This function is static. \n";
+ doc << "@brief writeType() - This method returns a new type from the class structure using "
+ "a type writer implementation. This function is static. \n";
doc << "@return - the result of the writer implementation";
- CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\nstatic typename WriterT::ReturnType writeType(WriterT& " + ARON_WRITER_ACCESSOR + ", std::vector<std::string> " + ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + " = {}, armarx::aron::type::Maybe "+ ARON_MAYBE_TYPE_ACCESSOR +" = ::armarx::aron::type::Maybe::NONE, const ::armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = ::armarx::aron::Path())", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(
+ "template<class WriterT>\nstatic typename WriterT::ReturnType writeType(WriterT& " +
+ ARON_WRITER_ACCESSOR + ", std::vector<std::string> " +
+ ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + " = {}, armarx::aron::type::Maybe " +
+ ARON_MAYBE_TYPE_ACCESSOR +
+ " = ::armarx::aron::type::Maybe::NONE, const ::armarx::aron::Path& " +
+ ARON_PATH_ACCESSOR + " = ::armarx::aron::Path())",
+ doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("using _Aron_T [[maybe_unused]] = typename WriterT::ReturnType;");
@@ -264,14 +304,20 @@ namespace armarx::aron::codegenerator::cpp
return m;
}
- CppMethodPtr Generator::toWriteMethod() const
+ CppMethodPtr
+ Generator::toWriteMethod() const
{
std::stringstream doc;
- doc << "@brief write() - This method returns a new type from the member data types using a data writer implementation. \n";
+ doc << "@brief write() - This method returns a new type from the member data types using a "
+ "data writer implementation. \n";
doc << "@param w - The writer implementation\n";
doc << "@return - the result of the writer implementation";
- CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\ntypename WriterT::ReturnType write(WriterT& " + ARON_WRITER_ACCESSOR + ", const ::armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path()) const", doc.str()));
+ CppMethodPtr m = CppMethodPtr(
+ new CppMethod("template<class WriterT>\ntypename WriterT::ReturnType write(WriterT& " +
+ ARON_WRITER_ACCESSOR + ", const ::armarx::aron::Path& " +
+ ARON_PATH_ACCESSOR + " = armarx::aron::Path()) const",
+ doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("using _Aron_T [[maybe_unused]] = typename WriterT::ReturnType;");
@@ -279,73 +325,106 @@ namespace armarx::aron::codegenerator::cpp
std::string dummy;
b->addBlock(this->getWriteBlock("", Path(), dummy));
b->addLine("catch(const std::exception& " + ARON_VARIABLE_PREFIX + "_e)");
- b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, std::string(\"An error occured during the write method of an aron generated class. The full error log was:\\n\") + " + ARON_VARIABLE_PREFIX + "_e.what());");
+ b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, "
+ "std::string(\"An error occured during the write method of an aron "
+ "generated class. The full error log was:\\n\") + " +
+ ARON_VARIABLE_PREFIX + "_e.what());");
m->setBlock(b);
m->setEnforceBlockGeneration(true);
return m;
}
- CppMethodPtr Generator::toReadMethod() const
+ CppMethodPtr
+ Generator::toReadMethod() const
{
std::stringstream doc;
- doc << "@brief read() - This method sets the struct members to new values given in a data reader implementation. \n";
+ doc << "@brief read() - This method sets the struct members to new values given in a data "
+ "reader implementation. \n";
doc << "@param r - The reader implementation\n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("template<class ReaderT>\nvoid read(ReaderT& " + ARON_READER_ACCESSOR + ", typename ReaderT::InputType& input)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod("template<class ReaderT>\nvoid read(ReaderT& " +
+ ARON_READER_ACCESSOR +
+ ", typename ReaderT::InputType& input)",
+ doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("using _Aron_T [[maybe_unused]] = typename ReaderT::InputType;");
b->addLine("using _Aron_TNonConst [[maybe_unused]] = typename ReaderT::InputTypeNonConst;");
b->addLine("this->resetSoft();");
- b->addLine("ARMARX_CHECK_AND_THROW(!" + ARON_READER_ACCESSOR + ".readNull(input), ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"The input to the read method must not be null.\"));");
+ b->addLine("ARMARX_CHECK_AND_THROW(!" + ARON_READER_ACCESSOR +
+ ".readNull(input), ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, "
+ "\"The input to the read method must not be null.\"));");
b->addLine("try");
b->addBlock(this->getReadBlock("", "input"));
b->addLine("catch(const std::exception& " + ARON_VARIABLE_PREFIX + "_e)");
- b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, std::string(\"An error occured during the read method of an aron generated class. The full error log was:\\n\") + " + ARON_VARIABLE_PREFIX + "_e.what());");
+ b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, "
+ "std::string(\"An error occured during the read method of an aron "
+ "generated class. The full error log was:\\n\") + " +
+ ARON_VARIABLE_PREFIX + "_e.what());");
m->setBlock(b);
m->setEnforceBlockGeneration(true);
return m;
}
- CppMethodPtr Generator::toSpecializedDataWriterMethod(const WriterInfo& info) const
+ CppMethodPtr
+ Generator::toSpecializedDataWriterMethod(const WriterInfo& info) const
{
std::stringstream doc;
- doc << "@brief " << info.methodName << "() - This method returns a new data from the member data types using a writer implementation. \n";
+ doc << "@brief " << info.methodName
+ << "() - This method returns a new data from the member data types using a writer "
+ "implementation. \n";
doc << "@return - the result of the writer implementation";
- CppMethodPtr m = CppMethodPtr(new CppMethod(info.returnType + " " + info.methodName + "() const" + (info.override ? " override" : ""), doc.str()));
+ CppMethodPtr m =
+ CppMethodPtr(new CppMethod(info.returnType + " " + info.methodName + "() const" +
+ (info.override ? " override" : ""),
+ doc.str()));
m->addLine(info.writerClassType + " writer;");
- m->addLine("return " + info.enforceConversion + "(this->write(writer))" + info.enforceMemberAccess + ";");
+ m->addLine("return " + info.enforceConversion + "(this->write(writer))" +
+ info.enforceMemberAccess + ";");
m->setEnforceBlockGeneration(true);
return m;
}
- CppMethodPtr Generator::toSpecializedDataReaderMethod(const ReaderInfo& info) const
+ CppMethodPtr
+ Generator::toSpecializedDataReaderMethod(const ReaderInfo& info) const
{
std::stringstream doc;
- doc << "@brief " << info.methodName << " - This method sets the struct members to new values given in a reader implementation. \n";
+ doc << "@brief " << info.methodName
+ << " - This method sets the struct members to new values given in a reader "
+ "implementation. \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("void " + info.methodName + "(const " + info.argumentType + "& input)" + (info.override ? " override" : ""), doc.str()));
+ CppMethodPtr m =
+ CppMethodPtr(new CppMethod("void " + info.methodName + "(const " + info.argumentType +
+ "& input)" + (info.override ? " override" : ""),
+ doc.str()));
m->addLine(info.readerClassType + " reader;");
- m->addLine("this->read(reader, " + info.enforceConversion + "(input)" + info.enforceMemberAccess + ");");
+ m->addLine("this->read(reader, " + info.enforceConversion + "(input)" +
+ info.enforceMemberAccess + ");");
m->setEnforceBlockGeneration(true);
return m;
}
- CppMethodPtr Generator::toSpecializedStaticDataReaderMethod(const StaticReaderInfo& info) const
+ CppMethodPtr
+ Generator::toSpecializedStaticDataReaderMethod(const StaticReaderInfo& info) const
{
std::stringstream doc;
- doc << "@brief " << info.methodName << "() - This method sets the struct members to new values given in a reader implementation. \n";
+ doc << "@brief " << info.methodName
+ << "() - This method sets the struct members to new values given in a reader "
+ "implementation. \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("static " + info.returnType + " " + info.methodName + "(const " + info.argumentType + "& input)", doc.str()));
+ CppMethodPtr m =
+ CppMethodPtr(new CppMethod("static " + info.returnType + " " + info.methodName +
+ "(const " + info.argumentType + "& input)",
+ doc.str()));
m->addLine(info.returnType + " t;");
m->addLine("t.fromAron(input);");
m->addLine("return t;");
@@ -354,28 +433,36 @@ namespace armarx::aron::codegenerator::cpp
return m;
}
- CppMethodPtr Generator::toSpecializedTypeWriterMethod(const WriterInfo& info) const
+ CppMethodPtr
+ Generator::toSpecializedTypeWriterMethod(const WriterInfo& info) const
{
std::stringstream doc;
- doc << "@brief " << info.methodName << "() - This method returns a new type from the member data types using a writer implementation. \n";
+ doc << "@brief " << info.methodName
+ << "() - This method returns a new type from the member data types using a writer "
+ "implementation. \n";
doc << "@return - the result of the writer implementation";
- CppMethodPtr m = CppMethodPtr(new CppMethod("static " + info.returnType + " " + info.methodName + "()", doc.str()));
+ CppMethodPtr m = CppMethodPtr(
+ new CppMethod("static " + info.returnType + " " + info.methodName + "()", doc.str()));
m->addLine(info.writerClassType + " writer;");
- m->addLine("return " + info.enforceConversion + "(writeType(writer))" + info.enforceMemberAccess + ";");
+ m->addLine("return " + info.enforceConversion + "(writeType(writer))" +
+ info.enforceMemberAccess + ";");
m->setEnforceBlockGeneration(true);
return m;
}
- CppMethodPtr Generator::toEqualsMethod() const
+ CppMethodPtr
+ Generator::toEqualsMethod() const
{
std::stringstream doc;
- doc << "@brief operator==() - This method checks whether all values equal another instance. \n";
+ doc << "@brief operator==() - This method checks whether all values equal another "
+ "instance. \n";
doc << "@param i - The other instance\n";
doc << "@return - true, if all members are the same, false otherwise";
- CppMethodPtr m = CppMethodPtr(new CppMethod("bool operator==(const " + this->getFullClassCppTypename() + "& i) const", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(
+ "bool operator==(const " + this->getFullClassCppTypename() + "& i) const", doc.str()));
CppBlockPtr b = this->getEqualsBlock("", "i");
b->addLine("return true;");
m->setBlock(b);
@@ -385,89 +472,111 @@ namespace armarx::aron::codegenerator::cpp
}
// defaulted implementations of the blocks
- std::vector<std::string> Generator::getRequiredIncludes() const
+ std::vector<std::string>
+ Generator::getRequiredIncludes() const
{
return {};
}
- std::vector<CppFieldPtr> Generator::getPublicVariableDeclarations(const std::string& name) const
+
+ std::vector<CppFieldPtr>
+ Generator::getPublicVariableDeclarations(const std::string& name) const
{
auto field = std::make_shared<CppField>(this->getFullInstantiatedCppTypename(), name);
return {field};
}
- std::pair<std::vector<std::pair<std::string, std::string>>, bool> Generator::getCtorInitializers(const std::string& name) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ Generator::getCtorInitializers(const std::string& name) const
{
- return {{{name, "{}"}}, false};
+ return {{}, false};
}
- CppBlockPtr Generator::getCtorBlock(const std::string&) const
+ CppBlockPtr
+ Generator::getCtorBlock(const std::string&) const
{
return std::make_shared<CppBlock>();
}
- std::pair<std::vector<std::pair<std::string, std::string>>, bool> Generator::getCopyCtorInitializers(const std::string& name) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ Generator::getCopyCtorInitializers(const std::string& name) const
{
const auto& t = getType();
if (t.getMaybe() == type::Maybe::UNIQUE_PTR) // unique ptrs cant be copied
{
- return {{{name, ARON_OTHER_ACCESSOR + "." + name + " ? " + resolveMaybeGenerator("*" + ARON_OTHER_ACCESSOR + "." + name) + " : nullptr"}}, true};
+ return {{{name,
+ ARON_OTHER_ACCESSOR + "." + name + " ? " +
+ resolveMaybeGenerator("*" + ARON_OTHER_ACCESSOR + "." + name) +
+ " : nullptr"}},
+ true};
}
return {{{name, ARON_OTHER_ACCESSOR + "." + name}}, false};
}
- CppBlockPtr Generator::getCopyCtorBlock(const std::string&) const
+ CppBlockPtr
+ Generator::getCopyCtorBlock(const std::string&) const
{
return std::make_shared<CppBlock>();
}
- CppBlockPtr Generator::getDtorBlock(const std::string&) const
+ CppBlockPtr
+ Generator::getDtorBlock(const std::string&) const
{
return std::make_shared<CppBlock>();
}
- CppBlockPtr Generator::getResetHardBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Generator::getResetHardBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine(cppAccessor + " = " + this->getFullInstantiatedCppTypename() + "();");
+ block_if_data->addLine(cppAccessor + " = " + this->getFullInstantiatedCppTypename() +
+ "();");
return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Generator::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Generator::getResetSoftBlock(const std::string& cppAccessor) const
{
auto block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine(cppAccessor + " = " + this->getFullInstantiatedCppTypename() + "();");
+ block_if_data->addLine(cppAccessor + " = " + this->getFullInstantiatedCppTypename() +
+ "();");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Generator::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ CppBlockPtr
+ Generator::getEqualsBlock(const std::string& accessor,
+ const std::string& otherInstanceAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string resolved_accessor = this->resolveMaybeAccessor(accessor);
- std::string other_instance_resolved_accessor = this->resolveMaybeAccessor(otherInstanceAccessor);
+ std::string other_instance_resolved_accessor =
+ this->resolveMaybeAccessor(otherInstanceAccessor);
- block_if_data->addLine("if (not (" + resolved_accessor + " == " + other_instance_resolved_accessor + "))");
+ block_if_data->addLine("if (not (" + resolved_accessor +
+ " == " + other_instance_resolved_accessor + "))");
block_if_data->addLineAsBlock("return false;");
return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
}
-
// Helper methods
- std::string Generator::resolveMaybeAccessor(const std::string& s) const
+ std::string
+ Generator::resolveMaybeAccessor(const std::string& s) const
{
const auto& t = getType();
if (t.getMaybe() == type::Maybe::OPTIONAL)
{
return s + ".value()";
}
- if (t.getMaybe() == type::Maybe::RAW_PTR || t.getMaybe() == type::Maybe::SHARED_PTR || t.getMaybe() == type::Maybe::UNIQUE_PTR)
+ if (t.getMaybe() == type::Maybe::RAW_PTR || t.getMaybe() == type::Maybe::SHARED_PTR ||
+ t.getMaybe() == type::Maybe::UNIQUE_PTR)
{
return "*" + s;
}
return s;
}
- std::string Generator::resolveMaybeGenerator(const std::string& args) const
+ std::string
+ Generator::resolveMaybeGenerator(const std::string& args) const
{
const auto& t = getType();
if (t.getMaybe() == type::Maybe::OPTIONAL)
@@ -487,10 +596,10 @@ namespace armarx::aron::codegenerator::cpp
return "std::make_unique<" + getInstantiatedCppTypename() + ">(" + args + ")";
}
return "";
-
}
- std::string Generator::resolveMaybeGeneratorWithSetter(const std::string& s, const std::string& args) const
+ std::string
+ Generator::resolveMaybeGeneratorWithSetter(const std::string& s, const std::string& args) const
{
auto gen = resolveMaybeGenerator(args);
if (!gen.empty())
@@ -500,7 +609,8 @@ namespace armarx::aron::codegenerator::cpp
return "";
}
- std::string Generator::nextEl() const
+ std::string
+ Generator::nextEl() const
{
const auto& type = getType();
switch (type.getMaybe())
@@ -513,10 +623,14 @@ namespace armarx::aron::codegenerator::cpp
case type::Maybe::UNIQUE_PTR:
return "->";
}
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Received unknown maybe enum", std::to_string((int) type.getMaybe()), type.getPath());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Received unknown maybe enum",
+ std::to_string((int)type.getMaybe()),
+ type.getPath());
}
- std::string Generator::toPointerAccessor(const std::string& cppAccessor) const
+ std::string
+ Generator::toPointerAccessor(const std::string& cppAccessor) const
{
const auto& type = getType();
switch (type.getMaybe())
@@ -528,12 +642,18 @@ namespace armarx::aron::codegenerator::cpp
return cppAccessor + ".get()";
case type::Maybe::NONE: //[[fallthrough]];
- case type::Maybe::OPTIONAL: break;
+ case type::Maybe::OPTIONAL:
+ break;
}
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Received invalid maybe enum (not a pointer?)", std::to_string((int) type.getMaybe()), type.getPath());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Received invalid maybe enum (not a pointer?)",
+ std::to_string((int)type.getMaybe()),
+ type.getPath());
}
- CppBlockPtr Generator::resolveMaybeResetHardBlock(const CppBlockPtr& block_if_data, const std::string& cppAccessor) const
+ CppBlockPtr
+ Generator::resolveMaybeResetHardBlock(const CppBlockPtr& block_if_data,
+ const std::string& cppAccessor) const
{
const auto& type = getType();
if (type.getMaybe() == type::Maybe::OPTIONAL)
@@ -542,7 +662,9 @@ namespace armarx::aron::codegenerator::cpp
b->addLine(cppAccessor + " = std::nullopt;");
return b;
}
- else if (type.getMaybe() == type::Maybe::RAW_PTR || type.getMaybe() == type::Maybe::SHARED_PTR || type.getMaybe() == type::Maybe::UNIQUE_PTR)
+ else if (type.getMaybe() == type::Maybe::RAW_PTR ||
+ type.getMaybe() == type::Maybe::SHARED_PTR ||
+ type.getMaybe() == type::Maybe::UNIQUE_PTR)
{
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine(cppAccessor + " = nullptr;");
@@ -551,10 +673,14 @@ namespace armarx::aron::codegenerator::cpp
return block_if_data;
}
- CppBlockPtr Generator::resolveMaybeResetSoftBlock(const CppBlockPtr& block_if_data, const std::string& cppAccessor) const
+ CppBlockPtr
+ Generator::resolveMaybeResetSoftBlock(const CppBlockPtr& block_if_data,
+ const std::string& cppAccessor) const
{
const auto& type = getType();
- if (type.getMaybe() == type::Maybe::OPTIONAL || type.getMaybe() == type::Maybe::RAW_PTR || type.getMaybe() == type::Maybe::SHARED_PTR || type.getMaybe() == type::Maybe::UNIQUE_PTR)
+ if (type.getMaybe() == type::Maybe::OPTIONAL || type.getMaybe() == type::Maybe::RAW_PTR ||
+ type.getMaybe() == type::Maybe::SHARED_PTR ||
+ type.getMaybe() == type::Maybe::UNIQUE_PTR)
{
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("if (" + cppAccessor + ") // if " + cppAccessor + " contains data");
@@ -564,10 +690,14 @@ namespace armarx::aron::codegenerator::cpp
return block_if_data;
}
- CppBlockPtr Generator::resolveMaybeWriteBlock(const CppBlockPtr& block_if_data, const std::string& cppAccessor) const
+ CppBlockPtr
+ Generator::resolveMaybeWriteBlock(const CppBlockPtr& block_if_data,
+ const std::string& cppAccessor) const
{
const auto& type = getType();
- if (type.getMaybe() == type::Maybe::OPTIONAL || type.getMaybe() == type::Maybe::RAW_PTR || type.getMaybe() == type::Maybe::SHARED_PTR || type.getMaybe() == type::Maybe::UNIQUE_PTR)
+ if (type.getMaybe() == type::Maybe::OPTIONAL || type.getMaybe() == type::Maybe::RAW_PTR ||
+ type.getMaybe() == type::Maybe::SHARED_PTR ||
+ type.getMaybe() == type::Maybe::UNIQUE_PTR)
{
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("if (" + cppAccessor + ") // if " + cppAccessor + " contains data");
@@ -577,13 +707,19 @@ namespace armarx::aron::codegenerator::cpp
return block_if_data;
}
- CppBlockPtr Generator::resolveMaybeReadBlock(const CppBlockPtr& block_if_data, const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ Generator::resolveMaybeReadBlock(const CppBlockPtr& block_if_data,
+ const std::string& cppAccessor,
+ const std::string& variantAccessor) const
{
const auto& type = getType();
- if (type.getMaybe() == type::Maybe::OPTIONAL || type.getMaybe() == type::Maybe::RAW_PTR || type.getMaybe() == type::Maybe::SHARED_PTR || type.getMaybe() == type::Maybe::UNIQUE_PTR)
+ if (type.getMaybe() == type::Maybe::OPTIONAL || type.getMaybe() == type::Maybe::RAW_PTR ||
+ type.getMaybe() == type::Maybe::SHARED_PTR ||
+ type.getMaybe() == type::Maybe::UNIQUE_PTR)
{
CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("if (not (" + ARON_READER_ACCESSOR + ".readNull(" + variantAccessor + "))) // if aron contains data");
+ b->addLine("if (not (" + ARON_READER_ACCESSOR + ".readNull(" + variantAccessor +
+ "))) // if aron contains data");
{
CppBlockPtr ifb = std::make_shared<CppBlock>();
ifb->addLine(resolveMaybeGeneratorWithSetter(cppAccessor));
@@ -595,18 +731,31 @@ namespace armarx::aron::codegenerator::cpp
return block_if_data;
}
- CppBlockPtr Generator::resolveMaybeEqualsBlock(const CppBlockPtr& block_if_data, const std::string& accessor, const std::string& otherInstanceAccessor) const
+ CppBlockPtr
+ Generator::resolveMaybeEqualsBlock(const CppBlockPtr& block_if_data,
+ const std::string& accessor,
+ const std::string& otherInstanceAccessor) const
{
const auto& type = getType();
- if (type.getMaybe() == type::Maybe::OPTIONAL || type.getMaybe() == type::Maybe::RAW_PTR || type.getMaybe() == type::Maybe::SHARED_PTR || type.getMaybe() == type::Maybe::UNIQUE_PTR)
+ if (type.getMaybe() == type::Maybe::RAW_PTR || type.getMaybe() == type::Maybe::SHARED_PTR || type.getMaybe() == type::Maybe::UNIQUE_PTR)
+ {
+ CppBlockPtr b = std::make_shared<CppBlock>();
+ b->addLine("if (not (static_cast<bool>(" + accessor + ") == static_cast<bool>(" + otherInstanceAccessor + "))) // check if both contain data");
+ b->addLineAsBlock("return false;");
+ b->addLine("if (static_cast<bool>(" + accessor + ") && static_cast<bool>(" + otherInstanceAccessor + "))");
+ b->addBlock(block_if_data);
+ return b;
+ }
+
+ if (type.getMaybe() == type::Maybe::OPTIONAL)
{
CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("if (not ((bool) " + accessor + " == (bool) " + otherInstanceAccessor + ")) // check if both contain data");
+ b->addLine("if (not ( " + accessor + ".has_value() == " + otherInstanceAccessor + ".has_value())) // check if both contain data");
b->addLineAsBlock("return false;");
- b->addLine("if ((bool) " + accessor + " && (bool) " + otherInstanceAccessor + ")");
+ b->addLine("if ( " + accessor + ".has_value() && " + otherInstanceAccessor + ".has_value())");
b->addBlock(block_if_data);
return b;
}
return block_if_data;
}
-}
+} // namespace armarx::aron::codegenerator::cpp
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.cpp
index 617a658199e8380a3c6fe02ad13a5a13fad1e886..1e0fbb5baf51815cbef21d496c1cb5d7a48a2cac 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.cpp
@@ -24,43 +24,54 @@
// Header
#include "Object.h"
-#include <SimoxUtility/meta/type_name.h>
#include <SimoxUtility/algorithm/vector.hpp>
+#include <SimoxUtility/meta/type_name.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- const std::map<std::string, DTOObjectReplacement> DTO_REPLACEMENTS =
- {
- { "armarx::arondto::DateTime", {"armarx::arondto::DateTime", "armarx::arondto::DateTime", "armarx::core::time::DateTime", "armarx::core::time::DateTime",
- {"<RobotAPI/libraries/aron/common/rw/time.h>"},
- {}}},
- { "armarx::arondto::Duration", {"armarx::arondto::Duration", "armarx::arondto::Duration", "armarx::core::time::Duration", "armarx::core::time::Duration",
- {"<RobotAPI/libraries/aron/common/rw/time.h>"},
- {"armarx::arondto::Frequency", "armarx::arondto::DateTime"}}},
- { "armarx::arondto::FramedPosition", {"armarx::arondto::FramedPosition", "armarx::arondto::FramedPosition", "armarx::FramedPosition", "armarx::FramedPosition",
- {"<RobotAPI/libraries/aron/common/rw/framed.h>"},
- {}}},
- { "armarx::arondto::FramedOrientation", {"armarx::arondto::FramedOrientation", "armarx::arondto::FramedOrientation", "armarx::FramedOrientation", "armarx::FramedOrientation",
- {"<RobotAPI/libraries/aron/common/rw/framed.h>"},
- {}}},
- { "armarx::arondto::FramedPose", {"armarx::arondto::FramedPose", "armarx::arondto::FramedPose", "armarx::FramedPose", "armarx::FramedPose",
- {"<RobotAPI/libraries/aron/common/rw/framed.h>"},
- {}}}
+ const std::map<std::string, DTOObjectReplacement> DTO_REPLACEMENTS = {
+ // ONLY CORE TYPE REPLACEMENTS HERE THAT REQUIRE A toAron fromAron CONVERSION
+ {"armarx::arondto::DateTime",
+ {"armarx::arondto::DateTime", // original typename
+ "armarx::arondto::DateTime", // original instantiated typename (in case of templates)
+ "armarx::core::time::DateTime", // replaced typename
+ "armarx::core::time::DateTime", // replaced instantiated typename
+ {"<RobotAPI/libraries/aron/common/rw/time.h>"}, // additional includes
+ {}}}, // disallowedBases
+ {"armarx::arondto::Duration",
+ {"armarx::arondto::Duration",
+ "armarx::arondto::Duration",
+ "armarx::core::time::Duration",
+ "armarx::core::time::Duration",
+ {"<RobotAPI/libraries/aron/common/rw/time.h>"},
+ {"armarx::arondto::Frequency", "armarx::arondto::DateTime"}}}
+
+ // {"armarx::armem::arondto::MemoryID",
+ // {"armarx::armem::arondto::MemoryID",
+ // "armarx::armem::arondto::MemoryID",
+ // "armarx::armem::MemoryID",
+ // "armarx::armem::MemoryID",
+ // {"<RobotAPI/libraries/aron/common/rw/memoryid.h>"},
+ // {}}}
};
- bool checkForAllowedReplacement(const type::Object& t)
+ bool
+ checkForAllowedReplacement(const type::Object& t)
{
std::string name = t.getObjectNameWithTemplates();
if (DTO_REPLACEMENTS.count(name) > 0)
{
auto x = DTO_REPLACEMENTS.at(name);
- bool b = std::find(x.disallowedBases.begin(), x.disallowedBases.end(), t.getPath().getRootIdentifier()) == x.disallowedBases.end();
+ bool b = std::find(x.disallowedBases.begin(),
+ x.disallowedBases.end(),
+ t.getPath().getRootIdentifier()) == x.disallowedBases.end();
return b;
}
return false;
}
- std::string checkForInstantiationTypenameReplacement(const type::Object& t)
+ std::string
+ checkForInstantiationTypenameReplacement(const type::Object& t)
{
std::string name = t.getObjectNameWithTemplates();
if (checkForAllowedReplacement(t) && DTO_REPLACEMENTS.count(name) > 0)
@@ -71,7 +82,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return t.getObjectNameWithTemplateInstantiations();
}
- std::string checkForClassNameReplacement(const type::Object& t)
+ std::string
+ checkForClassNameReplacement(const type::Object& t)
{
std::string name = t.getObjectNameWithTemplates();
if (checkForAllowedReplacement(t) && DTO_REPLACEMENTS.count(name) > 0)
@@ -82,7 +94,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return name;
}
- std::vector<std::string> checkForAdditionalIncludes(const type::Object& t)
+ std::vector<std::string>
+ checkForAdditionalIncludes(const type::Object& t)
{
std::string name = t.getObjectNameWithTemplates();
if (checkForAllowedReplacement(t) && DTO_REPLACEMENTS.count(name) > 0)
@@ -99,20 +112,21 @@ namespace armarx::aron::codegenerator::cpp::generator
checkForInstantiationTypenameReplacement(e),
checkForClassNameReplacement(e),
simox::meta::get_type_name<data::dto::Dict>(),
- simox::meta::get_type_name<type::dto::AronObject>(), e),
+ simox::meta::get_type_name<type::dto::AronObject>(),
+ e),
has_been_replaced(checkForAllowedReplacement(e))
{
}
-
- std::vector<std::string> Object::getRequiredIncludes() const
+ std::vector<std::string>
+ Object::getRequiredIncludes() const
{
std::vector<std::string> ret;
// check for includes of members
for (const auto& [key, child] : type.getMemberTypes())
{
- (void) key;
+ (void)key;
auto child_s = FromAronType(*child);
ret = simox::alg::appended(ret, child_s->getRequiredIncludes());
}
@@ -123,7 +137,11 @@ namespace armarx::aron::codegenerator::cpp::generator
return ret;
}
- CppBlockPtr Object::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Object::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
@@ -135,27 +153,35 @@ namespace armarx::aron::codegenerator::cpp::generator
templatesQuoted.push_back("\"" + t + "\"");
}
- b->addLine("auto " + variantAccessor + " = " + type.getObjectNameWithTemplateInstantiations() + "::writeType(" + ARON_WRITER_ACCESSOR + ", " +
- "{" + simox::alg::join(templatesQuoted, ", ") + "}, " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ b->addLine("auto " + variantAccessor + " = " +
+ type.getObjectNameWithTemplateInstantiations() + "::writeType(" +
+ ARON_WRITER_ACCESSOR + ", " + "{" + simox::alg::join(templatesQuoted, ", ") +
+ "}, " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return b;
}
- CppBlockPtr Object::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Object::getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- // if is dto replacement
- block_if_data->addLine("armarx::aron::write(" + ARON_WRITER_ACCESSOR + ", " + resolved_accessor + ", " + variantAccessor + ", " + "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ block_if_data->addLine("armarx::aron::write(" + ARON_WRITER_ACCESSOR + ", " +
+ resolved_accessor + ", " + variantAccessor + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Object::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ Object::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
@@ -166,14 +192,15 @@ namespace armarx::aron::codegenerator::cpp::generator
block_if_data->addLine(reset);
}
- block_if_data->addLine("armarx::aron::read(" + ARON_READER_ACCESSOR + ", " + variantAccessor + ", " + resolved_accessor + "); // of " + cppAccessor);
+ block_if_data->addLine("armarx::aron::read(" + ARON_READER_ACCESSOR + ", " +
+ variantAccessor + ", " + resolved_accessor + "); // of " +
+ cppAccessor);
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
-
-
- CppBlockPtr Object::getResetHardBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Object::getResetHardBlock(const std::string& cppAccessor) const
{
if (has_been_replaced)
{
@@ -185,7 +212,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Object::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Object::getResetSoftBlock(const std::string& cppAccessor) const
{
if (has_been_replaced)
{
@@ -196,5 +224,4 @@ namespace armarx::aron::codegenerator::cpp::generator
block_if_data->addLine(cppAccessor + nextEl() + "resetSoft();");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
-}
-
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
index 00a9ab35d7f52eb757963c97355403377459737e..dbff73cdf796524958269d256c5100ff95c304b9 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
@@ -26,7 +26,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
// constructors
@@ -35,18 +34,27 @@ namespace armarx::aron::codegenerator::cpp::generator
e.getEnumName(),
e.getEnumName(),
simox::meta::get_type_name<data::dto::NDArray>(),
- simox::meta::get_type_name<type::dto::IntEnum>(), e)
+ simox::meta::get_type_name<type::dto::IntEnum>(),
+ e)
+ {
+ }
+
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ IntEnum::getCtorInitializers(const std::string& name) const
{
+ return {{{name, type.getDefaultValueName()}}, true};
}
- CppBlockPtr IntEnum::getResetSoftBlock(const std::string& accessor) const
+ CppBlockPtr
+ IntEnum::getResetSoftBlock(const std::string& accessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(accessor + nextEl() + "resetSoft();");
return this->resolveMaybeResetSoftBlock(block_if_data, accessor);
}
- CppBlockPtr IntEnum::getResetHardBlock(const std::string& accessor) const
+ CppBlockPtr
+ IntEnum::getResetHardBlock(const std::string& accessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
if (type.getMaybe() != type::Maybe::NONE)
@@ -60,39 +68,51 @@ namespace armarx::aron::codegenerator::cpp::generator
return this->resolveMaybeResetHardBlock(b, accessor);
}
- CppBlockPtr IntEnum::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ IntEnum::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto "+variantAccessor+" = " +getInstantiatedCppTypename() + "::writeType(" + ARON_WRITER_ACCESSOR + ", " +
- "{}, " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ b->addLine("auto " + variantAccessor + " = " + getInstantiatedCppTypename() +
+ "::writeType(" + ARON_WRITER_ACCESSOR + ", " + "{}, " +
+ conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
+ ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
+ "})); // of " + cppAccessor);
return b;
}
- CppBlockPtr IntEnum::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ IntEnum::getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- block_if_data->addLine(variantAccessor+" = " + cppAccessor + nextEl() + "write(" + ARON_WRITER_ACCESSOR + ", "+
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ block_if_data->addLine(variantAccessor + " = " + cppAccessor + nextEl() + "write(" +
+ ARON_WRITER_ACCESSOR + ", " + "armarx::aron::Path(" +
+ ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
+ "})); // of " + cppAccessor);
return resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
- CppBlockPtr IntEnum::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ IntEnum::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
if (const auto reset = resolveMaybeGeneratorWithSetter(cppAccessor); !reset.empty())
{
block_if_data->addLine(reset);
}
- block_if_data->addLine(cppAccessor + nextEl() + "read(" + ARON_READER_ACCESSOR + ", " + variantAccessor + ");");
+ block_if_data->addLine(cppAccessor + nextEl() + "read(" + ARON_READER_ACCESSOR + ", " +
+ variantAccessor + ");");
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.h
index cd5b36bcef8300bbb57b88abdf14ca2bde9bc911..4fc6383beebab99aaf334b21a26498d3dfcb00ca 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.h
@@ -23,20 +23,18 @@
#pragma once
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h>
-#include <RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h>
-
#include <map>
#include <string>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h>
namespace armarx::aron::codegenerator::cpp::generator
{
class IntEnum;
typedef std::shared_ptr<IntEnum> IntEnumSerializerPtr;
- class IntEnum :
- public detail::SpecializedGeneratorBase<type::IntEnum, IntEnum>
+ class IntEnum : public detail::SpecializedGeneratorBase<type::IntEnum, IntEnum>
{
public:
// constructors
@@ -44,10 +42,18 @@ namespace armarx::aron::codegenerator::cpp::generator
virtual ~IntEnum() = default;
// virtual implementations
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string& name) const final;
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Image.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Image.cpp
index aae1c701a7e23804273f60346a77e346bfee1daf..f7e1a704c4903bf7c6be53bfd0bd7fb9110e123f 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Image.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Image.cpp
@@ -26,79 +26,103 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
- const std::map<type::image::PixelType, std::tuple<std::string, int, std::string>> PixelType2Cpp =
- {
- { type::image::PixelType::RGB24, {"CV_8UC3", 3, "::armarx::aron::type::image::PixelType::RGB24"}},
- { type::image::PixelType::DEPTH32, {"CV_32FC1", 4, "::armarx::aron::type::image::PixelType::DEPTH32"}},
+ const std::map<type::image::PixelType, std::tuple<std::string, int, std::string>>
+ PixelType2Cpp = {
+ {type::image::PixelType::RGB24,
+ {"CV_8UC3", 3, "::armarx::aron::type::image::PixelType::RGB24"}},
+ {type::image::PixelType::DEPTH32,
+ {"CV_32FC1", 4, "::armarx::aron::type::image::PixelType::DEPTH32"}},
};
-
// constructors
Image::Image(const type::Image& n) :
detail::NDArrayGenerator<type::Image, Image>(
"cv::Mat",
"cv::Mat",
simox::meta::get_type_name<data::dto::NDArray>(),
- simox::meta::get_type_name<type::dto::Image>(), n)
+ simox::meta::get_type_name<type::dto::Image>(),
+ n)
{
}
- std::vector<std::string> Image::getRequiredIncludes() const
+ std::vector<std::string>
+ Image::getRequiredIncludes() const
{
return {"<opencv2/core/core.hpp>"};
}
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ Image::getCtorInitializers(const std::string&) const
+ {
+ // TODO
+ return {{}, false};
+ }
- CppBlockPtr Image::getResetHardBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Image::getResetHardBlock(const std::string& cppAccessor) const
{
- CppBlockPtr block_if_data = detail::NDArrayGenerator<type::Image, Image>::getResetHardBlock(cppAccessor);
- block_if_data->addLine(cppAccessor + ".create(0, 0, " + std::get<0>(PixelType2Cpp.at(type.getPixelType())) + ");");
+ CppBlockPtr block_if_data =
+ detail::NDArrayGenerator<type::Image, Image>::getResetHardBlock(cppAccessor);
+ block_if_data->addLine(cppAccessor + ".create(0, 0, " +
+ std::get<0>(PixelType2Cpp.at(type.getPixelType())) + ");");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
-
- CppBlockPtr Image::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Image::getResetSoftBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine(cppAccessor + ".create(0, 0, " + std::get<0>(PixelType2Cpp.at(type.getPixelType())) + ");");
+ block_if_data->addLine(cppAccessor + ".create(0, 0, " +
+ std::get<0>(PixelType2Cpp.at(type.getPixelType())) + ");");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
-
- CppBlockPtr Image::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Image::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeImage(" + std::get<2>(PixelType2Cpp.at(type.getPixelType())) + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeImage(" +
+ std::get<2>(PixelType2Cpp.at(type.getPixelType())) + ", " + "\"" +
+ type.getDefaultValue() + "\", " +
+ conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
+ ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
+ "})); // of " + cppAccessor);
return b;
}
-
- CppBlockPtr Image::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Image::getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
const std::string shape_vec = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_imageShape";
- block_if_data->addLine("std::vector<int> " + shape_vec + "(" + cppAccessor + nextEl() + "size.p, " + cppAccessor + nextEl() + "size.p + " + cppAccessor + nextEl() + "dims);");
- block_if_data->addLine(shape_vec+".push_back(" + cppAccessor + nextEl() + "elemSize());");
-
- block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray(" + shape_vec + ", "+
- "std::to_string(" + cppAccessor + nextEl() + "type()), "+
- "reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() + "data), "+
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {" + simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
+ block_if_data->addLine("std::vector<int> " + shape_vec + "(" + cppAccessor + nextEl() +
+ "size.p, " + cppAccessor + nextEl() + "size.p + " + cppAccessor +
+ nextEl() + "dims);");
+ block_if_data->addLine(shape_vec + ".push_back(" + cppAccessor + nextEl() + "elemSize());");
+
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray(" +
+ shape_vec + ", " + "std::to_string(" + cppAccessor + nextEl() +
+ "type()), " + "reinterpret_cast<const unsigned char*>(" +
+ cppAccessor + nextEl() + "data), " + "armarx::aron::Path(" +
+ ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
+ "})); // of " + cppAccessor);
return resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
-
- CppBlockPtr Image::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ Image::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
@@ -109,23 +133,28 @@ namespace armarx::aron::codegenerator::cpp::generator
block_if_data->addLine("std::string " + type + ";");
block_if_data->addLine("std::vector<int> " + dims + ";");
block_if_data->addLine("std::vector<unsigned char> " + data + ";");
- block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
- block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(std::vector<int>({" + dims + ".begin(), std::prev(" + dims + ".end())}), std::stoi(" + type + "));");
- block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "data), "+data+".data(), "+data+".size());");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray(" + variantAccessor +
+ ", " + dims + ", " + type + ", " + data + "); // of " + cppAccessor);
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
+ "(std::vector<int>({" + dims + ".begin(), std::prev(" + dims +
+ ".end())}), std::stoi(" + type + "));");
+ block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor +
+ nextEl() + "data), " + data + ".data(), " + data + ".size());");
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
-
- CppBlockPtr Image::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ CppBlockPtr
+ Image::getEqualsBlock(const std::string& accessor,
+ const std::string& otherInstanceAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string resolved_accessor = this->resolveMaybeAccessor(accessor);
- std::string other_instance_resolved_accessor = this->resolveMaybeAccessor(otherInstanceAccessor);
+ std::string other_instance_resolved_accessor =
+ this->resolveMaybeAccessor(otherInstanceAccessor);
- block_if_data->addLine("if (cv::countNonZero(" + resolved_accessor + " != " + other_instance_resolved_accessor + ") != 0)");
+ block_if_data->addLine("if (cv::countNonZero(" + resolved_accessor +
+ " != " + other_instance_resolved_accessor + ") != 0)");
block_if_data->addLineAsBlock("return false;");
return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
}
-}
-
-
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Image.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Image.h
index 72f6947c9405a1a202e8d1b94b9f990fca3fefbf..532c4adc265d8593b29fe640bee81b7cf3fe73d5 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Image.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Image.h
@@ -23,33 +23,37 @@
#pragma once
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
-#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h>
-
#include <map>
#include <memory>
#include <string>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- class Image :
- public detail::NDArrayGenerator<type::Image, Image>
+ class Image : public detail::NDArrayGenerator<type::Image, Image>
{
public:
-
Image(const type::Image& n);
virtual ~Image() = default;
// virtual implementations
std::vector<std::string> getRequiredIncludes() const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const final;
-
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
index ce9389d138de2774555bb3c1b6bbc45b151d65cc..6ace1783e7b6cedbd7dc8312bb3c38265450c216 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
@@ -26,26 +26,34 @@
#include <SimoxUtility/meta/type_name.h>
+#include <RobotAPI/libraries/aron/codegeneration_util/TypeName.h>
+
namespace armarx::aron::codegenerator::cpp::generator
{
const std::map<type::matrix::ElementType, std::tuple<std::string, int, std::string>>
ElementType2Cpp = {
- // TODO: rename to float32 etc. but keep backward compability
- {type::matrix::INT16, {"short", 2, "::armarx::aron::type::matrix::INT16"}},
- {type::matrix::INT32, {"int", 4, "::armarx::aron::type::matrix::INT32"}},
- {type::matrix::INT64, {"long", 8, "::armarx::aron::type::matrix::INT64"}},
- {type::matrix::FLOAT32, {"float", 4, "::armarx::aron::type::matrix::FLOAT32"}},
- {type::matrix::FLOAT64, {"double", 8, "::armarx::aron::type::matrix::FLOAT64"}}};
+ {type::matrix::INT16,
+ {TypeName<short>::Get(), sizeof(short), "::armarx::aron::type::matrix::INT16"}},
+ {type::matrix::INT32,
+ {TypeName<int>::Get(), sizeof(int), "::armarx::aron::type::matrix::INT32"}},
+ {type::matrix::INT64,
+ {TypeName<long>::Get(), sizeof(long), "::armarx::aron::type::matrix::INT64"}},
+ {type::matrix::FLOAT32,
+ {TypeName<float>::Get(), sizeof(float), "::armarx::aron::type::matrix::FLOAT32"}},
+ {type::matrix::FLOAT64,
+ {TypeName<double>::Get(), sizeof(double), "::armarx::aron::type::matrix::FLOAT64"}}};
// constructors
Matrix::Matrix(const type::Matrix& n) :
detail::NDArrayGenerator<type::Matrix, Matrix>(
"Eigen::Matrix<" + std::get<0>(ElementType2Cpp.at(n.getElementType())) + ", " +
(n.getRows() == -1 ? "Eigen::Dynamic" : std::to_string(n.getRows())) + ", " +
- (n.getCols() == -1 ? "Eigen::Dynamic" : std::to_string(n.getCols())) + ">",
+ (n.getCols() == -1 ? "Eigen::Dynamic" : std::to_string(n.getCols())) +
+ (n.getCols() != 1 ? ", Eigen::RowMajor>" : ">"),
"Eigen::Matrix<" + std::get<0>(ElementType2Cpp.at(n.getElementType())) + ", " +
(n.getRows() == -1 ? "Eigen::Dynamic" : std::to_string(n.getRows())) + ", " +
- (n.getCols() == -1 ? "Eigen::Dynamic" : std::to_string(n.getCols())) + ">",
+ (n.getCols() == -1 ? "Eigen::Dynamic" : std::to_string(n.getCols())) +
+ (n.getCols() != 1 ? ", Eigen::RowMajor>" : ">"),
simox::meta::get_type_name<data::dto::NDArray>(),
simox::meta::get_type_name<type::dto::Matrix>(),
n)
@@ -55,7 +63,34 @@ namespace armarx::aron::codegenerator::cpp::generator
std::vector<std::string>
Matrix::getRequiredIncludes() const
{
- return {"<Eigen/Core>"};
+ return {"<RobotAPI/libraries/aron/common/rw/eigen.h>"};
+ }
+
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ Matrix::getCtorInitializers(const std::string& name) const
+ {
+ if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT)
+ {
+ return {{}, false};
+ }
+ if (type.getDefaultValue() == aron::type::matrix::default_value::IDENTITY)
+ {
+ return {{{name, getInstantiatedCppTypename() + "::Identity()"}}, true};
+ }
+ if (type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
+ {
+ return {{{name, getInstantiatedCppTypename() + "::Zero()"}}, true};
+ }
+ if (type.getDefaultValue() == aron::type::matrix::default_value::ONES)
+ {
+ return {{{name, getInstantiatedCppTypename() + "::One()"}}, true};
+ }
+ else
+ {
+ // try to parse num. We ensure from typereader that defaultValue is valid number
+ return {{{name, getInstantiatedCppTypename() + "::One() * " + type.getDefaultValue()}},
+ true};
+ }
}
CppBlockPtr
@@ -76,9 +111,10 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string escaped_accessor = EscapeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR +
- ".writeMatrix((int) " + std::to_string(type.getRows()) + ", " + "(int) " +
- std::to_string(type.getCols()) + ", " +
- std::get<2>(ElementType2Cpp.at(type.getElementType())) + ", " +
+ ".writeMatrix(static_cast<int>( " + std::to_string(type.getRows()) + "), " +
+ "static_cast<int>( " + std::to_string(type.getCols()) + "), " +
+ std::get<2>(ElementType2Cpp.at(type.getElementType())) + ", " + "\"" +
+ type.getDefaultValue() + "\", " +
conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
"})); // of " + cppAccessor);
@@ -93,16 +129,13 @@ namespace armarx::aron::codegenerator::cpp::generator
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- block_if_data->addLine(
- variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({(int) " + cppAccessor +
- nextEl() + "rows(), " + "(int) " + cppAccessor + nextEl() + "cols(), " +
- std::to_string(std::get<1>(ElementType2Cpp.at(type.getElementType()))) + "}, " + "\"" +
- std::get<0>(ElementType2Cpp.at(type.getElementType())) + "\", " +
- "reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() + "data()), " +
- "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
- simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
+ block_if_data->addLine("armarx::aron::write(" + ARON_WRITER_ACCESSOR + ", " +
+ resolved_accessor + ", " + variantAccessor + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
@@ -112,49 +145,17 @@ namespace armarx::aron::codegenerator::cpp::generator
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- std::string type = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_typeAsString";
- std::string dims = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_shape";
- std::string data = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_data";
-
- block_if_data->addLine("std::string " + type + ";");
- block_if_data->addLine("std::vector<int> " + dims + ";");
- block_if_data->addLine("std::vector<unsigned char> " + data + ";");
-
- block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray(" + variantAccessor +
- ", " + dims + ", " + type + ", " + data + "); // of " + cppAccessor);
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
- if ((this->type.getRows() == -1 or this->type.getRows() == Eigen::Dynamic) and
- this->type.getCols() != -1)
+ if (const auto reset = resolveMaybeGeneratorWithSetter(cppAccessor); !reset.empty())
{
- block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + ".at(0));");
+ block_if_data->addLine(reset);
}
- if (this->type.getRows() != -1 and
- (this->type.getCols() == -1 or this->type.getCols() == Eigen::Dynamic))
- {
- block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + ".at(1));");
- }
-
- if ((this->type.getRows() == -1 or this->type.getRows() == Eigen::Dynamic) and
- (this->type.getCols() == -1 or this->type.getCols() == Eigen::Dynamic))
- {
- block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + ".at(0), " + dims +
- ".at(1));");
- }
+ block_if_data->addLine("armarx::aron::read(" + ARON_READER_ACCESSOR + ", " +
+ variantAccessor + ", " + resolved_accessor + "); // of " +
+ cppAccessor);
- block_if_data->addLine("ARMARX_CHECK_AND_THROW(" + cppAccessor + nextEl() + "rows() == " +
- dims + ".at(0) and " + cppAccessor + nextEl() + "cols() == " + dims +
- ".at(1), ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, "
- "\"Received wrong dimensions for member '" +
- cppAccessor + "'.\"));");
- block_if_data->addLine("//ARMARX_CHECK_AND_THROW(" + type + " == \"" +
- std::get<0>(ElementType2Cpp.at(this->type.getElementType())) +
- "\", ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, "
- "\"Received wrong type for member '" +
- cppAccessor + "'.\"));");
-
- block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor +
- nextEl() + "data()), " + data + ".data(), " + data + ".size());");
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
index 9f56f2b986f199af4e662e342b5f7dc7040d778b..452ac7b53e5e9d996bc8cbc7ffa41555d7275e65 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
@@ -23,17 +23,15 @@
#pragma once
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
-#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h>
-
#include <map>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- class Matrix :
- public detail::NDArrayGenerator<type::Matrix, Matrix>
+ class Matrix : public detail::NDArrayGenerator<type::Matrix, Matrix>
{
public:
// constructors
@@ -42,10 +40,18 @@ namespace armarx::aron::codegenerator::cpp::generator
// virtual implementations
std::vector<std::string> getRequiredIncludes() const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/NDArray.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/NDArray.cpp
index 4198f74d41f91afe08a82270ba9eb4858f802cd6..4b1cba9cfbd143df8e98fd3e25cd98023e56af51 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/NDArray.cpp
@@ -26,7 +26,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
// constructors
@@ -35,33 +34,47 @@ namespace armarx::aron::codegenerator::cpp::generator
"NDArray",
"NDArray",
simox::meta::get_type_name<data::dto::NDArray>(),
- simox::meta::get_type_name<type::dto::NDArray>(), n)
+ simox::meta::get_type_name<type::dto::NDArray>(),
+ n)
{
}
- CppBlockPtr NDArray::getResetSoftBlock(const std::string& cppAccessor) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ NDArray::getCtorInitializers(const std::string&) const
+ {
+ return {{}, false};
+ }
+
+ CppBlockPtr
+ NDArray::getResetSoftBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine(cppAccessor + " = " + this->getFullInstantiatedCppTypename() + "();");
+ block_if_data->addLine(cppAccessor + " = " + this->getFullInstantiatedCppTypename() +
+ "();");
return resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
- CppBlockPtr NDArray::getWriteTypeBlock(const std::string&, const std::string&, const Path& p, std::string&) const
+ CppBlockPtr
+ NDArray::getWriteTypeBlock(const std::string&,
+ const std::string&,
+ const Path& p,
+ std::string&) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
return b;
}
- CppBlockPtr NDArray::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string&) const
+ CppBlockPtr
+ NDArray::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string&) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
return b;
}
- CppBlockPtr NDArray::getReadBlock(const std::string& cppAccessor, const std::string&) const
+ CppBlockPtr
+ NDArray::getReadBlock(const std::string& cppAccessor, const std::string&) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
return b;
}
-}
-
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/NDArray.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/NDArray.h
index 3b1d3d2ccc5129bfdbb53444d561b896a91d87f0..20ca2f31f34f3c1105f75785feaa3b388729e73a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/NDArray.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/NDArray.h
@@ -23,16 +23,14 @@
#pragma once
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
-#include <RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h>
-
#include <string>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- class NDArray :
- public detail::NDArrayGenerator<type::NDArray, NDArray>
+ class NDArray : public detail::NDArrayGenerator<type::NDArray, NDArray>
{
public:
@@ -41,9 +39,17 @@ namespace armarx::aron::codegenerator::cpp::generator
virtual ~NDArray() = default;
// virtual implementations
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
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 1fe84d735e74cd59f63801c0096ac19cbe1e9f9f..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
@@ -26,20 +26,31 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
- const std::map<type::pointcloud::VoxelType, std::tuple<std::string, int, std::string>> VoxelType2Cpp =
- {
- // see http://docs.ros.org/en/groovy/api/pcl/html/point__types_8hpp_source.html
- {type::pointcloud::VoxelType::POINT_XYZ, {"pcl::PointXYZ", 16, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZ"}},
- {type::pointcloud::VoxelType::POINT_XYZI, {"pcl::PointXYZI", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZI"}},
- {type::pointcloud::VoxelType::POINT_XYZL, {"pcl::PointXYZL", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZL"}},
- {type::pointcloud::VoxelType::POINT_XYZRGB, {"pcl::PointXYZRGB", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZRGB"}},
- {type::pointcloud::VoxelType::POINT_XYZRGBL, {"pcl::PointXYZRGBL", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZRGBL"}},
- {type::pointcloud::VoxelType::POINT_XYZRGBA, {"pcl::PointXYZRGBA", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZRGBA"}},
- {type::pointcloud::VoxelType::POINT_XYZHSV, {"pcl::PointXYZHSV", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZHSV"}}
- };
+ const std::map<type::pointcloud::VoxelType, std::tuple<std::string, int, std::string>>
+ VoxelType2Cpp = {
+ // see http://docs.ros.org/en/groovy/api/pcl/html/point__types_8hpp_source.html
+ {type::pointcloud::VoxelType::POINT_XYZ,
+ {"pcl::PointXYZ", 16, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZ"}},
+ {type::pointcloud::VoxelType::POINT_XYZI,
+ {"pcl::PointXYZI", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZI"}},
+ {type::pointcloud::VoxelType::POINT_XYZL,
+ {"pcl::PointXYZL", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZL"}},
+ {type::pointcloud::VoxelType::POINT_XYZRGB,
+ {"pcl::PointXYZRGB", 32, "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZRGB"}},
+ {type::pointcloud::VoxelType::POINT_XYZRGBL,
+ {"pcl::PointXYZRGBL",
+ 32,
+ "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZRGBL"}},
+ {type::pointcloud::VoxelType::POINT_XYZRGBA,
+ {"pcl::PointXYZRGBA",
+ 32,
+ "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZRGBA"}},
+ {type::pointcloud::VoxelType::POINT_XYZHSV,
+ {"pcl::PointXYZHSV",
+ 32,
+ "::armarx::aron::type::pointcloud::VoxelType::POINT_XYZHSV"}}};
// constructors
PointCloud::PointCloud(const type::PointCloud& n) :
@@ -52,45 +63,72 @@ namespace armarx::aron::codegenerator::cpp::generator
{
}
- std::vector<std::string> PointCloud::getRequiredIncludes() const
+ std::vector<std::string>
+ PointCloud::getRequiredIncludes() const
{
return {"<pcl/point_cloud.h>", "<pcl/point_types.h>"};
}
- CppBlockPtr PointCloud::getResetSoftBlock(const std::string& cppAccessor) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ PointCloud::getCtorInitializers(const std::string&) const
+ {
+ // TODO
+ return {{}, false};
+ }
+
+ CppBlockPtr
+ PointCloud::getResetSoftBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine(cppAccessor + " = " + getFullInstantiatedCppTypenameGenerator() + "(" + cppAccessor + nextEl() + "width, " + cppAccessor + nextEl() + "height);");
+ block_if_data->addLine(cppAccessor + " = " + getFullInstantiatedCppTypenameGenerator() +
+ "(" + cppAccessor + nextEl() + "width, " + cppAccessor + nextEl() +
+ "height);");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
- CppBlockPtr PointCloud::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ PointCloud::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePointCloud(" + std::get<2>(VoxelType2Cpp.at(type.getVoxelType())) + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePointCloud(" +
+ std::get<2>(VoxelType2Cpp.at(type.getVoxelType())) + ", " + "\"" +
+ type.getDefaultValue() + "\", " +
+ conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
+ ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
+ "})); // of " + cppAccessor);
return b;
}
- CppBlockPtr PointCloud::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ PointCloud::getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({" + cppAccessor + nextEl() + "width, " + 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() + "points.data()), "+
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {" + simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
+ block_if_data->addLine(
+ 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() +
+ "points.data()), " + "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
- CppBlockPtr PointCloud::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ PointCloud::getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
@@ -101,33 +139,48 @@ namespace armarx::aron::codegenerator::cpp::generator
block_if_data->addLine("std::string " + type + ";");
block_if_data->addLine("std::vector<int> " + dims + ";");
block_if_data->addLine("std::vector<unsigned char> " + data + ";");
- block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
-
- block_if_data->addLine("ARMARX_CHECK_AND_THROW(" + dims + ".size() == 3, ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Received wrong dimensions for member '"+cppAccessor+"'.\"));");
- block_if_data->addLine("ARMARX_CHECK_AND_THROW(" + type + " == \"" + std::get<0>(VoxelType2Cpp.at(this->type.getVoxelType())) + "\", ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Received wrong type for member '"+cppAccessor+"'.\"));");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray(" + variantAccessor +
+ ", " + dims + ", " + type + ", " + data + "); // of " + cppAccessor);
+
+ block_if_data->addLine(
+ "ARMARX_CHECK_AND_THROW(" + dims +
+ ".size() == 3, ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Received "
+ "wrong dimensions for member '" +
+ cppAccessor + "'.\"));");
+ block_if_data->addLine("ARMARX_CHECK_AND_THROW(" + type + " == \"" +
+ std::get<0>(VoxelType2Cpp.at(this->type.getVoxelType())) +
+ "\", ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, "
+ "\"Received wrong type for member '" +
+ cppAccessor + "'.\"));");
// assign is not availablePCL for PCL 1.8
// block_if_data->addLine(cppAccessor + nextEl() + "assign(" + dims + "[0], " + dims + "[1], " + std::get<0>(VoxelType2Cpp.at(this->type.getVoxelType())) + "());");
block_if_data->addLine(cppAccessor + nextEl() + "clear();");
// resize(width, height , value) is not available either
// block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + "[0], " + dims + "[1], " + std::get<0>(VoxelType2Cpp.at(this->type.getVoxelType())) + "()));");
- block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + "[0] * " + dims + "[1]);");
+ block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + "[0] * " + dims +
+ "[1]);");
block_if_data->addLine(cppAccessor + nextEl() + "width = " + dims + "[0];");
block_if_data->addLine(cppAccessor + nextEl() + "height = " + dims + "[1];");
- block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "points.data()), "+data+".data(), "+data+".size());");
+ block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor +
+ nextEl() + "points.data()), " + data + ".data(), " + data +
+ ".size());");
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
- CppBlockPtr PointCloud::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ CppBlockPtr
+ PointCloud::getEqualsBlock(const std::string& accessor,
+ const std::string& otherInstanceAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("if (" + accessor + nextEl() + "width != " + otherInstanceAccessor + nextEl() + "width || " + accessor + nextEl() + "height != " + otherInstanceAccessor + nextEl() + "height)");
+ block_if_data->addLine("if (" + accessor + nextEl() + "width != " + otherInstanceAccessor +
+ nextEl() + "width || " + accessor + nextEl() +
+ "height != " + otherInstanceAccessor + nextEl() + "height)");
block_if_data->addLineAsBlock("return false;");
//block_if_data->addLine("if (" + accessor + nextEl() + "points != " + otherInstanceAccessor + nextEl() + "points)");
//block_if_data->addLine("\t return false;");
return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
}
-}
-
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.h
index 9edc5a318356220a01b67c07d2f7a0e1d5acd49e..8514f807df630b16e8277a327b03096cd8839df3 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.h
@@ -23,16 +23,14 @@
#pragma once
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
-#include <RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h>
-
#include <map>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- class PointCloud :
- public detail::NDArrayGenerator<type::PointCloud, PointCloud>
+ class PointCloud : public detail::NDArrayGenerator<type::PointCloud, PointCloud>
{
public:
// constructors
@@ -41,10 +39,18 @@ namespace armarx::aron::codegenerator::cpp::generator
// virtual implementations
std::vector<std::string> getRequiredIncludes() const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
CppBlockPtr getEqualsBlock(const std::string& cppAccessor, const std::string&) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
index 32dae76ef8e2678e23e5fbec75133d5b4416f4ed..365b3c8824d6c38b60ccde7c4381aa41bb0945b7 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
@@ -26,14 +26,19 @@
#include <SimoxUtility/meta/type_name.h>
+#include <RobotAPI/libraries/aron/codegeneration_util/TypeName.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- const std::map<type::quaternion::ElementType, std::tuple<std::string, int, std::string>> ElementType2Cpp =
- {
- {type::quaternion::ElementType::FLOAT32, {"float", 4, "::armarx::aron::type::quaternion::ElementType::FLOAT32"}},
- {type::quaternion::ElementType::FLOAT64, {"double", 8, "::armarx::aron::type::quaternion::ElementType::FLOAT64"}}
- };
+ const std::map<type::quaternion::ElementType, std::tuple<std::string, int, std::string>>
+ ElementType2Cpp = {{type::quaternion::ElementType::FLOAT32,
+ {TypeName<float>::Get(),
+ sizeof(float),
+ "::armarx::aron::type::quaternion::ElementType::FLOAT32"}},
+ {type::quaternion::ElementType::FLOAT64,
+ {TypeName<double>::Get(),
+ sizeof(double),
+ "::armarx::aron::type::quaternion::ElementType::FLOAT64"}}};
// constructors
Quaternion::Quaternion(const type::Quaternion& n) :
@@ -46,65 +51,108 @@ namespace armarx::aron::codegenerator::cpp::generator
{
}
- std::vector<std::string> Quaternion::getRequiredIncludes() const
+ std::vector<std::string>
+ Quaternion::getRequiredIncludes() const
+ {
+ return {"<RobotAPI/libraries/aron/common/rw/eigen.h>"};
+ }
+
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ Quaternion::getCtorInitializers(const std::string& name) const
{
- return {"<Eigen/Core>", "<Eigen/Geometry>"};
+ if (type.getDefaultValue() == aron::type::quaternion::default_value::DEFAULT)
+ {
+ return {{}, false};
+ }
+ if (type.getDefaultValue() == aron::type::quaternion::default_value::ZEROS)
+ {
+ return {{{name, getInstantiatedCppTypename() + "(0, 0, 0, 0)"}}, true};
+ }
+ if (type.getDefaultValue() == aron::type::quaternion::default_value::ONES)
+ {
+ return {{{name, getInstantiatedCppTypename() + "(1, 1, 1, 1)"}}, true};
+ }
+ else
+ {
+ // try to parse num. We ensure from typereader that defaultValue is valid number
+ return {{{name, getInstantiatedCppTypename() + "(" + type.getDefaultValue() + ")"}},
+ true};
+ }
}
- CppBlockPtr Quaternion::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Quaternion::getResetSoftBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + nextEl() + "setIdentity();");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Quaternion::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Quaternion::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeQuaternion(" + std::get<2>(ElementType2Cpp.at(type.getElementType())) + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeQuaternion(" +
+ std::get<2>(ElementType2Cpp.at(type.getElementType())) + ", " + "\"" +
+ type.getDefaultValue() + "\", " +
+ conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
+ ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
+ "})); // of " + cppAccessor);
return b;
}
- CppBlockPtr Quaternion::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Quaternion::getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({1, 4, " + std::to_string(std::get<1>(ElementType2Cpp.at(type.getElementType()))) + "}, "+
- "\"" + std::get<0>(ElementType2Cpp.at(type.getElementType())) + "\", "+
- "reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() + "coeffs().data()), "+
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {" + simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
+ block_if_data->addLine("armarx::aron::write(" + ARON_WRITER_ACCESSOR + ", " +
+ resolved_accessor + ", " + variantAccessor + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Quaternion::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ Quaternion::getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- std::string type = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_typeAsString";
- std::string dims = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_shape";
- std::string data = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor + "_data";
-
- block_if_data->addLine("std::string " + type + ";");
- block_if_data->addLine("std::vector<int> " + dims + ";");
- block_if_data->addLine("std::vector<unsigned char> " + data + ";");
- block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
- block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "coeffs().data()), "+data+".data(), "+data+".size());");
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
+
+ if (const auto reset = resolveMaybeGeneratorWithSetter(cppAccessor); !reset.empty())
+ {
+ block_if_data->addLine(reset);
+ }
+
+ block_if_data->addLine("armarx::aron::read(" + ARON_READER_ACCESSOR + ", " +
+ variantAccessor + ", " + resolved_accessor + "); // of " +
+ cppAccessor);
+
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
- CppBlockPtr Quaternion::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ CppBlockPtr
+ Quaternion::getEqualsBlock(const std::string& accessor,
+ const std::string& otherInstanceAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" + otherInstanceAccessor + ")))");
+ block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" +
+ otherInstanceAccessor + ")))");
block_if_data->addLineAsBlock("return false;");
return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
index f744681595d1aea76b8e5b29224313e41b056baa..4fd41b7dbd382c0231337ea30ba661f9b1e9eab3 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
@@ -24,16 +24,14 @@
#pragma once
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
-#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h>
-
#include <map>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- class Quaternion :
- public detail::NDArrayGenerator<type::Quaternion, Quaternion>
+ class Quaternion : public detail::NDArrayGenerator<type::Quaternion, Quaternion>
{
public:
// constructors
@@ -42,10 +40,18 @@ namespace armarx::aron::codegenerator::cpp::generator
// virtual implementations
std::vector<std::string> getRequiredIncludes() const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp
index 7dcadbb33ae84d70a180e89763ca37a634946268..fc0ad8edf34ccb111da24edf9cdfb2c469945181 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp
@@ -25,7 +25,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
@@ -34,33 +33,56 @@ namespace armarx::aron::codegenerator::cpp::generator
"bool",
"bool",
simox::meta::get_type_name<data::dto::AronBool>(),
- simox::meta::get_type_name<type::dto::AronBool>(), e)
+ simox::meta::get_type_name<type::dto::AronBool>(),
+ e)
{
}
/* virtual implementations */
- CppBlockPtr Bool::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, const Path& p, std::string& variantAccessor) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ Bool::getCtorInitializers(const std::string& name) const
+ {
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, std::to_string(*def)}}, true};
+ }
+ return {{}, false};
+ }
+
+ CppBlockPtr
+ Bool::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& accessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeBool(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? std::to_string(*type.getDefaultValue())
+ : "std::nullopt";
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeBool(" +
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
- CppBlockPtr Bool::getResetHardBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Bool::getResetHardBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = false;");
return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Bool::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Bool::getResetSoftBlock(const std::string& cppAccessor) const
{
auto block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = false;");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.h
index 381adec7bc759c7a20aedd54229dd499253110d1..f218b9938bbc57d3595542c7915e021ebc386de7 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.h
@@ -23,24 +23,29 @@
#pragma once
-#include "../detail/PrimitiveGenerator.h"
-
#include <RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h>
+#include "../detail/PrimitiveGenerator.h"
namespace armarx::aron::codegenerator::cpp::generator
{
- class Bool :
- public detail::PrimitiveGenerator<type::Bool, Bool>
+ class Bool : public detail::PrimitiveGenerator<type::Bool, Bool>
{
public:
+ static const constexpr bool DEFAULT_DEFAULT = false;
+
/* constructors */
Bool(const type::Bool& e);
virtual ~Bool() = default;
/* virtual implementations */
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.cpp
index 735f3ad9dc4efdf0f00a34093671644aebb0eb3f..5dba5df3ef0c32fbeee4fad73c6e9bdc59ae5773 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.cpp
@@ -25,7 +25,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
@@ -40,28 +39,50 @@ namespace armarx::aron::codegenerator::cpp::generator
}
/* virtual implementations */
- CppBlockPtr Double::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, const Path& p, std::string& variantAccessor) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ Double::getCtorInitializers(const std::string& name) const
+ {
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, std::to_string(*def)}}, true};
+ }
+ return {{}, false};
+ }
+
+ CppBlockPtr
+ Double::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& accessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeDouble(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? std::to_string(*type.getDefaultValue())
+ : "std::nullopt";
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeDouble(" +
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
- CppBlockPtr Double::getResetHardBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Double::getResetHardBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = 0.0;");
return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Double::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Double::getResetSoftBlock(const std::string& cppAccessor) const
{
auto block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = 0.0;");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.h
index b9a96a077e225aad5b0871882e86e20f85fb3685..2ff98312264d0cfbd611544c13d3cfedddb3e884 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.h
@@ -26,11 +26,9 @@
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
#include <RobotAPI/libraries/aron/core/type/variant/primitive/Double.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
- class Double :
- public detail::PrimitiveGenerator<type::Double, Double>
+ class Double : public detail::PrimitiveGenerator<type::Double, Double>
{
public:
/* constructors */
@@ -38,8 +36,13 @@ namespace armarx::aron::codegenerator::cpp::generator
virtual ~Double() = default;
/* virtual implementations */
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.cpp
index d3d27ea6c34713b3d81074c1ab64e37ed696d69c..8b5913c5634e95e7a9a667b37d4bfac2c0175e14 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.cpp
@@ -25,7 +25,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
@@ -40,28 +39,50 @@ namespace armarx::aron::codegenerator::cpp::generator
}
/* virtual implementations */
- CppBlockPtr Float::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, const Path& p, std::string& variantAccessor) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ Float::getCtorInitializers(const std::string& name) const
+ {
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, std::to_string(*def)}}, true};
+ }
+ return {{}, false};
+ }
+
+ CppBlockPtr
+ Float::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& accessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeFloat(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? std::to_string(*type.getDefaultValue())
+ : "std::nullopt";
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeFloat(" +
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
- CppBlockPtr Float::getResetHardBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Float::getResetHardBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = 0.0f;");
return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Float::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Float::getResetSoftBlock(const std::string& cppAccessor) const
{
auto block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = 0.0f;");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.h
index f1c88c502f92b7d86afc7668eda0710f0f3351b7..7cb65fc6d2712107ea2c932d189f96532d5c04f9 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.h
@@ -26,11 +26,9 @@
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
#include <RobotAPI/libraries/aron/core/type/variant/primitive/Float.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
- class Float :
- public detail::PrimitiveGenerator<type::Float, Float>
+ class Float : public detail::PrimitiveGenerator<type::Float, Float>
{
public:
/* constructors */
@@ -38,8 +36,13 @@ namespace armarx::aron::codegenerator::cpp::generator
virtual ~Float() = default;
/* virtual implementations */
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.cpp
index 7a976bd83fffa4ad52bec05c2a5392bbf7400798..bcc9f869f25bdf1078de8c41ee0f2f80a53af98e 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.cpp
@@ -25,43 +25,63 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
Int::Int(const type::Int& e) :
- detail::PrimitiveGenerator<type::Int, Int>(
- "int",
- "int",
- simox::meta::get_type_name<data::dto::AronInt>(),
- simox::meta::get_type_name<type::dto::AronInt>(),
- e)
+ detail::PrimitiveGenerator<type::Int, Int>("int",
+ "int",
+ simox::meta::get_type_name<data::dto::AronInt>(),
+ simox::meta::get_type_name<type::dto::AronInt>(),
+ e)
{
}
/* virtual implementations */
- CppBlockPtr Int::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, const Path& p, std::string& variantAccessor) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ Int::getCtorInitializers(const std::string& name) const
+ {
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, std::to_string(*def)}}, true};
+ }
+ return {{}, false};
+ }
+
+ CppBlockPtr
+ Int::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& accessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeInt(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? std::to_string(*type.getDefaultValue())
+ : "std::nullopt";
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeInt(" +
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
- CppBlockPtr Int::getResetHardBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Int::getResetHardBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = 0;");
return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Int::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Int::getResetSoftBlock(const std::string& cppAccessor) const
{
auto block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = 0;");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.h
index b21abee9cef1850a1d55da0369a530b65585c210..d552f34ea914baba72f3a7c886c9ebce99d0658b 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.h
@@ -26,11 +26,9 @@
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
#include <RobotAPI/libraries/aron/core/type/variant/primitive/Int.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
- class Int :
- public detail::PrimitiveGenerator<type::Int, Int>
+ class Int : public detail::PrimitiveGenerator<type::Int, Int>
{
public:
/* constructors */
@@ -38,8 +36,13 @@ namespace armarx::aron::codegenerator::cpp::generator
virtual ~Int() = default;
/* virtual implementations */
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.cpp
index fe6cd5e87f6f3db26873c351987f91d86556d27b..edc30358bcf4cfc3e37636cd31f12021edd36693 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.cpp
@@ -25,7 +25,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
@@ -40,28 +39,50 @@ namespace armarx::aron::codegenerator::cpp::generator
}
/* virtual implementations */
- CppBlockPtr Long::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, const Path& p, std::string& variantAccessor) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ Long::getCtorInitializers(const std::string& name) const
+ {
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, std::to_string(*def)}}, true};
+ }
+ return {{}, false};
+ }
+
+ CppBlockPtr
+ Long::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& accessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeLong(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? std::to_string(*type.getDefaultValue())
+ : "std::nullopt";
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeLong(" +
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
- CppBlockPtr Long::getResetHardBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Long::getResetHardBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = 0;");
return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Long::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Long::getResetSoftBlock(const std::string& cppAccessor) const
{
auto block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = 0;");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.h
index 09d923e487108a7c2c0927156af77c21fddf79ec..8dd002ecb4680f40660bc6dc39c0583c556cf730 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.h
@@ -26,11 +26,9 @@
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
#include <RobotAPI/libraries/aron/core/type/variant/primitive/Long.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
- class Long :
- public detail::PrimitiveGenerator<type::Long, Long>
+ class Long : public detail::PrimitiveGenerator<type::Long, Long>
{
public:
/* constructors */
@@ -38,8 +36,13 @@ namespace armarx::aron::codegenerator::cpp::generator
virtual ~Long() = default;
/* virtual implementations */
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.cpp
index d51ee9ed159a5abb82442a5501ffebb3500aad15..0589e1881a56a73c946687d75952d5ff744d8dd7 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.cpp
@@ -25,7 +25,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
@@ -40,28 +39,50 @@ namespace armarx::aron::codegenerator::cpp::generator
}
/* virtual implementations */
- CppBlockPtr String::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, const Path& p, std::string& variantAccessor) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ String::getCtorInitializers(const std::string& name) const
+ {
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, "\"" + *def + "\""}}, true};
+ }
+ return {{}, false};
+ }
+
+ CppBlockPtr
+ String::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& accessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeString(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? ("\"" + *type.getDefaultValue() + "\"")
+ : "std::nullopt";
+
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeString(" +
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
- CppBlockPtr String::getResetHardBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ String::getResetHardBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = \"\";");
return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
}
- CppBlockPtr String::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ String::getResetSoftBlock(const std::string& cppAccessor) const
{
auto block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + " = \"\";");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.h
index fd854ccee807d549fbe1448643eb31a4f069bcc8..628f0af2552d732a5a1e39bd6eefc684030723af 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.h
@@ -26,11 +26,9 @@
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h>
#include <RobotAPI/libraries/aron/core/type/variant/primitive/String.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
- class String :
- public detail::PrimitiveGenerator<type::String, String>
+ class String : public detail::PrimitiveGenerator<type::String, String>
{
public:
/* constructors */
@@ -38,8 +36,13 @@ namespace armarx::aron::codegenerator::cpp::generator
virtual ~String() = default;
/* virtual implementations */
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp
index 0cddfd33161dcd0bf5f398fa87f4c0a88ec0a447..ade0f0ef42f57366ab2944014ecb2245eed29587 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp
@@ -26,7 +26,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
// constructors
@@ -40,11 +39,16 @@ namespace armarx::aron::codegenerator::cpp::generator
{
if (type.getMaybe() != type::Maybe::NONE)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Somehow the maybe flag of a top level int enum declaration is set. This is not valid!", std::to_string((int) type.getMaybe()), type.getPath());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Somehow the maybe flag of a top level int enum "
+ "declaration is set. This is not valid!",
+ std::to_string((int)type.getMaybe()),
+ type.getPath());
}
}
- std::vector<CppFieldPtr> IntEnumClass::getPublicVariableDeclarations(const std::string& className) const
+ std::vector<CppFieldPtr>
+ IntEnumClass::getPublicVariableDeclarations(const std::string& className) const
{
std::vector<CppFieldPtr> fields;
std::stringstream enum_to_name;
@@ -53,7 +57,11 @@ namespace armarx::aron::codegenerator::cpp::generator
std::stringstream value_to_enum;
// add legacy typedef
- fields.push_back(std::make_shared<CppField>("using", std::string(IMPL_ENUM), simox::alg::to_lower(className) + "_details::Enum", "Legacy typedef of enum"));
+ fields.push_back(
+ std::make_shared<CppField>("using",
+ std::string(IMPL_ENUM),
+ simox::alg::to_lower(className) + "_details::Enum",
+ "Legacy typedef of enum"));
ARMARX_CHECK(type.getAcceptedValueMap().size() > 0);
@@ -64,7 +72,8 @@ namespace armarx::aron::codegenerator::cpp::generator
for (const auto& [key, value] : type.getAcceptedValueMap())
{
std::string enumKeyWithNamespace = std::string(IMPL_ENUM) + "::" + key;
- fields.push_back(std::make_shared<CppField>("static constexpr " + std::string(IMPL_ENUM), key + " = " + enumKeyWithNamespace));
+ fields.push_back(std::make_shared<CppField>(
+ "static constexpr " + std::string(IMPL_ENUM), key + " = " + enumKeyWithNamespace));
enum_to_name << "\t\t{" << enumKeyWithNamespace << ", \"" << key << "\"}," << std::endl;
name_to_enum << "\t\t{\"" << key << "\", " << enumKeyWithNamespace << "}," << std::endl;
@@ -78,31 +87,56 @@ namespace armarx::aron::codegenerator::cpp::generator
enum_to_value << "\t}";
value_to_enum << "\t}";
- fields.push_back(std::make_shared<CppField>("static inline const std::map<" + std::string(IMPL_ENUM) + ", std::string>", "EnumToStringMap", enum_to_name.str(), "Mapping enum values to readable strings"));
- fields.push_back(std::make_shared<CppField>("static inline const std::map<std::string, " + std::string(IMPL_ENUM) + ">", "StringToEnumMap", name_to_enum.str(), "Mapping readable strings to enum values"));
- fields.push_back(std::make_shared<CppField>("static inline const std::map<" + std::string(IMPL_ENUM) + ", int>", "EnumToValueMap", enum_to_value.str(), "Mapping enum values to a int value"));
- fields.push_back(std::make_shared<CppField>("static inline const std::map<int, " + std::string(IMPL_ENUM) + ">", "ValueToEnumMap", value_to_enum.str(), "Mapping int values to a enum"));
-
- fields.push_back(std::make_shared<CppField>(std::string(IMPL_ENUM), "value", type.getAcceptedValueNames()[0], "The current value of the enum object"));
+ fields.push_back(std::make_shared<CppField>("static inline const std::map<" +
+ std::string(IMPL_ENUM) + ", std::string>",
+ "EnumToStringMap",
+ enum_to_name.str(),
+ "Mapping enum values to readable strings"));
+ fields.push_back(std::make_shared<CppField>("static inline const std::map<std::string, " +
+ std::string(IMPL_ENUM) + ">",
+ "StringToEnumMap",
+ name_to_enum.str(),
+ "Mapping readable strings to enum values"));
+ fields.push_back(std::make_shared<CppField>("static inline const std::map<" +
+ std::string(IMPL_ENUM) + ", int>",
+ "EnumToValueMap",
+ enum_to_value.str(),
+ "Mapping enum values to a int value"));
+ fields.push_back(std::make_shared<CppField>("static inline const std::map<int, " +
+ std::string(IMPL_ENUM) + ">",
+ "ValueToEnumMap",
+ value_to_enum.str(),
+ "Mapping int values to a enum"));
+
+ fields.push_back(std::make_shared<CppField>(std::string(IMPL_ENUM),
+ "value",
+ type.getAcceptedValueNames()[0],
+ "The current value of the enum object"));
return fields;
}
- CppBlockPtr IntEnumClass::getResetSoftBlock(const std::string& accessor) const
+ CppBlockPtr
+ IntEnumClass::getResetSoftBlock(const std::string& accessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("value = {};");
return block_if_data;
}
- CppBlockPtr IntEnumClass::getResetHardBlock(const std::string& accessor) const
+ CppBlockPtr
+ IntEnumClass::getResetHardBlock(const std::string& accessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("value = {};");
return block_if_data;
}
- CppBlockPtr IntEnumClass::getWriteTypeBlock(const std::string&, const std::string&, const Path& p, std::string&) const
+ CppBlockPtr
+ IntEnumClass::getWriteTypeBlock(const std::string&,
+ const std::string&,
+ const Path& p,
+ std::string&) const
{
static const std::string INT_ENUM_VALUE_MAP = ARON_VARIABLE_PREFIX + "_str2ValueMap";
@@ -113,40 +147,50 @@ namespace armarx::aron::codegenerator::cpp::generator
{
map_initializer.push_back("{\"" + key + "\", " + std::to_string(value) + "}");
}
- b->addLine("std::map<std::string, int> " + INT_ENUM_VALUE_MAP + " = {" + simox::alg::to_string(map_initializer, ", ") + "};");
- b->addLine("return " + ARON_WRITER_ACCESSOR + ".writeIntEnum(\"" + type.getEnumName() + "\", " +
- INT_ENUM_VALUE_MAP + ", " +
- ARON_MAYBE_TYPE_ACCESSOR + ", " +
- ARON_PATH_ACCESSOR + ");");
- return b;
-
+ b->addLine("std::map<std::string, int> " + INT_ENUM_VALUE_MAP + " = {" +
+ simox::alg::to_string(map_initializer, ", ") + "};");
+ b->addLine("return " + ARON_WRITER_ACCESSOR + ".writeIntEnum(\"" + type.getEnumName() +
+ "\", " + INT_ENUM_VALUE_MAP + ", " + "\"" + type.getDefaultValueName() + "\", " +
+ ARON_MAYBE_TYPE_ACCESSOR + ", " + ARON_PATH_ACCESSOR + ");");
return b;
}
- CppBlockPtr IntEnumClass::getWriteBlock(const std::string&, const Path& p, std::string&) const
+ CppBlockPtr
+ IntEnumClass::getWriteBlock(const std::string&, const Path& p, std::string&) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("return " + ARON_WRITER_ACCESSOR + ".writePrimitive(EnumToValueMap.at(value), " +
- ARON_PATH_ACCESSOR + "); // of top level enum " + getInstantiatedCppTypename());
+ block_if_data->addLine("return " + ARON_WRITER_ACCESSOR +
+ ".writePrimitive(EnumToValueMap.at(value), " + ARON_PATH_ACCESSOR +
+ "); // of top level enum " + getInstantiatedCppTypename());
return block_if_data;
}
- CppBlockPtr IntEnumClass::getReadBlock(const std::string&, const std::string& variantAccessor) const
+ CppBlockPtr
+ IntEnumClass::getReadBlock(const std::string&, const std::string& variantAccessor) const
{
static const std::string INT_ENUM_TMP_VALUE = ARON_VARIABLE_PREFIX + "_tmpValue";
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("int " + INT_ENUM_TMP_VALUE + ";");
- block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readPrimitive("+variantAccessor+", "+INT_ENUM_TMP_VALUE+"); // of top level enum " + getInstantiatedCppTypename());
-
- block_if_data->addLine("auto valueToEnumMap_iterator = ValueToEnumMap.find("+INT_ENUM_TMP_VALUE+");");
- block_if_data->addLine("ARMARX_CHECK_AND_THROW(valueToEnumMap_iterator != ValueToEnumMap.end(), ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Missing enum for value '\" + std::to_string(" + INT_ENUM_TMP_VALUE + ") + \"' in aron enum '" + getFullClassCppTypename() + "'.\"))");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readPrimitive(" + variantAccessor +
+ ", " + INT_ENUM_TMP_VALUE + "); // of top level enum " +
+ getInstantiatedCppTypename());
+
+ block_if_data->addLine("auto valueToEnumMap_iterator = ValueToEnumMap.find(" +
+ INT_ENUM_TMP_VALUE + ");");
+ block_if_data->addLine(
+ "ARMARX_CHECK_AND_THROW(valueToEnumMap_iterator != ValueToEnumMap.end(), "
+ "::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Missing enum for value "
+ "'\" + std::to_string(" +
+ INT_ENUM_TMP_VALUE + ") + \"' in aron enum '" + getFullClassCppTypename() + "'.\"))");
block_if_data->addLine("value = valueToEnumMap_iterator->second;");
return block_if_data;
}
- CppBlockPtr IntEnumClass::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ CppBlockPtr
+ IntEnumClass::getEqualsBlock(const std::string& accessor,
+ const std::string& otherInstanceAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("if (not (value == " + otherInstanceAccessor + ".value))");
@@ -154,7 +198,6 @@ namespace armarx::aron::codegenerator::cpp::generator
return block_if_data;
}
-
/*CppCtorPtr IntEnumClass::toCopyCtor(const std::string& name) const
{
CppCtorPtr c = std::make_shared<CppCtor>(name + "(const " + getFullInstantiatedCppTypename() + "& i)");
@@ -164,7 +207,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return c;
}*/
- CppCtorPtr IntEnumClass::toEnumCtor(const std::string& name) const
+ CppCtorPtr
+ IntEnumClass::toEnumCtor(const std::string& name) const
{
CppCtorPtr c = std::make_shared<CppCtor>(name + "(const " + std::string(IMPL_ENUM) + " e)");
std::vector<std::pair<std::string, std::string>> initList = {{"value", "e"}};
@@ -173,7 +217,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return c;
}
- std::vector<CppEnumFieldPtr> IntEnumClass::toEnumFields() const
+ std::vector<CppEnumFieldPtr>
+ IntEnumClass::toEnumFields() const
{
std::vector<CppEnumFieldPtr> e;
for (const auto& [key, value] : type.getAcceptedValueMap())
@@ -183,7 +228,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return e;
}
- CppMethodPtr IntEnumClass::toIntMethod() const
+ CppMethodPtr
+ IntEnumClass::toIntMethod() const
{
std::stringstream doc;
doc << "@brief int() - Converts the internally stored value to int representation \n";
@@ -196,13 +242,17 @@ namespace armarx::aron::codegenerator::cpp::generator
return m;
}
- CppMethodPtr IntEnumClass::toCopyAssignmentMethod() const
+ CppMethodPtr
+ IntEnumClass::toCopyAssignmentMethod() const
{
std::stringstream doc;
doc << "@brief operator=() - Assignment operator for copy \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod(getFullInstantiatedCppTypename() + "& operator=(const " + getFullInstantiatedCppTypename() + "& c)", doc.str()));
+ CppMethodPtr m =
+ CppMethodPtr(new CppMethod(getFullInstantiatedCppTypename() + "& operator=(const " +
+ getFullInstantiatedCppTypename() + "& c)",
+ doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("value = c.value;");
b->addLine("return *this;");
@@ -210,13 +260,16 @@ namespace armarx::aron::codegenerator::cpp::generator
return m;
}
- CppMethodPtr IntEnumClass::toEnumAssignmentMethod() const
+ CppMethodPtr
+ IntEnumClass::toEnumAssignmentMethod() const
{
std::stringstream doc;
doc << "@brief operator=() - Assignment operator for the internally defined enum \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod(getFullInstantiatedCppTypename() + "& operator=(" + std::string(IMPL_ENUM) + " v)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(
+ getFullInstantiatedCppTypename() + "& operator=(" + std::string(IMPL_ENUM) + " v)",
+ doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("value = v;");
b->addLine("return *this;");
@@ -224,17 +277,20 @@ namespace armarx::aron::codegenerator::cpp::generator
return m;
}
- CppMethodPtr IntEnumClass::toIntAssignmentMethod() const
+ CppMethodPtr
+ IntEnumClass::toIntAssignmentMethod() const
{
std::stringstream doc;
doc << "@brief operator=() - Assignment operator for the internally defined enum \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod(getFullInstantiatedCppTypename() + "& operator=(int v)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(
+ new CppMethod(getFullInstantiatedCppTypename() + "& operator=(int v)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("if (auto it = ValueToEnumMap.find(v); it == ValueToEnumMap.end())");
CppBlockPtr b2 = std::make_shared<CppBlock>();
- b2->addLine("throw armarx::LocalException(\"The input int is not valid. Could net set the enum to value '\" + std::to_string(v) + \"'\");");
+ b2->addLine("throw armarx::LocalException(\"The input int is not valid. Could net set the "
+ "enum to value '\" + std::to_string(v) + \"'\");");
b->addBlock(b2);
b->addLine("else");
CppBlockPtr b3 = std::make_shared<CppBlock>();
@@ -245,7 +301,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return m;
}
- CppMethodPtr IntEnumClass::toToStringMethod() const
+ CppMethodPtr
+ IntEnumClass::toToStringMethod() const
{
std::stringstream doc;
doc << "@brief toString() - Converts the internally stored value to string \n";
@@ -258,17 +315,21 @@ namespace armarx::aron::codegenerator::cpp::generator
return m;
}
- CppMethodPtr IntEnumClass::toFromStringMethod() const
+ CppMethodPtr
+ IntEnumClass::toFromStringMethod() const
{
std::stringstream doc;
- doc << "@brief fromString() - sets the internally stored value to the corrsponding enum of the input str \n";
+ doc << "@brief fromString() - sets the internally stored value to the corrsponding enum of "
+ "the input str \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("void fromString(const std::string& str)", doc.str()));
+ CppMethodPtr m =
+ CppMethodPtr(new CppMethod("void fromString(const std::string& str)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("if (auto it = StringToEnumMap.find(str); it == StringToEnumMap.end())");
CppBlockPtr b2 = std::make_shared<CppBlock>();
- b2->addLine("throw armarx::LocalException(\"The input name is not valid. Could net set the enum to value '\" + str + \"'\");");
+ b2->addLine("throw armarx::LocalException(\"The input name is not valid. Could net set the "
+ "enum to value '\" + str + \"'\");");
b->addBlock(b2);
b->addLine("else");
CppBlockPtr b3 = std::make_shared<CppBlock>();
@@ -278,4 +339,4 @@ namespace armarx::aron::codegenerator::cpp::generator
m->setBlock(b);
return m;
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h
index fb5e22671ba678ea69072e7b97fee54dbdd395e9..555f1e48be7600da97ec828fbff4e07c77333df2 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h
@@ -23,17 +23,15 @@
#pragma once
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h>
-#include <RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h>
-
#include <map>
-#include <utility> // std::pair
+#include <utility> // std::pair
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- class IntEnumClass :
- public detail::SpecializedGeneratorBase<type::IntEnum, IntEnumClass>
+ class IntEnumClass : public detail::SpecializedGeneratorBase<type::IntEnum, IntEnumClass>
{
public:
// constructors
@@ -45,9 +43,15 @@ namespace armarx::aron::codegenerator::cpp::generator
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const final;
// TODO: Move some of those methods to upper class for enums (if we want to support multiple enums)
@@ -66,4 +70,4 @@ namespace armarx::aron::codegenerator::cpp::generator
static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
static constexpr const char* IMPL_ENUM = "ImplEnum";
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
index 605c7b8655db3accb2dd1999d7c62e1cfc48730e..6bfc0100f0738f06ea16052a666893c774f19868 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
@@ -24,9 +24,8 @@
// Header
#include "ObjectClass.h"
-#include <SimoxUtility/meta/type_name.h>
#include <SimoxUtility/algorithm/vector.hpp>
-
+#include <SimoxUtility/meta/type_name.h>
namespace armarx::aron::codegenerator::cpp::generator
{
@@ -36,27 +35,54 @@ namespace armarx::aron::codegenerator::cpp::generator
e.getObjectNameWithTemplateInstantiations(), // should be similar to the object name
e.getObjectNameWithTemplates(),
simox::meta::get_type_name<data::dto::Dict>(),
- simox::meta::get_type_name<type::dto::AronObject>(), e)
+ simox::meta::get_type_name<type::dto::AronObject>(),
+ e)
{
if (type.getMaybe() != type::Maybe::NONE)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Somehow the maybe flag of a top level object declaration is set. This is not valid!", std::to_string((int) type.getMaybe()) + " aka " + type::defaultconversion::string::Maybe2String.at(type.getMaybe()), type.getPath());
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__,
+ "Somehow the maybe flag of a top level object declaration is set. This is not "
+ "valid!",
+ std::to_string((int)type.getMaybe()) + " aka " +
+ type::defaultconversion::string::Maybe2String.at(type.getMaybe()),
+ type.getPath());
+ }
+ }
+
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ ObjectClass::getCtorInitializers(const std::string& name) const
+ {
+ std::vector<std::pair<std::string, std::string>> ret;
+ bool any = false;
+ for (const auto& [key, child] : type.getDirectMemberTypes())
+ {
+ auto child_s = FromAronType(*child);
+ auto p = child_s->getCtorInitializers(key);
+ for (const auto& el : p.first)
+ {
+ ret.push_back(el);
+ }
+ any = any or p.second;
}
+ return {ret, any};
}
- std::vector<std::string> ObjectClass::getRequiredIncludes() const
+ std::vector<std::string>
+ ObjectClass::getRequiredIncludes() const
{
std::vector<std::string> ret;
for (const auto& [key, child] : type.getDirectMemberTypes())
{
- (void) key;
+ (void)key;
auto child_s = FromAronType(*child);
ret = simox::alg::appended(ret, child_s->getRequiredIncludes());
}
return ret;
}
- std::vector<CppFieldPtr> ObjectClass::getPublicVariableDeclarations(const std::string&) const
+ std::vector<CppFieldPtr>
+ ObjectClass::getPublicVariableDeclarations(const std::string&) const
{
std::vector<CppFieldPtr> fields;
for (const auto& [key, member] : type.getDirectMemberTypes())
@@ -68,7 +94,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return fields;
}
- CppBlockPtr ObjectClass::getResetSoftBlock(const std::string& accessor) const
+ CppBlockPtr
+ ObjectClass::getResetSoftBlock(const std::string& accessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
if (type.getExtends() != nullptr)
@@ -86,7 +113,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return block_if_data;
}
- CppBlockPtr ObjectClass::getResetHardBlock(const std::string& accessor) const
+ CppBlockPtr
+ ObjectClass::getResetHardBlock(const std::string& accessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
if (type.getExtends() != nullptr)
@@ -104,7 +132,11 @@ namespace armarx::aron::codegenerator::cpp::generator
return block_if_data;
}
- CppBlockPtr ObjectClass::getWriteTypeBlock(const std::string&, const std::string&, const Path& p, std::string&) const
+ CppBlockPtr
+ ObjectClass::getWriteTypeBlock(const std::string&,
+ const std::string&,
+ const Path& p,
+ std::string&) const
{
static const std::string OBJECT_MEMBERS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectMembers";
static const std::string OBJECT_EXTENDS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectExtends";
@@ -116,10 +148,11 @@ namespace armarx::aron::codegenerator::cpp::generator
{
const auto extends_s = FromAronType(*type.getExtends());
b->addLine("// get base class of " + this->getFullInstantiatedCppTypename());
- b->addLine("auto " + OBJECT_EXTENDS_ACCESSOR + " = " + extends_s->getFullInstantiatedCppTypename() + "::writeType(" +
- ARON_WRITER_ACCESSOR + ", " +
- "{" + simox::alg::join(type.getExtends()->getTemplateInstantiations(), ", ") + "}, " +
- "::armarx::aron::type::Maybe::NONE);");
+ b->addLine("auto " + OBJECT_EXTENDS_ACCESSOR + " = " +
+ extends_s->getFullInstantiatedCppTypename() + "::writeType(" +
+ ARON_WRITER_ACCESSOR + ", " + "{" +
+ simox::alg::join(type.getExtends()->getTemplateInstantiations(), ", ") +
+ "}, " + "::armarx::aron::type::Maybe::NONE);");
}
else
{
@@ -132,9 +165,11 @@ namespace armarx::aron::codegenerator::cpp::generator
const auto child_s = FromAronType(*child);
std::string child_return_variant;
Path nextPath = p.withElement(key, true);
- CppBlockPtr child_b = child_s->getWriteTypeBlock(child_s->getFullInstantiatedCppTypename(), key, nextPath, child_return_variant);
+ CppBlockPtr child_b = child_s->getWriteTypeBlock(
+ child_s->getFullInstantiatedCppTypename(), key, nextPath, child_return_variant);
b->appendBlock(child_b);
- b->addLine(OBJECT_MEMBERS_ACCESSOR + ".emplace(\"" + key + "\", " + child_return_variant + ");");
+ b->addLine(OBJECT_MEMBERS_ACCESSOR + ".emplace(\"" + key + "\", " +
+ child_return_variant + ");");
}
std::vector<std::string> templatesQuoted;
std::vector<std::string> templateIntantiationsQuoted;
@@ -147,18 +182,19 @@ namespace armarx::aron::codegenerator::cpp::generator
templateIntantiationsQuoted.push_back("\"" + t + "\"");
}
- b->addLine("return " + ARON_WRITER_ACCESSOR + ".writeObject(\"" + type.getObjectName() + "\", " +
- "{" + simox::alg::join(templatesQuoted, ", ") + "}, " +
- ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + ", " +
- OBJECT_MEMBERS_ACCESSOR + ", " +
- OBJECT_EXTENDS_ACCESSOR + ", " +
- ARON_MAYBE_TYPE_ACCESSOR + ", " +
- ARON_PATH_ACCESSOR + "); // of top level object " + getInstantiatedCppTypename());;
+ b->addLine("return " + ARON_WRITER_ACCESSOR + ".writeObject(\"" + type.getObjectName() +
+ "\", " + "{" + simox::alg::join(templatesQuoted, ", ") + "}, " +
+ ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + ", " + OBJECT_MEMBERS_ACCESSOR + ", " +
+ OBJECT_EXTENDS_ACCESSOR + ", " + ARON_MAYBE_TYPE_ACCESSOR + ", " +
+ ARON_PATH_ACCESSOR + "); // of top level object " +
+ getInstantiatedCppTypename());
+ ;
return b;
}
- CppBlockPtr ObjectClass::getWriteBlock(const std::string&, const Path& p, std::string&) const
+ CppBlockPtr
+ ObjectClass::getWriteBlock(const std::string&, const Path& p, std::string&) const
{
static const std::string OBJECT_MEMBERS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectMembers";
static const std::string OBJECT_EXTENDS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectExtends";
@@ -171,8 +207,11 @@ namespace armarx::aron::codegenerator::cpp::generator
if (type.getExtends() != nullptr)
{
const auto extends_s = FromAronType(*type.getExtends());
- block_if_data->addLine("// write base class of " + this->getFullInstantiatedCppTypename());
- block_if_data->addLine(OBJECT_EXTENDS_ACCESSOR + " = " + extends_s->getFullInstantiatedCppTypename() + "::write(" + ARON_WRITER_ACCESSOR + ");");
+ block_if_data->addLine("// write base class of " +
+ this->getFullInstantiatedCppTypename());
+ block_if_data->addLine(OBJECT_EXTENDS_ACCESSOR + " = " +
+ extends_s->getFullInstantiatedCppTypename() + "::write(" +
+ ARON_WRITER_ACCESSOR + ");");
}
block_if_data->addLine("// members of " + this->getFullInstantiatedCppTypename());
@@ -183,51 +222,67 @@ namespace armarx::aron::codegenerator::cpp::generator
Path nextPath = p.withElement(key, true);
CppBlockPtr child_b = child_s->getWriteBlock(key, nextPath, child_return_variant);
- block_if_data->addLine("auto " + child_return_variant + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ block_if_data->addLine("auto " + child_return_variant + " = " + ARON_WRITER_ACCESSOR +
+ ".writeNull();");
block_if_data->appendBlock(child_b);
- block_if_data->addLine(OBJECT_MEMBERS_ACCESSOR + ".emplace(\"" + key + "\", " + child_return_variant + ");");
+ block_if_data->addLine(OBJECT_MEMBERS_ACCESSOR + ".emplace(\"" + key + "\", " +
+ child_return_variant + ");");
}
- block_if_data->addLine("return " + ARON_WRITER_ACCESSOR + ".writeDict("+OBJECT_MEMBERS_ACCESSOR+", " +
- OBJECT_EXTENDS_ACCESSOR + ", "+
- ARON_PATH_ACCESSOR + "); // of top level object " + getInstantiatedCppTypename());
+ block_if_data->addLine("return " + ARON_WRITER_ACCESSOR + ".writeDict(" +
+ OBJECT_MEMBERS_ACCESSOR + ", " + OBJECT_EXTENDS_ACCESSOR + ", " +
+ ARON_PATH_ACCESSOR + "); // of top level object " +
+ getInstantiatedCppTypename());
return block_if_data;
}
- CppBlockPtr ObjectClass::getReadBlock(const std::string&, const std::string& variantAccessor) const
+ CppBlockPtr
+ ObjectClass::getReadBlock(const std::string&, const std::string& variantAccessor) const
{
static const std::string OBJECT_MEMBERS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectMembers";
static const std::string OBJECT_EXTENDS_ACCESSOR = ARON_VARIABLE_PREFIX + "_objectExtends";
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- block_if_data->addLine("std::map<std::string, _Aron_TNonConst> " + OBJECT_MEMBERS_ACCESSOR + ";");
+ block_if_data->addLine("std::map<std::string, _Aron_TNonConst> " + OBJECT_MEMBERS_ACCESSOR +
+ ";");
if (type.getExtends() != nullptr)
{
const auto extends_s = FromAronType(*type.getExtends());
- block_if_data->addLine(extends_s->getFullInstantiatedCppTypename() + "::read(" + ARON_READER_ACCESSOR + ", "+variantAccessor+");");
+ block_if_data->addLine(extends_s->getFullInstantiatedCppTypename() + "::read(" +
+ ARON_READER_ACCESSOR + ", " + variantAccessor + ");");
}
- block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readDict("+variantAccessor+", "+OBJECT_MEMBERS_ACCESSOR+"); // of top level object " + getInstantiatedCppTypename());
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readDict(" + variantAccessor + ", " +
+ OBJECT_MEMBERS_ACCESSOR + "); // of top level object " +
+ getInstantiatedCppTypename());
for (const auto& [key, child] : type.getDirectMemberTypes())
{
const auto child_s = FromAronType(*child);
std::string child_accessor = OBJECT_MEMBERS_ACCESSOR + "_" + key + "_iterator";
- block_if_data->addLine("auto " + child_accessor + " = " + OBJECT_MEMBERS_ACCESSOR + ".find(\"" + key + "\");");
- block_if_data->addLine("ARMARX_CHECK_AND_THROW(" + child_accessor + " != " + OBJECT_MEMBERS_ACCESSOR + ".end(), ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Missing member '" + key + "' in aron object '" + getFullClassCppTypename() + "'.\"));");
+ block_if_data->addLine("auto " + child_accessor + " = " + OBJECT_MEMBERS_ACCESSOR +
+ ".find(\"" + key + "\");");
+ block_if_data->addLine(
+ "ARMARX_CHECK_AND_THROW(" + child_accessor + " != " + OBJECT_MEMBERS_ACCESSOR +
+ ".end(), ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Missing "
+ "member '" +
+ key + "' in aron object '" + getFullClassCppTypename() + "'.\"));");
block_if_data->appendBlock(child_s->getReadBlock(key, child_accessor + "->second"));
}
return block_if_data;
}
- CppBlockPtr ObjectClass::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
+ CppBlockPtr
+ ObjectClass::getEqualsBlock(const std::string& accessor,
+ const std::string& otherInstanceAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
if (type.getExtends() != nullptr)
{
const auto extends_s = FromAronType(*type.getExtends());
- block_if_data->addLine("if (not (" + extends_s->getFullInstantiatedCppTypename() + "::operator== (" + otherInstanceAccessor + ")))");
+ block_if_data->addLine("if (not (" + extends_s->getFullInstantiatedCppTypename() +
+ "::operator== (" + otherInstanceAccessor + ")))");
block_if_data->addLineAsBlock("return false;");
}
for (const auto& [key, child] : type.getDirectMemberTypes())
@@ -239,7 +294,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return block_if_data;
}
- std::pair<std::vector<std::pair<std::string, std::string>>, bool> ObjectClass::getCopyCtorInitializers(const std::string&) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ ObjectClass::getCopyCtorInitializers(const std::string&) const
{
std::vector<std::pair<std::string, std::string>> ret;
@@ -259,4 +315,4 @@ namespace armarx::aron::codegenerator::cpp::generator
}
return {ret, anyComplex};
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h
index 772d138cb7cc521753ce6bd3086bb076d475b462..34d0f16c058cca25a7fd2905e0c43f926ea365b7 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h
@@ -26,11 +26,9 @@
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
- class ObjectClass :
- public detail::SpecializedGeneratorBase<type::Object, ObjectClass>
+ class ObjectClass : public detail::SpecializedGeneratorBase<type::Object, ObjectClass>
{
public:
// constructors
@@ -41,13 +39,20 @@ namespace armarx::aron::codegenerator::cpp::generator
std::vector<std::string> getRequiredIncludes() const final;
std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const final;
- std::pair<std::vector<std::pair<std::string, std::string>>, bool> getCopyCtorInitializers(const std::string&) const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const final;
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCopyCtorInitializers(const std::string&) const final;
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string&) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr
+ getWriteBlock(const std::string& cppAccessor, const Path&, std::string&) const final;
CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string&) const final;
CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/BaseClassTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/BaseClassTest.xml
index d8ca3c690bce46d85a09ea7ce4a31372ad62cd9e..ad363bcf7627ad0d1f4b5d4549e6568a8959d81f 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/BaseClassTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/BaseClassTest.xml
@@ -15,7 +15,7 @@
<ObjectChild key='base_class_member2'>
<List>
- <Float />
+ <float32 />
</List>
</ObjectChild>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/DerivedClassTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/DerivedClassTest.xml
index 47c12680ff9adb38b8b26fa91c526ccca04bd21e..d151851349369f44bd500b5d683be43fa2103c20 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/DerivedClassTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/DerivedClassTest.xml
@@ -9,7 +9,7 @@
<Object name='armarx::DerivedClassTest' extends="armarx::BaseClassTest" doc-brief="This is a brief doc" doc-author="Itse me, Mario!">
<ObjectChild key='derived_class_member1' doc-brief="This is a member doc string">
- <Int />
+ <int32 />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/DictTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/DictTest.xml
index fadec8226455e878c6a98ed7a76131c66a3944e6..7e1df867abee6fce3c159d447cacef90afd04c32 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/DictTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/DictTest.xml
@@ -5,7 +5,7 @@
<Object name='armarx::DictTest'>
<objectchild key='dict'>
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/HumanPoseTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/HumanPoseTest.xml
index 78f4fd166c505e6e8b2d0cd4f4ac5c73f391d371..0f39658f744563187ef21188726155c5c72b67b2 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/HumanPoseTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/HumanPoseTest.xml
@@ -9,7 +9,7 @@
<ObjectChild key='jointValues'>
<List>
- <Float />
+ <float32 />
</List>
</ObjectChild>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/ListTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/ListTest.xml
index 1cde35422162d4d3e3d3387cabbe3d8845343113..11c3cc482c006d712806c0994de29a20a13f0424 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/ListTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/ListTest.xml
@@ -7,32 +7,32 @@
<Bool />
</ObjectChild>
<ObjectChild key='element1'>
- <Int />
+ <int32 />
</ObjectChild>
<ObjectChild key='element2'>
- <Double />
+ <float64 />
</ObjectChild>
</Object>
<Object name='armarx::ListTest'>
<ObjectChild key='floatList'>
<List>
- <Float />
+ <float32 />
</List>
</ObjectChild>
<ObjectChild key='intList'>
<List>
- <Int />
+ <int32 />
</List>
</ObjectChild>
<ObjectChild key='doubleList'>
<List>
- <Double />
+ <float64 />
</List>
</ObjectChild>
<ObjectChild key='longList'>
<List>
- <Long />
+ <int64 />
</List>
</ObjectChild>
<ObjectChild key='stringList'>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/NaturalIKTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/NaturalIKTest.xml
index 769a899a51e82d2c25e533b96bd1054f66565eef..7b707b61223978cefcf26f8fac96ec823f5f7ab8 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/NaturalIKTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/NaturalIKTest.xml
@@ -19,13 +19,9 @@
<ObjectChild key='jointValues'>
<List>
- <Float />
+ <float32 />
</List>
</ObjectChild>
</Object>
</GenerateTypes>
</AronTypeDefinition>
-
-
-
-
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/ObjectTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/ObjectTest.xml
index d0b514701e3d6647becf3ea469c13fbf349dcd3f..73e0232fd1a7b87d03da45b816dd6f3c24feb6cf 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/ObjectTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/ObjectTest.xml
@@ -14,7 +14,7 @@
<bool />
</objectchild>
<objectchild key='the_int'>
- <int />
+ <int32 />
</objectchild>
</object>
@@ -23,7 +23,7 @@
<bool />
</objectchild>
<objectchild key='the_int'>
- <int />
+ <int32 />
</objectchild>
</object>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/OptionalTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/OptionalTest.xml
index 29b58d4594a99b5845c25fcbfe003d97bb89b763..ec4f0bef2f719c0c1921a6725b8c8496fd4a2468 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/OptionalTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/OptionalTest.xml
@@ -7,7 +7,7 @@
<GenerateTypes>
<Object name="armarx::OptionalTestElement">
<ObjectChild key="val">
- <Float />
+ <float32 />
</ObjectChild>
</Object>
@@ -24,7 +24,7 @@
<ObjectChild key='some_dict'>
<Dict optional="true">
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
@@ -36,7 +36,7 @@
<ObjectChild key='some_list'>
<List optional="true">
- <Double />
+ <float64 />
</List>
</ObjectChild>
@@ -46,8 +46,8 @@
</List>
</ObjectChild>
- <ObjectChild key='some_eigen_matrix'>
- <Matrix rows="25" cols="10" type="int64" optional="true"/>
+ <ObjectChild key='some_eigen_matrix'> <!-- not optional -->
+ <Matrix rows="25" cols="10" type="int64" />
</ObjectChild>
<ObjectChild key='some_obj'>
@@ -63,7 +63,7 @@
</ObjectChild>
<ObjectChild key="an_optional_pose">
- <Pose />
+ <Pose optional="true"/>
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/OrientationTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/OrientationTest.xml
index 5c23de5f95be8b7052529ed4e8e0d190854e66db..6b91d76c6245e54bca4ba11033a8e462d496f4a4 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/OrientationTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/OrientationTest.xml
@@ -7,6 +7,10 @@
<Orientation />
</ObjectChild>
+ <ObjectChild key='framedorientation'>
+ <FramedOrientation />
+ </ObjectChild>
+
</Object>
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/PoseTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/PoseTest.xml
index cb0204202933cf976c0c2d6b52513df6fbdbba5c..a04872d282559b2f17b169d9f3386f57427a40c8 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/PoseTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/PoseTest.xml
@@ -7,6 +7,10 @@
<Pose />
</ObjectChild>
+ <ObjectChild key='framedpose'>
+ <FramedPose />
+ </ObjectChild>
+
</Object>
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/PositionTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/PositionTest.xml
index d479592007a94e1e9cf07ddf354611d30dfe0ea9..6bde176dff76513bc4abf4dfde1a42128d42ce5a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/PositionTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/PositionTest.xml
@@ -7,6 +7,10 @@
<Position />
</ObjectChild>
+ <ObjectChild key='framedposition'>
+ <FramedPosition />
+ </ObjectChild>
+
</Object>
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/PrimitiveTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/PrimitiveTest.xml
index 1770306699daf5f27d4fbe9d820c47a04b6b68b6..549e34181085626b9aec8958964a2cdcedf247c7 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/PrimitiveTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/PrimitiveTest.xml
@@ -4,22 +4,22 @@
<GenerateTypes>
<Object name='armarx::PrimitiveTest'>
<objectchild key='the_int'>
- <int />
+ <int default="45"/>
</ObjectChild>
<objectchild key='the_long'>
- <long />
+ <long default="44"/>
</ObjectChild>
<objectchild key='the_float'>
- <float />
+ <float default="43"/>
</ObjectChild>
<objectchild key='the_double'>
- <double />
+ <double default="42"/>
</ObjectChild>
<objectchild key='the_string'>
- <string />
+ <string default="lalala" />
</ObjectChild>
<objectchild key='the_bool'>
- <bool />
+ <bool default="false"/>
</ObjectChild>
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aron/TemplateTest.xml b/source/RobotAPI/libraries/aron/codegeneration/test/aron/TemplateTest.xml
index 304ae4193d5710f9d397705a083abc81b088a55e..2391afa20a0857ab51be63c72c4f08d69a121b79 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/TemplateTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/TemplateTest.xml
@@ -32,7 +32,7 @@
</ObjectChild>
<ObjectChild key='da_int'>
- <Int />
+ <int32 />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aronCodeGenerationTest.cpp b/source/RobotAPI/libraries/aron/codegeneration/test/aronCodeGenerationTest.cpp
index e9c8ad44b3de2bb5af9dc2248f32ac628611c4ee..de489f82e25a53bcc061a89e514d2b40113ead21 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aronCodeGenerationTest.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aronCodeGenerationTest.cpp
@@ -25,49 +25,49 @@
#define ARMARX_BOOST_TEST
// STD/STL
-#include <iostream>
#include <cstdlib>
#include <ctime>
+#include <iostream>
#include <numeric>
// Test
#include <RobotAPI/Test.h>
// ArmarX
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+
#include <RobotAPI/libraries/aron/core/Exception.h>
// Generated File
-#include <RobotAPI/libraries/aron/codegeneration/test/aron/TemplateTest.aron.generated.h>
#include <RobotAPI/libraries/aron/codegeneration/test/aron/AnyTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/codegeneration/test/aron/ListTest.aron.generated.h>
#include <RobotAPI/libraries/aron/codegeneration/test/aron/DictTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/codegeneration/test/aron/PrimitiveTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/codegeneration/test/aron/ObjectTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/DtoTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/EnumTest.aron.generated.h>
#include <RobotAPI/libraries/aron/codegeneration/test/aron/ImageTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/ListTest.aron.generated.h>
#include <RobotAPI/libraries/aron/codegeneration/test/aron/MatrixTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/codegeneration/test/aron/QuaternionTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/codegeneration/test/aron/PointCloudTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/codegeneration/test/aron/PositionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/ObjectTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/OptionalTest.aron.generated.h>
#include <RobotAPI/libraries/aron/codegeneration/test/aron/OrientationTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/PointCloudTest.aron.generated.h>
#include <RobotAPI/libraries/aron/codegeneration/test/aron/PoseTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/codegeneration/test/aron/EnumTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/codegeneration/test/aron/OptionalTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/codegeneration/test/aron/DtoTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/PositionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/PrimitiveTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/QuaternionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/TemplateTest.aron.generated.h>
using namespace armarx;
using namespace aron;
-
namespace std
{
- ostream& operator<<(ostream& os, const type_info& i)
+ ostream&
+ operator<<(ostream& os, const type_info& i)
{
return os << "'" << i.name() << "'";
}
-}
-
+} // namespace std
BOOST_AUTO_TEST_CASE(AronCodeGenerationAnyTest)
{
@@ -83,7 +83,8 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationAnyTest)
auto aron = p.toAron();
auto any_object_member = aron->getElement("any_object");
auto any_object_member_casted = aron::data::Dict::DynamicCastAndCheck(any_object_member);
- auto any_object_int_var = aron::data::Int::DynamicCastAndCheck(any_object_member_casted->getElement("hello_world"));
+ auto any_object_int_var =
+ aron::data::Int::DynamicCastAndCheck(any_object_member_casted->getElement("hello_world"));
BOOST_CHECK_EQUAL(any_object_int_var->getValue(), 5);
AnyTest p2;
@@ -92,7 +93,6 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationAnyTest)
BOOST_CHECK_EQUAL(int_var2->getValue(), 5);
}
-
BOOST_AUTO_TEST_CASE(AronCodeGenerationTemplateTest)
{
std::cout << "Running Code Gen Tmpl test" << std::endl;
@@ -109,13 +109,16 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationTemplateTest)
auto t_daint = aron::data::Int::DynamicCastAndCheck(aron->getElement("da_int"));
BOOST_CHECK_EQUAL(t_daint->getValue(), 1234);
- auto t_dabar_dastring = aron::data::String::DynamicCastAndCheck(t_dabar->getElement("da_string"));
+ auto t_dabar_dastring =
+ aron::data::String::DynamicCastAndCheck(t_dabar->getElement("da_string"));
BOOST_CHECK_EQUAL(t_dabar_dastring->getValue(), "da_bar");
- auto t_dafoo_dastring = aron::data::String::DynamicCastAndCheck(t_dafoo->getElement("da_string"));
+ auto t_dafoo_dastring =
+ aron::data::String::DynamicCastAndCheck(t_dafoo->getElement("da_string"));
BOOST_CHECK_EQUAL(t_dafoo_dastring->getValue(), "da_foo");
auto t_daimpl_dablarg = aron::data::Dict::DynamicCastAndCheck(t_daimpl->getElement("da_blarg"));
- auto t_daimpl_dablarg_dastring = aron::data::String::DynamicCastAndCheck(t_daimpl_dablarg->getElement("da_string"));
+ auto t_daimpl_dablarg_dastring =
+ aron::data::String::DynamicCastAndCheck(t_daimpl_dablarg->getElement("da_string"));
BOOST_CHECK_EQUAL(t_daimpl_dablarg_dastring->getValue(), "da_tmpl.da_blarg");
auto aronT = TemplateTest2<WithoutTemplateTest, WithoutTemplateTest>::ToAronType();
@@ -137,15 +140,16 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationTemplateTest)
BOOST_CHECK_EQUAL(tt_dabar->getTemplateInstantiations(), std::vector<std::string>{});
BOOST_CHECK_EQUAL(tt_dafoo->getTemplateInstantiations(), std::vector<std::string>{});
- BOOST_CHECK_EQUAL(tt_datmpl->getTemplateInstantiations(), std::vector<std::string>{"armarx::WithoutTemplateTest"});
+ BOOST_CHECK_EQUAL(tt_datmpl->getTemplateInstantiations(),
+ std::vector<std::string>{"armarx::WithoutTemplateTest"});
- auto tt_daimpl_dablarg = aron::type::Object::DynamicCastAndCheck(tt_datmpl->getMemberType("da_blarg"));
+ auto tt_daimpl_dablarg =
+ aron::type::Object::DynamicCastAndCheck(tt_datmpl->getMemberType("da_blarg"));
BOOST_CHECK_EQUAL(tt_daimpl_dablarg->getObjectName(), "armarx::WithoutTemplateTest");
BOOST_CHECK_EQUAL(tt_daimpl_dablarg->getTemplates(), std::vector<std::string>{});
BOOST_CHECK_EQUAL(tt_daimpl_dablarg->getTemplateInstantiations(), std::vector<std::string>{});
}
-
BOOST_AUTO_TEST_CASE(AronCodeGenerationListTest)
{
std::cout << "Running Code Gen List test" << std::endl;
@@ -170,7 +174,6 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationListTest)
BOOST_CHECK_EQUAL(p.objectList.size(), 0);
}
-
BOOST_AUTO_TEST_CASE(AronCodeGenerationDictTest)
{
std::cout << "Running Code Gen Dict test" << std::endl;
@@ -181,7 +184,6 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationDictTest)
BOOST_CHECK_EQUAL(p.dict.size(), 0);
}
-
BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenMatrixTest)
{
std::cout << "Running Code Gen EigenMatrix test" << std::endl;
@@ -201,7 +203,6 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenMatrixTest)
BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.cols(), 1);
}
-
BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenQuaternionTest)
{
std::cout << "Running Code Gen EigenQuaternion test" << std::endl;
@@ -215,7 +216,6 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenQuaternionTest)
//BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.z(), 0);
}
-
BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenPositionTest)
{
std::cout << "Running Code Gen EigenPosition test" << std::endl;
@@ -227,7 +227,6 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenPositionTest)
BOOST_CHECK_EQUAL(p.position.cols(), 1);
}
-
BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenPoseTest)
{
std::cout << "Running Code Gen EigenPose test" << std::endl;
@@ -239,7 +238,6 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenPoseTest)
BOOST_CHECK_EQUAL(p.pose.cols(), 4);
}
-
BOOST_AUTO_TEST_CASE(AronCodeGenerationIntEnumTest)
{
std::cout << "Running Code Gen IntEnum test" << std::endl;
@@ -250,7 +248,6 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationIntEnumTest)
BOOST_CHECK(p.the_int_enum.value == TheIntEnum::INT_ENUM_VALUE_0);
}
-
BOOST_AUTO_TEST_CASE(AronCodeGenerationPrimitiveTest)
{
std::cout << "Running Code Gen Primitive test" << std::endl;
@@ -259,14 +256,13 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationPrimitiveTest)
BOOST_CHECK(p_tmp == p);
BOOST_CHECK_EQUAL(p.the_bool, false);
- BOOST_CHECK_EQUAL(p.the_double, 0.0);
- BOOST_CHECK_EQUAL(p.the_float, 0.0);
- BOOST_CHECK_EQUAL(p.the_int, 0);
- BOOST_CHECK_EQUAL(p.the_long, 0);
- BOOST_CHECK_EQUAL(p.the_string, "");
+ BOOST_CHECK_EQUAL(p.the_double, 42);
+ BOOST_CHECK_EQUAL(p.the_float, 43);
+ BOOST_CHECK_EQUAL(p.the_int, 45);
+ BOOST_CHECK_EQUAL(p.the_long, 44);
+ BOOST_CHECK_EQUAL(p.the_string, "lalala");
}
-
BOOST_AUTO_TEST_CASE(AronCodeGenerationOptionalTest)
{
std::cout << "Running Code Gen Optional test" << std::endl;
@@ -275,17 +271,22 @@ BOOST_AUTO_TEST_CASE(AronCodeGenerationOptionalTest)
BOOST_CHECK(p_tmp == p);
BOOST_CHECK_EQUAL(typeid(p.some_dict), typeid(std::optional<std::map<std::string, float>>));
- BOOST_CHECK_EQUAL(typeid(p.some_dict_with_optional_type), typeid(std::map<std::string, std::optional<float>>));
- BOOST_CHECK_EQUAL(typeid(p.some_eigen_matrix), typeid(std::optional<Eigen::Matrix<long, 25, 10>>));
+ BOOST_CHECK_EQUAL(typeid(p.some_dict_with_optional_type),
+ typeid(std::map<std::string, std::optional<float>>));
+ BOOST_CHECK_EQUAL(typeid(p.some_eigen_matrix), typeid(Eigen::Matrix<long, 25, 10>));
BOOST_CHECK_EQUAL(typeid(p.some_float), typeid(std::optional<float>));
BOOST_CHECK_EQUAL(typeid(p.some_list), typeid(std::optional<std::vector<double>>));
- BOOST_CHECK_EQUAL(typeid(p.some_list_with_optional_list), typeid(std::optional<std::vector<std::optional<std::vector<std::optional<float>>>>>));
- BOOST_CHECK_EQUAL(typeid(p.some_list_with_optional_type), typeid(std::vector<std::optional<double>>));
+ BOOST_CHECK_EQUAL(
+ typeid(p.some_list_with_optional_list),
+ typeid(std::optional<std::vector<std::optional<std::vector<std::optional<float>>>>>));
+ BOOST_CHECK_EQUAL(typeid(p.some_list_with_optional_type),
+ typeid(std::vector<std::optional<double>>));
BOOST_CHECK_EQUAL(typeid(p.some_obj), typeid(std::optional<armarx::OptionalTestElement>));
BOOST_CHECK_EQUAL(typeid(p.some_string), typeid(std::optional<std::string>));
auto aronType = p.ToAronType();
- BOOST_CHECK_EQUAL(aronType->getMemberType("some_float")->getMaybe(), aron::type::Maybe::OPTIONAL);
+ BOOST_CHECK_EQUAL(aronType->getMemberType("some_float")->getMaybe(),
+ aron::type::Maybe::OPTIONAL);
BOOST_CHECK(not p.some_float.has_value());
auto aron = p.toAron();
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.cpp b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.cpp
index 925582621433aa3f679dccee50af2b7e2dcad6d7..5f409eb05e322029765e3d8322be49af2430be93 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.cpp
@@ -24,12 +24,128 @@
#include "Data.h"
#include <SimoxUtility/algorithm/string/string_tools.h>
+
#include <RobotAPI/libraries/aron/core/Exception.h>
namespace armarx::aron::typereader::xml
{
+ bool
+ util::CheckStrIsInt(const std::string& s)
+ {
+ try
+ {
+ simox::alg::to_<int>(s);
+ }
+ catch (simox::error::SimoxError&)
+ {
+ return false;
+ }
+ return true;
+ }
+
+ void
+ util::EnforceStrIsInt(const std::string& s)
+ {
+ if (!CheckStrIsInt(s))
+ {
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not convert a string to an integer", s);
+ }
+ }
+
+ bool
+ util::CheckStrIsFloat(const std::string& s)
+ {
+ try
+ {
+ simox::alg::to_<float>(s);
+ }
+ catch (simox::error::SimoxError&)
+ {
+ return false;
+ }
+ return true;
+ }
+
+ void
+ util::EnforceStrIsFloat(const std::string& s)
+ {
+ if (!CheckStrIsFloat(s))
+ {
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not convert a string to a floating point", s);
+ }
+ }
+
+ bool
+ util::CheckStrIsNumber(const std::string& s)
+ {
+ return CheckStrIsInt(s) || CheckStrIsFloat(s);
+ }
+
+ void
+ util::EnforceStrIsNumber(const std::string& s)
+ {
+ if (!CheckStrIsNumber(s))
+ {
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not convert a string to a number", s);
+ }
+ }
+
+ bool
+ util::CheckStrIsNumberSeq(const std::string& s)
+ {
+ std::vector<std::string> split = simox::alg::split(s, ",");
+ for (const auto& e : split)
+ {
+ if (!CheckStrIsNumber(e))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ void
+ util::EnforceStrIsNumberSeq(const std::string& s)
+ {
+ if (!CheckStrIsNumberSeq(s))
+ {
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__,
+ "Could not convert a string to a sequence of numbers. Note that number sequences "
+ "have to separated with ','",
+ s);
+ }
+ }
+
+ bool
+ util::CheckStrIsBool(const std::string& s)
+ {
+ try
+ {
+ simox::alg::to_<bool>(s);
+ }
+ catch (simox::error::SimoxError&)
+ {
+ return false;
+ }
+ return true;
+ }
+
+ void
+ util::EnforceStrIsBool(const std::string& s)
+ {
+ if (!CheckStrIsBool(s))
+ {
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not convert a string to a bool", s);
+ }
+ }
- std::optional<RapidXmlReaderNode> util::GetFirstNodeWithTag(const RapidXmlReaderNode& node, const std::string& name)
+ std::optional<RapidXmlReaderNode>
+ util::GetFirstNodeWithTag(const RapidXmlReaderNode& node, const std::string& name)
{
for (const auto& n : node.nodes())
{
@@ -42,26 +158,35 @@ namespace armarx::aron::typereader::xml
return std::nullopt;
}
- void util::EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ void
+ util::EnforceAttribute(const RapidXmlReaderNode& node, const std::string& att)
{
if (!HasAttribute(node, att))
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "A <" + node.name() + ">-tag does not have the '"+att+"' attribute", simox::alg::to_string(node.get_all_attributes()));
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "A <" + node.name() + ">-tag does not have the '" +
+ att + "' attribute",
+ simox::alg::to_string(node.get_all_attributes()));
}
}
- bool util::HasAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ bool
+ util::HasAttribute(const RapidXmlReaderNode& node, const std::string& att)
{
return node.has_attribute(att.c_str());
}
- std::string util::GetAttribute(const RapidXmlReaderNode& node, const std::string& att)
+ std::string
+ util::GetAttribute(const RapidXmlReaderNode& node, const std::string& att)
{
EnforceAttribute(node, att);
return node.attribute_value(att.c_str());
}
- std::string util::GetAttributeWithDefault(const armarx::RapidXmlReaderNode& node, const std::string& att, const std::string& def)
+ std::string
+ util::GetAttributeWithDefault(const armarx::RapidXmlReaderNode& node,
+ const std::string& att,
+ const std::string& def)
{
if (!(HasAttribute(node, att)))
{
@@ -70,7 +195,8 @@ namespace armarx::aron::typereader::xml
return node.attribute_value(att.c_str());
}
- bool util::AttributeIsTrue(const armarx::RapidXmlReaderNode& node, const std::string& att)
+ bool
+ util::AttributeIsTrue(const armarx::RapidXmlReaderNode& node, const std::string& att)
{
if (HasAttribute(node, att))
{
@@ -83,20 +209,24 @@ namespace armarx::aron::typereader::xml
return false;
}
- bool util::HasTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
+ bool
+ util::HasTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
{
return (simox::alg::to_lower(name) == simox::alg::to_lower(node.name()));
}
- void util::EnforceTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
+ void
+ util::EnforceTagName(const armarx::RapidXmlReaderNode& node, const std::string& name)
{
if (!(HasTagName(node, name)))
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node has the wrong tag. Expected: " + name, node.name());
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "The node has the wrong tag. Expected: " + name, node.name());
}
}
- void util::EnforceTagNames(const RapidXmlReaderNode& node, const std::vector<std::string>& names)
+ void
+ util::EnforceTagNames(const RapidXmlReaderNode& node, const std::vector<std::string>& names)
{
for (const auto& name : names)
{
@@ -105,57 +235,86 @@ namespace armarx::aron::typereader::xml
return;
}
}
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node has the wrong tag. Expected one of: " + simox::alg::to_string(names, ", "), node.name());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "The node has the wrong tag. Expected one of: " +
+ simox::alg::to_string(names, ", "),
+ node.name());
}
- std::string util::GetTagName(const armarx::RapidXmlReaderNode& node)
+ std::string
+ util::GetTagName(const armarx::RapidXmlReaderNode& node)
{
return simox::alg::to_lower(node.name());
}
- void util::EnforceChildSizeSmaller(const RapidXmlReaderNode& node, const size_t size)
+ void
+ util::EnforceChildSizeSmaller(const RapidXmlReaderNode& node, const size_t size)
{
const size_t s = node.nodes().size();
if (s >= size)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong number of children", std::to_string(s), std::to_string(size));
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "The node <" + node.name() +
+ "> has the wrong number of children",
+ std::to_string(s),
+ std::to_string(size));
}
}
- void util::EnforceChildSizeSmallerEqual(const RapidXmlReaderNode& node, const size_t size)
+ void
+ util::EnforceChildSizeSmallerEqual(const RapidXmlReaderNode& node, const size_t size)
{
const size_t s = node.nodes().size();
if (s > size)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong number of children", std::to_string(s), std::to_string(size));
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "The node <" + node.name() +
+ "> has the wrong number of children",
+ std::to_string(s),
+ std::to_string(size));
}
}
- void util::EnforceChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
+ void
+ util::EnforceChildSize(const armarx::RapidXmlReaderNode& node, const size_t size)
{
const size_t s = node.nodes().size();
if (s != size)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong number of children", std::to_string(s), std::to_string(size));
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "The node <" + node.name() +
+ "> has the wrong number of children",
+ std::to_string(s),
+ std::to_string(size));
}
}
- void util::EnforceChildSizeGreaterEqual(const RapidXmlReaderNode& node, const size_t size)
+ void
+ util::EnforceChildSizeGreaterEqual(const RapidXmlReaderNode& node, const size_t size)
{
const size_t s = node.nodes().size();
if (s < size)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong number of children", std::to_string(s), std::to_string(size));
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "The node <" + node.name() +
+ "> has the wrong number of children",
+ std::to_string(s),
+ std::to_string(size));
}
}
- void util::EnforceChildSizeGreater(const RapidXmlReaderNode& node, const size_t size)
+ void
+ util::EnforceChildSizeGreater(const RapidXmlReaderNode& node, const size_t size)
{
const size_t s = node.nodes().size();
if (s <= size)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The node <" + node.name() + "> has the wrong number of children", std::to_string(s), std::to_string(size));
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "The node <" + node.name() +
+ "> has the wrong number of children",
+ std::to_string(s),
+ std::to_string(size));
}
}
-}
+} // namespace armarx::aron::typereader::xml
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.h b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.h
index bc557478142396f30a2d92191b469e2f7fcf438f..cb4a9bad4ce82f4d07ac1655cfb2edd3c0c15157 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.h
@@ -41,70 +41,102 @@ namespace armarx::aron::typereader::xml
namespace constantes
{
// Definition tags
- const std::string TYPE_DEFINITION_TAG = "arontypedefinition";
+ const constexpr auto TYPE_DEFINITION_TAG = "arontypedefinition";
- const std::string CODE_INCLUDES_TAG = "codeincludes";
- const std::string ARON_INCLUDES_TAG = "aronincludes";
- const std::string INCLUDES_TAG = "includes";
- const std::string SYSTEM_INCLUDE_TAG = "systeminclude";
- const std::string PACKAGE_PATH_TAG = "packagepath";
- const std::string INCLUDE_TAG = "include";
- const std::string GENERATE_TYPES_TAG = "generatetypes";
+ const constexpr auto CODE_INCLUDES_TAG = "codeincludes";
+ const constexpr auto ARON_INCLUDES_TAG = "aronincludes";
+ const constexpr auto INCLUDES_TAG = "includes";
+ const constexpr auto SYSTEM_INCLUDE_TAG = "systeminclude";
+ const constexpr auto PACKAGE_PATH_TAG = "packagepath";
+ const constexpr auto INCLUDE_TAG = "include";
+ const constexpr auto GENERATE_TYPES_TAG = "generatetypes";
// Attribute names
- const std::string ARON_ATTRIBUTE_NAME = "aron";
- const std::string CODE_ATTRIBUTE_NAME = "code";
- const std::string METHOD_ATTRIBUTE_NAME = "method";
- const std::string RETURN_ATTRIBUTE_NAME = "return";
- const std::string ARGUMENT_TYPE_ATTRIBUTE_NAME = "argumenttype";
- const std::string INCLUDE_ATTRIBUTE_NAME = "include";
- const std::string READER_ATTRIBUTE_NAME = "reader";
- const std::string WRITER_ATTRIBUTE_NAME = "writer";
- const std::string EXTENDS_ATTRIBUTE_NAME = "extends";
- const std::string NAME_ATTRIBUTE_NAME = "name";
- const std::string VALUE_ATTRIBUTE_NAME = "value";
- const std::string TEMPLATE_ATTRIBUTE_NAME = "template";
- const std::string KEY_ATTRIBUTE_NAME = "key";
- const std::string TYPE_ATTRIBUTE_NAME = "type";
- const std::string WIDTH_ATTRIBUTE_NAME = "width";
- const std::string HEIGHT_ATTRIBUTE_NAME = "height";
- const std::string ROWS_ATTRIBUTE_NAME = "rows";
- const std::string COLS_ATTRIBUTE_NAME = "cols";
- const std::string DIMENSIONS_ATTRIBUTE_NAME = "dimensions";
- const std::string SHAPE_ATTRIBUTE_NAME = "shape";
- const std::string OPTIONAL_ATTRIBUTE_NAME = "optional";
- const std::string RAW_PTR_ATTRIBUTE_NAME = "raw_ptr";
- const std::string SHARED_PTR_ATTRIBUTE_NAME = "shared_ptr";
- const std::string UNIQUE_PTR_ATTRIBUTE_NAME = "unique_ptr";
- const std::string PACKAGE_ATTRIBUTE_NAME = "package";
- const std::string DOC_BRIEF_ATTRIBUTE_NAME = "doc-brief";
- const std::string DOC_AUTHOR_ATTRIBUTE_NAME = "doc-author";
- const std::string DOC_PARAM_ATTRIBUTE_NAME = "doc-param";
- const std::string PATH_ATTRIBUTE_NAME = "path";
+ const constexpr auto ARON_ATTRIBUTE_NAME = "aron";
+ const constexpr auto CODE_ATTRIBUTE_NAME = "code";
+ const constexpr auto METHOD_ATTRIBUTE_NAME = "method";
+ const constexpr auto RETURN_ATTRIBUTE_NAME = "return";
+ const constexpr auto ARGUMENT_TYPE_ATTRIBUTE_NAME = "argumenttype";
+ const constexpr auto INCLUDE_ATTRIBUTE_NAME = "include";
+ const constexpr auto READER_ATTRIBUTE_NAME = "reader";
+ const constexpr auto WRITER_ATTRIBUTE_NAME = "writer";
+ const constexpr auto EXTENDS_ATTRIBUTE_NAME = "extends";
+ const constexpr auto NAME_ATTRIBUTE_NAME = "name";
+ const constexpr auto VALUE_ATTRIBUTE_NAME = "value";
+ const constexpr auto TEMPLATE_ATTRIBUTE_NAME = "template";
+ const constexpr auto KEY_ATTRIBUTE_NAME = "key";
+ const constexpr auto TYPE_ATTRIBUTE_NAME = "type";
+ const constexpr auto WIDTH_ATTRIBUTE_NAME = "width";
+ const constexpr auto HEIGHT_ATTRIBUTE_NAME = "height";
+ const constexpr auto ROWS_ATTRIBUTE_NAME = "rows";
+ const constexpr auto COLS_ATTRIBUTE_NAME = "cols";
+ const constexpr auto DIMENSIONS_ATTRIBUTE_NAME = "dimensions";
+ const constexpr auto SHAPE_ATTRIBUTE_NAME = "shape";
+ const constexpr auto OPTIONAL_ATTRIBUTE_NAME = "optional";
+ const constexpr auto RAW_PTR_ATTRIBUTE_NAME = "raw_ptr";
+ const constexpr auto SHARED_PTR_ATTRIBUTE_NAME = "shared_ptr";
+ const constexpr auto UNIQUE_PTR_ATTRIBUTE_NAME = "unique_ptr";
+ const constexpr auto PACKAGE_ATTRIBUTE_NAME = "package";
+ const constexpr auto DOC_BRIEF_ATTRIBUTE_NAME = "doc-brief";
+ const constexpr auto DOC_AUTHOR_ATTRIBUTE_NAME = "doc-author";
+ const constexpr auto DOC_PARAM_ATTRIBUTE_NAME = "doc-param";
+ const constexpr auto PATH_ATTRIBUTE_NAME = "path";
+ const constexpr auto DEFAULT_ATTRIBUTE_NAME = "default";
+
+ // Slugs
+ const constexpr auto IDENTITY_SLUG = "identity";
+ const constexpr auto ZEROS_SLUG = "zeros";
+ const constexpr auto ONES_SLUG = "ones";
+ const constexpr auto RGB_SLUG = "rgb24";
+ const constexpr auto DEPTH_SLUG = "depth32";
+ const constexpr auto POINT_XYZ_SLUG = "pointxyz";
+ const constexpr auto POINT_XYZI_SLUG = "pointxyzi";
+ const constexpr auto POINT_XYZL_SLUG = "pointxyzl";
+ const constexpr auto POINT_XYZRGB_SLUG = "pointxyzrgb";
+ const constexpr auto POINT_XYZRGBL_SLUG = "pointxyzrgbl";
+ const constexpr auto POINT_XYZRGBA_SLUG = "pointxyzrgba";
+ const constexpr auto POINT_XYZHSV_SLUG = "pointxyzhsv";
+
+ const constexpr auto CHAR_SLUG = "int8";
+ const constexpr auto UCHAR_SLUG = "uint8";
+ const constexpr auto SHORT_SLUG = "int16";
+ const constexpr auto USHORT_SLUG = "uint16";
+ const constexpr auto INT_SLUG = "int32";
+ const constexpr auto UINT_SLUG = "uint32";
+ const constexpr auto LONG_SLUG = "int64";
+ const constexpr auto ULONG_SLUG = "uint64";
+ const constexpr auto FLOAT_SLUG = "float32";
+ const constexpr auto DOUBLE_SLUG = "float64";
// Second level tags. Only important if in specific top level tag
- const std::string OBJECT_CHILD_TAG = "objectchild";
- const std::string ENUM_VALUE_TAG = "enumvalue";
+ const constexpr auto OBJECT_CHILD_TAG = "objectchild";
+ const constexpr auto ENUM_VALUE_TAG = "enumvalue";
// Top Level type tags
- const std::string LIST_TAG = "list";
- const std::string DICT_TAG = "dict";
- const std::string OBJECT_TAG = "object";
- const std::string PAIR_TAG = "pair";
- const std::string TUPLE_TAG = "tuple";
- const std::string INT_ENUM_TAG = "intenum";
- const std::string NDARRAY_TAG = "ndarray";
- const std::string MATRIX_TAG = "matrix";
- const std::string QUATERNION_TAG = "quaternion";
- const std::string IMAGE_TAG = "image";
- const std::string POINT_CLOUD_TAG = "pointcloud";
- const std::string INT_TAG = "int32";
- const std::string LONG_TAG = "int64";
- const std::string FLOAT_TAG = "float32";
- const std::string DOUBLE_TAG = "float64";
- const std::string STRING_TAG = "string";
- const std::string BOOL_TAG = "bool";
- const std::string ANY_OBJECT_TAG = "anyobject";
+ const constexpr auto LIST_TAG = "list";
+ const constexpr auto DICT_TAG = "dict";
+ const constexpr auto OBJECT_TAG = "object";
+ const constexpr auto PAIR_TAG = "pair";
+ const constexpr auto TUPLE_TAG = "tuple";
+ const constexpr auto INT_ENUM_TAG = "intenum";
+ const constexpr auto NDARRAY_TAG = "ndarray";
+ const constexpr auto MATRIX_TAG = "matrix";
+ const constexpr auto QUATERNION_TAG = "quaternion";
+ const constexpr auto IMAGE_TAG = "image";
+ const constexpr auto POINT_CLOUD_TAG = "pointcloud";
+ const constexpr auto CHAR_TAG = "int8";
+ const constexpr auto UCHAR_TAG = "uint8";
+ const constexpr auto SHORT_TAG = "int16";
+ const constexpr auto USHORT_TAG = "uint16";
+ const constexpr auto INT_TAG = "int32";
+ const constexpr auto UINT_TAG = "uint32";
+ const constexpr auto LONG_TAG = "int64";
+ const constexpr auto ULONG_TAG = "uint64";
+ const constexpr auto FLOAT_TAG = "float32";
+ const constexpr auto DOUBLE_TAG = "float64";
+ const constexpr auto STRING_TAG = "string";
+ const constexpr auto BOOL_TAG = "bool";
+ const constexpr auto ANY_OBJECT_TAG = "anyobject";
// others
const std::vector<std::string> WHATEVER_VALUES = {"?"};
@@ -118,70 +150,83 @@ namespace armarx::aron::typereader::xml
// Replacements ({tagName, {replacementsTag, additionalAronDTOXMLIncludePackagePath}, deprecationwarning})
const std::map<std::string, Replacement> REPLACEMENTS = {
- {"int",
- {"<int32 />", {}, "The <int />-tag is deprecated. Please use <int32 /> instead."}},
+ {"int", {"int32", {}, "The <int />-tag is deprecated. Please use <int32 /> instead."}},
{"long",
- {"<int64 />", {}, "The <long />-tag is deprecated. Please use <int64 /> instead."}},
+ {"int64", {}, "The <long />-tag is deprecated. Please use <int64 /> instead."}},
{"float",
- {"<float32 />",
- {},
- "The <float />-tag is deprecated. Please use <float32 /> instead."}},
+ {"float32", {}, "The <float />-tag is deprecated. Please use <float32 /> instead."}},
{"double",
- {"<float64 />",
- {},
- "The <double />-tag is deprecated. Please use <float64 /> instead."}},
+ {"float64", {}, "The <double />-tag is deprecated. Please use <float64 /> instead."}},
// should that be deprecated?
- {"position", {"<matrix rows='3' cols='1' type='float32' />", {}, ""}},
- {"pose", {"<matrix rows='4' cols='4' type='float32' />", {}, ""}},
- {"orientation", {"<quaternion type='float32' />", {}, ""}},
+ {"position", {"matrix rows='3' cols='1' type='float32'", {}, ""}},
+ {"pose", {"matrix rows='4' cols='4' type='float32'", {}, ""}},
+ {"orientation", {"quaternion type='float32'", {}, ""}},
// values similar to eigen
- {"quaternionf", {"<quaternion type='float32' />", {}, ""}},
- {"quaterniond", {"<quaternion type='float64' />", {}, ""}},
- {"vector2f", {"<matrix rows='2' cols='1' type='float32' />", {}, ""}},
- {"vector2d", {"<matrix rows='2' cols='1' type='float64' />", {}, ""}},
- {"vector3f", {"<matrix rows='3' cols='1' type='float32' />", {}, ""}},
- {"vector3d", {"<matrix rows='3' cols='1' type='float64' />", {}, ""}},
- {"vector4f", {"<matrix rows='4' cols='1' type='float32' />", {}, ""}},
- {"vector4d", {"<matrix rows='4' cols='1' type='float64' />", {}, ""}},
- {"matrix2f", {"<matrix rows='2' cols='2' type='float32' />", {}, ""}},
- {"matrix2d", {"<matrix rows='2' cols='2' type='float64' />", {}, ""}},
- {"matrix3f", {"<matrix rows='3' cols='3' type='float32' />", {}, ""}},
- {"matrix3d", {"<matrix rows='3' cols='3' type='float64' />", {}, ""}},
- {"matrix4f", {"<matrix rows='4' cols='4' type='float32' />", {}, ""}},
- {"matrix4d", {"<matrix rows='4' cols='4' type='float64' />", {}, ""}},
+ {"quaternionf", {"quaternion type='float32'", {}, ""}},
+ {"quaterniond", {"quaternion type='float64'", {}, ""}},
+ {"vector2f", {"matrix rows='2' cols='1' type='float32'", {}, ""}},
+ {"vector2d", {"matrix rows='2' cols='1' type='float64'", {}, ""}},
+ {"vector3f", {"matrix rows='3' cols='1' type='float32'", {}, ""}},
+ {"vector3d", {"matrix rows='3' cols='1' type='float64'", {}, ""}},
+ {"vector4f", {"matrix rows='4' cols='1' type='float32'", {}, ""}},
+ {"vector4d", {"matrix rows='4' cols='1' type='float64'", {}, ""}},
+ {"matrix2f", {"matrix rows='2' cols='2' type='float32'", {}, ""}},
+ {"matrix2d", {"matrix rows='2' cols='2' type='float64'", {}, ""}},
+ {"matrix3f", {"matrix rows='3' cols='3' type='float32'", {}, ""}},
+ {"matrix3d", {"matrix rows='3' cols='3' type='float64'", {}, ""}},
+ {"matrix4f", {"matrix rows='4' cols='4' type='float32'", {}, ""}},
+ {"matrix4d", {"matrix rows='4' cols='4' type='float64'", {}, ""}},
// You can also add replacements for arondtos here!
- // structure: {xml-identifier, {replacement, auto-include}, deprecationwarning}
+ // structure: {xml-identifier, {replacement, auto-ARON-include (NO CPP SPECIFIC STUFF HERE!)}, deprecationwarning}
{"datetime",
- {"<armarx::arondto::DateTime />",
+ {"armarx::arondto::DateTime",
{"RobotAPI", "libraries/aron/common/aron/time.xml"},
""}},
{"time",
- {"<armarx::arondto::DateTime />",
+ {"armarx::arondto::DateTime",
{"RobotAPI", "libraries/aron/common/aron/time.xml"},
""}},
{"duration",
- {"<armarx::arondto::Duration />",
+ {"armarx::arondto::Duration",
{"RobotAPI", "libraries/aron/common/aron/time.xml"},
""}},
+
+ {"memoryid",
+ {"armarx::armem::arondto::MemoryID",
+ {"RobotAPI", "libraries/armem/aron/MemoryID.xml"},
+ ""}},
+
+ // TODO: Replace with geometry package
{"framedposition",
- {"<armarx::arondto::FramedPosition />",
+ {"armarx::arondto::FramedPosition",
{"RobotAPI", "libraries/aron/common/aron/framed.xml"},
""}},
{"framedorientation",
- {"<armarx::arondto::FramedOrientation />",
+ {"armarx::arondto::FramedOrientation",
{"RobotAPI", "libraries/aron/common/aron/framed.xml"},
""}},
{"framedpose",
- {"<armarx::arondto::FramedPose />",
+ {"armarx::arondto::FramedPose",
{"RobotAPI", "libraries/aron/common/aron/framed.xml"},
""}}};
} // namespace constantes
namespace util
{
+ bool CheckStrIsInt(const std::string&);
+ void EnforceStrIsInt(const std::string&);
+ bool CheckStrIsFloat(const std::string&);
+ void EnforceStrIsFloat(const std::string&);
+ bool CheckStrIsNumber(const std::string&);
+ void EnforceStrIsNumber(const std::string&);
+ bool CheckStrIsNumberSeq(const std::string&);
+ void EnforceStrIsNumberSeq(const std::string&);
+ bool CheckStrIsBool(const std::string&);
+ void EnforceStrIsBool(const std::string&);
+
std::optional<RapidXmlReaderNode> GetFirstNodeWithTag(const RapidXmlReaderNode& node,
const std::string& name);
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp
index 8409c87ba128e86bab38cf78f0994d34f35844fe..4f77481df505023025c4315481b98e78b47fb49e 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp
@@ -31,20 +31,35 @@
// ArmarX
#include <ArmarXCore/core/time/DateTime.h>
+
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
#include "Data.h"
namespace armarx::aron::typereader::xml
{
- void printDeprecationWarning(const std::string& warning)
+ void
+ printDeprecationWarning(const std::string& warning)
{
- std::cout << "\033[1;33mAron deprecation warning: " << warning << "\033[0m" << std::endl;
+ auto v = armarx::aron::Version();
+ auto v_str =
+ std::to_string(v.MAJOR) + "." + std::to_string(v.MINOR) + "." + std::to_string(v.PATCH);
+ std::cout
+ << "\033[1;33m[**Aron v" + v_str + " deprecation warning**]: " << warning
+ << "\nPlease note that a future version of aron might not support this anymore.\033[0m"
+ << std::endl;
}
- type::VariantPtr ReaderFactory::create(const RapidXmlReaderNode& node, const Path& path)
+ void
+ printWarning(const std::string& warning)
{
- static const std::map<std::string, type::Descriptor> String2Descriptor =
- {
+ std::cout << "\033[1;33m[**Aron warning**]: " << warning << "\033[0m" << std::endl;
+ }
+
+ type::VariantPtr
+ ReaderFactory::create(const RapidXmlReaderNode& node, const Path& path)
+ {
+ static const std::map<std::string, type::Descriptor> String2Descriptor = {
{constantes::LIST_TAG, type::Descriptor::LIST},
{constantes::OBJECT_TAG, type::Descriptor::OBJECT},
{constantes::TUPLE_TAG, type::Descriptor::TUPLE},
@@ -62,13 +77,13 @@ namespace armarx::aron::typereader::xml
{constantes::DOUBLE_TAG, type::Descriptor::DOUBLE},
{constantes::STRING_TAG, type::Descriptor::STRING},
{constantes::BOOL_TAG, type::Descriptor::BOOL},
- {constantes::ANY_OBJECT_TAG, type::Descriptor::ANY_OBJECT}
- };
+ {constantes::ANY_OBJECT_TAG, type::Descriptor::ANY_OBJECT}};
RapidXmlReaderNode nodeToUse = node;
// check for replacement
- if (auto it = constantes::REPLACEMENTS.find(simox::alg::to_lower(nodeToUse.name())); it != constantes::REPLACEMENTS.end())
+ if (auto it = constantes::REPLACEMENTS.find(simox::alg::to_lower(nodeToUse.name()));
+ it != constantes::REPLACEMENTS.end())
{
auto replacement = it->second;
if (!replacement.deprecatedWarning.empty())
@@ -76,47 +91,75 @@ namespace armarx::aron::typereader::xml
printDeprecationWarning(replacement.deprecatedWarning);
}
+ std::string tag = replacement.replacementTag;
+ if (util::HasAttribute(nodeToUse, constantes::DEFAULT_ATTRIBUTE_NAME))
+ {
+ tag += " " + std::string(constantes::DEFAULT_ATTRIBUTE_NAME) + "=\"" +
+ util::GetAttribute(nodeToUse, constantes::DEFAULT_ATTRIBUTE_NAME) + "\"";
+ }
+
// We make the system believe there is another string in the xml
- RapidXmlReaderPtr reader = RapidXmlReader::FromXmlString(replacement.replacementTag);
+ RapidXmlReaderPtr reader = RapidXmlReader::FromXmlString("<" + tag + " />");
nodeToUse = reader->getRoot();
}
auto it = String2Descriptor.find(simox::alg::to_lower(nodeToUse.name()));
auto descriptor = (it == String2Descriptor.end() ? type::Descriptor::UNKNOWN : it->second);
- switch(descriptor)
- {
- case type::Descriptor::LIST: return createList(nodeToUse, path);
- case type::Descriptor::DICT: return createDict(nodeToUse, path);
- case type::Descriptor::OBJECT: return createObject(nodeToUse, path);
- case type::Descriptor::TUPLE: return createTuple(nodeToUse, path);
- case type::Descriptor::PAIR: return createPair(nodeToUse, path);
- case type::Descriptor::NDARRAY: return createNDArray(nodeToUse, path);
- case type::Descriptor::MATRIX: return createMatrix(nodeToUse, path);
- case type::Descriptor::QUATERNION: return createQuaternion(nodeToUse, path);
- case type::Descriptor::IMAGE: return createImage(nodeToUse, path);
- case type::Descriptor::POINTCLOUD: return createPointCloud(nodeToUse, path);
- case type::Descriptor::INT_ENUM: return createIntEnum(nodeToUse, path);
- case type::Descriptor::INT: return createInt(nodeToUse, path);
- case type::Descriptor::LONG: return createLong(nodeToUse, path);
- case type::Descriptor::FLOAT: return createFloat(nodeToUse, path);
- case type::Descriptor::DOUBLE: return createDouble(nodeToUse, path);
- case type::Descriptor::STRING: return createString(nodeToUse, path);
- case type::Descriptor::BOOL: return createBool(nodeToUse, path);
- case type::Descriptor::ANY_OBJECT: return createAnyObject(nodeToUse, path);
- case type::Descriptor::UNKNOWN: return findExistingObject(nodeToUse.name(), path);
+ switch (descriptor)
+ {
+ case type::Descriptor::LIST:
+ return createList(nodeToUse, path);
+ case type::Descriptor::DICT:
+ return createDict(nodeToUse, path);
+ case type::Descriptor::OBJECT:
+ return createObject(nodeToUse, path);
+ case type::Descriptor::TUPLE:
+ return createTuple(nodeToUse, path);
+ case type::Descriptor::PAIR:
+ return createPair(nodeToUse, path);
+ case type::Descriptor::NDARRAY:
+ return createNDArray(nodeToUse, path);
+ case type::Descriptor::MATRIX:
+ return createMatrix(nodeToUse, path);
+ case type::Descriptor::QUATERNION:
+ return createQuaternion(nodeToUse, path);
+ case type::Descriptor::IMAGE:
+ return createImage(nodeToUse, path);
+ case type::Descriptor::POINTCLOUD:
+ return createPointCloud(nodeToUse, path);
+ case type::Descriptor::INT_ENUM:
+ return createIntEnum(nodeToUse, path);
+ case type::Descriptor::INT:
+ return createInt(nodeToUse, path);
+ case type::Descriptor::LONG:
+ return createLong(nodeToUse, path);
+ case type::Descriptor::FLOAT:
+ return createFloat(nodeToUse, path);
+ case type::Descriptor::DOUBLE:
+ return createDouble(nodeToUse, path);
+ case type::Descriptor::STRING:
+ return createString(nodeToUse, path);
+ case type::Descriptor::BOOL:
+ return createBool(nodeToUse, path);
+ case type::Descriptor::ANY_OBJECT:
+ return createAnyObject(nodeToUse, path);
+ case type::Descriptor::UNKNOWN:
+ return findExistingObject(nodeToUse.name(), path);
}
throw aron::error::AronEOFException(__PRETTY_FUNCTION__);
}
- type::VariantPtr ReaderFactory::findExistingObject(const std::string& name, const Path& path) const
+ type::VariantPtr
+ ReaderFactory::findExistingObject(const std::string& name, const Path& path) const
{
const auto public_intenum_it = allGeneratedPublicIntEnums.find(name);
if (public_intenum_it != allGeneratedPublicIntEnums.end())
{
// copy the navigator
- auto v = type::Variant::FromAronDTO(*public_intenum_it->second.correspondingType->toAronDTO(), path);
+ auto v = type::Variant::FromAronDTO(
+ *public_intenum_it->second.correspondingType->toAronDTO(), path);
return v;
}
@@ -124,31 +167,44 @@ namespace armarx::aron::typereader::xml
if (public_obj_it != allGeneratedPublicObjects.end())
{
// copy the navigator and set instantiation template args
- auto v = type::Variant::FromAronDTO(*public_obj_it->second.correspondingType->toAronDTO(), path);
+ auto v = type::Variant::FromAronDTO(
+ *public_obj_it->second.correspondingType->toAronDTO(), path);
return v;
}
- const auto public_known_it = std::find(allPreviouslyKnownPublicTypes.begin(), allPreviouslyKnownPublicTypes.end(), name);
- if(public_known_it != allPreviouslyKnownPublicTypes.end())
+ const auto public_known_it = std::find(
+ allPreviouslyKnownPublicTypes.begin(), allPreviouslyKnownPublicTypes.end(), name);
+ if (public_known_it != allPreviouslyKnownPublicTypes.end())
{
// create an empty navigator
- auto v = std::make_shared<aron::type::Object>(*public_known_it, std::vector<std::string>(), std::vector<std::string>(), std::map<std::string, type::VariantPtr>(), path);
+ auto v = std::make_shared<aron::type::Object>(path);
+ v->setObjectName(*public_known_it);
+ v->setTemplates({});
+ v->setTemplateInstantiations({});
+ v->setMemberTypes({});
return v;
}
// only works during generation process (not for top-level). Used for templates.
- const auto private_known_it = std::find(allPreviouslyKnownPrivateTypes.begin(), allPreviouslyKnownPrivateTypes.end(), name);
- if(private_known_it != allPreviouslyKnownPrivateTypes.end())
+ const auto private_known_it = std::find(
+ allPreviouslyKnownPrivateTypes.begin(), allPreviouslyKnownPrivateTypes.end(), name);
+ if (private_known_it != allPreviouslyKnownPrivateTypes.end())
{
// create an empty navigator
- auto v = std::make_shared<aron::type::Object>(*private_known_it, std::vector<std::string>(), std::vector<std::string>(), std::map<std::string, type::VariantPtr>(), path);
+ auto v = std::make_shared<aron::type::Object>(path);
+ v->setObjectName(*private_known_it);
+ v->setTemplates({});
+ v->setTemplateInstantiations({});
+ v->setMemberTypes({});
return v;
}
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Cannot find a valid object.", name);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Cannot find a valid object.", name);
}
- type::Maybe ReaderFactory::getMaybe(const RapidXmlReaderNode& n) const
+ type::Maybe
+ ReaderFactory::getMaybe(const RapidXmlReaderNode& n) const
{
if (util::AttributeIsTrue(n, constantes::OPTIONAL_ATTRIBUTE_NAME))
{
@@ -169,35 +225,51 @@ namespace armarx::aron::typereader::xml
return type::Maybe::NONE;
}
- void ReaderFactory::checkObjectMemberName(const std::string& s) const
+ void
+ ReaderFactory::checkObjectMemberName(const std::string& s) const
{
if (simox::alg::starts_with(s, "_") || simox::alg::ends_with(s, "_"))
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "You used an invalid membername - '_' as starting or ending char is not allowed.", s);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__,
+ "You used an invalid membername - '_' as starting or ending char is not allowed.",
+ s);
}
if (simox::alg::starts_with(s, "aron"))
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "You used an invalid membername - The prefix 'aron' is used for codegeneration.", s);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__,
+ "You used an invalid membername - The prefix 'aron' is used for codegeneration.",
+ s);
}
}
- type::VariantPtr ReaderFactory::createObject(const RapidXmlReaderNode& node, const Path& path)
+ type::VariantPtr
+ ReaderFactory::createObject(const RapidXmlReaderNode& node, const Path& path)
{
if (path.hasElement())
+ // ensured we are toplevel!
{
- throw error::AronException(__PRETTY_FUNCTION__, "Having an inner class is not supported anymore since Aron Version 'beta 0.2.3'. Please move the inner class definition to the <" + constantes::GENERATE_TYPES_TAG + ">-tag.");
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Having an inner class is not supported anymore since Aron Version 'beta 0.2.3'. "
+ "Please move the inner class definition to the <" +
+ std::string(constantes::GENERATE_TYPES_TAG) + ">-tag.");
}
- // ensured we are toplevel!
const std::string name = util::GetAttribute(node, constantes::NAME_ATTRIBUTE_NAME);
- const std::string extends = util::GetAttributeWithDefault(node, constantes::EXTENDS_ATTRIBUTE_NAME, "");
- auto templates = simox::alg::split(util::GetAttributeWithDefault(node, constantes::TEMPLATE_ATTRIBUTE_NAME, ""), ",");
+ const std::string extends =
+ util::GetAttributeWithDefault(node, constantes::EXTENDS_ATTRIBUTE_NAME, "");
+ auto templates = simox::alg::split(
+ util::GetAttributeWithDefault(node, constantes::TEMPLATE_ATTRIBUTE_NAME, ""), ",");
auto newObjectInfo = typereader::GenerateObjectInfo();
newObjectInfo.typeName = name;
- newObjectInfo.doc_brief = util::GetAttributeWithDefault(node, constantes::DOC_BRIEF_ATTRIBUTE_NAME, "");
- newObjectInfo.doc_author = util::GetAttributeWithDefault(node, constantes::DOC_AUTHOR_ATTRIBUTE_NAME, "");
+ newObjectInfo.doc_brief =
+ util::GetAttributeWithDefault(node, constantes::DOC_BRIEF_ATTRIBUTE_NAME, "");
+ newObjectInfo.doc_author =
+ util::GetAttributeWithDefault(node, constantes::DOC_AUTHOR_ATTRIBUTE_NAME, "");
// reset private known types
allPreviouslyKnownPrivateTypes = templates;
@@ -219,7 +291,8 @@ namespace armarx::aron::typereader::xml
if (util::HasAttribute(objectChild, constantes::DOC_BRIEF_ATTRIBUTE_NAME))
{
- newObjectInfo.doc_members.insert({key, util::GetAttribute(objectChild, constantes::DOC_BRIEF_ATTRIBUTE_NAME)});
+ newObjectInfo.doc_members.insert(
+ {key, util::GetAttribute(objectChild, constantes::DOC_BRIEF_ATTRIBUTE_NAME)});
}
std::vector<RapidXmlReaderNode> children = objectChild.nodes();
@@ -230,7 +303,10 @@ namespace armarx::aron::typereader::xml
if (childNavigator->getDescriptor() == aron::type::Descriptor::OBJECT)
{
// check if template args are present
- std::vector<std::string> templates = simox::alg::split(util::GetAttributeWithDefault(children[0], constantes::TEMPLATE_ATTRIBUTE_NAME, ""), ",");
+ std::vector<std::string> templates =
+ simox::alg::split(util::GetAttributeWithDefault(
+ children[0], constantes::TEMPLATE_ATTRIBUTE_NAME, ""),
+ ",");
auto obj = aron::type::Object::DynamicCastAndCheck(childNavigator);
for (const auto& t : templates)
{
@@ -243,7 +319,11 @@ namespace armarx::aron::typereader::xml
}
// set the new object
- auto aronObjectType = std::make_shared<type::Object>(name, templates, std::vector<std::string>(), members, objPath);
+ auto aronObjectType = std::make_shared<type::Object>(objPath);
+ aronObjectType->setObjectName(name);
+ aronObjectType->setTemplates(templates);
+ aronObjectType->setTemplateInstantiations({});
+ aronObjectType->setMemberTypes(members);
if (extends != "")
{
@@ -258,7 +338,8 @@ namespace armarx::aron::typereader::xml
return aronObjectType;
}
- type::VariantPtr ReaderFactory::createList(const RapidXmlReaderNode &node, const Path &path)
+ type::VariantPtr
+ ReaderFactory::createList(const RapidXmlReaderNode& node, const Path& path)
{
util::EnforceChildSize(node, 1);
@@ -267,11 +348,13 @@ namespace armarx::aron::typereader::xml
type::VariantPtr type = create(typeNode, path.withAcceptedType());
type->setMaybe(getMaybe(typeNode));
- auto o = std::make_shared<type::List>(type, path);
+ auto o = std::make_shared<type::List>(path);
+ o->setAcceptedType(type);
return o;
}
- type::VariantPtr ReaderFactory::createDict(const RapidXmlReaderNode &node, const Path &path)
+ type::VariantPtr
+ ReaderFactory::createDict(const RapidXmlReaderNode& node, const Path& path)
{
util::EnforceChildSize(node, 1);
@@ -280,11 +363,13 @@ namespace armarx::aron::typereader::xml
type::VariantPtr type = create(typeNode, path.withAcceptedType());
type->setMaybe(getMaybe(typeNode));
- auto o = std::make_shared<type::Dict>(type, path);
+ auto o = std::make_shared<type::Dict>(path);
+ o->setAcceptedType(type);
return o;
}
- type::VariantPtr ReaderFactory::createTuple(const RapidXmlReaderNode& node, const Path& path)
+ type::VariantPtr
+ ReaderFactory::createTuple(const RapidXmlReaderNode& node, const Path& path)
{
util::EnforceChildSizeGreater(node, 1);
@@ -304,11 +389,13 @@ namespace armarx::aron::typereader::xml
elementTypes.push_back(type);
}
- auto o = std::make_shared<type::Tuple>(elementTypes, path);
+ auto o = std::make_shared<type::Tuple>(path);
+ o->setAcceptedTypes(elementTypes);
return o;
}
- type::VariantPtr ReaderFactory::createPair(const RapidXmlReaderNode& node, const Path& path)
+ type::VariantPtr
+ ReaderFactory::createPair(const RapidXmlReaderNode& node, const Path& path)
{
util::EnforceChildSize(node, 2);
@@ -322,158 +409,268 @@ namespace armarx::aron::typereader::xml
type::VariantPtr type2 = create(type2Node, path.withAcceptedTypeIndex(1));
type2->setMaybe(getMaybe(type2Node));
- auto o = std::make_shared<type::Pair>(type1, type2, path);
+ auto o = std::make_shared<type::Pair>(path);
+ o->setFirstAcceptedType(type1);
+ o->setSecondAcceptedType(type2);
return o;
}
- type::VariantPtr ReaderFactory::createNDArray(const RapidXmlReaderNode& node, const Path& path) const
+ type::VariantPtr
+ ReaderFactory::createNDArray(const RapidXmlReaderNode& node, const Path& path) const
{
- static const std::map<std::string, type::ndarray::ElementType> String2NDArrayType =
- {
- {"int8", type::ndarray::ElementType::INT8},
- {"int16", type::ndarray::ElementType::INT16},
- {"int32", type::ndarray::ElementType::INT32},
- {"uint8", type::ndarray::ElementType::UINT8},
- {"uint16", type::ndarray::ElementType::UINT16},
- {"uint32", type::ndarray::ElementType::UINT32},
- {"float32", type::ndarray::ElementType::FLOAT32},
- {"float64", type::ndarray::ElementType::FLOAT64}
- };
+ static const std::map<std::string, type::ndarray::ElementType> String2NDArrayType = {
+ {constantes::CHAR_SLUG, type::ndarray::ElementType::INT8},
+ {constantes::SHORT_SLUG, type::ndarray::ElementType::INT16},
+ {constantes::INT_SLUG, type::ndarray::ElementType::INT32},
+ {constantes::LONG_SLUG, type::ndarray::ElementType::UINT8},
+ {constantes::USHORT_SLUG, type::ndarray::ElementType::UINT16},
+ {constantes::UINT_SLUG, type::ndarray::ElementType::UINT32},
+ {constantes::FLOAT_SLUG, type::ndarray::ElementType::FLOAT32},
+ {constantes::DOUBLE_SLUG, type::ndarray::ElementType::FLOAT64}};
+
+ static const std::map<std::string, std::string> StringSlug2NDArrayDefaultValue = {
+ {constantes::ONES_SLUG, type::ndarray::default_value::ONES},
+ {constantes::ZEROS_SLUG, type::ndarray::default_value::ZEROS}};
+
+ auto o = std::make_shared<type::NDArray>(path);
+ util::EnforceChildSize(node, 0);
+ util::EnforceAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
+
+ // TODO
return nullptr;
}
- type::VariantPtr ReaderFactory::createMatrix(const RapidXmlReaderNode &node, const Path &path) const
+ type::VariantPtr
+ ReaderFactory::createMatrix(const RapidXmlReaderNode& node, const Path& path) const
{
- static const std::map<std::string, type::matrix::ElementType> String2MatrixType =
- {
- {"int16", type::matrix::ElementType::INT16},
- {"int32", type::matrix::ElementType::INT32},
- {"int64", type::matrix::ElementType::INT64},
- {"float32", type::matrix::ElementType::FLOAT32},
- {"float64", type::matrix::ElementType::FLOAT64}
- };
+ static const std::map<std::string, type::matrix::ElementType> StringSlug2MatrixType = {
+ {constantes::SHORT_SLUG, type::matrix::ElementType::INT16},
+ {constantes::INT_SLUG, type::matrix::ElementType::INT32},
+ {constantes::LONG_SLUG, type::matrix::ElementType::INT64},
+ {constantes::FLOAT_SLUG, type::matrix::ElementType::FLOAT32},
+ {constantes::DOUBLE_SLUG, type::matrix::ElementType::FLOAT64}};
+
+ static const std::map<std::string, std::string> StringSlug2MatrixDefaultValue = {
+ {constantes::IDENTITY_SLUG, type::matrix::default_value::IDENTITY},
+ {constantes::ONES_SLUG, type::matrix::default_value::ONES},
+ {constantes::ZEROS_SLUG, type::matrix::default_value::ZEROS}};
auto o = std::make_shared<type::Matrix>(path);
util::EnforceChildSize(node, 0);
- util::EnforceAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
auto rows_str = util::GetAttributeWithDefault(node, constantes::ROWS_ATTRIBUTE_NAME, "4");
auto cols_str = util::GetAttributeWithDefault(node, constantes::COLS_ATTRIBUTE_NAME, "4");
- if (std::find(constantes::WHATEVER_VALUES.begin(), constantes::WHATEVER_VALUES.end(), simox::alg::to_lower(rows_str)) != constantes::WHATEVER_VALUES.end())
+ // check for unknown size
+ if (std::find(constantes::WHATEVER_VALUES.begin(),
+ constantes::WHATEVER_VALUES.end(),
+ simox::alg::to_lower(rows_str)) != constantes::WHATEVER_VALUES.end())
{
rows_str = "-1";
}
- if (std::find(constantes::WHATEVER_VALUES.begin(), constantes::WHATEVER_VALUES.end(), simox::alg::to_lower(cols_str)) != constantes::WHATEVER_VALUES.end())
+ if (std::find(constantes::WHATEVER_VALUES.begin(),
+ constantes::WHATEVER_VALUES.end(),
+ simox::alg::to_lower(cols_str)) != constantes::WHATEVER_VALUES.end())
{
cols_str = "-1";
}
const int rows = std::stoi(rows_str);
const int cols = std::stoi(cols_str);
- std::string type = util::GetAttributeWithDefault(node, constantes::TYPE_ATTRIBUTE_NAME, "???");
+ // set rows and cols
o->setRows(rows);
o->setCols(cols);
- try
+
+ // get type
+ std::string type = util::GetAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
+
+ ARMARX_CHECK_AND_THROW(
+ StringSlug2MatrixType.count(simox::alg::to_lower(type)),
+ error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not resolve type", type));
+
+ // set elem type
+ o->setElementType(StringSlug2MatrixType.at(simox::alg::to_lower(type)));
+
+ // set default
+ const std::string def = simox::alg::to_lower(util::GetAttributeWithDefault(
+ node, constantes::DEFAULT_ATTRIBUTE_NAME, type::matrix::default_value::DEFAULT));
+
+ if (const auto it = StringSlug2MatrixDefaultValue.find(def);
+ it != StringSlug2MatrixDefaultValue.end())
+ {
+ o->setDefaultValue(it->second);
+ }
+ else if (def == type::matrix::default_value::DEFAULT)
{
- o->setElementType(String2MatrixType.at(simox::alg::to_lower(type)));
+ o->setDefaultValue(def);
}
- catch (std::out_of_range& e)
+ else
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The type tag is not valid", type);
+ util::EnforceStrIsNumber(def);
+ o->setDefaultValue(def);
}
+
return o;
}
- type::VariantPtr ReaderFactory::createQuaternion(const RapidXmlReaderNode &node, const Path &path) const
+ type::VariantPtr
+ ReaderFactory::createQuaternion(const RapidXmlReaderNode& node, const Path& path) const
{
- static const std::map<std::string, type::quaternion::ElementType> String2QuaternionType =
- {
- {"float32", type::quaternion::ElementType::FLOAT32},
- {"float64", type::quaternion::ElementType::FLOAT64}
- };
+ static const std::map<std::string, type::quaternion::ElementType>
+ StringSlug2QuaternionType = {
+ {constantes::FLOAT_SLUG, type::quaternion::ElementType::FLOAT32},
+ {constantes::DOUBLE_SLUG, type::quaternion::ElementType::FLOAT64}};
+
+ static const std::map<std::string, std::string> StringSlug2QuaternionDefaultValue = {
+ {constantes::ONES_SLUG, type::quaternion::default_value::ONES},
+ {constantes::ZEROS_SLUG, type::quaternion::default_value::ZEROS}};
auto o = std::make_shared<type::Quaternion>(path);
util::EnforceChildSize(node, 0);
util::EnforceAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
- std::string type = util::GetAttributeWithDefault(node, constantes::TYPE_ATTRIBUTE_NAME, "???");
+ // set elem type
+ std::string type = util::GetAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
+ ARMARX_CHECK_AND_THROW(
+ StringSlug2QuaternionType.count(simox::alg::to_lower(type)),
+ error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not resolve type", type));
- try
+ o->setElementType(StringSlug2QuaternionType.at(simox::alg::to_lower(type)));
+
+ // set default
+ const std::string def = simox::alg::to_lower(util::GetAttributeWithDefault(
+ node, constantes::DEFAULT_ATTRIBUTE_NAME, type::quaternion::default_value::DEFAULT));
+
+ if (const auto it = StringSlug2QuaternionDefaultValue.find(def);
+ it != StringSlug2QuaternionDefaultValue.end())
+ {
+ o->setDefaultValue(it->second);
+ }
+ else if (def == type::quaternion::default_value::DEFAULT)
{
- o->setElementType(String2QuaternionType.at(simox::alg::to_lower(type)));
+ o->setDefaultValue(def);
}
- catch (std::out_of_range& e)
+ else
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The type tag is not valid", type);
+ util::EnforceStrIsNumberSeq(def);
+ o->setDefaultValue(def);
}
+
return o;
}
-
- type::VariantPtr ReaderFactory::createImage(const RapidXmlReaderNode &node, const Path &path) const
+ type::VariantPtr
+ ReaderFactory::createImage(const RapidXmlReaderNode& node, const Path& path) const
{
- static const std::map<std::string, type::image::PixelType> String2PixelType =
- {
- {"rgb24", type::image::PixelType::RGB24},
- {"depth32", type::image::PixelType::DEPTH32}
- };
+ static const std::map<std::string, type::image::PixelType> String2PixelType = {
+ {constantes::RGB_SLUG, type::image::PixelType::RGB24},
+ {constantes::DEPTH_SLUG, type::image::PixelType::DEPTH32}};
+
+ static const std::map<std::string, std::string> StringSlug2ImageDefaultValue = {
+ {constantes::IDENTITY_SLUG, type::image::default_value::IDENTITY},
+ {constantes::ONES_SLUG, type::image::default_value::ONES},
+ {constantes::ZEROS_SLUG, type::image::default_value::ZEROS}};
auto o = std::make_shared<type::Image>(path);
util::EnforceChildSize(node, 0);
util::EnforceAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
- std::string type = util::GetAttributeWithDefault(node, constantes::TYPE_ATTRIBUTE_NAME, "???");
+ // set elem type
+ std::string type = util::GetAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
+
+ ARMARX_CHECK_AND_THROW(
+ String2PixelType.count(simox::alg::to_lower(type)),
+ error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not resolve type", type));
- try
+ o->setPixelType(String2PixelType.at(simox::alg::to_lower(type)));
+
+ // set default
+ const std::string def = simox::alg::to_lower(util::GetAttributeWithDefault(
+ node, constantes::DEFAULT_ATTRIBUTE_NAME, type::image::default_value::DEFAULT));
+
+ if (const auto it = StringSlug2ImageDefaultValue.find(def);
+ it != StringSlug2ImageDefaultValue.end())
+ {
+ o->setDefaultValue(it->second);
+ }
+ else if (def == type::image::default_value::DEFAULT)
{
- o->setPixelType(String2PixelType.at(simox::alg::to_lower(type)));
+ o->setDefaultValue(def);
}
- catch (std::out_of_range& e)
+ else
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The type tag is not valid", type);
+ util::EnforceStrIsNumber(def);
+ o->setDefaultValue(def);
}
+
return o;
}
- type::VariantPtr ReaderFactory::createPointCloud(const RapidXmlReaderNode &node, const Path &path) const
+ type::VariantPtr
+ ReaderFactory::createPointCloud(const RapidXmlReaderNode& node, const Path& path) const
{
- static const std::map<std::string, type::pointcloud::VoxelType> String2VoxelType =
- {
- {"pointxyz", type::pointcloud::VoxelType::POINT_XYZ},
- {"pointxyzi", type::pointcloud::VoxelType::POINT_XYZI},
- {"pointxyzl", type::pointcloud::VoxelType::POINT_XYZL},
- {"pointxyzrgb", type::pointcloud::VoxelType::POINT_XYZRGB},
- {"pointxyzrgbl", type::pointcloud::VoxelType::POINT_XYZRGBL},
- {"pointxyzrgba", type::pointcloud::VoxelType::POINT_XYZRGBA},
- {"pointxyzhsv", type::pointcloud::VoxelType::POINT_XYZHSV}
- };
+ static const std::map<std::string, type::pointcloud::VoxelType> StringSlug2VoxelType = {
+ {constantes::POINT_XYZ_SLUG, type::pointcloud::VoxelType::POINT_XYZ},
+ {constantes::POINT_XYZI_SLUG, type::pointcloud::VoxelType::POINT_XYZI},
+ {constantes::POINT_XYZL_SLUG, type::pointcloud::VoxelType::POINT_XYZL},
+ {constantes::POINT_XYZRGB_SLUG, type::pointcloud::VoxelType::POINT_XYZRGB},
+ {constantes::POINT_XYZRGBL_SLUG, type::pointcloud::VoxelType::POINT_XYZRGBL},
+ {constantes::POINT_XYZRGBA_SLUG, type::pointcloud::VoxelType::POINT_XYZRGBA},
+ {constantes::POINT_XYZHSV_SLUG, type::pointcloud::VoxelType::POINT_XYZHSV}};
+
+ static const std::map<std::string, std::string> StringSlug2PointCloudDefaultValue = {
+ {constantes::IDENTITY_SLUG, type::pointcloud::default_value::IDENTITY},
+ {constantes::ONES_SLUG, type::pointcloud::default_value::ONES},
+ {constantes::ZEROS_SLUG, type::pointcloud::default_value::ZEROS}};
auto o = std::make_shared<type::PointCloud>(path);
util::EnforceChildSize(node, 0);
- util::EnforceAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
- std::string type = util::GetAttributeWithDefault(node, constantes::TYPE_ATTRIBUTE_NAME, "???");
- try
+ // set elem type
+ std::string type = util::GetAttribute(node, constantes::TYPE_ATTRIBUTE_NAME);
+
+ ARMARX_CHECK_AND_THROW(
+ StringSlug2VoxelType.count(simox::alg::to_lower(type)),
+ error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not resolve type", type));
+
+ o->setVoxelType(StringSlug2VoxelType.at(simox::alg::to_lower(type)));
+
+ // set default
+ const std::string def = simox::alg::to_lower(util::GetAttributeWithDefault(
+ node, constantes::DEFAULT_ATTRIBUTE_NAME, type::pointcloud::default_value::DEFAULT));
+
+ if (const auto it = StringSlug2PointCloudDefaultValue.find(def);
+ it != StringSlug2PointCloudDefaultValue.end())
{
- o->setVoxelType(String2VoxelType.at(simox::alg::to_lower(type)));
+ o->setDefaultValue(it->second);
}
- catch (std::out_of_range& e)
+ else if (def == type::pointcloud::default_value::DEFAULT)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The type tag is not valid", type);
+ o->setDefaultValue(def);
}
+ else
+ {
+ util::EnforceStrIsNumber(def);
+ o->setDefaultValue(def);
+ }
+
return o;
}
- type::VariantPtr ReaderFactory::createIntEnum(const RapidXmlReaderNode& node, const Path& path)
+ type::VariantPtr
+ ReaderFactory::createIntEnum(const RapidXmlReaderNode& node, const Path& path)
{
if (path.hasElement())
+ // ensured we are top-level!
{
- throw error::AronException(__PRETTY_FUNCTION__, "Having an inner int-enum is not supported anymore since Aron Version 'beta 0.2.3'. Please move the inner int-enum definition to the <" + constantes::GENERATE_TYPES_TAG + ">-tag.");
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Having an inner int-enum is not supported anymore since Aron Version 'beta "
+ "0.2.3'. Please move the inner int-enum definition to the <" +
+ std::string(constantes::GENERATE_TYPES_TAG) + ">-tag.");
}
- // ensured we are top-level!
const std::string name = util::GetAttribute(node, constantes::NAME_ATTRIBUTE_NAME);
@@ -482,6 +679,10 @@ namespace armarx::aron::typereader::xml
allPreviouslyKnownPrivateTypes.clear();
+ std::string defaultValue = util::GetAttributeWithDefault(
+ node, constantes::DEFAULT_ATTRIBUTE_NAME, type::aron_enum::default_value::DEFAULT);
+
+ // get accepted values
std::map<std::string, int> acceptedValues;
for (const RapidXmlReaderNode& valueChild : node.nodes())
{
@@ -492,18 +693,44 @@ namespace armarx::aron::typereader::xml
if (util::HasAttribute(valueChild, constantes::DOC_BRIEF_ATTRIBUTE_NAME))
{
- newEnumInfo.doc_values.insert({key, util::GetAttribute(valueChild, constantes::DOC_BRIEF_ATTRIBUTE_NAME)});
+ newEnumInfo.doc_values.insert(
+ {key, util::GetAttribute(valueChild, constantes::DOC_BRIEF_ATTRIBUTE_NAME)});
}
- const std::string value = util::GetAttribute(valueChild, constantes::VALUE_ATTRIBUTE_NAME);
+ const std::string value =
+ util::GetAttribute(valueChild, constantes::VALUE_ATTRIBUTE_NAME);
+
+ // Overwrite default value if child is tagged as default
+ if (util::AttributeIsTrue(valueChild, constantes::DEFAULT_ATTRIBUTE_NAME))
+ {
+ defaultValue = key;
+ }
acceptedValues.emplace(key, std::stoi(value));
}
+ if (acceptedValues.size() == 0)
+ {
+ printWarning("Found enum '" + name + "' without any members...");
+ }
+
+ // check default value
+ if (defaultValue != type::aron_enum::default_value::DEFAULT and
+ not acceptedValues.count(defaultValue))
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "An int enum has a non existing default value set",
+ defaultValue);
+ }
+
+
// create the int enum
aron::Path enumPath = path; // copy, just in case there are special things set
enumPath.setRootIdentifier(name); // set the root identifier to the obj name
- auto o = std::make_shared<type::IntEnum>(name, acceptedValues, enumPath);
+ auto o = std::make_shared<type::IntEnum>(enumPath);
+ o->setEnumName(name);
+ o->setAcceptedValueMap(acceptedValues);
+ o->setDefaultValueName(defaultValue);
newEnumInfo.correspondingType = o;
allGeneratedPublicIntEnums.emplace(newEnumInfo.typeName, newEnumInfo);
@@ -512,38 +739,98 @@ namespace armarx::aron::typereader::xml
return o;
}
- type::VariantPtr ReaderFactory::createInt(const RapidXmlReaderNode &node, const Path &path) const
+ type::VariantPtr
+ ReaderFactory::createInt(const RapidXmlReaderNode& node, const Path& path) const
{
- return std::make_shared<type::Int>(path);
+ auto o = std::make_shared<type::Int>(path);
+
+ if (util::HasAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME))
+ {
+ const std::string def = util::GetAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME);
+ util::EnforceStrIsInt(def);
+ o->setDefaultValue(simox::alg::to_<int>(def));
+ }
+
+ return o;
}
- type::VariantPtr ReaderFactory::createLong(const RapidXmlReaderNode &node, const Path &path) const
+ type::VariantPtr
+ ReaderFactory::createLong(const RapidXmlReaderNode& node, const Path& path) const
{
- return std::make_shared<type::Long>(path);
+ auto o = std::make_shared<type::Long>(path);
+
+ if (util::HasAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME))
+ {
+ const std::string def = util::GetAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME);
+ util::EnforceStrIsInt(def);
+ o->setDefaultValue(simox::alg::to_<long>(def));
+ }
+
+ return o;
}
- type::VariantPtr ReaderFactory::createFloat(const RapidXmlReaderNode &node, const Path &path) const
+ type::VariantPtr
+ ReaderFactory::createFloat(const RapidXmlReaderNode& node, const Path& path) const
{
- return std::make_shared<type::Float>(path);
+ auto o = std::make_shared<type::Float>(path);
+
+ if (util::HasAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME))
+ {
+ const std::string def = util::GetAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME);
+ util::EnforceStrIsFloat(def);
+ o->setDefaultValue(simox::alg::to_<float>(def));
+ }
+
+ return o;
}
- type::VariantPtr ReaderFactory::createDouble(const RapidXmlReaderNode &node, const Path &path) const
+ type::VariantPtr
+ ReaderFactory::createDouble(const RapidXmlReaderNode& node, const Path& path) const
{
- return std::make_shared<type::Double>(path);
+ auto o = std::make_shared<type::Double>(path);
+
+ if (util::HasAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME))
+ {
+ const std::string def = util::GetAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME);
+ util::EnforceStrIsFloat(def);
+ o->setDefaultValue(simox::alg::to_<double>(def));
+ }
+
+ return o;
}
- type::VariantPtr ReaderFactory::createString(const RapidXmlReaderNode &node, const Path &path) const
+ type::VariantPtr
+ ReaderFactory::createString(const RapidXmlReaderNode& node, const Path& path) const
{
- return std::make_shared<type::String>(path);
+ auto o = std::make_shared<type::String>(path);
+
+ if (util::HasAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME))
+ {
+ const std::string def = util::GetAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME);
+ o->setDefaultValue(def);
+ }
+
+ return o;
}
- type::VariantPtr ReaderFactory::createBool(const RapidXmlReaderNode &node, const Path &path) const
+ type::VariantPtr
+ ReaderFactory::createBool(const RapidXmlReaderNode& node, const Path& path) const
{
- return std::make_shared<type::Bool>(path);
+ auto o = std::make_shared<type::Bool>(path);
+
+ if (util::HasAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME))
+ {
+ const std::string def = util::GetAttribute(node, constantes::DEFAULT_ATTRIBUTE_NAME);
+ util::EnforceStrIsBool(def);
+ o->setDefaultValue(simox::alg::to_<bool>(def));
+ }
+
+ return o;
}
- type::VariantPtr ReaderFactory::createAnyObject(const RapidXmlReaderNode& node, const Path& path) const
+ type::VariantPtr
+ ReaderFactory::createAnyObject(const RapidXmlReaderNode& node, const Path& path) const
{
return std::make_shared<type::AnyObject>(path);
}
-}
+} // namespace armarx::aron::typereader::xml
diff --git a/source/RobotAPI/libraries/aron/codegeneration_util/CMakeLists.txt b/source/RobotAPI/libraries/aron/codegeneration_util/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9df89f3a56b58967674bb4eeb5a18c45af0256ab
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/codegeneration_util/CMakeLists.txt
@@ -0,0 +1,23 @@
+set(LIB_NAME aroncodegenerationutil)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox not available")
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # RobotAPI
+ RobotAPICore
+
+ HEADERS
+ TypeName.h
+
+ SOURCES
+ TypeName.cpp
+)
+
+add_library(AronCodegenerationUtil ALIAS "${LIB_NAME}")
diff --git a/source/RobotAPI/libraries/aron/codegeneration_util/TypeName.cpp b/source/RobotAPI/libraries/aron/codegeneration_util/TypeName.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98ea1f26861f3e6f5443c52ef93d057f69512540
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/codegeneration_util/TypeName.cpp
@@ -0,0 +1 @@
+#include "TypeName.h"
diff --git a/source/RobotAPI/libraries/aron/codegeneration_util/TypeName.h b/source/RobotAPI/libraries/aron/codegeneration_util/TypeName.h
new file mode 100644
index 0000000000000000000000000000000000000000..8018b67a20f32b7185c0ed82f5ebe3c2bea15175
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/codegeneration_util/TypeName.h
@@ -0,0 +1,89 @@
+#pragma once
+
+#include <cstdint> // TODO
+#include <string>
+#include <typeinfo>
+
+namespace armarx::aron
+{
+ // default implementation
+ template <typename T>
+ struct TypeName
+ {
+ static const char*
+ Get()
+ {
+ return typeid(T).name();
+ }
+ };
+
+ template <>
+ struct TypeName<short>
+ {
+ static const char*
+ Get()
+ {
+ return "short";
+ }
+ };
+
+ template <>
+ struct TypeName<int>
+ {
+ static const char*
+ Get()
+ {
+ return "int";
+ }
+ };
+
+ template <>
+ struct TypeName<float>
+ {
+ static const char*
+ Get()
+ {
+ return "float";
+ }
+ };
+
+ template <>
+ struct TypeName<long>
+ {
+ static const char*
+ Get()
+ {
+ return "long";
+ }
+ };
+
+ template <>
+ struct TypeName<double>
+ {
+ static const char*
+ Get()
+ {
+ return "double";
+ }
+ };
+
+ template <>
+ struct TypeName<std::string>
+ {
+ static const char*
+ Get()
+ {
+ return "string";
+ }
+ };
+
+ template <>
+ struct TypeName<bool>
+ {
+ static const char*
+ Get()
+ {
+ return "bool";
+ }
+ };
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index 8dc8e85556003093661f8adc0e4352ed5f660726..f8109b4a31f9aa10cfb3caae5b557e4a0c96a391 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -11,6 +11,7 @@ armarx_add_library(
# RobotAPI
RobotAPICore
aron
+ aroncodegenerationutil
HEADERS
aron_conversions.h
@@ -29,6 +30,7 @@ armarx_add_library(
json_conversions/armarx.h
rw/time.h
+ rw/eigen.h
rw/framed.h
util/object_finders.h
@@ -47,6 +49,7 @@ armarx_add_library(
json_conversions/armarx.cpp
rw/time.cpp
+ rw/eigen.cpp
rw/framed.cpp
util/object_finders.cpp
diff --git a/source/RobotAPI/libraries/aron/common/aron/Color.xml b/source/RobotAPI/libraries/aron/common/aron/Color.xml
index 180a9b372464de917400387032bdff9d4c086f79..a23c5a01c7a774a39e6f2d24756772da33a30b31 100644
--- a/source/RobotAPI/libraries/aron/common/aron/Color.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/Color.xml
@@ -5,19 +5,19 @@
<Object name='simox::arondto::Color'>
<ObjectChild key='r'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='g'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='b'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='a'>
- <int />
+ <int32 />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml b/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
index ac8b68bf36ca9e14ae5c9d22802823892a2de76a..f9315ae6ddfb4511194e2d2c106bd9bfcecb3299 100644
--- a/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
@@ -4,11 +4,6 @@ The ARON DTO of simox::OrientedBoxf.
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
- <CodeIncludes>
- <SystemInclude include="Eigen/Core" />
- <SystemInclude include="Eigen/Geometry" />
- </CodeIncludes>
-
<GenerateTypes>
<Object name="simox::arondto::OrientedBox">
diff --git a/source/RobotAPI/libraries/aron/common/aron/color.xml b/source/RobotAPI/libraries/aron/common/aron/color.xml
index f84c31bbe8e465c63cb0b456c77fdc5b3f831e44..4e36c32867bb05d98911b7b17e6d1674c83b7809 100644
--- a/source/RobotAPI/libraries/aron/common/aron/color.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/color.xml
@@ -4,49 +4,49 @@
<GenerateTypes>
<Object name='armarx::arondto::DrawColor'>
<ObjectChild key='r'>
- <float />
+ <float32 />
</ObjectChild>
<ObjectChild key='g'>
- <float />
+ <float32 />
</ObjectChild>
<ObjectChild key='b'>
- <float />
+ <float32 />
</ObjectChild>
<ObjectChild key='a'>
- <float />
+ <float32 />
</ObjectChild>
</Object>
<Object name='armarx::arondto::DrawColor24Bit'>
<ObjectChild key='r'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='g'>
- <int />
+ <int32 />
</ObjectChild>
<ObjectChild key='b'>
- <int />
+ <int32 />
</ObjectChild>
</Object>
<Object name='armarx::arondto::HsvColor'>
<ObjectChild key='h'>
- <float />
+ <float32 />
</ObjectChild>
<ObjectChild key='s'>
- <float />
+ <float32 />
</ObjectChild>
<ObjectChild key='v'>
- <float />
+ <float32 />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/common/aron/time.xml b/source/RobotAPI/libraries/aron/common/aron/time.xml
index 390e677cda4a528839abc613b1d147511511e9e9..38c2c5300adceeb78eaca66bce80b8d087e74713 100644
--- a/source/RobotAPI/libraries/aron/common/aron/time.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/time.xml
@@ -12,7 +12,7 @@
<Object name="armarx::arondto::Duration">
<ObjectChild key='microSeconds'>
- <long />
+ <int64 />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/common/aron/trajectory.xml b/source/RobotAPI/libraries/aron/common/aron/trajectory.xml
index beb4f2788e35f2ec91b2feeb8f58674c13ae5892..dba65554e81b41889aa9af251cfa643203d132a5 100644
--- a/source/RobotAPI/libraries/aron/common/aron/trajectory.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/trajectory.xml
@@ -7,7 +7,7 @@ My nice data, representing nice information.
<Object name="armarx::arondto::TSElement">
<ObjectChild key="timestep">
- <Float />
+ <float32 />
</ObjectChild>
<ObjectChild key="pose">
<Pose />
@@ -25,11 +25,11 @@ My nice data, representing nice information.
<Object name="armarx::arondto::JSElement">
<ObjectChild key="timestep">
- <Float />
+ <float32 />
</ObjectChild>
<ObjectChild key="jointValues">
<List>
- <Float />
+ <float32 />
</List>
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
index d12c47afe385fee4a2665a744915d9aa44f94466..6b4d0fde2cd85d155ba391430f5f0a3ce08f9595 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
@@ -1,11 +1,13 @@
#pragma once
-#include "packagepath.h"
+#include "core.h"
+#include "eigen.h"
#include "framed.h"
+#include "packagepath.h"
+#include "stl.h"
#include "time.h"
-
namespace armarx
{
-} // namespace armarx
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
index 8b738d098d56b40796fc427f6d69dd744beab9cc..1db8c876a280cb87641cfe65897cf74e73f38f3d 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
@@ -1,38 +1,42 @@
#include "eigen.h"
-namespace armarx::aron
+namespace armarx::aron::eigen
{
- using AronPose = Eigen::Matrix<float, 4, 4>;
-
- void fromAron(const AronPose& dto, Eigen::Affine3f& bo)
+ void
+ fromAron(const AronPosef& dto, Eigen::Affine3f& bo)
{
bo.matrix() = dto;
}
- void toAron(AronPose& dto, const Eigen::Affine3f& bo)
+ void
+ toAron(AronPosef& dto, const Eigen::Affine3f& bo)
{
dto = bo.matrix();
}
- void fromAron(const AronPose& dto, Eigen::Isometry3f& bo)
+ void
+ fromAron(const AronPosef& dto, Eigen::Isometry3f& bo)
{
bo.matrix() = dto;
}
- void toAron(AronPose& dto, const Eigen::Isometry3f& bo)
+ void
+ toAron(AronPosef& dto, const Eigen::Isometry3f& bo)
{
dto = bo.matrix();
}
- void fromAron(const AronPose& dto, Eigen::Isometry3d& bo)
+ void
+ fromAron(const AronPosef& dto, Eigen::Isometry3d& bo)
{
bo.matrix() = dto.cast<double>();
}
- void toAron(AronPose& dto, const Eigen::Isometry3d& bo)
+ void
+ toAron(AronPosef& dto, const Eigen::Isometry3d& bo)
{
dto = bo.matrix().cast<float>();
}
-} // namespace armarx::aron
+} // namespace armarx::aron::eigen
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
index e60f080c8a5defcb98268379d3df37bb40cb1a73..9fbbce95f071d05ea9a9bc3d1932e625f67fd902 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
@@ -1,20 +1,51 @@
#pragma once
+#include <Eigen/Core>
#include <Eigen/Geometry>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
-namespace armarx::aron
+namespace armarx::aron::eigen
{
- using AronPose = Eigen::Matrix<float, 4, 4>;
+ using AronPosef = Eigen::Matrix<float, 4, 4, Eigen::RowMajor>;
+ using AronPositionf = Eigen::Matrix<float, 3, 1, Eigen::RowMajor>;
+ using AronOrientationf = Eigen::Quaternion<float>;
+
+ template <class EigenT, int rows, int cols>
+ void
+ fromAron(const Eigen::Matrix<EigenT, rows, cols, Eigen::RowMajor>& dto,
+ Eigen::Matrix<EigenT, rows, cols>& bo)
+ {
+ bo = dto;
+ }
+
+ template <class EigenT, int rows, int cols>
+ void
+ toAron(Eigen::Matrix<EigenT, rows, cols, Eigen::RowMajor>& dto,
+ const Eigen::Matrix<EigenT, rows, cols>& bo)
+ {
+ dto = bo;
+ }
+
+ void fromAron(const AronPosef& dto, Eigen::Affine3f& bo);
+ void toAron(AronPosef& dto, const Eigen::Affine3f& bo);
- void fromAron(const AronPose& dto, Eigen::Affine3f& bo);
- void toAron(AronPose& dto, const Eigen::Affine3f& bo);
+ void fromAron(const AronPosef& dto, Eigen::Isometry3f& bo);
+ void toAron(AronPosef& dto, const Eigen::Isometry3f& bo);
- void fromAron(const AronPose& dto, Eigen::Isometry3f& bo);
- void toAron(AronPose& dto, const Eigen::Isometry3f& bo);
+ void fromAron(const AronPosef& dto, Eigen::Isometry3d& bo);
+ void toAron(AronPosef& dto, const Eigen::Isometry3d& bo);
- void fromAron(const AronPose& dto, Eigen::Isometry3d& bo);
- void toAron(AronPose& dto, const Eigen::Isometry3d& bo);
+} // namespace armarx::aron::eigen
-} // namespace armarx
+namespace armarx::aron
+{
+ using eigen::fromAron;
+ using eigen::toAron;
+} // namespace armarx::aron
+
+namespace armarx
+{
+ using aron::eigen::fromAron;
+ using aron::eigen::toAron;
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
index 857c3a46b65fce9c001520b39a90130ac892a76d..27ab8bff523e82ccd52f02bede86a33b51e79225 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/framed.cpp
@@ -2,9 +2,10 @@
#include <RobotAPI/libraries/aron/common/aron/framed.aron.generated.h>
-namespace armarx
+namespace armarx::aron::framed
{
- void fromAron(const arondto::FramedPosition& dto, armarx::FramedPosition& bo)
+ void
+ fromAron(const arondto::FramedPosition& dto, armarx::FramedPosition& bo)
{
bo.frame = dto.header.frame;
bo.agent = dto.header.agent;
@@ -13,7 +14,8 @@ namespace armarx
bo.z = dto.position.z();
}
- void toAron(arondto::FramedPosition& dto, const armarx::FramedPosition& bo)
+ void
+ toAron(arondto::FramedPosition& dto, const armarx::FramedPosition& bo)
{
dto.header.frame = bo.frame;
dto.header.agent = bo.agent;
@@ -22,7 +24,8 @@ namespace armarx
dto.position.z() = bo.z;
}
- void fromAron(const arondto::FramedOrientation& dto, armarx::FramedOrientation& bo)
+ void
+ fromAron(const arondto::FramedOrientation& dto, armarx::FramedOrientation& bo)
{
bo.frame = dto.header.frame;
bo.agent = dto.header.agent;
@@ -32,7 +35,8 @@ namespace armarx
bo.qz = dto.orientation.z();
}
- void toAron(arondto::FramedOrientation& dto, const armarx::FramedOrientation& bo)
+ void
+ toAron(arondto::FramedOrientation& dto, const armarx::FramedOrientation& bo)
{
dto.header.frame = bo.frame;
dto.header.agent = bo.agent;
@@ -42,25 +46,28 @@ namespace armarx
dto.orientation.z() = bo.qz;
}
- void fromAron(const arondto::FramedPose& dto, armarx::FramedPose& bo)
+ void
+ fromAron(const arondto::FramedPose& dto, armarx::FramedPose& bo)
{
bo.frame = dto.header.frame;
bo.agent = dto.header.agent;
- auto vec = Eigen::Vector3f(dto.pose.block<3,1>(0,3));
+ auto vec = Eigen::Vector3f(dto.pose.block<3, 1>(0, 3));
bo.position->x = vec.x();
bo.position->y = vec.y();
bo.position->z = vec.z();
- auto quat = Eigen::Quaternionf(dto.pose.block<3,3>(0,0));
+ auto quat = Eigen::Quaternionf(dto.pose.block<3, 3>(0, 0));
bo.orientation->qw = quat.w();
bo.orientation->qx = quat.x();
bo.orientation->qy = quat.y();
bo.orientation->qz = quat.z();
}
- void toAron(arondto::FramedPose& dto, const armarx::FramedPose& bo)
+ void
+ toAron(arondto::FramedPose& dto, const armarx::FramedPose& bo)
{
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;
@@ -70,7 +77,7 @@ namespace armarx
quat.x() = bo.orientation->qx;
quat.y() = bo.orientation->qy;
quat.z() = bo.orientation->qz;
- dto.pose.block<3,1>(0,3) = vec;
- dto.pose.block<3,3>(0,0) = quat.toRotationMatrix();
+ dto.pose.block<3, 1>(0, 3) = vec;
+ dto.pose.block<3, 3>(0, 0) = quat.toRotationMatrix();
}
-}
+} // namespace armarx::aron::framed
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/framed.h b/source/RobotAPI/libraries/aron/common/aron_conversions/framed.h
index f8654e6827d2c5344457320ee97be143f1d93966..39c39967dd495c4d0956806f7814b4e74c4a63b0 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/framed.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/framed.h
@@ -10,10 +10,9 @@ namespace armarx::arondto
class FramedOrientation;
class FramedPose;
class FrameID;
-}
+} // namespace armarx::arondto
-
-namespace armarx
+namespace armarx::aron::framed
{
void fromAron(const arondto::FramedPosition& dto, armarx::FramedPosition& bo);
void toAron(arondto::FramedPosition& dto, const armarx::FramedPosition& bo);
@@ -23,4 +22,16 @@ namespace armarx
void fromAron(const arondto::FramedPose& dto, armarx::FramedPose& bo);
void toAron(arondto::FramedPose& dto, const armarx::FramedPose& bo);
-}
+} // namespace armarx::aron::framed
+
+namespace armarx::aron
+{
+ using framed::fromAron;
+ using framed::toAron;
+} // namespace armarx::aron
+
+namespace armarx
+{
+ using aron::framed::fromAron;
+ using aron::framed::toAron;
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/packagepath.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/packagepath.cpp
index a1168648b87d631707271f956bf4fe6558f9116f..3abc66443091202264fda47c070393f5b9398e74 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/packagepath.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/packagepath.cpp
@@ -4,14 +4,16 @@
/* PackagePath */
-void armarx::fromAron(const arondto::PackagePath& dto, PackagePath& bo)
+void
+armarx::aron::packagepath::fromAron(const arondto::PackagePath& dto, PackagePath& bo)
{
bo = {dto.package, dto.path};
}
-void armarx::toAron(arondto::PackagePath& dto, const PackagePath& bo)
+void
+armarx::aron::packagepath::toAron(arondto::PackagePath& dto, const PackagePath& bo)
{
- const data::PackagePath icedto = bo.serialize();
- dto.package = icedto.package;
- dto.path = icedto.path;
+ const armarx::data::PackagePath icedto = bo.serialize();
+ dto.package = icedto.package;
+ dto.path = icedto.path;
}
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/packagepath.h b/source/RobotAPI/libraries/aron/common/aron_conversions/packagepath.h
index 3dea6b7f3b442217a9afdc629c81b3f944f0865e..0bbb2d1e63541716fa4b747057314430b4c822bd 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/packagepath.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/packagepath.h
@@ -1,18 +1,31 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/aron_conversions.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <RobotAPI/libraries/aron/core/aron_conversions.h>
+
// arondto forward declarations
namespace armarx::arondto
{
class PackagePath;
}
-namespace armarx
+namespace armarx::aron::packagepath
{
void fromAron(const arondto::PackagePath& dto, PackagePath& bo);
void toAron(arondto::PackagePath& dto, const PackagePath& bo);
-} // namespace armarx
+} // namespace armarx::aron::packagepath
+
+namespace armarx::aron
+{
+ using packagepath::fromAron;
+ using packagepath::toAron;
+} // namespace armarx::aron
+
+namespace armarx
+{
+ using aron::packagepath::fromAron;
+ using aron::packagepath::toAron;
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp
index f8629190d5f2dac1dd8189f4eb2be2425479464e..ae43b2f70b3588ca5c0b27920926a016edf6346a 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp
@@ -4,34 +4,38 @@
#include <RobotAPI/libraries/aron/common/aron/Color.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>
-
-void simox::fromAron(const arondto::AxisAlignedBoundingBox& dto, AxisAlignedBoundingBox& bo)
+void
+armarx::aron::simox::fromAron(const arondto::AxisAlignedBoundingBox& dto,
+ ::simox::AxisAlignedBoundingBox& bo)
{
bo.center() = dto.center;
bo.extents() = dto.extents;
}
-void simox::toAron(arondto::AxisAlignedBoundingBox& dto, const AxisAlignedBoundingBox& bo)
+void
+armarx::aron::simox::toAron(arondto::AxisAlignedBoundingBox& dto,
+ const ::simox::AxisAlignedBoundingBox& bo)
{
dto.center = bo.center();
dto.extents = bo.extents();
}
-
-void simox::fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo)
+void
+armarx::aron::simox::fromAron(const arondto::OrientedBox& dto, ::simox::OrientedBoxf& bo)
{
- bo = OrientedBoxf(dto.center, dto.orientation, dto.extents);
+ bo = ::simox::OrientedBoxf(dto.center, dto.orientation, dto.extents);
}
-void simox::toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo)
+void
+armarx::aron::simox::toAron(arondto::OrientedBox& dto, const ::simox::OrientedBoxf& bo)
{
dto.center = bo.center();
dto.orientation = bo.rotation();
dto.extents = bo.dimensions();
}
-
-void simox::fromAron(const arondto::Color& dto, Color& bo)
+void
+armarx::aron::simox::fromAron(const arondto::Color& dto, ::simox::Color& bo)
{
bo.r = dto.r;
bo.g = dto.g;
@@ -39,7 +43,8 @@ void simox::fromAron(const arondto::Color& dto, Color& bo)
bo.a = dto.a;
}
-void simox::toAron(arondto::Color& dto, const Color& bo)
+void
+armarx::aron::simox::toAron(arondto::Color& dto, const ::simox::Color& bo)
{
dto.r = bo.r;
dto.g = bo.g;
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h
index 68a7fada4109bdd90355d585cdade3bd530c5f34..0680db017b4b7086f7f8e1e5241cce0634e85516 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h
@@ -1,23 +1,46 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/aron_conversions.h>
-
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <RobotAPI/libraries/aron/common/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/aron_conversions.h>
-
-namespace simox
+namespace armarx::aron::simox
{
- void fromAron(const arondto::AxisAlignedBoundingBox& dto, AxisAlignedBoundingBox& bo);
- void toAron(arondto::AxisAlignedBoundingBox& dto, const AxisAlignedBoundingBox& bo);
+ namespace arondto
+ {
+ using AxisAlignedBoundingBox = ::simox::arondto::AxisAlignedBoundingBox;
+ using OrientedBox = ::simox::arondto::OrientedBox;
+ using Color = ::simox::arondto::Color;
+ } // namespace arondto
+
+ void fromAron(const arondto::AxisAlignedBoundingBox& dto, ::simox::AxisAlignedBoundingBox& bo);
+ void toAron(arondto::AxisAlignedBoundingBox& dto, const ::simox::AxisAlignedBoundingBox& bo);
- void fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo);
- void toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo);
+ void fromAron(const arondto::OrientedBox& dto, ::simox::OrientedBoxf& bo);
+ void toAron(arondto::OrientedBox& dto, const ::simox::OrientedBoxf& bo);
- void fromAron(const arondto::Color& dto, Color& bo);
- void toAron(arondto::Color& dto, const Color& bo);
-}
+ void fromAron(const arondto::Color& dto, ::simox::Color& bo);
+ void toAron(arondto::Color& dto, const ::simox::Color& bo);
+} // namespace armarx::aron::simox
+namespace armarx::aron
+{
+ using simox::fromAron;
+ using simox::toAron;
+} // namespace armarx::aron
+
+namespace armarx
+{
+ using aron::simox::fromAron;
+ using aron::simox::toAron;
+} // namespace armarx
+
+// Legacy support
+namespace simox
+{
+ using ::armarx::aron::simox::fromAron;
+ using ::armarx::aron::simox::toAron;
+} // namespace simox
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/time.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/time.cpp
index a1d0825a4ed26670e94ac545b2319b3f98fe56a5..56944236dc504a0e9a03a7f7115007f6a38f9c2b 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/time.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/time.cpp
@@ -2,32 +2,36 @@
#include <IceUtil/Time.h>
-#include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
#include <ArmarXCore/core/time/ice_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
-void IceUtil::fromAron(const long& dto, IceUtil::Time& bo)
+void
+IceUtil::fromAron(const long& dto, IceUtil::Time& bo)
{
bo = IceUtil::Time::microSeconds(dto);
}
-void IceUtil::toAron(long& dto, const IceUtil::Time& bo)
+void
+IceUtil::toAron(long& dto, const IceUtil::Time& bo)
{
dto = bo.toMicroSeconds();
}
-
-void IceUtil::fromAron(const IceUtil::Time& dto, armarx::DateTime& bo)
+void
+IceUtil::fromAron(const IceUtil::Time& dto, armarx::DateTime& bo)
{
fromIce(dto, bo);
}
-void IceUtil::toAron(IceUtil::Time& dto, const armarx::DateTime& bo)
+void
+IceUtil::toAron(IceUtil::Time& dto, const armarx::DateTime& bo)
{
toIce(dto, bo);
}
-void armarx::fromAron(const arondto::ClockType& dto, armarx::ClockType& bo)
+void
+armarx::aron::time::fromAron(const arondto::ClockType& dto, armarx::ClockType& bo)
{
switch (dto.value)
{
@@ -46,7 +50,8 @@ void armarx::fromAron(const arondto::ClockType& dto, armarx::ClockType& bo)
}
}
-void armarx::toAron(arondto::ClockType& dto, const armarx::ClockType& bo)
+void
+armarx::aron::time::toAron(arondto::ClockType& dto, const armarx::ClockType& bo)
{
switch (bo)
{
@@ -65,17 +70,20 @@ void armarx::toAron(arondto::ClockType& dto, const armarx::ClockType& bo)
}
}
-void armarx::fromAron(const arondto::Duration& dto, armarx::Duration& bo)
+void
+armarx::aron::time::fromAron(const arondto::Duration& dto, armarx::Duration& bo)
{
bo = Duration::MicroSeconds(dto.microSeconds);
}
-void armarx::toAron(arondto::Duration& dto, const armarx::Duration& bo)
+void
+armarx::aron::time::toAron(arondto::Duration& dto, const armarx::Duration& bo)
{
dto.microSeconds = bo.toMicroSeconds();
}
-void armarx::fromAron(const arondto::Frequency& dto, armarx::Frequency& bo)
+void
+armarx::aron::time::fromAron(const arondto::Frequency& dto, armarx::Frequency& bo)
{
Duration cycleDuration;
fromAron(dto.cycleDuration, cycleDuration);
@@ -83,12 +91,14 @@ void armarx::fromAron(const arondto::Frequency& dto, armarx::Frequency& bo)
bo = Frequency(cycleDuration);
}
-void armarx::toAron(arondto::Frequency& dto, const armarx::Frequency& bo)
+void
+armarx::aron::time::toAron(arondto::Frequency& dto, const armarx::Frequency& bo)
{
toAron(dto.cycleDuration, bo.toCycleDuration());
}
-void armarx::fromAron(const arondto::DateTime& dto, armarx::DateTime& bo)
+void
+armarx::aron::time::fromAron(const arondto::DateTime& dto, armarx::DateTime& bo)
{
ClockType clockType;
fromAron(dto.clockType, clockType);
@@ -99,7 +109,8 @@ void armarx::fromAron(const arondto::DateTime& dto, armarx::DateTime& bo)
bo = DateTime(timeSinceEpoch, clockType, dto.hostname);
}
-void armarx::toAron(arondto::DateTime& dto, const armarx::DateTime& bo)
+void
+armarx::aron::time::toAron(arondto::DateTime& dto, const armarx::DateTime& bo)
{
toAron(dto.clockType, bo.clockType());
toAron(dto.timeSinceEpoch, bo.toDurationSinceEpoch());
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/time.h b/source/RobotAPI/libraries/aron/common/aron_conversions/time.h
index 271508d8dd58f9e0f78f220cf2206ad9c24dd331..6bf8a0f1c7712adddc8becb8d475f015ad603fec 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/time.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/time.h
@@ -1,10 +1,11 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/aron_conversions.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Frequency.h>
+#include <RobotAPI/libraries/aron/core/aron_conversions.h>
+
// arondto forward declarations
namespace armarx::arondto
{
@@ -12,27 +13,31 @@ namespace armarx::arondto
class Duration;
class DateTime;
class Frequency;
-}
-
+} // namespace armarx::arondto
namespace IceUtil
{
class Time;
/* Deprecated stuff from old time type */
- [[deprecated("Using IceUtil::Time in ArmarX is deprecated. Use armarx::DateTime instead.")]]
- void fromAron(const long& dto, IceUtil::Time& bo);
+ [[deprecated(
+ "Using IceUtil::Time in ArmarX is deprecated. Use armarx::DateTime instead.")]] void
+ fromAron(const long& dto, IceUtil::Time& bo);
- [[deprecated("Using IceUtil::Time in ArmarX is deprecated. Use armarx::DateTime instead.")]]
- void toAron(long& dto, const IceUtil::Time& bo);
+ [[deprecated(
+ "Using IceUtil::Time in ArmarX is deprecated. Use armarx::DateTime instead.")]] void
+ toAron(long& dto, const IceUtil::Time& bo);
- [[deprecated("Using IceUtil::Time in ArmarX is deprecated. Use armarx::DateTime instead.")]]
- void fromAron(const IceUtil::Time& dto, armarx::DateTime& bo);
+ [[deprecated(
+ "Using IceUtil::Time in ArmarX is deprecated. Use armarx::DateTime instead.")]] void
+ fromAron(const IceUtil::Time& dto, armarx::DateTime& bo);
- [[deprecated("Using IceUtil::Time in ArmarX is deprecated. Use armarx::DateTime instead.")]]
- void toAron(IceUtil::Time& dto, const armarx::DateTime& bo);
-}
-namespace armarx
+ [[deprecated(
+ "Using IceUtil::Time in ArmarX is deprecated. Use armarx::DateTime instead.")]] void
+ toAron(IceUtil::Time& dto, const armarx::DateTime& bo);
+} // namespace IceUtil
+
+namespace armarx::aron::time
{
void fromAron(const arondto::ClockType& dto, ClockType& bo);
void toAron(arondto::ClockType& dto, const ClockType& bo);
@@ -46,4 +51,16 @@ namespace armarx
void fromAron(const arondto::DateTime& dto, DateTime& bo);
void toAron(arondto::DateTime& dto, const DateTime& bo);
-} // namespace armarx
+} // namespace armarx::aron::time
+
+namespace armarx::aron
+{
+ using time::fromAron;
+ using time::toAron;
+} // namespace armarx::aron
+
+namespace armarx
+{
+ using aron::time::fromAron;
+ using aron::time::toAron;
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/rw/eigen.cpp b/source/RobotAPI/libraries/aron/common/rw/eigen.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..709d149cc26ddfe787ee2c0af97ee77c61f013cd
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/rw/eigen.cpp
@@ -0,0 +1 @@
+#include "eigen.h"
diff --git a/source/RobotAPI/libraries/aron/common/rw/eigen.h b/source/RobotAPI/libraries/aron/common/rw/eigen.h
new file mode 100644
index 0000000000000000000000000000000000000000..69fd857063b85792e46f7e197e933a71b93c46e9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/rw/eigen.h
@@ -0,0 +1,172 @@
+#pragma once
+
+#include <RobotAPI/libraries/aron/codegeneration_util/TypeName.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/rw/Reader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/Writer.h>
+#include <RobotAPI/libraries/aron/core/type/rw/Writer.h>
+
+#include "../aron_conversions/eigen.h"
+
+namespace armarx
+{
+ // Helper methods for code generation to convert json/aron to bo and vice versa
+ namespace aron
+ {
+ template <class ReaderT, class EigenT, int rows, int cols, int options>
+ requires data::isReader<ReaderT>
+
+ void
+ read(ReaderT& aron_r,
+ typename ReaderT::InputType& input,
+ Eigen::Matrix<EigenT, rows, cols, options>& ret)
+ {
+ std::string typeAsString;
+ std::vector<int> shape;
+ std::vector<unsigned char> data;
+ aron_r.readNDArray(input, shape, typeAsString, data);
+
+ ARMARX_CHECK_AND_THROW(
+ ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
+ ::armarx::aron::error::AronException(
+ __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
+ ARMARX_CHECK_AND_THROW(
+ typeAsString == TypeName<EigenT>::Get(),
+ ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Received wrong typename",
+ typeAsString,
+ TypeName<EigenT>::Get()));
+ std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
+ }
+
+ template <class ReaderT, class EigenT, int cols, int options>
+ requires data::isReader<ReaderT>
+
+ void
+ read(ReaderT& aron_r,
+ typename ReaderT::InputType& input,
+ Eigen::Matrix<EigenT, Eigen::Dynamic, cols, options>& ret)
+ {
+ std::string typeAsString;
+ std::vector<int> shape;
+ std::vector<unsigned char> data;
+ aron_r.readNDArray(input, shape, typeAsString, data);
+
+ ret.resize(shape.at(0));
+
+ ARMARX_CHECK_AND_THROW(
+ ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
+ ::armarx::aron::error::AronException(
+ __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
+ ARMARX_CHECK_AND_THROW(
+ typeAsString == TypeName<EigenT>::Get(),
+ ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Received wrong typename",
+ typeAsString,
+ TypeName<EigenT>::Get()));
+ std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
+ }
+
+ template <class ReaderT, class EigenT, int rows, int options>
+ requires data::isReader<ReaderT>
+
+ void
+ read(ReaderT& aron_r,
+ typename ReaderT::InputType& input,
+ Eigen::Matrix<EigenT, rows, Eigen::Dynamic, options>& ret)
+ {
+ std::string typeAsString;
+ std::vector<int> shape;
+ std::vector<unsigned char> data;
+ aron_r.readNDArray(input, shape, typeAsString, data);
+
+ ret.resize(shape.at(1));
+
+ ARMARX_CHECK_AND_THROW(
+ ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
+ ::armarx::aron::error::AronException(
+ __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
+ ARMARX_CHECK_AND_THROW(
+ typeAsString == TypeName<EigenT>::Get(),
+ ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Received wrong typename",
+ typeAsString,
+ TypeName<EigenT>::Get()));
+ std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
+ }
+
+ template <class ReaderT, class EigenT, int options>
+ requires data::isReader<ReaderT>
+
+ void
+ read(ReaderT& aron_r,
+ typename ReaderT::InputType& input,
+ Eigen::Matrix<EigenT, Eigen::Dynamic, Eigen::Dynamic, options>& ret)
+ {
+ std::string typeAsString;
+ std::vector<int> shape;
+ std::vector<unsigned char> data;
+ aron_r.readNDArray(input, shape, typeAsString, data);
+
+ ret.resize(shape.at(0), shape.at(1));
+
+ ARMARX_CHECK_AND_THROW(
+ ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
+ ::armarx::aron::error::AronException(
+ __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
+ ARMARX_CHECK_AND_THROW(
+ typeAsString == TypeName<EigenT>::Get(),
+ ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Received wrong typename",
+ typeAsString,
+ TypeName<EigenT>::Get()));
+ std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
+ }
+
+ template <class WriterT, class EigenT, int rows, int cols, int options>
+ requires data::isWriter<WriterT>
+
+ void
+ write(WriterT& aron_w,
+ const Eigen::Matrix<EigenT, rows, cols, options>& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p = armarx::aron::Path())
+ {
+ ret = aron_w.writeNDArray(
+ {static_cast<int>(input.rows()), static_cast<int>(input.cols()), sizeof(EigenT)},
+ TypeName<EigenT>::Get(),
+ reinterpret_cast<const unsigned char*>(input.data()),
+ aron_p);
+ }
+
+ template <class ReaderT, class EigenT>
+ requires data::isReader<ReaderT>
+
+ void
+ read(ReaderT& aron_r, typename ReaderT::InputType& input, Eigen::Quaternion<EigenT>& ret)
+ {
+ std::string typeAsString;
+ std::vector<int> shape;
+ std::vector<unsigned char> data;
+ aron_r.readNDArray(input, shape, typeAsString, data);
+ std::memcpy(
+ reinterpret_cast<unsigned char*>(ret.coeffs().data()), data.data(), data.size());
+ }
+
+ template <class WriterT, class EigenT>
+ requires data::isWriter<WriterT>
+
+ void
+ write(WriterT& aron_w,
+ const Eigen::Quaternion<EigenT>& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p = armarx::aron::Path())
+ {
+ ret = aron_w.writeNDArray({1, 4, sizeof(EigenT)},
+ TypeName<EigenT>::Get(),
+ reinterpret_cast<const unsigned char*>(input.coeffs().data()),
+ aron_p);
+ }
+ } // namespace aron
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/rw/framed.h b/source/RobotAPI/libraries/aron/common/rw/framed.h
index 21d3aae5a942774a8fc4598c7e780150c81f9451..a0c84c292bd69484209627e369bdd42e4f426f10 100644
--- a/source/RobotAPI/libraries/aron/common/rw/framed.h
+++ b/source/RobotAPI/libraries/aron/common/rw/framed.h
@@ -1,15 +1,22 @@
#pragma once
-#include "../aron_conversions/framed.h"
#include <RobotAPI/libraries/aron/common/aron/framed.aron.generated.h>
+#include "../aron_conversions/framed.h"
+
namespace armarx
{
// Helper methods for code generation to convert json/aron to bo and vice versa
+ // UPDATE 29.06.2023 IT MAKES NO SENSE TO MERGE ARMARX::VARIANTS WITH ARON. THEREOFRE WE REMOVED THE ARMARX::VARIANTS.
+ // AS SOON AS CHRISTIANS GEOMETRY PACKAGE IN SIMOX IS READY, THE ARON TYPES SHOULD BE REPLACED WITH CORE TYPES FROM SIMOX!
+ // AND THESE METHODS NEED TO BE UPDATED!
namespace aron
{
- template<class ReaderT>
- void read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::FramedPosition& ret)
+ template <class ReaderT>
+ requires data::isReader<ReaderT>
+
+ void
+ read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::FramedPosition& ret)
{
arondto::FramedPosition aron;
aron.read(aron_r, input);
@@ -17,16 +24,25 @@ namespace armarx
armarx::fromAron(aron, ret);
}
- template<class WriterT>
- void write(WriterT& aron_w, const armarx::FramedPosition& input, typename WriterT::ReturnType& ret, const armarx::aron::Path& aron_p = armarx::aron::Path())
+ template <class WriterT>
+ requires data::isWriter<WriterT>
+
+ void
+ write(WriterT& aron_w,
+ const armarx::FramedPosition& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p = armarx::aron::Path())
{
arondto::FramedPosition aron;
armarx::toAron(aron, input);
ret = aron.write(aron_w, aron_p);
}
- template<class ReaderT>
- void read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::FramedOrientation& ret)
+ template <class ReaderT>
+ requires data::isReader<ReaderT>
+
+ void
+ read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::FramedOrientation& ret)
{
arondto::FramedOrientation aron;
aron.read(aron_r, input);
@@ -34,16 +50,25 @@ namespace armarx
armarx::fromAron(aron, ret);
}
- template<class WriterT>
- void write(WriterT& aron_w, const armarx::FramedOrientation& input, typename WriterT::ReturnType& ret, const armarx::aron::Path& aron_p = armarx::aron::Path())
+ template <class WriterT>
+ requires data::isWriter<WriterT>
+
+ void
+ write(WriterT& aron_w,
+ const armarx::FramedOrientation& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p = armarx::aron::Path())
{
arondto::FramedOrientation aron;
armarx::toAron(aron, input);
ret = aron.write(aron_w, aron_p);
}
- template<class ReaderT>
- void read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::FramedPose& ret)
+ template <class ReaderT>
+ requires data::isReader<ReaderT>
+
+ void
+ read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::FramedPose& ret)
{
arondto::FramedPose aron;
aron.read(aron_r, input);
@@ -51,15 +76,19 @@ namespace armarx
armarx::fromAron(aron, ret);
}
- template<class WriterT>
- void write(WriterT& aron_w, const armarx::FramedPose& input, typename WriterT::ReturnType& ret, const armarx::aron::Path& aron_p = armarx::aron::Path())
+ template <class WriterT>
+ requires data::isWriter<WriterT>
+
+ void
+ write(WriterT& aron_w,
+ const armarx::FramedPose& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p = armarx::aron::Path())
{
arondto::FramedPose aron;
armarx::toAron(aron, input);
ret = aron.write(aron_w, aron_p);
}
- }
-
-} // namespace armarx
-
+ } // namespace aron
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/rw/time.h b/source/RobotAPI/libraries/aron/common/rw/time.h
index 0549d82f1b3411c9b5dc9c8c30fd8c7d640ce4cd..c31b81991444b957b173066b27634456d0452529 100644
--- a/source/RobotAPI/libraries/aron/common/rw/time.h
+++ b/source/RobotAPI/libraries/aron/common/rw/time.h
@@ -1,15 +1,22 @@
#pragma once
-#include "../aron_conversions/time.h"
#include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/rw/Reader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/Writer.h>
+
+#include "../aron_conversions/time.h"
namespace armarx
{
// Helper methods for code generation to convert json/aron to bo and vice versa
namespace aron
{
- template<class ReaderT>
- void read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::core::time::DateTime& ret)
+ template <class ReaderT>
+ requires data::isReader<ReaderT>
+
+ void
+ read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::core::time::DateTime& ret)
{
arondto::DateTime aron;
aron.read(aron_r, input);
@@ -17,18 +24,25 @@ namespace armarx
armarx::fromAron(aron, ret);
}
- template<class WriterT>
- void write(WriterT& aron_w, const armarx::core::time::DateTime& input, typename WriterT::ReturnType& ret, const armarx::aron::Path& aron_p = armarx::aron::Path())
+ template <class WriterT>
+ requires data::isWriter<WriterT>
+
+ void
+ write(WriterT& aron_w,
+ const armarx::core::time::DateTime& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p = armarx::aron::Path())
{
arondto::DateTime aron;
armarx::toAron(aron, input);
ret = aron.write(aron_w, aron_p);
}
+ template <class ReaderT>
+ requires data::isReader<ReaderT>
-
- template<class ReaderT>
- void read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::core::time::Duration& ret)
+ void
+ read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::core::time::Duration& ret)
{
arondto::Duration aron;
aron.read(aron_r, input);
@@ -36,14 +50,19 @@ namespace armarx
armarx::fromAron(aron, ret);
}
- template<class WriterT>
- void write(WriterT& aron_w, const armarx::core::time::Duration& input, typename WriterT::ReturnType& ret, const armarx::aron::Path& aron_p = armarx::aron::Path())
+ template <class WriterT>
+ requires data::isWriter<WriterT>
+
+ void
+ write(WriterT& aron_w,
+ const armarx::core::time::Duration& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p = armarx::aron::Path())
{
arondto::Duration aron;
armarx::toAron(aron, input);
ret = aron.write(aron_w, aron_p);
}
- }
-
-} // namespace armarx
+ } // namespace aron
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 79164ab84cea2848fd5737d301f30df6bdc0cf5d..398e5bcdbf53a3fd059eaa792e8caa16344a3b4e 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -111,7 +111,7 @@ namespace armarx::aron::data::converter
using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
const auto dims = nav.getShape();
- ARMARX_CHECK_EQUAL(dims.size(), 2); // for now ...
+ ARMARX_CHECK(dims.size() == 2 or dims.size() == 3); // for now ...
Eigen::Map<MatrixT> map(reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1));
return map;
@@ -197,7 +197,7 @@ namespace armarx::aron::data::converter
return ConvertToDynamicArray<T>(nav);
}
- checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
+ checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToArray");
auto dims = nav.getShape();
Eigen::Map<Eigen::Array<T, Rows, Cols>> ret(
diff --git a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
index b4682df50a19597d8c0057b1cecd2b11978fae54..c94ff951aff27a7cc3ab740728d80795ececce23 100644
--- a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
@@ -66,8 +66,7 @@ namespace armarx::aron::type::converter
AronNlohmannJSONConverter::ConvertFromNlohmannJSONTypeObject(const nlohmann::json& j,
const armarx::aron::Path& p)
{
- type::ObjectPtr foo =
- std::make_shared<aron::type::Object>("foo"); // foo is just a placeholder
+ type::ObjectPtr foo = std::make_shared<aron::type::Object>();
type::VariantPtr aron = std::make_shared<aron::type::Object>(*foo, p);
ConvertFromNlohmannJSON(aron, j);
return type::Object::DynamicCastAndCheck(aron);
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 49a75cb664c32ff731824b10d86c134fd0b168ef..863fc5e6efcbad4b7661487d653113199cb4012b 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -82,6 +82,9 @@ set(LIB_FILES
type/rw/reader/variant/VariantReader.cpp
type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+ type/rw/json/Data.cpp
+ data/rw/json/Data.cpp
+
data/visitor/Visitor.cpp
data/visitor/RecursiveVisitor.cpp
data/visitor/variant/VariantVisitor.cpp
diff --git a/source/RobotAPI/libraries/aron/core/Exception.h b/source/RobotAPI/libraries/aron/core/Exception.h
index fc080bafd09e2d7424bf0fc18538539f680329da..49e7ad4444286b115f71f199c553359b97dcdfe4 100644
--- a/source/RobotAPI/libraries/aron/core/Exception.h
+++ b/source/RobotAPI/libraries/aron/core/Exception.h
@@ -26,53 +26,58 @@
// STD/STL
// ArmarX
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+
#include <RobotAPI/interface/aron.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/exceptions/Exception.h>
-#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
-
+#include <RobotAPI/libraries/aron/core/Path.h>
namespace armarx::aron::error
{
/**
* @brief A base class for aron exceptions. All aron exceptions inherit from this class
*/
- class AronException :
- public armarx::LocalException
+ class AronException : public armarx::LocalException
{
public:
AronException() = delete;
+
AronException(const std::string& prettymethod, const std::string& reason) :
LocalException(prettymethod + ": " + reason + ".")
{
}
- AronException(const std::string& prettymethod, const std::string& reason, const Path& path) :
+ AronException(const std::string& prettymethod,
+ const std::string& reason,
+ const Path& path) :
LocalException(prettymethod + ": " + reason + ". The path was: " + path.toString())
{
}
/// call operator to append a message to the exception. Used by ARMARX_CHECK_AND_THROW
- AronException& operator()(const std::string& additionalMessage = "")
+ AronException&
+ operator()(const std::string& additionalMessage = "")
{
auto currentReason = getReason();
- setReason(currentReason + ". Additional Message: " + additionalMessage);
+ if (not additionalMessage.empty())
+ {
+ setReason(currentReason + ". Additional Message: " + additionalMessage);
+ }
return *this;
}
};
- /**
- * @brief A base class for aron exceptions. All aron exceptions inherit from this class
- */
- class AronEOFException :
- public AronException
+ class AronEOFException : public AronException
{
public:
AronEOFException() = delete;
+
AronEOFException(const std::string& prettymethod) :
- AronException(prettymethod, "REACHED THE END OF A NON VOID METHOD. PERHAPS YOU FORGOT TO ADD A VALUE TO SOME SWITCH-CASE STATEMEMT?.")
+ AronException(prettymethod,
+ "REACHED THE END OF A NON VOID METHOD. PERHAPS YOU FORGOT TO ADD A VALUE "
+ "TO SOME SWITCH-CASE STATEMEMT?.")
{
}
};
@@ -80,11 +85,11 @@ namespace armarx::aron::error
/**
* @brief The NotImplementedYetException class
*/
- class NotImplementedYetException :
- public AronException
+ class NotImplementedYetException : public AronException
{
public:
NotImplementedYetException() = delete;
+
NotImplementedYetException(const std::string& prettymethod) :
AronException(prettymethod, "This method is not yet implemented!")
{
@@ -94,66 +99,86 @@ namespace armarx::aron::error
/**
* @brief The AronNotValidException class. Takes a dto object as input
*/
- class AronNotValidException :
- public AronException
+ class AronNotValidException : public AronException
{
public:
AronNotValidException() = delete;
- AronNotValidException(const std::string& prettymethod, const std::string& reason, const data::dto::GenericDataPtr& data) :
+
+ AronNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const data::dto::GenericDataPtr& data) :
AronException(prettymethod, reason + ". The ice_id of the data was: " + data->ice_id())
{
-
}
- AronNotValidException(const std::string& prettymethod, const std::string& reason, const data::dto::GenericDataPtr& data, const Path& path) :
- AronException(prettymethod, reason + ". The ice_id of the data was: " + data->ice_id(), path)
+ AronNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const data::dto::GenericDataPtr& data,
+ const Path& path) :
+ AronException(prettymethod,
+ reason + ". The ice_id of the data was: " + data->ice_id(),
+ path)
{
-
}
- AronNotValidException(const std::string& prettymethod, const std::string& reason, const type::dto::GenericTypePtr& type) :
+ AronNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const type::dto::GenericTypePtr& type) :
AronException(prettymethod, reason + ". The ice_id of the type was: " + type->ice_id())
{
-
}
- AronNotValidException(const std::string& prettymethod, const std::string& reason, const type::dto::GenericTypePtr& type, const Path& path) :
- AronException(prettymethod, reason + ". The ice_id of the type was: " + type->ice_id(), path)
+ AronNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const type::dto::GenericTypePtr& type,
+ const Path& path) :
+ AronException(prettymethod,
+ reason + ". The ice_id of the type was: " + type->ice_id(),
+ path)
{
-
}
};
/**
* @brief The ValueNotValidException class. Only takes strings as input (convert before)
*/
- class ValueNotValidException :
- public AronException
+ class ValueNotValidException : public AronException
{
public:
ValueNotValidException() = delete;
- ValueNotValidException(const std::string& prettymethod, const std::string& reason, const std::string& input) :
+
+ ValueNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const std::string& input) :
AronException(prettymethod, reason + ". Got: " + input)
{
-
}
- ValueNotValidException(const std::string& prettymethod, const std::string& reason, const std::string& input, const Path& path) :
+ ValueNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const std::string& input,
+ const Path& path) :
AronException(prettymethod, reason + ". Got: " + input, path)
{
-
}
- ValueNotValidException(const std::string& prettymethod, const std::string& reason, const std::string& input, const std::string& expectation) :
+ ValueNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const std::string& input,
+ const std::string& expectation) :
AronException(prettymethod, reason + ". Got: " + input + ". Expected: " + expectation)
{
-
}
- ValueNotValidException(const std::string& prettymethod, const std::string& reason, const std::string& input, const std::string& expectation, const Path& path) :
- AronException(prettymethod, reason + ". Got: " + input + ". Expected: " + expectation, path)
+ ValueNotValidException(const std::string& prettymethod,
+ const std::string& reason,
+ const std::string& input,
+ const std::string& expectation,
+ const Path& path) :
+ AronException(prettymethod,
+ reason + ". Got: " + input + ". Expected: " + expectation,
+ path)
{
-
}
};
-}
+} // namespace armarx::aron::error
diff --git a/source/RobotAPI/libraries/aron/core/Path.cpp b/source/RobotAPI/libraries/aron/core/Path.cpp
index 8d15ca0c810a94fb2972c451ca87cf01758088f7..c80e00f53c989b8aadd4618757aaa3c6a588dc4a 100644
--- a/source/RobotAPI/libraries/aron/core/Path.cpp
+++ b/source/RobotAPI/libraries/aron/core/Path.cpp
@@ -235,4 +235,38 @@ namespace armarx::aron
ret.setDelimeter(delimeter);
return ret;
}
+
+ bool
+ Path::hasPrefix(const Path& e) const
+ {
+ const auto& p = e.getPath();
+
+ if (path.size() < p.size())
+ {
+ // prefix is impossible
+ return false;
+ }
+
+ int i = 0;
+ for (const auto& el : path)
+ {
+ if (el != path[i++])
+ {
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ bool
+ Path::hasDirectPrefix(const Path& e) const
+ {
+ const auto& p = e.getPath();
+ if (p.size() + 1 != path.size())
+ {
+ return false;
+ }
+ return hasPrefix(e);
+ }
} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/core/Path.h b/source/RobotAPI/libraries/aron/core/Path.h
index a88a00d84317ee20526d06334ebac5e829020591..888327fd929309cde7575133be27649b1a57bcd9 100644
--- a/source/RobotAPI/libraries/aron/core/Path.h
+++ b/source/RobotAPI/libraries/aron/core/Path.h
@@ -80,6 +80,9 @@ namespace armarx::aron
Path withDetachedLastElement() const;
Path getWithoutPrefix(const Path&) const;
+ bool hasPrefix(const Path&) const;
+ bool hasDirectPrefix(const Path&) const;
+
private:
void append(const std::string&);
diff --git a/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h b/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h
index 8372d89c318cf821a9056c6e5f00f58f372492db..c42a18210927db1d08233ef591283ed581586acf 100644
--- a/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h
@@ -24,26 +24,22 @@
#pragma once
// STD/STL
+#include <map>
#include <memory>
#include <string>
#include <vector>
-#include <map>
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
-
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/rw/Reader.h>
#include <RobotAPI/libraries/aron/core/data/rw/Writer.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/rw/Writer.h>
// ARON
#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/aron_conversions.h>
-#include <RobotAPI/libraries/aron/core/rw.h>
-
namespace armarx::aron::cpp
{
@@ -54,15 +50,15 @@ namespace armarx::aron::cpp
virtual ~AronGeneratedClass() = default;
/// Reset all member values of this class to default (as stated in the XML). This may mean that maybe types are null or false and images may be created as headers_only
- virtual void resetHard() {};
+ virtual void resetHard(){};
/// Reset all member values of this class softly, meaning if a maybe type has a value, we reset only the value (not the full maybe type) and if an image has data (width, height) we keep the data and width and height and only reset teh pixel values
- virtual void resetSoft() {};
+ virtual void resetSoft(){};
};
template <class T>
concept isAronGeneratedClass = std::is_base_of<AronGeneratedClass, T>::value;
-}
+} // namespace armarx::aron::cpp
namespace armarx::aron::codegenerator::cpp
{
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/json/Data.cpp b/source/RobotAPI/libraries/aron/core/data/rw/json/Data.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..df60876fbdd629f5c43d7a3fd2efb3f2a28ba1f0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/data/rw/json/Data.cpp
@@ -0,0 +1,21 @@
+/*
+* 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 Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Data.h"
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
index f458e3d9973bf34dd2dc13e49b5f505db4e403e4..c03b3c37fec376834168d1795dbc6fc390874bea 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
@@ -33,7 +33,8 @@ namespace armarx::aron::data
{
// Access method
- std::unique_ptr<data::Variant> VariantFactory::create(const data::dto::GenericDataPtr& aron, const Path& path) const
+ std::unique_ptr<data::Variant>
+ VariantFactory::create(const data::dto::GenericDataPtr& aron, const Path& path) const
{
// if aron is null ==> e.g. optional or ptr not set
if (!aron)
@@ -41,20 +42,54 @@ namespace armarx::aron::data
return nullptr;
};
+ // compare versions
+ auto v = aron->VERSION;
+ auto v_str =
+ std::to_string(v.MAJOR) + "." + std::to_string(v.MINOR) + "." + std::to_string(v.PATCH);
+
+ auto d = aron::Version();
+ auto d_str =
+ std::to_string(d.MAJOR) + "." + std::to_string(d.MINOR) + "." + std::to_string(d.PATCH);
+
+ if (v != d)
+ {
+ ARMARX_WARNING
+ << "Found inconsistencies in aron versions. This may lead to ice marshalling "
+ "exceptions. Please make sure to update the sender and receiver packages. "
+ << v_str << " != " << d_str;
+ }
+
auto descriptor = data::Aron2Descriptor(*aron);
- switch(descriptor)
+ switch (descriptor)
{
- case data::Descriptor::LIST: return std::make_unique<data::List>(data::dto::ListPtr::dynamicCast(aron), path);
- case data::Descriptor::DICT: return std::make_unique<data::Dict>(data::dto::DictPtr::dynamicCast(aron), path);
- case data::Descriptor::NDARRAY: return std::make_unique<data::NDArray>(data::dto::NDArrayPtr::dynamicCast(aron), path);
- case data::Descriptor::INT: return std::make_unique<data::Int>(data::dto::AronIntPtr::dynamicCast(aron), path);
- case data::Descriptor::LONG: return std::make_unique<data::Long>(data::dto::AronLongPtr::dynamicCast(aron), path);
- case data::Descriptor::FLOAT: return std::make_unique<data::Float>(data::dto::AronFloatPtr::dynamicCast(aron), path);
- case data::Descriptor::DOUBLE: return std::make_unique<data::Double>(data::dto::AronDoublePtr::dynamicCast(aron), path);
- case data::Descriptor::STRING: return std::make_unique<data::String>(data::dto::AronStringPtr::dynamicCast(aron), path);
- case data::Descriptor::BOOL: return std::make_unique<data::Bool>(data::dto::AronBoolPtr::dynamicCast(aron), path);
- case data::Descriptor::UNKNOWN: break;
+ case data::Descriptor::LIST:
+ return std::make_unique<data::List>(data::dto::ListPtr::dynamicCast(aron), path);
+ case data::Descriptor::DICT:
+ return std::make_unique<data::Dict>(data::dto::DictPtr::dynamicCast(aron), path);
+ case data::Descriptor::NDARRAY:
+ return std::make_unique<data::NDArray>(data::dto::NDArrayPtr::dynamicCast(aron),
+ path);
+ case data::Descriptor::INT:
+ return std::make_unique<data::Int>(data::dto::AronIntPtr::dynamicCast(aron), path);
+ case data::Descriptor::LONG:
+ return std::make_unique<data::Long>(data::dto::AronLongPtr::dynamicCast(aron),
+ path);
+ case data::Descriptor::FLOAT:
+ return std::make_unique<data::Float>(data::dto::AronFloatPtr::dynamicCast(aron),
+ path);
+ case data::Descriptor::DOUBLE:
+ return std::make_unique<data::Double>(data::dto::AronDoublePtr::dynamicCast(aron),
+ path);
+ case data::Descriptor::STRING:
+ return std::make_unique<data::String>(data::dto::AronStringPtr::dynamicCast(aron),
+ path);
+ case data::Descriptor::BOOL:
+ return std::make_unique<data::Bool>(data::dto::AronBoolPtr::dynamicCast(aron),
+ path);
+ case data::Descriptor::UNKNOWN:
+ break;
}
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "", std::to_string((int) descriptor), path);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "", std::to_string((int)descriptor), path);
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Variant.h b/source/RobotAPI/libraries/aron/core/data/variant/Variant.h
index 1fab338ab2df57c267c841ac9297c533632bc46a..cc01ef83979306defb363ce9bdbf0089920ad747 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/Variant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Variant.h
@@ -30,10 +30,9 @@
// ArmarX
#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Path.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
-
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
namespace armarx::aron::data
@@ -66,16 +65,19 @@ namespace armarx::aron::data
public:
// constructors
Variant() = delete;
+
Variant(const data::Descriptor& descriptor, const Path& path) :
- descriptor(descriptor),
- path(path)
+ descriptor(descriptor), path(path)
{
}
+
virtual ~Variant() = default;
// operators
virtual bool operator==(const Variant& other) const = 0;
- bool operator==(const VariantPtr& other) const
+
+ bool
+ operator==(const VariantPtr& other) const
{
if (!other)
{
@@ -90,25 +92,29 @@ namespace armarx::aron::data
static VariantPtr FromAronDTO(const data::dto::GenericDataPtr&, const Path& = Path());
/// create a list of variants from a list of dto objects
- static std::vector<VariantPtr> FromAronDTO(const std::vector<data::dto::GenericDataPtr>&, const Path& = Path());
+ static std::vector<VariantPtr> FromAronDTO(const std::vector<data::dto::GenericDataPtr>&,
+ const Path& = Path());
/// return a list of dto objects from a list of variant objects
static std::vector<data::dto::GenericDataPtr> ToAronDTO(const std::vector<VariantPtr>&);
/// getter for the descriptor enum
- data::Descriptor getDescriptor() const
+ data::Descriptor
+ getDescriptor() const
{
return descriptor;
}
/// get the path
- Path getPath() const
+ Path
+ getPath() const
{
return path;
}
/// get the path as string
- std::string pathToString() const
+ std::string
+ pathToString() const
{
return path.toString();
}
@@ -116,6 +122,7 @@ namespace armarx::aron::data
// virtual definitions
/// get a pointer to a copy of this variant
virtual VariantPtr cloneAsVariant() const = 0;
+ virtual VariantPtr cloneAsVariant(const Path& newPath) const = 0;
/// get the children of a data variant
virtual std::vector<VariantPtr> getChildren() const = 0;
@@ -154,4 +161,4 @@ namespace armarx::aron::data
template <class T>
concept isVariant = std::is_base_of<Variant, T>::value;
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
index 483088aa7ffaf706271072d5e13d55f26b8e4ec4..dfad132e5f6a45f828dd00942b8c5de4c1f427f3 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -35,23 +35,31 @@ namespace armarx::aron::data
{
// constructors
NDArray::NDArray(const Path& path) :
- detail::ComplexVariant<data::dto::NDArray, NDArray>::ComplexVariant(data::Descriptor::NDARRAY, path)
+ detail::ComplexVariant<data::dto::NDArray, NDArray>::ComplexVariant(
+ data::Descriptor::NDARRAY,
+ path)
{
-
}
NDArray::NDArray(const data::dto::NDArrayPtr& o, const Path& path) :
- detail::ComplexVariant<data::dto::NDArray, NDArray>::ComplexVariant(o, data::Descriptor::NDARRAY, path)
+ detail::ComplexVariant<data::dto::NDArray, NDArray>::ComplexVariant(
+ o,
+ data::Descriptor::NDARRAY,
+ path)
{
}
- NDArray::NDArray(const std::vector<int>& dim, const std::string& t, const std::vector<unsigned char>& data, const Path& path) :
- NDArray(data::dto::NDArrayPtr(new data::dto::NDArray(aron::VERSION, dim, t, data)), path)
+ NDArray::NDArray(const std::vector<int>& dim,
+ const std::string& t,
+ const std::vector<unsigned char>& data,
+ const Path& path) :
+ NDArray(data::dto::NDArrayPtr(new data::dto::NDArray(aron::Version(), dim, t, data)), path)
{
}
// operators
- bool NDArray::operator==(const NDArray& other) const
+ bool
+ NDArray::operator==(const NDArray& other) const
{
const auto otherAron = other.toNDArrayDTO();
if (aron->shape != otherAron->shape)
@@ -69,7 +77,9 @@ namespace armarx::aron::data
}
return true;
}
- bool NDArray::operator==(const NDArrayPtr& other) const
+
+ bool
+ NDArray::operator==(const NDArrayPtr& other) const
{
if (!other)
{
@@ -78,8 +88,16 @@ namespace armarx::aron::data
return *this == *other;
}
+ NDArrayPtr
+ NDArray::clone(const Path& p) const
+ {
+ NDArrayPtr ret(new NDArray(getShape(), getType(), getDataAsVector(), p));
+ return ret;
+ }
+
// static methods
- NDArrayPtr NDArray::FromNDArrayDTO(const data::dto::NDArrayPtr& aron)
+ NDArrayPtr
+ NDArray::FromNDArrayDTO(const data::dto::NDArrayPtr& aron)
{
if (!aron)
{
@@ -88,13 +106,15 @@ namespace armarx::aron::data
return std::make_shared<NDArray>(aron);
}
- data::dto::NDArrayPtr NDArray::ToNDArrayDTO(const NDArrayPtr& variant)
+ data::dto::NDArrayPtr
+ NDArray::ToNDArrayDTO(const NDArrayPtr& variant)
{
return variant ? variant->toNDArrayDTO() : nullptr;
}
// public member functions
- DictPtr NDArray::getAsDict() const
+ DictPtr
+ NDArray::getAsDict() const
{
auto dict = std::make_shared<Dict>();
auto copy_this = FromAronDTO(toAronDTO(), getPath());
@@ -102,43 +122,51 @@ namespace armarx::aron::data
return dict;
}
- unsigned char* NDArray::getData() const
+ unsigned char*
+ NDArray::getData() const
{
return aron->data.data();
}
- void NDArray::setData(unsigned int elements, const unsigned char* src)
+ void
+ NDArray::setData(unsigned int elements, const unsigned char* src)
{
aron->data = std::vector<unsigned char>(elements);
std::memcpy(aron->data.data(), src, elements);
}
- std::vector<unsigned char> NDArray::getDataAsVector() const
+ std::vector<unsigned char>
+ NDArray::getDataAsVector() const
{
return aron->data;
}
- std::vector<int> NDArray::getShape() const
+ std::vector<int>
+ NDArray::getShape() const
{
return aron->shape;
}
- void NDArray::setShape(const std::vector<int>& d)
+ void
+ NDArray::setShape(const std::vector<int>& d)
{
aron->shape = d;
}
- void NDArray::addToShape(int i)
+ void
+ NDArray::addToShape(int i)
{
aron->shape.push_back(i);
}
- std::string NDArray::getType() const
+ std::string
+ NDArray::getType() const
{
return aron->type;
}
- void NDArray::setType(const std::string& t)
+ void
+ NDArray::setType(const std::string& t)
{
if (t.empty())
{
@@ -149,76 +177,85 @@ namespace armarx::aron::data
aron->type = t;
}
- data::dto::NDArrayPtr NDArray::toNDArrayDTO() const
+ data::dto::NDArrayPtr
+ NDArray::toNDArrayDTO() const
{
return aron;
}
// virtual implementations
- std::string NDArray::getShortName() const
+ std::string
+ NDArray::getShortName() const
{
return "NDArray";
}
- std::string NDArray::getFullName() const
+ std::string
+ NDArray::getFullName() const
{
- return "armarx::aron::data::NDArray<" + simox::alg::to_string(aron->shape, ", ") + ", " + aron->type + ">";
+ return "armarx::aron::data::NDArray<" + simox::alg::to_string(aron->shape, ", ") + ", " +
+ aron->type + ">";
}
- type::VariantPtr NDArray::recalculateType() const
+ type::VariantPtr
+ NDArray::recalculateType() const
{
ARMARX_TRACE;
throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
}
- bool NDArray::fullfillsType(const type::VariantPtr& type) const
+ bool
+ NDArray::fullfillsType(const type::VariantPtr& type) const
{
- if (!type) return true;
+ if (!type)
+ return true;
type::Descriptor typeDesc = type->getDescriptor();
switch (typeDesc)
{
- case type::Descriptor::MATRIX:
- {
- auto casted = type::Matrix::DynamicCastAndCheck(type);
- ARMARX_TRACE;
- return (aron->shape.size() == 3 && aron->shape[0] == casted->getRows() && aron->shape[1] == casted->getCols());
- }
- case type::Descriptor::QUATERNION:
- {
- auto casted = type::Quaternion::DynamicCastAndCheck(type);
- ARMARX_TRACE;
- return (aron->shape.size() == 3 && aron->shape[0] == 1 && aron->shape[1] == 4);
- }
- case type::Descriptor::POINTCLOUD:
- {
- auto casted = type::PointCloud::DynamicCastAndCheck(type);
- ARMARX_TRACE;
- return (aron->shape.size() == 3);
- }
- case type::Descriptor::IMAGE:
- {
- auto casted = type::Image::DynamicCastAndCheck(type);
- ARMARX_TRACE;
- return (aron->shape.size() == 3);
- }
- case type::Descriptor::NDARRAY:
- {
- auto casted = type::NDArray::DynamicCastAndCheck(type);
- ARMARX_TRACE;
- return (aron->shape.size() == (unsigned int) casted->getNumberDimensions());
- }
- default:
- ARMARX_TRACE;
- return false;
+ case type::Descriptor::MATRIX:
+ {
+ auto casted = type::Matrix::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
+ return (aron->shape.size() == 3 && aron->shape[0] == casted->getRows() &&
+ aron->shape[1] == casted->getCols());
+ }
+ case type::Descriptor::QUATERNION:
+ {
+ auto casted = type::Quaternion::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
+ return (aron->shape.size() == 3 && aron->shape[0] == 1 && aron->shape[1] == 4);
+ }
+ case type::Descriptor::POINTCLOUD:
+ {
+ auto casted = type::PointCloud::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
+ return (aron->shape.size() == 3);
+ }
+ case type::Descriptor::IMAGE:
+ {
+ auto casted = type::Image::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
+ return (aron->shape.size() == 3);
+ }
+ case type::Descriptor::NDARRAY:
+ {
+ auto casted = type::NDArray::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
+ return (aron->shape.size() == (unsigned int)casted->getNumberDimensions());
+ }
+ default:
+ ARMARX_TRACE;
+ return false;
}
}
- std::string NDArray::DimensionsToString(const std::vector<int>& dimensions)
+ std::string
+ NDArray::DimensionsToString(const std::vector<int>& dimensions)
{
std::stringstream ss;
ss << "(" << simox::alg::join(simox::alg::multi_to_string(dimensions), ", ") << ")";
return ss.str();
}
-} // namespace armarx::aron::datanavigator
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
index 068fd1a85cebc436ca6c1330c59c6d88ba9f183c..1cfc673719f6a4808a9f7e91bfc9b25a46cffb72 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h
@@ -26,8 +26,8 @@
// STD/STL
#include <cstddef>
#include <functional>
-#include <memory>
#include <map>
+#include <memory>
#include <numeric>
#include <vector>
@@ -40,22 +40,25 @@
// Types
#include "../../../type/variant/ndarray/All.h"
-
namespace armarx::aron::data
{
class NDArray;
using NDArrayPtr = std::shared_ptr<NDArray>;
- class NDArray :
- public detail::ComplexVariant<data::dto::NDArray, NDArray>
+ class NDArray : public detail::ComplexVariant<data::dto::NDArray, NDArray>
{
public:
// constructors
NDArray(const Path& path = Path());
NDArray(const data::dto::NDArrayPtr&, const Path& path = Path());
- NDArray(const std::vector<int>&, const std::string&, const std::vector<unsigned char>&, const Path& path = Path());
+ NDArray(const std::vector<int>&,
+ const std::string&,
+ const std::vector<unsigned char>&,
+ const Path& path = Path());
// operators
+ using detail::ComplexVariant<data::dto::NDArray, NDArray>::operator==;
+
virtual bool operator==(const NDArray&) const override;
bool operator==(const NDArrayPtr&) const override;
@@ -84,11 +87,8 @@ namespace armarx::aron::data
data::dto::NDArrayPtr toNDArrayDTO() const;
// virtual implementations
- NDArrayPtr clone() const override
- {
- NDArrayPtr ret(new NDArray(getShape(), getType(), getDataAsVector(), getPath()));
- return ret;
- }
+ using Base::clone;
+ NDArrayPtr clone(const Path& p) const override;
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
@@ -96,13 +96,14 @@ namespace armarx::aron::data
virtual type::VariantPtr recalculateType() const override;
virtual bool fullfillsType(const type::VariantPtr&) const override;
};
-}
+} // namespace armarx::aron::data
namespace armarx::aron
{
- template<typename... _Args>
- aron::data::NDArrayPtr make_ndarray(_Args&&... args)
+ template <typename... _Args>
+ aron::data::NDArrayPtr
+ make_ndarray(_Args&&... args)
{
return std::make_shared<aron::data::NDArray>(args...);
}
-}
+} // namespace armarx::aron
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 9724ae6279a2b2a2140d5964d7d101f6e9fbabc8..1d85a7f800b038befc8dedd21561c133a1f13439 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -36,12 +36,15 @@ namespace armarx::aron::data
// constructors
Dict::Dict(const Path& path) :
- detail::ContainerVariant<data::dto::Dict, Dict>::ContainerVariant(data::Descriptor::DICT, path)
+ detail::ContainerVariant<data::dto::Dict, Dict>::ContainerVariant(data::Descriptor::DICT,
+ path)
{
}
Dict::Dict(const data::dto::DictPtr& o, const Path& path) :
- detail::ContainerVariant<data::dto::Dict, Dict>::ContainerVariant(o, data::Descriptor::DICT, path)
+ detail::ContainerVariant<data::dto::Dict, Dict>::ContainerVariant(o,
+ data::Descriptor::DICT,
+ path)
{
for (const auto& [key, dataPtr] : this->aron->elements)
{
@@ -49,8 +52,7 @@ namespace armarx::aron::data
}
}
- Dict::Dict(const std::map<std::string, VariantPtr>& m, const Path& path) :
- Dict(path)
+ Dict::Dict(const std::map<std::string, VariantPtr>& m, const Path& path) : Dict(path)
{
for (const auto& [key, dataPtr] : m)
{
@@ -59,7 +61,8 @@ namespace armarx::aron::data
}
// operators
- bool Dict::operator==(const Dict& other) const
+ bool
+ Dict::operator==(const Dict& other) const
{
for (const auto& [key, nav] : childrenNavigators)
{
@@ -69,7 +72,7 @@ namespace armarx::aron::data
}
if (!nav)
{
- return !((bool) other.getElement(key));
+ return !((bool)other.getElement(key));
}
if (not(*nav == *other.getElement(key)))
{
@@ -78,7 +81,9 @@ namespace armarx::aron::data
}
return true;
}
- bool Dict::operator==(const DictPtr& other) const
+
+ bool
+ Dict::operator==(const DictPtr& other) const
{
if (!other)
{
@@ -86,19 +91,42 @@ namespace armarx::aron::data
}
return *this == *other;
}
- VariantPtr Dict::operator[](const std::string& s) const
+
+ VariantPtr
+ Dict::operator[](const std::string& s) const
{
return getElement(s);
}
- Dict& Dict::operator=(const Dict& other)
+
+ Dict&
+ Dict::operator=(const Dict& other)
{
this->aron = other.aron;
this->childrenNavigators = other.childrenNavigators;
return *this;
}
+ DictPtr
+ Dict::clone(const Path& p) const
+ {
+ DictPtr ret(new Dict(p));
+ for (const auto& [key, val] : getElements())
+ {
+ if (val)
+ {
+ ret->addElement(key, val->cloneAsVariant());
+ }
+ else
+ {
+ ret->addElement(key, nullptr);
+ }
+ }
+ return ret;
+ }
+
// static methods
- DictPtr Dict::FromAronDictDTO(const data::dto::DictPtr& aron)
+ DictPtr
+ Dict::FromAronDictDTO(const data::dto::DictPtr& aron)
{
if (!aron)
{
@@ -107,18 +135,21 @@ namespace armarx::aron::data
return std::make_shared<Dict>(aron);
}
- data::dto::DictPtr Dict::ToAronDictDTO(const DictPtr& navigator)
+ data::dto::DictPtr
+ Dict::ToAronDictDTO(const DictPtr& navigator)
{
return navigator ? navigator->toAronDictDTO() : nullptr;
}
- data::dto::DictPtr Dict::toAronDictDTO() const
+ data::dto::DictPtr
+ Dict::toAronDictDTO() const
{
return aron;
}
// public member functions
- std::vector<std::string> Dict::getAllKeys() const
+ std::vector<std::string>
+ Dict::getAllKeys() const
{
std::vector<std::string> ret;
for (const auto& [key, _] : childrenNavigators)
@@ -128,44 +159,114 @@ namespace armarx::aron::data
return ret;
}
- std::string Dict::getAllKeysAsString() const
+ std::string
+ Dict::getAllKeysAsString() const
{
return simox::alg::to_string(getAllKeys(), ", ");
}
- void Dict::addElement(const std::string& key, const VariantPtr& data)
+ void
+ Dict::addElement(const std::string& key, const VariantPtr& data)
{
+ // Please note that the data may be null (indicating that a non existing maybetype has been added)
+
if (hasElement(key))
{
ARMARX_TRACE;
- throw error::AronException(__PRETTY_FUNCTION__, "The key '"+key+"' already exists in a aron dict.");
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "The key '" + key + "' already exists in a aron dict.");
}
setElement(key, data);
}
- bool Dict::hasElement(const std::string& key) const
+ void
+ Dict::addElementCopy(const std::string& key, const VariantPtr& data)
+ {
+ // Please note that the data may be null (indicating that a non existing maybetype has been added)
+
+ if (hasElement(key))
+ {
+ ARMARX_TRACE;
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "The key '" + key + "' already exists in a aron dict.");
+ }
+ 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
{
return childrenNavigators.count(key) > 0;
}
- VariantPtr Dict::getElement(const std::string& key) const
+ VariantPtr
+ Dict::getElement(const std::string& key) const
{
auto it = childrenNavigators.find(key);
if (it == childrenNavigators.end())
{
ARMARX_TRACE;
- throw error::AronException(__PRETTY_FUNCTION__, "Could not find key '" + key + "'. But I found the following keys: [" + simox::alg::join(this->getAllKeys(), ", ") + "]", getPath());
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "Could not find key '" + key +
+ "'. But I found the following keys: [" +
+ simox::alg::join(this->getAllKeys(), ", ") + "]",
+ getPath());
}
return it->second;
}
- std::map<std::string, VariantPtr> Dict::getElements() const
+ std::map<std::string, VariantPtr>
+ Dict::getElements() const
{
return childrenNavigators;
}
- void Dict::setElement(const std::string& key, const VariantPtr& data)
+ void
+ Dict::setElement(const std::string& key, const VariantPtr& data)
{
+ // Please note that the data may be null (indicating that a non existing maybetype has been added)
+
+ if (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.";
+ }
+ }
+
this->childrenNavigators[key] = data;
if (data)
{
@@ -177,44 +278,66 @@ namespace armarx::aron::data
}
}
- void Dict::removeElement(const std::string& key)
+ void
+ Dict::setElementCopy(const std::string& key, const VariantPtr& data)
+ {
+ // Please note that the data may be null (indicating that a non existing maybetype has been added)
+
+ VariantPtr copy = nullptr;
+ if (data)
+ {
+ Path newPath = getPath().withElement(key);
+ copy = data->cloneAsVariant(newPath);
+ }
+ setElement(key, copy);
+ }
+
+ void
+ Dict::removeElement(const std::string& key)
{
childrenNavigators.erase(key);
aron->elements.erase(key);
}
- void Dict::clear()
+ void
+ Dict::clear()
{
childrenNavigators.clear();
aron->elements.clear();
}
- VariantPtr Dict::at(const std::string& s) const
+ VariantPtr
+ Dict::at(const std::string& s) const
{
return getElement(s);
}
// virtual implementations
- std::string Dict::getShortName() const
+ std::string
+ Dict::getShortName() const
{
return "Dict";
}
- std::string Dict::getFullName() const
+ std::string
+ Dict::getFullName() const
{
return "armarx::aron::data::Dict";
}
// TODO
- type::VariantPtr Dict::recalculateType() const
+ type::VariantPtr
+ Dict::recalculateType() const
{
ARMARX_TRACE;
throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
}
- bool Dict::fullfillsType(const type::VariantPtr& type) const
+ bool
+ Dict::fullfillsType(const type::VariantPtr& type) const
{
- if (!type) return true;
+ if (!type)
+ return true;
type::Descriptor typeDesc = type->getDescriptor();
@@ -224,70 +347,74 @@ 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;
+ (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;
}
}
- std::vector<VariantPtr> Dict::getChildren() const
+ std::vector<VariantPtr>
+ Dict::getChildren() const
{
std::vector<VariantPtr> ret(childrenNavigators.size());
for (const auto& [key, nav] : childrenNavigators)
@@ -297,23 +424,27 @@ namespace armarx::aron::data
return ret;
}
- size_t Dict::childrenSize() const
+ size_t
+ Dict::childrenSize() const
{
return childrenNavigators.size();
}
- VariantPtr Dict::navigateAbsolute(const Path& path) const
+ VariantPtr
+ Dict::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
{
ARMARX_TRACE;
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
}
std::string el = path.getFirstElement();
if (!hasElement(el))
{
ARMARX_TRACE;
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
}
if (path.size() == 1)
@@ -326,9 +457,12 @@ namespace armarx::aron::data
if (!childrenNavigators.at(el))
{
ARMARX_TRACE;
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "Could not navigate into a NULL member. Seems like the "
+ "member is optional and not set.",
+ next);
}
return childrenNavigators.at(el)->navigateAbsolute(next);
}
}
-}
+} // namespace armarx::aron::data
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 5006a0c97a5b830f7d0998024ce2113d38af59a5..a49add482dc4e7e6f1a1d5308d76e571785dbe73 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.h
@@ -24,25 +24,24 @@
#pragma once
// STD/STL
-#include <memory>
#include <map>
+#include <memory>
#include <utility>
// Base class
#include "../detail/ContainerVariant.h"
// ArmarX
+#include "../../../type/variant/any/AnyObject.h"
#include "../../../type/variant/container/Dict.h"
#include "../../../type/variant/container/Object.h"
-#include "../../../type/variant/any/AnyObject.h"
namespace armarx::aron::data
{
class Dict;
typedef std::shared_ptr<Dict> DictPtr;
- class Dict :
- public detail::ContainerVariant<data::dto::Dict, Dict>
+ class Dict : public detail::ContainerVariant<data::dto::Dict, Dict>
{
public:
// constructors
@@ -51,6 +50,8 @@ namespace armarx::aron::data
Dict(const std::map<std::string, VariantPtr>&, const Path& path = Path());
// operators
+ using detail::ContainerVariant<data::dto::Dict, Dict>::operator==;
+
bool operator==(const Dict&) const override;
bool operator==(const DictPtr&) const override;
VariantPtr operator[](const std::string&) const;
@@ -65,25 +66,23 @@ namespace armarx::aron::data
std::string getAllKeysAsString() const;
void addElement(const std::string& key, const VariantPtr& = nullptr);
+ void addElementCopy(const std::string& key, const VariantPtr& = nullptr);
bool hasElement(const std::string&) const;
void setElement(const std::string&, const VariantPtr& = nullptr);
+ void setElementCopy(const std::string&, const VariantPtr& = nullptr);
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();
- // virtual implementations
- DictPtr clone() const override
- {
- DictPtr ret(new Dict(getPath()));
- for (const auto& [key, val] : getElements())
- {
- ret->addElement(key, val->cloneAsVariant());
- }
- return ret;
- }
+ // virtual implementations
+ using Base::clone;
+ DictPtr clone(const Path& p) const override;
std::string getShortName() const override;
std::string getFullName() const override;
@@ -99,13 +98,14 @@ namespace armarx::aron::data
// members
std::map<std::string, VariantPtr> childrenNavigators;
};
-}
+} // namespace armarx::aron::data
namespace armarx::aron
{
- template<typename... _Args>
- aron::data::DictPtr make_dict(_Args&&... args)
+ template <typename... _Args>
+ aron::data::DictPtr
+ make_dict(_Args&&... args)
{
return std::make_shared<aron::data::Dict>(args...);
}
-}
+} // namespace armarx::aron
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 c53b51ee1b04c3e903b4ddd718b9793d75303412..82955eabfa8414f5e984c6ad0de704d7cab09a00 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
@@ -37,12 +37,15 @@ namespace armarx::aron::data
{
// constructors
List::List(const Path& path) :
- detail::ContainerVariant<data::dto::List, List>::ContainerVariant(data::Descriptor::LIST, path)
+ detail::ContainerVariant<data::dto::List, List>::ContainerVariant(data::Descriptor::LIST,
+ path)
{
}
List::List(const data::dto::ListPtr& l, const Path& path) :
- detail::ContainerVariant<data::dto::List, List>::ContainerVariant(l, data::Descriptor::LIST, path)
+ detail::ContainerVariant<data::dto::List, List>::ContainerVariant(l,
+ data::Descriptor::LIST,
+ path)
{
unsigned int i = 0;
for (const auto& dataPtr : l->elements)
@@ -51,8 +54,7 @@ namespace armarx::aron::data
}
}
- List::List(const std::vector<VariantPtr>& n, const Path& path) :
- List(path)
+ List::List(const std::vector<VariantPtr>& n, const Path& path) : List(path)
{
for (const auto& dataPtr : n)
{
@@ -61,7 +63,8 @@ namespace armarx::aron::data
}
// operators
- bool List::operator==(const List& other) const
+ bool
+ List::operator==(const List& other) const
{
unsigned int i = 0;
for (const auto& nav : childrenNavigators)
@@ -85,7 +88,9 @@ namespace armarx::aron::data
}
return true;
}
- bool List::operator==(const ListPtr& other) const
+
+ bool
+ List::operator==(const ListPtr& other) const
{
if (!other)
{
@@ -94,8 +99,20 @@ namespace armarx::aron::data
return *this == *other;
}
+ ListPtr
+ List::clone(const Path& p) const
+ {
+ ListPtr ret(new List(p));
+ for (const auto& val : getElements())
+ {
+ ret->addElement(val->cloneAsVariant());
+ }
+ return ret;
+ }
+
// static methods
- ListPtr List::FromAronListDTO(const data::dto::ListPtr& aron)
+ ListPtr
+ List::FromAronListDTO(const data::dto::ListPtr& aron)
{
if (!aron)
{
@@ -104,13 +121,15 @@ namespace armarx::aron::data
return std::make_shared<List>(aron);
}
- data::dto::ListPtr List::ToAronListDTO(const ListPtr& navigator)
+ data::dto::ListPtr
+ List::ToAronListDTO(const ListPtr& navigator)
{
return navigator ? navigator->toAronListDTO() : nullptr;
}
// public member functions
- DictPtr List::getAsDict() const
+ DictPtr
+ List::getAsDict() const
{
auto dict = std::make_shared<Dict>();
auto copy_this = FromAronDTO(toAronDTO(), getPath());
@@ -118,17 +137,22 @@ namespace armarx::aron::data
return dict;
}
- void List::addElement(const VariantPtr& n)
+ void
+ List::addElement(const VariantPtr& n)
{
setElement(aron->elements.size(), n);
}
- void List::setElement(unsigned int i, const VariantPtr& n)
+ void
+ List::setElement(unsigned int i, const VariantPtr& n)
{
if (i > aron->elements.size())
{
ARMARX_TRACE;
- throw error::AronException(__PRETTY_FUNCTION__, "Cannot set a listelement at index " + std::to_string(i) + " because this list has size " + std::to_string(aron->elements.size()));
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "Cannot set a listelement at index " + std::to_string(i) +
+ " because this list has size " +
+ std::to_string(aron->elements.size()));
}
if (i == aron->elements.size())
@@ -157,66 +181,81 @@ namespace armarx::aron::data
}
}
- bool List::hasElement(unsigned int i) const
+ bool
+ List::hasElement(unsigned int i) const
{
return i < childrenNavigators.size();
}
- VariantPtr List::getElement(unsigned int i) const
+ VariantPtr
+ List::getElement(unsigned int i) const
{
if (i >= childrenNavigators.size())
{
ARMARX_TRACE;
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The index is out of bounds (size = " + std::to_string(childrenNavigators.size()) + ")", std::to_string(i), getPath());
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "The index is out of bounds (size = " +
+ std::to_string(childrenNavigators.size()) + ")",
+ std::to_string(i),
+ getPath());
}
return childrenNavigators[i];
}
- std::vector<VariantPtr> List::getElements() const
+ std::vector<VariantPtr>
+ List::getElements() const
{
return childrenNavigators;
}
- void List::removeElement(unsigned int i)
+ void
+ List::removeElement(unsigned int i)
{
// Use with care since this function will not work in a loop with increasing indexes
- i = std::min((unsigned int) childrenNavigators.size()-1, i);
+ i = std::min((unsigned int)childrenNavigators.size() - 1, i);
childrenNavigators.erase(childrenNavigators.begin() + i);
aron->elements.erase(aron->elements.begin() + i);
}
- void List::clear()
+ void
+ List::clear()
{
childrenNavigators.clear();
aron->elements.clear();
}
- data::dto::ListPtr List::toAronListDTO() const
+ data::dto::ListPtr
+ List::toAronListDTO() const
{
return aron;
}
// virtual implementations
- std::string List::getShortName() const
+ std::string
+ List::getShortName() const
{
return "List";
}
- std::string List::getFullName() const
+ std::string
+ List::getFullName() const
{
return "armarx::aron::data::List";
}
// TODO
- type::VariantPtr List::recalculateType() const
+ type::VariantPtr
+ List::recalculateType() const
{
ARMARX_TRACE;
throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
}
- bool List::fullfillsType(const type::VariantPtr& type) const
+ bool
+ List::fullfillsType(const type::VariantPtr& type) const
{
- if (!type) return true;
+ if (!type)
+ return true;
type::Descriptor typeDesc = type->getDescriptor();
switch (typeDesc)
@@ -232,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();
@@ -292,25 +323,19 @@ 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);
+ return childrenNavigators[0]->fullfillsType(firstChildTypeNav) &&
+ childrenNavigators[1]->fullfillsType(secondChildTypeNav);
}
default:
ARMARX_TRACE;
@@ -318,27 +343,35 @@ namespace armarx::aron::data
}
}
- std::vector<VariantPtr> List::getChildren() const
+ std::vector<VariantPtr>
+ List::getChildren() const
{
return childrenNavigators;
}
- size_t List::childrenSize() const
+ size_t
+ List::childrenSize() const
{
return childrenNavigators.size();
}
- VariantPtr List::navigateAbsolute(const Path& path) const
+ VariantPtr
+ List::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path. The path was empty.");
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Could not navigate without a valid path. The path was empty.");
}
unsigned int i = std::stoi(path.getFirstElement());
if (!hasElement(i))
{
ARMARX_TRACE;
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", std::to_string(i), std::to_string(childrenSize()));
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Could not find an element of a path.",
+ std::to_string(i),
+ std::to_string(childrenSize()));
}
if (path.size() == 1)
@@ -351,9 +384,12 @@ namespace armarx::aron::data
if (!childrenNavigators.at(i))
{
ARMARX_TRACE;
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "Could not navigate into a NULL member. Seems like the "
+ "member is optional and not set.",
+ next);
}
return childrenNavigators.at(i)->navigateAbsolute(next);
}
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
index 142f42ee66940c6a5fa6d99a616d4845719234e2..df103ffd694f5e5f7773ef16e358747058eb976b 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
@@ -30,18 +30,17 @@
#include "../detail/SpecializedVariant.h"
// ArmarX
-#include "Dict.h"
#include "../../../type/variant/container/List.h"
#include "../../../type/variant/container/Pair.h"
#include "../../../type/variant/container/Tuple.h"
+#include "Dict.h"
namespace armarx::aron::data
{
class List;
typedef std::shared_ptr<List> ListPtr;
- class List :
- public detail::ContainerVariant<data::dto::List, List>
+ class List : public detail::ContainerVariant<data::dto::List, List>
{
public:
// constructors
@@ -50,6 +49,8 @@ namespace armarx::aron::data
List(const std::vector<VariantPtr>&, const Path& path = Path());
// operators
+ using detail::ContainerVariant<data::dto::List, List>::operator==;
+
bool operator==(const List&) const override;
bool operator==(const ListPtr&) const override;
@@ -73,15 +74,8 @@ namespace armarx::aron::data
void clear();
// virtual implementations
- ListPtr clone() const override
- {
- ListPtr ret(new List(getPath()));
- for (const auto& val : getElements())
- {
- ret->addElement(val->cloneAsVariant());
- }
- return ret;
- }
+ using detail::ContainerVariant<data::dto::List, List>::clone;
+ ListPtr clone(const Path& p) const override;
std::string getShortName() const override;
std::string getFullName() const override;
@@ -96,13 +90,14 @@ namespace armarx::aron::data
private:
std::vector<VariantPtr> childrenNavigators;
};
-}
+} // namespace armarx::aron::data
namespace armarx::aron
{
- template<typename... _Args>
- aron::data::ListPtr make_list(_Args&&... args)
+ template <typename... _Args>
+ aron::data::ListPtr
+ make_list(_Args&&... args)
{
return std::make_shared<aron::data::List>(args...);
}
-}
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/ComplexVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/ComplexVariant.h
index 868b7a8df691a47458d1f8a73b319034627cbfb7..afececc9372a6b5494ac2d686594b553b4752233 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/ComplexVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/ComplexVariant.h
@@ -33,29 +33,36 @@
namespace armarx::aron::data::detail
{
- template<typename AronDataT, typename DerivedT>
- class ComplexVariant :
- public SpecializedVariantBase<AronDataT, DerivedT>
+ template <typename AronDataT, typename DerivedT>
+ class ComplexVariant : public SpecializedVariantBase<AronDataT, DerivedT>
{
public:
- using SpecializedVariantBase<AronDataT, DerivedT>::SpecializedVariantBase;
+ using Base = SpecializedVariantBase<AronDataT, DerivedT>;
+ using Base::SpecializedVariantBase;
virtual ~ComplexVariant() = default;
// virtual implementations
- VariantPtr navigateAbsolute(const Path &path) const override
+ VariantPtr
+ navigateAbsolute(const Path& path) const override
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through a non container navigator. The input path was: " + path.toString(), Variant::getPath());
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Could not navigate through a non container navigator. The input path was: " +
+ path.toString(),
+ Variant::getPath());
}
- std::vector<VariantPtr> getChildren() const override
+ std::vector<VariantPtr>
+ getChildren() const override
{
return {};
}
- size_t childrenSize() const override
+ size_t
+ childrenSize() const override
{
return 0;
}
};
-}
+} // namespace armarx::aron::data::detail
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/ContainerVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/ContainerVariant.h
index 69fff7c78e637d961eebad2253327be056f4b7a0..042ef1227918ce2930c1df755958633daf8c26fa 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/ContainerVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/ContainerVariant.h
@@ -33,13 +33,13 @@
namespace armarx::aron::data::detail
{
- template<typename AronDataT, typename DerivedT>
- class ContainerVariant :
- public SpecializedVariantBase<AronDataT, DerivedT>
+ template <typename AronDataT, typename DerivedT>
+ class ContainerVariant : public SpecializedVariantBase<AronDataT, DerivedT>
{
public:
- using SpecializedVariantBase<AronDataT, DerivedT>::SpecializedVariantBase;
+ using Base = SpecializedVariantBase<AronDataT, DerivedT>;
+ using Base::SpecializedVariantBase;
virtual ~ContainerVariant() = default;
};
-}
+} // namespace armarx::aron::data::detail
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 3b36c6310386a27122ae7607e41a137fc6420833..b5444266d3a2bfbc09e2cc151755eb90402e511a 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
@@ -36,19 +36,21 @@
namespace armarx::aron::data::detail
{
- template<typename AronDataT, typename ValueT, typename DerivedT>
- class PrimitiveVariant :
- public SpecializedVariantBase<AronDataT, DerivedT>
+ template <typename AronDataT, typename ValueT, typename DerivedT>
+ class PrimitiveVariant : public SpecializedVariantBase<AronDataT, DerivedT>
{
public:
+ using Base = SpecializedVariantBase<AronDataT, DerivedT>;
using PointerType = std::shared_ptr<DerivedT>;
using ValueType = ValueT;
public:
- using SpecializedVariantBase<AronDataT, DerivedT>::SpecializedVariantBase;
+ using typename Base::SpecializedVariantBase;
- PrimitiveVariant(const ValueT& v, const data::Descriptor descriptor, const Path& path = Path()):
- SpecializedVariantBase<AronDataT, DerivedT>(descriptor, path)
+ PrimitiveVariant(const ValueT& v,
+ const data::Descriptor descriptor,
+ const Path& path = Path()) :
+ Base(descriptor, path)
{
this->aron->value = v;
}
@@ -60,7 +62,8 @@ namespace armarx::aron::data::detail
return this->aron->value;
}
- DerivedT& operator=(const ValueT& x)
+ DerivedT&
+ operator=(const ValueT& x)
{
this->aron->value = x;
return *this;
@@ -76,23 +79,33 @@ namespace armarx::aron::data::detail
virtual void fromString(const std::string& setter) = 0;
// virtual implementations
- VariantPtr navigateAbsolute(const Path &path) const override
+ VariantPtr
+ navigateAbsolute(const Path& path) const override
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through a non container navigator. The input path was: " + path.toString(), Variant::getPath());
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Could not navigate through a non container navigator. The input path was: " +
+ path.toString(),
+ Variant::getPath());
}
- PointerType clone() const override
+ using Base::clone;
+
+ PointerType
+ clone(const Path& newPath) const override
{
- PointerType ret(new DerivedT(getValue(), this->getPath()));
+ PointerType ret(new DerivedT(getValue(), newPath));
return ret;
}
- std::vector<VariantPtr> getChildren() const override
+ std::vector<VariantPtr>
+ getChildren() const override
{
return {};
}
- size_t childrenSize() const override
+ size_t
+ childrenSize() const override
{
return 0;
}
@@ -100,7 +113,8 @@ namespace armarx::aron::data::detail
// static methods
/* public member functions */
- DictPtr getAsDict() const
+ DictPtr
+ getAsDict() const
{
auto dict = std::make_shared<Dict>();
auto copy_this = FromAronDTO(this->toAronDTO(), this->getPath());
@@ -108,18 +122,22 @@ namespace armarx::aron::data::detail
return dict;
}
- void setValue(const ValueT& x)
+ void
+ setValue(const ValueT& x)
{
this->aron->value = x;
}
- ValueT getValue() const
+
+ ValueT
+ getValue() const
{
return this->aron->value;
}
- ValueT& getValue()
+ ValueT&
+ getValue()
{
return this->aron->value;
}
};
-}
+} // namespace armarx::aron::data::detail
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
index d7e6e4a55cd9e96fed989b8a1a4f60e6a1c5d8b2..031d58ae16d06a2cdfefcfa180f36d753bb5030c 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
@@ -35,11 +35,11 @@
namespace armarx::aron::data::detail
{
- template<typename AronDataT, typename DerivedT>
- class SpecializedVariantBase :
- public data::Variant
+ template <typename AronDataT, typename DerivedT>
+ class SpecializedVariantBase : public data::Variant
{
public:
+ using Base = data::Variant;
using PointerType = std::shared_ptr<DerivedT>;
using AronDataType = AronDataT;
@@ -47,15 +47,14 @@ namespace armarx::aron::data::detail
SpecializedVariantBase() = delete;
SpecializedVariantBase(const data::Descriptor descriptor, const Path& path = Path()) :
- Variant(descriptor, path),
- aron(new AronDataType())
+ Variant(descriptor, path), aron(new AronDataType())
{
- aron->VERSION = aron::VERSION;
}
- SpecializedVariantBase(const typename AronDataType::PointerType& o, const data::Descriptor descriptor, const Path& path = Path()) :
- Variant(descriptor, path),
- aron(o)
+ SpecializedVariantBase(const typename AronDataType::PointerType& o,
+ const data::Descriptor descriptor,
+ const Path& path = Path()) :
+ Variant(descriptor, path), aron(o)
{
ARMARX_CHECK_NOT_NULL(aron);
}
@@ -68,7 +67,8 @@ namespace armarx::aron::data::detail
return aron;
}
- bool operator==(const Variant& other) const override
+ bool
+ operator==(const Variant& other) const override
{
const auto& n = DerivedT::DynamicCast(other);
return *this == n;
@@ -78,43 +78,61 @@ namespace armarx::aron::data::detail
virtual bool operator==(const PointerType& other) const = 0;
// new interface methods
- virtual PointerType clone() const = 0;
+ virtual PointerType
+ clone() const
+ {
+ return clone(getPath());
+ }
+
+ virtual PointerType clone(const Path& newPath) const = 0;
// virtual implementations
- VariantPtr cloneAsVariant() const override
+ VariantPtr
+ cloneAsVariant() const override
{
return clone();
}
- data::dto::GenericDataPtr toAronDTO() const override
+ VariantPtr
+ cloneAsVariant(const Path& newPath) const override
+ {
+ return clone(newPath);
+ }
+
+ data::dto::GenericDataPtr
+ toAronDTO() const override
{
return aron;
}
- VariantPtr navigateRelative(const Path& path) const override
+ VariantPtr
+ navigateRelative(const Path& path) const override
{
Path absoluteFromHere = path.getWithoutPrefix(getPath());
return navigateAbsolute(absoluteFromHere);
}
-
// static methods
- static PointerType DynamicCast(const VariantPtr& n)
+ static PointerType
+ DynamicCast(const VariantPtr& n)
{
return std::dynamic_pointer_cast<DerivedT>(n);
}
- static DerivedT& DynamicCast(Variant& n)
+ static DerivedT&
+ DynamicCast(Variant& n)
{
return dynamic_cast<DerivedT&>(n);
}
- static const DerivedT& DynamicCast(const Variant& n)
+ static const DerivedT&
+ DynamicCast(const Variant& n)
{
return dynamic_cast<const DerivedT&>(n);
}
- static PointerType DynamicCastAndCheck(const VariantPtr& n)
+ static PointerType
+ DynamicCastAndCheck(const VariantPtr& n)
{
if (!n)
{
@@ -122,13 +140,17 @@ namespace armarx::aron::data::detail
}
PointerType casted = DerivedT::DynamicCast(n);
- ARMARX_CHECK_NOT_NULL(casted) << "The path was: " << n->getPath().toString() << ".\n"
- << "It has the descriptor: '" << data::defaultconversion::string::Descriptor2String.at(n->getDescriptor()) << "'.\n"
- << "And the typeid: " << typeid(n).name() << ". Tried to cast to " << typeid(PointerType).name();
+ ARMARX_CHECK_NOT_NULL(casted)
+ << "The path was: " << n->getPath().toString() << ".\n"
+ << "It has the descriptor: '"
+ << data::defaultconversion::string::Descriptor2String.at(n->getDescriptor())
+ << "'.\n"
+ << "And the typeid: " << typeid(n).name() << ". Tried to cast to "
+ << typeid(PointerType).name();
return casted;
}
protected:
typename AronDataType::PointerType aron;
};
-}
+} // namespace armarx::aron::data::detail
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
index 2f7fe6fc8e486c43c32875c1cf072e721146c402..ed29f88c343ba218155b7510c28e496882349fe2 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
@@ -27,22 +27,31 @@ namespace armarx::aron::data
{
/* constructors */
Bool::Bool(const data::dto::AronBoolPtr& o, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>::PrimitiveVariant(o, data::Descriptor::BOOL, path)
+ detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>::PrimitiveVariant(
+ o,
+ data::Descriptor::BOOL,
+ path)
{
}
Bool::Bool(const Path& path) :
- detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>::PrimitiveVariant(data::Descriptor::BOOL, path)
+ detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>::PrimitiveVariant(
+ data::Descriptor::BOOL,
+ path)
{
}
Bool::Bool(const bool d, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>::PrimitiveVariant(d, data::Descriptor::BOOL, path)
+ detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>::PrimitiveVariant(
+ d,
+ data::Descriptor::BOOL,
+ path)
{
}
/* operators */
- bool Bool::operator==(const Bool& other) const
+ bool
+ Bool::operator==(const Bool& other) const
{
const auto& otherAron = other.toAronBoolDTO();
if (aron->value != otherAron->value)
@@ -51,7 +60,9 @@ namespace armarx::aron::data
}
return true;
}
- bool Bool::operator==(const BoolPtr& other) const
+
+ bool
+ Bool::operator==(const BoolPtr& other) const
{
if (!other)
{
@@ -60,9 +71,9 @@ namespace armarx::aron::data
return *this == *other;
}
-
/* static methods */
- BoolPtr Bool::FromAronBoolDTO(const data::dto::AronBoolPtr& aron)
+ BoolPtr
+ Bool::FromAronBoolDTO(const data::dto::AronBoolPtr& aron)
{
if (!aron)
{
@@ -71,19 +82,22 @@ namespace armarx::aron::data
return std::make_shared<Bool>(aron);
}
- data::dto::AronBoolPtr Bool::ToAronBoolDTO(const BoolPtr& navigator)
+ data::dto::AronBoolPtr
+ Bool::ToAronBoolDTO(const BoolPtr& navigator)
{
return navigator ? navigator->toAronBoolDTO() : nullptr;
}
/* public member functions */
- data::dto::AronBoolPtr Bool::toAronBoolDTO() const
+ data::dto::AronBoolPtr
+ Bool::toAronBoolDTO() const
{
return aron;
}
/* virtual implementations */
- void Bool::fromString(const std::string& setter)
+ void
+ Bool::fromString(const std::string& setter)
{
if (setter == "true" || setter == "1" || setter == "yes")
{
@@ -96,27 +110,36 @@ namespace armarx::aron::data
else
{
ARMARX_TRACE;
- throw error::AronException(__PRETTY_FUNCTION__, "Could not set from string. Got: '" + setter + "'");
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "Could not set from string. Got: '" + setter + "'");
}
}
- std::string Bool::getShortName() const
+ std::string
+ Bool::getShortName() const
{
return "Bool";
}
- std::string Bool::getFullName() const
+
+ std::string
+ Bool::getFullName() const
{
return "armarx::aron::data::Bool";
}
- bool Bool::fullfillsType(const type::VariantPtr& type) const
+ bool
+ Bool::fullfillsType(const type::VariantPtr& type) const
{
- if (!type) return true;
+ if (!type)
+ return true;
return type->getDescriptor() == type::Descriptor::BOOL;
}
- type::VariantPtr Bool::recalculateType() const
+ type::VariantPtr
+ Bool::recalculateType() const
{
- return std::make_shared<type::Bool>(getPath());
+ ARMARX_TRACE;
+ throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
+ //return std::make_shared<type::Bool>(getPath());
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
index 5e07b8a8e33577b268ea8ae8366291664b3012e6..94349fb2c608936b6e395a1af2090713e8372148 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
@@ -48,6 +48,8 @@ namespace armarx::aron::data
Bool(const bool, const Path& = Path());
// operators
+ using detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>::operator==;
+
bool operator==(const Bool& other) const override;
bool operator==(const BoolPtr&) const override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
index 87ea9eeacbabc0356f13476714860756424f457d..66b23d4e11892841f17fbb520a61b80e54a498e2 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
@@ -31,22 +31,31 @@ namespace armarx::aron::data
{
/* constructors */
Double::Double(const data::dto::AronDoublePtr& o, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronDouble, double, Double>::PrimitiveVariant(o, data::Descriptor::DOUBLE, path)
+ detail::PrimitiveVariant<data::dto::AronDouble, double, Double>::PrimitiveVariant(
+ o,
+ data::Descriptor::DOUBLE,
+ path)
{
}
Double::Double(const Path& path) :
- detail::PrimitiveVariant<data::dto::AronDouble, double, Double>::PrimitiveVariant(data::Descriptor::DOUBLE, path)
+ detail::PrimitiveVariant<data::dto::AronDouble, double, Double>::PrimitiveVariant(
+ data::Descriptor::DOUBLE,
+ path)
{
}
Double::Double(const double d, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronDouble, double, Double>::PrimitiveVariant(d, data::Descriptor::DOUBLE, path)
+ detail::PrimitiveVariant<data::dto::AronDouble, double, Double>::PrimitiveVariant(
+ d,
+ data::Descriptor::DOUBLE,
+ path)
{
}
/* operators */
- bool Double::operator==(const Double& other) const
+ bool
+ Double::operator==(const Double& other) const
{
const auto& otherAron = other.toAronDoubleDTO();
if (this->aron->value != otherAron->value)
@@ -55,7 +64,9 @@ namespace armarx::aron::data
}
return true;
}
- bool Double::operator==(const DoublePtr& other) const
+
+ bool
+ Double::operator==(const DoublePtr& other) const
{
if (!other)
{
@@ -65,7 +76,8 @@ namespace armarx::aron::data
}
/* static methods */
- DoublePtr Double::FromAronDoubleDTO(const data::dto::AronDoublePtr& aron)
+ DoublePtr
+ Double::FromAronDoubleDTO(const data::dto::AronDoublePtr& aron)
{
if (!aron)
{
@@ -74,41 +86,51 @@ namespace armarx::aron::data
return std::make_shared<Double>(aron);
}
- data::dto::AronDoublePtr Double::ToAronDoubleDTO(const DoublePtr& navigator)
+ data::dto::AronDoublePtr
+ Double::ToAronDoubleDTO(const DoublePtr& navigator)
{
return navigator ? navigator->toAronDoubleDTO() : nullptr;
}
-
/* public member functions */
- data::dto::AronDoublePtr Double::toAronDoubleDTO() const
+ data::dto::AronDoublePtr
+ Double::toAronDoubleDTO() const
{
return aron;
}
/* virtual implementations */
- void Double::fromString(const std::string& setter)
+ void
+ Double::fromString(const std::string& setter)
{
setValue(std::stod(setter));
}
- std::string Double::getShortName() const
+ std::string
+ Double::getShortName() const
{
return "Double";
}
- std::string Double::getFullName() const
+
+ std::string
+ Double::getFullName() const
{
return "armarx::aron::data::Double";
}
- bool Double::fullfillsType(const type::VariantPtr& type) const
+ bool
+ Double::fullfillsType(const type::VariantPtr& type) const
{
- if (!type) return true;
+ if (!type)
+ return true;
return type->getDescriptor() == type::Descriptor::DOUBLE;
}
- type::VariantPtr Double::recalculateType() const
+ type::VariantPtr
+ Double::recalculateType() const
{
- return std::make_shared<type::Double>(getPath());
+ ARMARX_TRACE;
+ throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
+ //return std::make_shared<type::Double>(getPath());
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
index fb7a6880a9782397239243d38939d1ac22688fd4..59447e7def921da7face8213f24372bf5d5865df 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
@@ -48,6 +48,8 @@ namespace armarx::aron::data
Double(const double, const Path& = Path());
/* operators */
+ using detail::PrimitiveVariant<data::dto::AronDouble, double, Double>::operator==;
+
bool operator==(const Double&) const override;
bool operator==(const DoublePtr&) const override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
index 8dd7721c4adf5e5e0ec86b04325ea6971b8413ad..26d08b4b50d88fae2045b9b1bcbcb6ea3fb237de 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
@@ -27,22 +27,31 @@ namespace armarx::aron::data
{
/* constructors */
Float::Float(const data::dto::AronFloatPtr& o, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronFloat, float, Float>::PrimitiveVariant(o, data::Descriptor::FLOAT, path)
+ detail::PrimitiveVariant<data::dto::AronFloat, float, Float>::PrimitiveVariant(
+ o,
+ data::Descriptor::FLOAT,
+ path)
{
}
Float::Float(const Path& path) :
- detail::PrimitiveVariant<data::dto::AronFloat, float, Float>::PrimitiveVariant(data::Descriptor::FLOAT, path)
+ detail::PrimitiveVariant<data::dto::AronFloat, float, Float>::PrimitiveVariant(
+ data::Descriptor::FLOAT,
+ path)
{
}
Float::Float(const float d, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronFloat, float, Float>::PrimitiveVariant(d, data::Descriptor::FLOAT, path)
+ detail::PrimitiveVariant<data::dto::AronFloat, float, Float>::PrimitiveVariant(
+ d,
+ data::Descriptor::FLOAT,
+ path)
{
}
/* operators */
- bool Float::operator==(const Float& other) const
+ bool
+ Float::operator==(const Float& other) const
{
const auto& otherAron = other.toAronFloatDTO();
if (this->aron->value != otherAron->value)
@@ -51,7 +60,9 @@ namespace armarx::aron::data
}
return true;
}
- bool Float::operator==(const FloatPtr& other) const
+
+ bool
+ Float::operator==(const FloatPtr& other) const
{
if (!other)
{
@@ -60,9 +71,9 @@ namespace armarx::aron::data
return *this == *other;
}
-
/* static methods */
- FloatPtr Float::FromAronFloatDTO(const data::dto::AronFloatPtr& aron)
+ FloatPtr
+ Float::FromAronFloatDTO(const data::dto::AronFloatPtr& aron)
{
if (!aron)
{
@@ -71,41 +82,51 @@ namespace armarx::aron::data
return std::make_shared<Float>(aron);
}
- data::dto::AronFloatPtr Float::ToAronFloatDTO(const FloatPtr& navigator)
+ data::dto::AronFloatPtr
+ Float::ToAronFloatDTO(const FloatPtr& navigator)
{
return navigator ? navigator->toAronFloatDTO() : nullptr;
}
/* public member functions */
- data::dto::AronFloatPtr Float::toAronFloatDTO() const
+ data::dto::AronFloatPtr
+ Float::toAronFloatDTO() const
{
return aron;
}
-
/* virtual implementations */
- void Float::fromString(const std::string& setter)
+ void
+ Float::fromString(const std::string& setter)
{
setValue(std::stof(setter));
}
- std::string Float::getShortName() const
+ std::string
+ Float::getShortName() const
{
return "Float";
}
- std::string Float::getFullName() const
+
+ std::string
+ Float::getFullName() const
{
return "armarx::aron::data::Float";
}
- bool Float::fullfillsType(const type::VariantPtr& type) const
+ bool
+ Float::fullfillsType(const type::VariantPtr& type) const
{
- if (!type) return true;
+ if (!type)
+ return true;
return type->getDescriptor() == type::Descriptor::FLOAT;
}
- type::VariantPtr Float::recalculateType() const
+ type::VariantPtr
+ Float::recalculateType() const
{
- return std::make_shared<type::Float>(getPath());
+ ARMARX_TRACE;
+ throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
+ //return std::make_shared<type::Float>(getPath());
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
index 2d0cecb7e609df5496da3e616ffcd0fe6e249800..96f84a14c7978ab6675661c6d5fab261e0ad3b92 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
@@ -48,6 +48,8 @@ namespace armarx::aron::data
Float(const float, const Path& = Path());
/* operators */
+ using detail::PrimitiveVariant<data::dto::AronFloat, float, Float>::operator==;
+
bool operator==(const Float&) const override;
bool operator==(const FloatPtr&) const override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
index cc4d9dc2ec4285f3791e4f45752d578b6a304408..870cbcd22877a82237e143bc79bf1528e64d35de 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
@@ -30,22 +30,31 @@ namespace armarx::aron::data
{
/* constructors */
Int::Int(const data::dto::AronIntPtr& o, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronInt, int, Int>::PrimitiveVariant(o, data::Descriptor::INT, path)
+ detail::PrimitiveVariant<data::dto::AronInt, int, Int>::PrimitiveVariant(
+ o,
+ data::Descriptor::INT,
+ path)
{
}
Int::Int(const Path& path) :
- detail::PrimitiveVariant<data::dto::AronInt, int, Int>::PrimitiveVariant(data::Descriptor::INT, path)
+ detail::PrimitiveVariant<data::dto::AronInt, int, Int>::PrimitiveVariant(
+ data::Descriptor::INT,
+ path)
{
}
Int::Int(const int d, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronInt, int, Int>::PrimitiveVariant(d, data::Descriptor::INT, path)
+ detail::PrimitiveVariant<data::dto::AronInt, int, Int>::PrimitiveVariant(
+ d,
+ data::Descriptor::INT,
+ path)
{
}
/* operators */
- bool Int::operator==(const Int& other) const
+ bool
+ Int::operator==(const Int& other) const
{
const auto& otherAron = other.toAronIntDTO();
if (this->aron->value != otherAron->value)
@@ -54,7 +63,9 @@ namespace armarx::aron::data
}
return true;
}
- bool Int::operator==(const IntPtr& other) const
+
+ bool
+ Int::operator==(const IntPtr& other) const
{
if (!other)
{
@@ -63,9 +74,9 @@ namespace armarx::aron::data
return *this == *other;
}
-
/* static methods */
- IntPtr Int::FromAronIntDTO(const data::dto::AronIntPtr& aron)
+ IntPtr
+ Int::FromAronIntDTO(const data::dto::AronIntPtr& aron)
{
if (!aron)
{
@@ -74,41 +85,52 @@ namespace armarx::aron::data
return std::make_shared<Int>(aron);
}
- data::dto::AronIntPtr Int::ToAronIntDTO(const IntPtr& navigator)
+ data::dto::AronIntPtr
+ Int::ToAronIntDTO(const IntPtr& navigator)
{
return navigator ? navigator->toAronIntDTO() : nullptr;
}
/* public member functions */
- data::dto::AronIntPtr Int::toAronIntDTO() const
+ data::dto::AronIntPtr
+ Int::toAronIntDTO() const
{
return aron;
}
-
/* virtual implementations */
- void Int::fromString(const std::string& setter)
+ void
+ Int::fromString(const std::string& setter)
{
setValue(std::stoi(setter));
}
- std::string Int::getShortName() const
+ std::string
+ Int::getShortName() const
{
return "Int";
}
- std::string Int::getFullName() const
+
+ std::string
+ Int::getFullName() const
{
return "armarx::aron::data::Int";
}
- bool Int::fullfillsType(const type::VariantPtr& type) const
+ bool
+ Int::fullfillsType(const type::VariantPtr& type) const
{
- if (!type) return true;
- return type->getDescriptor() == type::Descriptor::INT || type->getDescriptor() == type::Descriptor::INT_ENUM;
+ if (!type)
+ return true;
+ return type->getDescriptor() == type::Descriptor::INT ||
+ type->getDescriptor() == type::Descriptor::INT_ENUM;
}
- type::VariantPtr Int::recalculateType() const
+ type::VariantPtr
+ Int::recalculateType() const
{
- return std::make_shared<type::Int>(getPath());
+ ARMARX_TRACE;
+ throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
+ //return std::make_shared<type::Int>(getPath());
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
index 5ee4a270f60bd2b21d8e2257c814f5dab8a0b759..738496f71bc51b4f0bdca92f57405a84a0dc2d22 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
@@ -49,6 +49,8 @@ namespace armarx::aron::data
Int(const int, const Path& = Path());
/* operators */
+ using detail::PrimitiveVariant<data::dto::AronInt, int, Int>::operator==;
+
bool operator==(const Int&) const override;
bool operator==(const IntPtr&) const override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
index 6a271cdc821d275c8a9afec1b65f73436010ad2f..2f62027e1ca65333d48447c4490932ecc032b543 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
@@ -31,22 +31,31 @@ namespace armarx::aron::data
{
/* constructors */
Long::Long(const data::dto::AronLongPtr& o, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronLong, long, Long>::PrimitiveVariant(o, data::Descriptor::LONG, path)
+ detail::PrimitiveVariant<data::dto::AronLong, long, Long>::PrimitiveVariant(
+ o,
+ data::Descriptor::LONG,
+ path)
{
}
Long::Long(const Path& path) :
- detail::PrimitiveVariant<data::dto::AronLong, long, Long>::PrimitiveVariant(data::Descriptor::LONG, path)
+ detail::PrimitiveVariant<data::dto::AronLong, long, Long>::PrimitiveVariant(
+ data::Descriptor::LONG,
+ path)
{
}
Long::Long(const long d, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronLong, long, Long>::PrimitiveVariant(d, data::Descriptor::LONG, path)
+ detail::PrimitiveVariant<data::dto::AronLong, long, Long>::PrimitiveVariant(
+ d,
+ data::Descriptor::LONG,
+ path)
{
}
/* operators */
- bool Long::operator==(const Long& other) const
+ bool
+ Long::operator==(const Long& other) const
{
const auto& otherAron = other.toAronLongDTO();
if (this->aron->value != otherAron->value)
@@ -55,7 +64,9 @@ namespace armarx::aron::data
}
return true;
}
- bool Long::operator==(const LongPtr& other) const
+
+ bool
+ Long::operator==(const LongPtr& other) const
{
if (!other)
{
@@ -64,9 +75,9 @@ namespace armarx::aron::data
return *this == *other;
}
-
/* static methods */
- LongPtr Long::FromAronLongDTO(const data::dto::AronLongPtr& aron)
+ LongPtr
+ Long::FromAronLongDTO(const data::dto::AronLongPtr& aron)
{
if (!aron)
{
@@ -75,41 +86,51 @@ namespace armarx::aron::data
return std::make_shared<Long>(aron);
}
- data::dto::AronLongPtr Long::ToAronLongDTO(const LongPtr& navigator)
+ data::dto::AronLongPtr
+ Long::ToAronLongDTO(const LongPtr& navigator)
{
return navigator ? navigator->toAronLongDTO() : nullptr;
}
/* public member functions */
- data::dto::AronLongPtr Long::toAronLongDTO() const
+ data::dto::AronLongPtr
+ Long::toAronLongDTO() const
{
return aron;
}
-
/* virtual implementations */
- void Long::fromString(const std::string& setter)
+ void
+ Long::fromString(const std::string& setter)
{
setValue(std::stol(setter));
}
- std::string Long::getShortName() const
+ std::string
+ Long::getShortName() const
{
return "Long";
}
- std::string Long::getFullName() const
+
+ std::string
+ Long::getFullName() const
{
return "armarx::aron::data::Long";
}
- bool Long::fullfillsType(const type::VariantPtr& type) const
+ bool
+ Long::fullfillsType(const type::VariantPtr& type) const
{
- if (!type) return true;
+ if (!type)
+ return true;
return type->getDescriptor() == type::Descriptor::LONG;
}
- type::VariantPtr Long::recalculateType() const
+ type::VariantPtr
+ Long::recalculateType() const
{
- return std::make_shared<type::Long>(getPath());
+ ARMARX_TRACE;
+ throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
+ //return std::make_shared<type::Long>(getPath());
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
index 97e3496c6b984ededccbc19422ecad3b5c65f17b..c28b603aa93f3b23750a310e84e9bcc6a9e7d501 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
@@ -48,6 +48,8 @@ namespace armarx::aron::data
Long(const long, const Path& = Path());
/* operators */
+ using detail::PrimitiveVariant<data::dto::AronLong, long, Long>::operator==;
+
bool operator==(const Long&) const override;
bool operator==(const LongPtr&) const override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
index 6415245f36a08faf2b02364ff4a298d33a4c9ac9..9f765ed209f40a84e6554eebb26e1671d1a04e8b 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
@@ -27,22 +27,31 @@ namespace armarx::aron::data
{
/* constructors */
String::String(const data::dto::AronStringPtr& o, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronString, std::string, String>::PrimitiveVariant(o, data::Descriptor::STRING, path)
+ detail::PrimitiveVariant<data::dto::AronString, std::string, String>::PrimitiveVariant(
+ o,
+ data::Descriptor::STRING,
+ path)
{
}
String::String(const Path& path) :
- detail::PrimitiveVariant<data::dto::AronString, std::string, String>::PrimitiveVariant(data::Descriptor::STRING, path)
+ detail::PrimitiveVariant<data::dto::AronString, std::string, String>::PrimitiveVariant(
+ data::Descriptor::STRING,
+ path)
{
}
String::String(const std::string& d, const Path& path) :
- detail::PrimitiveVariant<data::dto::AronString, std::string, String>::PrimitiveVariant(d, data::Descriptor::STRING, path)
+ detail::PrimitiveVariant<data::dto::AronString, std::string, String>::PrimitiveVariant(
+ d,
+ data::Descriptor::STRING,
+ path)
{
}
/* operators */
- bool String::operator==(const String& other) const
+ bool
+ String::operator==(const String& other) const
{
const auto& otherAron = other.toAronStringDTO();
if (this->aron->value != otherAron->value)
@@ -51,7 +60,9 @@ namespace armarx::aron::data
}
return true;
}
- bool String::operator==(const StringPtr& other) const
+
+ bool
+ String::operator==(const StringPtr& other) const
{
if (!other)
{
@@ -60,9 +71,9 @@ namespace armarx::aron::data
return *this == *other;
}
-
/* static methods */
- StringPtr String::FromAronStringDTO(const data::dto::AronStringPtr& aron)
+ StringPtr
+ String::FromAronStringDTO(const data::dto::AronStringPtr& aron)
{
if (!aron)
{
@@ -71,41 +82,51 @@ namespace armarx::aron::data
return std::make_shared<String>(aron);
}
- data::dto::AronStringPtr String::ToAronStringDTO(const StringPtr& navigator)
+ data::dto::AronStringPtr
+ String::ToAronStringDTO(const StringPtr& navigator)
{
return navigator ? navigator->toAronStringDTO() : nullptr;
}
/* public member functions */
- data::dto::AronStringPtr String::toAronStringDTO() const
+ data::dto::AronStringPtr
+ String::toAronStringDTO() const
{
return aron;
}
-
/* virtual implementations */
- void String::fromString(const std::string& setter)
+ void
+ String::fromString(const std::string& setter)
{
setValue(setter);
}
- std::string String::getShortName() const
+ std::string
+ String::getShortName() const
{
return "String";
}
- std::string String::getFullName() const
+
+ std::string
+ String::getFullName() const
{
return "armarx::aron::data::String";
}
- bool String::fullfillsType(const type::VariantPtr& type) const
+ bool
+ String::fullfillsType(const type::VariantPtr& type) const
{
- if (!type) return true;
+ if (!type)
+ return true;
return type->getDescriptor() == type::Descriptor::STRING;
}
- type::VariantPtr String::recalculateType() const
+ type::VariantPtr
+ String::recalculateType() const
{
- return std::make_shared<type::String>(getPath());
+ ARMARX_TRACE;
+ throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
+ //return std::make_shared<type::String>(getPath());
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
index 21c23698c114262b3d71658288cef82e12e4747f..def1c80315934c55073314483fcda063fbd50e76 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
@@ -48,6 +48,7 @@ namespace armarx::aron::data
String(const std::string&, const Path& = Path());
/* operators */
+ using detail::PrimitiveVariant<data::dto::AronString, std::string, String>::operator==;
bool operator==(const String&) const override;
bool operator==(const StringPtr&) const override;
diff --git a/source/RobotAPI/libraries/aron/core/rw.h b/source/RobotAPI/libraries/aron/core/rw.h
index 95b6aac606f47d3c46de34ac5ec077ded28c41a4..7f0f69b5556b0e3c33b998a1e47457789865fef4 100644
--- a/source/RobotAPI/libraries/aron/core/rw.h
+++ b/source/RobotAPI/libraries/aron/core/rw.h
@@ -6,28 +6,40 @@
#include <vector>
#include "aron_conversions.h"
+#include "codegeneration/cpp/AronGeneratedClass.h"
#include "data/rw/Reader.h"
#include "data/rw/Writer.h"
namespace armarx::aron
{
- template<class ReaderT, class T>
- requires (data::isReader<ReaderT>)
- inline void read(ReaderT& aron_r, typename ReaderT::InputType& input, T& ret)
+ template <class ReaderT, class T>
+
+ requires(data::isReader<ReaderT>&& armarx::aron::cpp::isAronGeneratedClass<T>) inline void read(
+ ReaderT& aron_r,
+ typename ReaderT::InputType& input,
+ T& ret)
{
ret.read(aron_r, input);
}
- template<class WriterT, class T>
- requires (data::isWriter<WriterT>)
- inline void write(WriterT& aron_w, const T& input, typename WriterT::ReturnType& ret, const armarx::aron::Path& aron_p = armarx::aron::Path())
+ template <class WriterT, class T>
+
+ requires(data::isWriter<WriterT>&& armarx::aron::cpp::isAronGeneratedClass<
+ T>) inline void write(WriterT& aron_w,
+ const T& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p = armarx::aron::Path())
{
ret = input.write(aron_w, aron_p);
}
- template<class ReaderT, class DtoT, class BoT>
- requires (data::isReader<ReaderT> && !detail::DtoAndBoAreSame<DtoT, BoT>)
- inline void read(ReaderT& aron_r, typename ReaderT::InputType& input, BoT& ret)
+ template <class ReaderT, class DtoT, class BoT>
+
+ requires(
+ data::isReader<ReaderT>&& armarx::aron::cpp::isAronGeneratedClass<DtoT> &&
+ !detail::DtoAndBoAreSame<DtoT, BoT>) inline void read(ReaderT& aron_r,
+ typename ReaderT::InputType& input,
+ BoT& ret)
{
DtoT aron;
aron.read(aron_r, input);
@@ -35,12 +47,18 @@ namespace armarx::aron
armarx::fromAron(aron, ret);
}
- template<class WriterT, class DtoT, class BoT>
- requires (data::isWriter<WriterT> && !detail::DtoAndBoAreSame<DtoT, BoT>)
- inline void write(WriterT& aron_w, const BoT& input, typename WriterT::ReturnType& ret, const armarx::aron::Path& aron_p = armarx::aron::Path())
+ template <class WriterT, class DtoT, class BoT>
+
+ requires(
+ data::isWriter<WriterT>&& armarx::aron::cpp::isAronGeneratedClass<DtoT> &&
+ !detail::DtoAndBoAreSame<DtoT, BoT>) inline void write(WriterT& aron_w,
+ const BoT& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p =
+ armarx::aron::Path())
{
DtoT aron;
armarx::toAron(aron, input);
ret = aron.write(aron_w, aron_p);
}
-}
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/core/type/converter/Converter.h b/source/RobotAPI/libraries/aron/core/type/converter/Converter.h
index e166ab4327c0dd9f8f800a6a0eadea3bf001408a..7e6f0c84dd3d9503f8e0f0d14429395d31867ace 100644
--- a/source/RobotAPI/libraries/aron/core/type/converter/Converter.h
+++ b/source/RobotAPI/libraries/aron/core/type/converter/Converter.h
@@ -23,9 +23,9 @@
#pragma once
-#include "../visitor/Visitor.h"
#include "../rw/Reader.h"
#include "../rw/Writer.h"
+#include "../visitor/Visitor.h"
namespace armarx::aron::type
{
@@ -35,16 +35,20 @@ namespace armarx::aron::type
struct Converter;
template <class T>
- concept isConverter = std::is_base_of<Converter<typename T::ReaderType, typename T::WriterType, typename T::This>, T>::value;
+ concept isConverter =
+ std::is_base_of<Converter<typename T::ReaderType, typename T::WriterType, typename T::This>,
+ T>::value;
template <class ConverterImplementation>
requires isConverter<ConverterImplementation>
- typename ConverterImplementation::WriterReturnType readAndWrite(typename ConverterImplementation::ReaderInputType& o);
+ typename ConverterImplementation::WriterReturnType
+ readAndWrite(typename ConverterImplementation::ReaderInputType& o);
/// Converter struct providing the needed methods.
/// WriterImplementation is a writer class, TODO: add concepts
template <class ReaderImplementation, class WriterImplementation, class DerivedT>
requires isReader<ReaderImplementation> && isWriter<WriterImplementation>
+
struct Converter : virtual public Visitor<typename ReaderImplementation::InputType>
{
using WriterType = WriterImplementation;
@@ -60,12 +64,14 @@ namespace armarx::aron::type
virtual ~Converter() = default;
- type::Descriptor getDescriptor(ReaderInputType& o) final
+ type::Descriptor
+ getDescriptor(ReaderInputType& o) final
{
return r.getDescriptor(o);
}
- void visitObject(ReaderInputType& o) final
+ void
+ visitObject(ReaderInputType& o) final
{
std::map<std::string, ReaderInputTypeNonConst> elementsOfInput;
std::string name;
@@ -81,10 +87,12 @@ namespace armarx::aron::type
elementsReturn.insert({key, converted});
}
- last_returned = w.writeObject(name, templates, templateInstantiations, elementsReturn, std::nullopt, maybe, p);
+ last_returned = w.writeObject(
+ name, templates, templateInstantiations, elementsReturn, std::nullopt, maybe, p);
}
- void visitDict(ReaderInputType& o) final
+ void
+ visitDict(ReaderInputType& o) final
{
ReaderInputTypeNonConst acceptedType;
type::Maybe maybe;
@@ -96,7 +104,8 @@ namespace armarx::aron::type
last_returned = w.writeDict(converted, maybe, p);
};
- void visitList(ReaderInputType& o) final
+ void
+ visitList(ReaderInputType& o) final
{
ReaderInputTypeNonConst acceptedType;
type::Maybe maybe;
@@ -108,7 +117,8 @@ namespace armarx::aron::type
last_returned = w.writeList(converted, maybe, p);
};
- void visitPair(ReaderInputType& o) final
+ void
+ visitPair(ReaderInputType& o) final
{
ReaderInputTypeNonConst acceptedType1;
ReaderInputTypeNonConst acceptedType2;
@@ -122,7 +132,8 @@ namespace armarx::aron::type
last_returned = w.writePair(converted1, converted2, maybe, p);
};
- void visitTuple(ReaderInputType& o) final
+ void
+ visitTuple(ReaderInputType& o) final
{
std::vector<ReaderInputTypeNonConst> acceptedTypes;
std::vector<WriterReturnType> elementsReturn;
@@ -139,117 +150,143 @@ namespace armarx::aron::type
last_returned = w.writeTuple(elementsReturn, maybe, p);
};
- void visitNDArray(ReaderInputType& o) final
+ void
+ visitNDArray(ReaderInputType& o) final
{
type::Maybe maybe;
type::ndarray::ElementType type;
+ std::string defaultValue;
int ndim;
Path p;
- r.readNDArray(o, ndim, type, maybe, p);
- last_returned = w.writeNDArray(ndim, type, maybe, p);
+ r.readNDArray(o, ndim, type, defaultValue, maybe, p);
+ last_returned = w.writeNDArray(ndim, type, defaultValue, maybe, p);
};
- void visitMatrix(ReaderInputType& o) final
+ void
+ visitMatrix(ReaderInputType& o) final
{
type::Maybe maybe;
type::matrix::ElementType type;
+ std::string defaultValue;
int rows;
int cols;
Path p;
- r.readMatrix(o, rows, cols, type, maybe, p);
- last_returned = w.writeMatrix(rows, cols, type, maybe, p);
+ r.readMatrix(o, rows, cols, type, defaultValue, maybe, p);
+ last_returned = w.writeMatrix(rows, cols, type, defaultValue, maybe, p);
};
- void visitQuaternion(ReaderInputType& o) final
+ void
+ visitQuaternion(ReaderInputType& o) final
{
type::Maybe maybe;
type::quaternion::ElementType type;
+ std::string defaultValue;
Path p;
- r.readQuaternion(o, type, maybe, p);
- last_returned = w.writeQuaternion(type, maybe, p);
+ r.readQuaternion(o, type, defaultValue, maybe, p);
+ last_returned = w.writeQuaternion(type, defaultValue, maybe, p);
};
- void visitImage(ReaderInputType& o) final
+ void
+ visitImage(ReaderInputType& o) final
{
type::Maybe maybe;
type::image::PixelType type;
+ std::string defaultValue;
Path p;
- r.readImage(o, type, maybe, p);
- last_returned = w.writeImage(type, maybe, p);
+ r.readImage(o, type, defaultValue, maybe, p);
+ last_returned = w.writeImage(type, defaultValue, maybe, p);
};
- void visitPointCloud(ReaderInputType& o) final
+ void
+ visitPointCloud(ReaderInputType& o) final
{
type::Maybe maybe;
type::pointcloud::VoxelType type;
+ std::string defaultValue;
Path p;
- r.readPointCloud(o, type, maybe, p);
- last_returned = w.writePointCloud(type, maybe, p);
+ r.readPointCloud(o, type, defaultValue, maybe, p);
+ last_returned = w.writePointCloud(type, defaultValue, maybe, p);
};
- void visitIntEnum(ReaderInputType& o) final
+ void
+ visitIntEnum(ReaderInputType& o) final
{
type::Maybe maybe;
std::string name;
std::map<std::string, int> values;
+ std::string defaultValue;
Path p;
- r.readIntEnum(o, name, values, maybe, p);
- last_returned = w.writeIntEnum(name, values, maybe, p);
+ r.readIntEnum(o, name, values, defaultValue, maybe, p);
+ last_returned = w.writeIntEnum(name, values, defaultValue, maybe, p);
};
- void visitInt(ReaderInputType& o) final
+ void
+ visitInt(ReaderInputType& o) final
{
type::Maybe maybe;
Path p;
- r.readInt(o, maybe, p);
- last_returned = w.writeInt(maybe, p);
+ std::optional<int> defaultValue;
+ r.readInt(o, defaultValue, maybe, p);
+ last_returned = w.writeInt(defaultValue, maybe, p);
};
- void visitLong(ReaderInputType& o) final
+ void
+ visitLong(ReaderInputType& o) final
{
type::Maybe maybe;
Path p;
- r.readLong(o, maybe, p);
- last_returned = w.writeLong(maybe, p);
+ std::optional<long> defaultValue;
+ r.readLong(o, defaultValue, maybe, p);
+ last_returned = w.writeLong(defaultValue, maybe, p);
};
- void visitFloat(ReaderInputType& o) final
+ void
+ visitFloat(ReaderInputType& o) final
{
type::Maybe maybe;
Path p;
- r.readFloat(o, maybe, p);
- last_returned = w.writeFloat(maybe, p);
+ std::optional<float> defaultValue;
+ r.readFloat(o, defaultValue, maybe, p);
+ last_returned = w.writeFloat(defaultValue, maybe, p);
};
- void visitDouble(ReaderInputType& o) final
+ void
+ visitDouble(ReaderInputType& o) final
{
type::Maybe maybe;
Path p;
- r.readDouble(o, maybe, p);
- last_returned = w.writeDouble(maybe, p);
+ std::optional<double> defaultValue;
+ r.readDouble(o, defaultValue, maybe, p);
+ last_returned = w.writeDouble(defaultValue, maybe, p);
};
- void visitBool(ReaderInputType& o) final
+ void
+ visitBool(ReaderInputType& o) final
{
type::Maybe maybe;
Path p;
- r.readBool(o, maybe, p);
- last_returned = w.writeBool(maybe, p);
+ std::optional<bool> defaultValue;
+ r.readBool(o, defaultValue, maybe, p);
+ last_returned = w.writeBool(defaultValue, maybe, p);
};
- void visitString(ReaderInputType& o) final
+ void
+ visitString(ReaderInputType& o) final
{
type::Maybe maybe;
Path p;
- r.readString(o, maybe, p);
- last_returned = w.writeString(maybe, p);
+ std::optional<std::string> defaultValue;
+ r.readString(o, defaultValue, maybe, p);
+ last_returned = w.writeString(defaultValue, maybe, p);
};
- void visitUnknown(ReaderInputType& o) final
+ void
+ visitUnknown(ReaderInputType& o) final
{
if (!r.readNull(o))
{
- throw error::AronException(__PRETTY_FUNCTION__, "A visitor got type but the enum is unknown.");
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "A visitor got type but the enum is unknown.");
}
w.writeNull();
}
@@ -259,10 +296,12 @@ namespace armarx::aron::type
/// returns the returntype of T
template <class ConverterImplementation>
requires isConverter<ConverterImplementation>
- typename ConverterImplementation::WriterReturnType readAndWrite(typename ConverterImplementation::ReaderInputType& o)
+
+ typename ConverterImplementation::WriterReturnType
+ readAndWrite(typename ConverterImplementation::ReaderInputType& o)
{
ConverterImplementation v;
type::visit(v, o);
return v.last_returned;
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Reader.h b/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
index 790a0d9fb7f9ed77f00ca40686582db063fd3fbe..9b9b5cf97723bd1b79f53775ebb7e485627c013f 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
@@ -22,13 +22,13 @@
// STD/STL
#include <memory>
+#include <optional>
#include <string>
// ArmarX
#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
-
namespace armarx::aron::type
{
/**
@@ -46,61 +46,127 @@ namespace armarx::aron::type
virtual type::Descriptor getDescriptor(InputType& input) = 0;
/// Extract information from an Object type
- virtual void readObject(const InputType& input, std::string& name, std::vector<std::string>& templates, std::vector<std::string>& templateInstantiations, std::map<std::string, InputTypeNonConst>& memberTypes, type::Maybe& maybe, Path& p) = 0;
+ virtual void readObject(const InputType& input,
+ std::string& name,
+ std::vector<std::string>& templates,
+ std::vector<std::string>& templateInstantiations,
+ std::map<std::string, InputTypeNonConst>& memberTypes,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from a list type
- virtual void readList(const InputType& input, InputTypeNonConst& acceptedType, type::Maybe& maybe, Path& p) = 0;
+ virtual void readList(const InputType& input,
+ InputTypeNonConst& acceptedType,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from a dict type
- virtual void readDict(const InputType& input, InputTypeNonConst& acceptedType, type::Maybe& maybe, Path& p) = 0;
+ virtual void readDict(const InputType& input,
+ InputTypeNonConst& acceptedType,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from a tuple type
- virtual void readTuple(const InputType& input, std::vector<InputTypeNonConst>& acceptedTypes, type::Maybe& maybe, Path& p) = 0;
+ virtual void readTuple(const InputType& input,
+ std::vector<InputTypeNonConst>& acceptedTypes,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from a pair type
- virtual void readPair(const InputType& input, InputTypeNonConst& acceptedTypes1, InputTypeNonConst& acceptedTypes2, type::Maybe& maybe, Path& p) = 0;
+ virtual void readPair(const InputType& input,
+ InputTypeNonConst& acceptedTypes1,
+ InputTypeNonConst& acceptedTypes2,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from a ndarray type
- virtual void readNDArray(const InputType& input, int& ndim, type::ndarray::ElementType& type, type::Maybe& maybe, Path& p) = 0;
+ virtual void readNDArray(const InputType& input,
+ int& ndim,
+ type::ndarray::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from a matrix type
- virtual void readMatrix(const InputType& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe, Path& p) = 0;
+ virtual void readMatrix(const InputType& input,
+ int& rows,
+ int& cols,
+ type::matrix::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from a quaternion type
- virtual void readQuaternion(const InputType& input, type::quaternion::ElementType& type, type::Maybe& maybe, Path& p) = 0;
+ virtual void readQuaternion(const InputType& input,
+ type::quaternion::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from a pointcloud type
- virtual void readPointCloud(const InputType& input, type::pointcloud::VoxelType& type, type::Maybe& maybe, Path& p) = 0;
+ virtual void readPointCloud(const InputType& input,
+ type::pointcloud::VoxelType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from an image type
- virtual void readImage(const InputType& input, type::image::PixelType& type, type::Maybe& maybe, Path& p) = 0;
+ virtual void readImage(const InputType& input,
+ type::image::PixelType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from an int enum type
- virtual void readIntEnum(const InputType& input, std::string& name, std::map<std::string, int>& acceptedValues, type::Maybe& maybe, Path& p) = 0;
+ virtual void readIntEnum(const InputType& input,
+ std::string& name,
+ std::map<std::string, int>& acceptedValues,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from an int type
- virtual void readInt(const InputType& input, type::Maybe& maybe, Path& p) = 0;
+ virtual void readInt(const InputType& input,
+ std::optional<int>& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from an long type
- virtual void readLong(const InputType& input, type::Maybe& maybe, Path& p) = 0;
+ virtual void readLong(const InputType& input,
+ std::optional<long>& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from an float type
- virtual void readFloat(const InputType& input, type::Maybe& maybe, Path& p) = 0;
+ virtual void readFloat(const InputType& input,
+ std::optional<float>& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from an double type
- virtual void readDouble(const InputType& input, type::Maybe& maybe, Path& p) = 0;
+ virtual void readDouble(const InputType& input,
+ std::optional<double>& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from an string type
- virtual void readString(const InputType& input, type::Maybe& maybe, Path& p) = 0;
+ virtual void readString(const InputType& input,
+ std::optional<std::string>& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from an bool type
- virtual void readBool(const InputType& input, type::Maybe& maybe, Path& p) = 0;
+ virtual void readBool(const InputType& input,
+ std::optional<bool>& defaultValue,
+ type::Maybe& maybe,
+ Path& p) = 0;
/// Extract information from an time type
virtual void readAnyObject(const InputType& input, type::Maybe& maybe, Path& p) = 0;
/// Check if input is null
- virtual bool readNull(InputType& input) // defaulted implementation
+ virtual bool
+ readNull(InputType& input) // defaulted implementation
{
InputType nil = {};
return input == nil;
@@ -218,4 +284,4 @@ namespace armarx::aron::type
template <class T>
concept isReader = std::is_base_of<ReaderInterface<typename T::InputType>, T>::value;
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
index 5cba5dd672c7a2ebcde343485e64bc781ce26a1f..27bcfcd0da522ce9defa148017ec57f6ea28e9bb 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
@@ -22,14 +22,13 @@
// STD/STL
#include <memory>
-#include <string>
#include <optional>
+#include <string>
// ArmarX
#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
-
namespace armarx::aron::type
{
/**
@@ -47,67 +46,115 @@ namespace armarx::aron::type
virtual type::Descriptor getDescriptor(ReturnTypeConst& input) = 0;
/// Construct an object from the params
- virtual ReturnType writeObject(const std::string& name, const std::vector<std::string>& templates, const std::vector<std::string>& templateInstantiations, const std::map<std::string, ReturnType>& memberTypes, const std::optional<ReturnType>& extends, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeObject(const std::string& name,
+ const std::vector<std::string>& templates,
+ const std::vector<std::string>& templateInstantiations,
+ const std::map<std::string, ReturnType>& memberTypes,
+ const std::optional<ReturnType>& extends,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a list from the params
- virtual ReturnType writeList(const ReturnType& acceptedType, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType
+ writeList(const ReturnType& acceptedType, const type::Maybe maybe, const Path& p) = 0;
/// Construct a dict from the params
- virtual ReturnType writeDict(const ReturnType& acceptedType, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType
+ writeDict(const ReturnType& acceptedType, const type::Maybe maybe, const Path& p) = 0;
/// Construct a pair from the params
- virtual ReturnType writePair(const ReturnType& acceptedType1, const ReturnType& acceptedType2, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writePair(const ReturnType& acceptedType1,
+ const ReturnType& acceptedType2,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a tuple from the params
- virtual ReturnType writeTuple(const std::vector<ReturnType>& acceptedTypes, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeTuple(const std::vector<ReturnType>& acceptedTypes,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a ndarray from the params
- virtual ReturnType writeNDArray(const int ndim, const type::ndarray::ElementType, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeNDArray(const int ndim,
+ const type::ndarray::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a matrix from the params
- virtual ReturnType writeMatrix(const int rows, const int cols, const type::matrix::ElementType type, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeMatrix(const int rows,
+ const int cols,
+ const type::matrix::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a quaternion from the params
- virtual ReturnType writeQuaternion(const type::quaternion::ElementType, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeQuaternion(const type::quaternion::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a image from the params
- virtual ReturnType writeImage(const type::image::PixelType, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeImage(const type::image::PixelType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a pointcloud from the params
- virtual ReturnType writePointCloud(const type::pointcloud::VoxelType, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writePointCloud(const type::pointcloud::VoxelType,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a int enum from the params
- virtual ReturnType writeIntEnum(const std::string& name, const std::map<std::string, int>& acceptedValues, const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeIntEnum(const std::string& name,
+ const std::map<std::string, int>& acceptedValues,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a int from the params
- virtual ReturnType writeInt(const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeInt(const std::optional<int>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a long from the params
- virtual ReturnType writeLong(const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeLong(const std::optional<long>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a float from the params
- virtual ReturnType writeFloat(const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeFloat(const std::optional<float>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a double from the params
- virtual ReturnType writeDouble(const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeDouble(const std::optional<double>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a string from the params
- virtual ReturnType writeString(const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeString(const std::optional<std::string>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a bool from the params
- virtual ReturnType writeBool(const type::Maybe maybe, const Path& p) = 0;
+ virtual ReturnType writeBool(const std::optional<bool>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p) = 0;
/// Construct a time from the params
virtual ReturnType writeAnyObject(const type::Maybe maybe, const Path& p) = 0;
/// write a null
- virtual ReturnType writeNull(const Path& p = Path()) // defaulted implementation
+ virtual ReturnType
+ writeNull(const Path& p = Path()) // defaulted implementation
{
- (void) p;
+ (void)p;
return {};
}
};
template <class T>
concept isWriter = std::is_base_of<WriterInterface<typename T::ReturnType>, T>::value;
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/json/Data.cpp b/source/RobotAPI/libraries/aron/core/type/rw/json/Data.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..df60876fbdd629f5c43d7a3fd2efb3f2a28ba1f0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/type/rw/json/Data.cpp
@@ -0,0 +1,21 @@
+/*
+* 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 Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "Data.h"
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h b/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h
index a3dbadf76ff6c5d74e20f9b4ad66ae25b171d9aa..834b0b462f7f6de3e1bb6d1d4a1212773995f4ab 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h
@@ -34,45 +34,46 @@ namespace armarx::aron::type::rw::json
{
namespace constantes
{
- const std::string MAYBE_SLUG = "_ARON_MAYBE";
- const std::string TYPE_SLUG = "_ARON_TYPE";
- const std::string PATH_SLUG = "_ARON_PATH";
- const std::string VERSION_SLUG = "_ARON_VERSION";
+ const constexpr auto MAYBE_SLUG = "_ARON_MAYBE";
+ const constexpr auto TYPE_SLUG = "_ARON_TYPE";
+ const constexpr auto PATH_SLUG = "_ARON_PATH";
+ const constexpr auto VERSION_SLUG = "_ARON_VERSION";
- const std::string KEY_SLUG = "_ARON_KEY";
- const std::string VALUE_SLUG = "_ARON_VALUE";
- const std::string MEMBERS_SLUG = "_ARON_MEMBERS";
- const std::string ELEMENTS_SLUG = "_ARON_ELEMENTS";
+ const constexpr auto KEY_SLUG = "_ARON_KEY";
+ const constexpr auto VALUE_SLUG = "_ARON_VALUE";
+ const constexpr auto MEMBERS_SLUG = "_ARON_MEMBERS";
+ const constexpr auto ELEMENTS_SLUG = "_ARON_ELEMENTS";
- const std::string NAME_SLUG = "_ARON_NAME";
- const std::string EXTENDS_SLUG = "_ARON_EXTENDS";
- const std::string ACCEPTED_TYPE_SLUG = "_ARON_ACCEPTED_TYPE";
- const std::string DIMENSIONS_SLUG = "_ARON_DIMESIONS";
- const std::string DATA_SLUG = "_ARON_DATA";
- const std::string USED_TYPE_SLUG = "_ARON_USED_TYPE";
- const std::string TEMPLATES_SLUG = "_ARON_TEMPLATES";
- const std::string TEMPLATE_INSTANTIATIONS_SLUG = "_ARON_TEMPLATE_INSTANTIATION";
+ const constexpr auto NAME_SLUG = "_ARON_NAME";
+ const constexpr auto EXTENDS_SLUG = "_ARON_EXTENDS";
+ const constexpr auto ACCEPTED_TYPE_SLUG = "_ARON_ACCEPTED_TYPE";
+ const constexpr auto DIMENSIONS_SLUG = "_ARON_DIMESIONS";
+ const constexpr auto DATA_SLUG = "_ARON_DATA";
+ const constexpr auto USED_TYPE_SLUG = "_ARON_USED_TYPE";
+ const constexpr auto TEMPLATES_SLUG = "_ARON_TEMPLATES";
+ const constexpr auto TEMPLATE_INSTANTIATIONS_SLUG = "_ARON_TEMPLATE_INSTANTIATION";
+ const constexpr auto DEFAULT_SLUG = "_ARON_DEFAULT";
- const std::string LIST_TYPENAME_SLUG = "_ARON_LIST";
- const std::string DICT_TYPENAME_SLUG = "_ARON_DICT";
- const std::string OBJECT_TYPENAME_SLUG = "_ARON_OBJECT";
- const std::string TUPLE_TYPENAME_SLUG = "_ARON_TUPLE";
- const std::string PAIR_TYPENAME_SLUG = "_ARON_PAIR";
- const std::string NDARRAY_TYPENAME_SLUG = "_ARON_NDARRAY";
- const std::string IMAGE_TYPENAME_SLUG = "_ARON_IMAGE";
- const std::string MATRIX_TYPENAME_SLUG = "_ARON_MATRIX";
- const std::string QUATERNION_TYPENAME_SLUG = "_ARON_QUATERNION";
- const std::string POINT_CLOUD_TYPENAME_SLUG = "_ARON_POINT_CLOUD";
- const std::string INT_ENUM_TYPENAME_SLUG = "_ARON_INT_ENUM";
- const std::string INT_TYPENAME_SLUG = "_ARON_INT";
- const std::string LONG_TYPENAME_SLUG = "_ARON_LONG";
- const std::string FLOAT_TYPENAME_SLUG = "_ARON_FLOAT";
- const std::string DOUBLE_TYPENAME_SLUG = "_ARON_DOUBLE";
- const std::string STRING_TYPENAME_SLUG = "_ARON_STRING";
- const std::string BOOL_TYPENAME_SLUG = "_ARON_BOOL";
- const std::string TIME_TYPENAME_SLUG = "_ARON_TIME";
- const std::string ANY_OBJECT_TYPENAME_SLUG = "_ARON_ANY_OBJECT";
- }
+ const constexpr auto LIST_TYPENAME_SLUG = "_ARON_LIST";
+ const constexpr auto DICT_TYPENAME_SLUG = "_ARON_DICT";
+ const constexpr auto OBJECT_TYPENAME_SLUG = "_ARON_OBJECT";
+ const constexpr auto TUPLE_TYPENAME_SLUG = "_ARON_TUPLE";
+ const constexpr auto PAIR_TYPENAME_SLUG = "_ARON_PAIR";
+ const constexpr auto NDARRAY_TYPENAME_SLUG = "_ARON_NDARRAY";
+ const constexpr auto IMAGE_TYPENAME_SLUG = "_ARON_IMAGE";
+ const constexpr auto MATRIX_TYPENAME_SLUG = "_ARON_MATRIX";
+ const constexpr auto QUATERNION_TYPENAME_SLUG = "_ARON_QUATERNION";
+ const constexpr auto POINT_CLOUD_TYPENAME_SLUG = "_ARON_POINT_CLOUD";
+ const constexpr auto INT_ENUM_TYPENAME_SLUG = "_ARON_INT_ENUM";
+ const constexpr auto INT_TYPENAME_SLUG = "_ARON_INT";
+ const constexpr auto LONG_TYPENAME_SLUG = "_ARON_LONG";
+ const constexpr auto FLOAT_TYPENAME_SLUG = "_ARON_FLOAT";
+ const constexpr auto DOUBLE_TYPENAME_SLUG = "_ARON_DOUBLE";
+ const constexpr auto STRING_TYPENAME_SLUG = "_ARON_STRING";
+ const constexpr auto BOOL_TYPENAME_SLUG = "_ARON_BOOL";
+ const constexpr auto TIME_TYPENAME_SLUG = "_ARON_TIME";
+ const constexpr auto ANY_OBJECT_TYPENAME_SLUG = "_ARON_ANY_OBJECT";
+ } // namespace constantes
namespace conversion
{
@@ -94,22 +95,18 @@ namespace armarx::aron::type::rw::json
{type::Descriptor::DOUBLE, rw::json::constantes::DOUBLE_TYPENAME_SLUG},
{type::Descriptor::BOOL, rw::json::constantes::BOOL_TYPENAME_SLUG},
{type::Descriptor::STRING, rw::json::constantes::STRING_TYPENAME_SLUG},
- {type::Descriptor::ANY_OBJECT, rw::json::constantes::ANY_OBJECT_TYPENAME_SLUG}
- };
+ {type::Descriptor::ANY_OBJECT, rw::json::constantes::ANY_OBJECT_TYPENAME_SLUG}};
const auto String2Descriptor = aron::conversion::util::InvertMap(Descriptor2String);
- const std::map<type::Maybe, std::string> Maybe2String =
- {
+ const std::map<type::Maybe, std::string> Maybe2String = {
{type::Maybe::NONE, "type::maybe::NONE"},
{type::Maybe::OPTIONAL, "type::maybe::OPTIONAL"},
{type::Maybe::RAW_PTR, "type::maybe::RAW_PTR"},
{type::Maybe::SHARED_PTR, "type::maybe::SHARED_PTR"},
- {type::Maybe::UNIQUE_PTR, "type::maybe::UNIQUE_PTR"}
- };
+ {type::Maybe::UNIQUE_PTR, "type::maybe::UNIQUE_PTR"}};
const auto String2Maybe = aron::conversion::util::InvertMap(Maybe2String);
- const std::map<type::ndarray::ElementType, std::string> NDArrayType2String =
- {
+ const std::map<type::ndarray::ElementType, std::string> NDArrayType2String = {
{type::ndarray::ElementType::INT8, "type::ndarray::INT8"},
{type::ndarray::ElementType::INT16, "type::ndarray::INT16"},
{type::ndarray::ElementType::INT32, "type::ndarray::INT32"},
@@ -117,44 +114,35 @@ namespace armarx::aron::type::rw::json
{type::ndarray::ElementType::UINT16, "type::ndarray::UINT16"},
{type::ndarray::ElementType::UINT32, "type::ndarray::UINT32"},
{type::ndarray::ElementType::FLOAT32, "type::ndarray::FLOAT32"},
- {type::ndarray::ElementType::FLOAT64, "type::ndarray::FLOAT64"}
- };
+ {type::ndarray::ElementType::FLOAT64, "type::ndarray::FLOAT64"}};
const auto String2NDArrayType = aron::conversion::util::InvertMap(NDArrayType2String);
- const std::map<type::matrix::ElementType, std::string> MatrixType2String =
- {
+ const std::map<type::matrix::ElementType, std::string> MatrixType2String = {
{type::matrix::ElementType::INT16, "type::matrix::INT16"},
{type::matrix::ElementType::INT32, "type::matrix::INT32"},
{type::matrix::ElementType::INT64, "type::matrix::INT64"},
{type::matrix::ElementType::FLOAT32, "type::matrix::FLOAT32"},
- {type::matrix::ElementType::FLOAT64, "type::matrix::FLOAT64"}
- };
+ {type::matrix::ElementType::FLOAT64, "type::matrix::FLOAT64"}};
const auto String2MatrixType = aron::conversion::util::InvertMap(MatrixType2String);
- const std::map<type::quaternion::ElementType, std::string> QuaternionType2String =
- {
+ const std::map<type::quaternion::ElementType, std::string> QuaternionType2String = {
{type::quaternion::ElementType::FLOAT32, "type::quaternion::FLOAT32"},
- {type::quaternion::ElementType::FLOAT64, "type::quaternion::FLOAT64"}
- };
+ {type::quaternion::ElementType::FLOAT64, "type::quaternion::FLOAT64"}};
const auto String2QuaternionType = aron::conversion::util::InvertMap(QuaternionType2String);
- const std::map<type::image::PixelType, std::string> PixelType2String =
- {
+ const std::map<type::image::PixelType, std::string> PixelType2String = {
{type::image::PixelType::RGB24, "type::image::RGB24"},
- {type::image::PixelType::DEPTH32, "type::image::DEPTH32"}
- };
+ {type::image::PixelType::DEPTH32, "type::image::DEPTH32"}};
const auto String2PixelType = aron::conversion::util::InvertMap(PixelType2String);
- const std::map<type::pointcloud::VoxelType, std::string> VoxelType2String =
- {
+ const std::map<type::pointcloud::VoxelType, std::string> VoxelType2String = {
{type::pointcloud::VoxelType::POINT_XYZ, "type::pointcloud::POINT_XYZ"},
{type::pointcloud::VoxelType::POINT_XYZI, "type::pointcloud::POINT_XYZI"},
{type::pointcloud::VoxelType::POINT_XYZL, "type::pointcloud::POINT_XYZL"},
{type::pointcloud::VoxelType::POINT_XYZRGB, "type::pointcloud::POINT_XYZRGB"},
{type::pointcloud::VoxelType::POINT_XYZRGBA, "type::pointcloud::POINT_XYZRGBA"},
{type::pointcloud::VoxelType::POINT_XYZRGBL, "type::pointcloud::POINT_XYZRGBL"},
- {type::pointcloud::VoxelType::POINT_XYZHSV, "type::pointcloud::POINT_XYZHSV"}
- };
+ {type::pointcloud::VoxelType::POINT_XYZHSV, "type::pointcloud::POINT_XYZHSV"}};
const auto String2VoxelType = aron::conversion::util::InvertMap(VoxelType2String);
- }
-}
+ } // namespace conversion
+} // namespace armarx::aron::type::rw::json
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
index 028b6681ce8ee3f57db6a33861eb0c28d4228811..9b0ae2b3e526ecac3739c196616bbbb132f43d48 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -27,33 +27,48 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/Exception.h>
-#include "../../json/Data.h"
#include <RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.h>
+#include "../../json/Data.h"
+
namespace armarx::aron::type::reader
{
namespace
{
/// Throw an exception if the type is other than expected
- void getAronMetaInformationForType(const nlohmann::json& input, const std::string& expectedType, Path& p)
+ void
+ getAronMetaInformationForType(const nlohmann::json& input,
+ const std::string& expectedType,
+ Path& p)
{
if (input[rw::json::constantes::TYPE_SLUG] != expectedType)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Wrong type in json encountered.", input[rw::json::constantes::TYPE_SLUG], expectedType);
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Wrong type in json encountered.",
+ input[rw::json::constantes::TYPE_SLUG],
+ expectedType);
}
// get Path
std::vector<std::string> pathElements = input[rw::json::constantes::PATH_SLUG];
p = Path(pathElements);
}
- }
+ } // namespace
- type::Descriptor NlohmannJSONReader::getDescriptor(InputType& input)
+ type::Descriptor
+ NlohmannJSONReader::getDescriptor(InputType& input)
{
return ConstNlohmannJSONVisitor::GetDescriptor(input);
}
- void NlohmannJSONReader::readObject(const nlohmann::json& input, std::string& name, std::vector<std::string>& templates, std::vector<std::string>& templateInstantiations, std::map<std::string, nlohmann::json>& memberTypes, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readObject(const nlohmann::json& input,
+ std::string& name,
+ std::vector<std::string>& templates,
+ std::vector<std::string>& templateInstantiations,
+ std::map<std::string, nlohmann::json>& memberTypes,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::OBJECT_TYPENAME_SLUG, p);
@@ -64,14 +79,21 @@ namespace armarx::aron::type::reader
}
if (input.count(rw::json::constantes::TEMPLATE_INSTANTIATIONS_SLUG))
{
- templateInstantiations = input[rw::json::constantes::TEMPLATE_INSTANTIATIONS_SLUG].get<std::vector<std::string>>();
+ templateInstantiations = input[rw::json::constantes::TEMPLATE_INSTANTIATIONS_SLUG]
+ .get<std::vector<std::string>>();
}
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- memberTypes = input[rw::json::constantes::MEMBERS_SLUG].get<std::map<std::string, nlohmann::json>>();
+ memberTypes =
+ input[rw::json::constantes::MEMBERS_SLUG].get<std::map<std::string, nlohmann::json>>();
}
- void NlohmannJSONReader::readList(const nlohmann::json& input, nlohmann::json& acceptedType, type::Maybe& maybe, Path& p)
+
+ void
+ NlohmannJSONReader::readList(const nlohmann::json& input,
+ nlohmann::json& acceptedType,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::LIST_TYPENAME_SLUG, p);
@@ -79,7 +101,11 @@ namespace armarx::aron::type::reader
acceptedType = input[rw::json::constantes::ACCEPTED_TYPE_SLUG];
}
- void NlohmannJSONReader::readDict(const nlohmann::json& input, nlohmann::json& acceptedType, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readDict(const nlohmann::json& input,
+ nlohmann::json& acceptedType,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::DICT_TYPENAME_SLUG, p);
@@ -87,25 +113,42 @@ namespace armarx::aron::type::reader
acceptedType = input[rw::json::constantes::ACCEPTED_TYPE_SLUG];
}
- void NlohmannJSONReader::readTuple(const nlohmann::json& input, std::vector<nlohmann::json>& acceptedTypes, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readTuple(const nlohmann::json& input,
+ std::vector<nlohmann::json>& acceptedTypes,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::TUPLE_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- acceptedTypes = input[rw::json::constantes::ACCEPTED_TYPE_SLUG].get<std::vector<nlohmann::json>>();
+ acceptedTypes =
+ input[rw::json::constantes::ACCEPTED_TYPE_SLUG].get<std::vector<nlohmann::json>>();
}
- void NlohmannJSONReader::readPair(const nlohmann::json& input, nlohmann::json& acceptedType1, nlohmann::json& acceptedType2, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readPair(const nlohmann::json& input,
+ nlohmann::json& acceptedType1,
+ nlohmann::json& acceptedType2,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::PAIR_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- auto list = input[rw::json::constantes::ACCEPTED_TYPE_SLUG].get<std::vector<nlohmann::json>>();
+ auto list =
+ input[rw::json::constantes::ACCEPTED_TYPE_SLUG].get<std::vector<nlohmann::json>>();
acceptedType1 = list[0];
acceptedType2 = list[1];
}
- void NlohmannJSONReader::readNDArray(const nlohmann::json& input, int& ndim, type::ndarray::ElementType& type, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readNDArray(const nlohmann::json& input,
+ int& ndim,
+ type::ndarray::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::NDARRAY_TYPENAME_SLUG, p);
@@ -114,9 +157,17 @@ namespace armarx::aron::type::reader
std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
type = armarx::aron::type::rw::json::conversion::String2NDArrayType.at(t);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readMatrix(const nlohmann::json& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readMatrix(const nlohmann::json& input,
+ int& rows,
+ int& cols,
+ type::matrix::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::MATRIX_TYPENAME_SLUG, p);
@@ -127,9 +178,15 @@ namespace armarx::aron::type::reader
std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
type = armarx::aron::type::rw::json::conversion::String2MatrixType.at(t);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readQuaternion(const nlohmann::json& input, type::quaternion::ElementType& type, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readQuaternion(const nlohmann::json& input,
+ type::quaternion::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::QUATERNION_TYPENAME_SLUG, p);
@@ -137,9 +194,15 @@ namespace armarx::aron::type::reader
std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
type = armarx::aron::type::rw::json::conversion::String2QuaternionType.at(t);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readPointCloud(const nlohmann::json& input, type::pointcloud::VoxelType& type, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readPointCloud(const nlohmann::json& input,
+ type::pointcloud::VoxelType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::POINT_CLOUD_TYPENAME_SLUG, p);
@@ -147,9 +210,15 @@ namespace armarx::aron::type::reader
std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
type = armarx::aron::type::rw::json::conversion::String2VoxelType.at(t);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readImage(const nlohmann::json& input, type::image::PixelType& type, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readImage(const nlohmann::json& input,
+ type::image::PixelType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::IMAGE_TYPENAME_SLUG, p);
@@ -157,64 +226,105 @@ namespace armarx::aron::type::reader
std::string t = input[rw::json::constantes::USED_TYPE_SLUG];
type = armarx::aron::type::rw::json::conversion::String2PixelType.at(t);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readIntEnum(const nlohmann::json& input, std::string& name, std::map<std::string, int>& acceptedValues, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readIntEnum(const nlohmann::json& input,
+ std::string& name,
+ std::map<std::string, int>& acceptedValues,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::INT_ENUM_TYPENAME_SLUG, p);
name = input[rw::json::constantes::NAME_SLUG];
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- acceptedValues = input[rw::json::constantes::ELEMENTS_SLUG].get<std::map<std::string, int>>();
+ acceptedValues =
+ input[rw::json::constantes::ELEMENTS_SLUG].get<std::map<std::string, int>>();
+
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readInt(const nlohmann::json& input, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readInt(const nlohmann::json& input,
+ std::optional<int>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::INT_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readLong(const nlohmann::json& input, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readLong(const nlohmann::json& input,
+ std::optional<long>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::LONG_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readFloat(const nlohmann::json& input, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readFloat(const nlohmann::json& input,
+ std::optional<float>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::FLOAT_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readDouble(const nlohmann::json& input, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readDouble(const nlohmann::json& input,
+ std::optional<double>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::DOUBLE_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readString(const nlohmann::json& input, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readString(const nlohmann::json& input,
+ std::optional<std::string>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::STRING_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readBool(const nlohmann::json& input, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readBool(const nlohmann::json& input,
+ std::optional<bool>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::BOOL_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
+ defaultValue = input[rw::json::constantes::DEFAULT_SLUG];
}
- void NlohmannJSONReader::readAnyObject(const nlohmann::json& input, type::Maybe& maybe, Path& p)
+ void
+ NlohmannJSONReader::readAnyObject(const nlohmann::json& input, type::Maybe& maybe, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::ANY_OBJECT_TYPENAME_SLUG, p);
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
}
-}
+} // namespace armarx::aron::type::reader
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h
index 3775977b41ff3bc07c6c7763ed87dc34123fb1aa..66eb89af601e7ef2acf213497fcf993a89b15cd0 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h
@@ -32,8 +32,7 @@
namespace armarx::aron::type::reader
{
- class NlohmannJSONReader :
- public ReaderInterface<const nlohmann::json>
+ class NlohmannJSONReader : public ReaderInterface<const nlohmann::json>
{
public:
// constructors
@@ -41,27 +40,92 @@ namespace armarx::aron::type::reader
type::Descriptor getDescriptor(InputType& input) final;
- void readObject(InputType& input, std::string& name, std::vector<std::string>& templates, std::vector<std::string>& templateInstantiations, std::map<std::string, InputTypeNonConst>& memberTypes, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readList(InputType& input, InputTypeNonConst& acceptedType, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readDict(InputType& input, InputTypeNonConst& acceptedType, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readTuple(InputType& input, std::vector<InputTypeNonConst>& acceptedTypes, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readPair(InputType& input, InputTypeNonConst& acceptedType1, InputTypeNonConst& acceptedType2, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readObject(InputType& input,
+ std::string& name,
+ std::vector<std::string>& templates,
+ std::vector<std::string>& templateInstantiations,
+ std::map<std::string, InputTypeNonConst>& memberTypes,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readList(InputType& input,
+ InputTypeNonConst& acceptedType,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readDict(InputType& input,
+ InputTypeNonConst& acceptedType,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readTuple(InputType& input,
+ std::vector<InputTypeNonConst>& acceptedTypes,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readPair(InputType& input,
+ InputTypeNonConst& acceptedType1,
+ InputTypeNonConst& acceptedType2,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readNDArray(InputType& input, int& ndim, type::ndarray::ElementType& type, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readMatrix(InputType& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readQuaternion(InputType& input, type::quaternion::ElementType& type, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readPointCloud(InputType& input, type::pointcloud::VoxelType& type, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readImage(InputType& input, type::image::PixelType& type, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readNDArray(InputType& input,
+ int& ndim,
+ type::ndarray::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readMatrix(InputType& input,
+ int& rows,
+ int& cols,
+ type::matrix::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readQuaternion(InputType& input,
+ type::quaternion::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readPointCloud(InputType& input,
+ type::pointcloud::VoxelType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readImage(InputType& input,
+ type::image::PixelType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readIntEnum(InputType& input, std::string& name, std::map<std::string, int>& acceptedValues, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readIntEnum(InputType& input,
+ std::string& name,
+ std::map<std::string, int>& acceptedValues,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readInt(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readLong(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readFloat(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readDouble(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readString(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readBool(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readInt(InputType& input,
+ std::optional<int>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readLong(InputType& input,
+ std::optional<long>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readFloat(InputType& input,
+ std::optional<float>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readDouble(InputType& input,
+ std::optional<double>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readString(InputType& input,
+ std::optional<std::string>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readBool(InputType& input,
+ std::optional<bool>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
void readAnyObject(InputType& input, type::Maybe& maybe, Path& p) override;
};
-}
+} // namespace armarx::aron::type::reader
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.cpp b/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.cpp
index 452f240a5373678a3523dbe418d670e065e62423..6e2ebb31fc7d914f3231a11ef35a9d5ff755cd9e 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.cpp
@@ -33,12 +33,20 @@
namespace armarx::aron::type::reader
{
- type::Descriptor VariantReader::getDescriptor(InputType& input)
+ type::Descriptor
+ VariantReader::getDescriptor(InputType& input)
{
return ConstVariantVisitor::GetDescriptor(input);
}
- void VariantReader::readObject(const aron::type::VariantPtr& input, std::string& name, std::vector<std::string>& templates, std::vector<std::string>& templateInstantiations, std::map<std::string, aron::type::VariantPtr>& memberTypes, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readObject(const aron::type::VariantPtr& input,
+ std::string& name,
+ std::vector<std::string>& templates,
+ std::vector<std::string>& templateInstantiations,
+ std::map<std::string, aron::type::VariantPtr>& memberTypes,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Object::DynamicCastAndCheck(input);
@@ -50,7 +58,12 @@ namespace armarx::aron::type::reader
memberTypes = o->getMemberTypes();
p = o->getPath();
}
- void VariantReader::readList(const aron::type::VariantPtr& input, aron::type::VariantPtr& acceptedType, type::Maybe& maybe, Path& p)
+
+ void
+ VariantReader::readList(const aron::type::VariantPtr& input,
+ aron::type::VariantPtr& acceptedType,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::List::DynamicCastAndCheck(input);
@@ -60,7 +73,11 @@ namespace armarx::aron::type::reader
p = o->getPath();
}
- void VariantReader::readDict(const aron::type::VariantPtr& input, aron::type::VariantPtr& acceptedType, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readDict(const aron::type::VariantPtr& input,
+ aron::type::VariantPtr& acceptedType,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Dict::DynamicCastAndCheck(input);
@@ -70,7 +87,11 @@ namespace armarx::aron::type::reader
p = o->getPath();
}
- void VariantReader::readTuple(const aron::type::VariantPtr& input, std::vector<aron::type::VariantPtr>& acceptedTypes, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readTuple(const aron::type::VariantPtr& input,
+ std::vector<aron::type::VariantPtr>& acceptedTypes,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Tuple::DynamicCastAndCheck(input);
@@ -80,7 +101,12 @@ namespace armarx::aron::type::reader
p = o->getPath();
}
- void VariantReader::readPair(const aron::type::VariantPtr& input, aron::type::VariantPtr& acceptedType1, aron::type::VariantPtr& acceptedType2, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readPair(const aron::type::VariantPtr& input,
+ aron::type::VariantPtr& acceptedType1,
+ aron::type::VariantPtr& acceptedType2,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Pair::DynamicCastAndCheck(input);
@@ -91,7 +117,13 @@ namespace armarx::aron::type::reader
p = o->getPath();
}
- void VariantReader::readNDArray(const aron::type::VariantPtr& input, int& ndim, type::ndarray::ElementType& type, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readNDArray(const aron::type::VariantPtr& input,
+ int& ndim,
+ type::ndarray::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::NDArray::DynamicCastAndCheck(input);
@@ -99,10 +131,18 @@ namespace armarx::aron::type::reader
maybe = o->getMaybe();
ndim = o->getNumberDimensions();
type = o->getElementType();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readMatrix(const aron::type::VariantPtr& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readMatrix(const aron::type::VariantPtr& input,
+ int& rows,
+ int& cols,
+ type::matrix::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Matrix::DynamicCastAndCheck(input);
@@ -111,40 +151,65 @@ namespace armarx::aron::type::reader
rows = o->getRows();
cols = o->getCols();
type = o->getElementType();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readQuaternion(const aron::type::VariantPtr& input, type::quaternion::ElementType& type, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readQuaternion(const aron::type::VariantPtr& input,
+ type::quaternion::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Quaternion::DynamicCastAndCheck(input);
maybe = o->getMaybe();
type = o->getElementType();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readPointCloud(const aron::type::VariantPtr& input, type::pointcloud::VoxelType& type, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readPointCloud(const aron::type::VariantPtr& input,
+ type::pointcloud::VoxelType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::PointCloud::DynamicCastAndCheck(input);
maybe = o->getMaybe();
type = o->getVoxelType();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readImage(const aron::type::VariantPtr& input, type::image::PixelType& type, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readImage(const aron::type::VariantPtr& input,
+ type::image::PixelType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Image::DynamicCastAndCheck(input);
maybe = o->getMaybe();
type = o->getPixelType();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readIntEnum(const aron::type::VariantPtr& input, std::string& name, std::map<std::string, int>& acceptedValues, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readIntEnum(const aron::type::VariantPtr& input,
+ std::string& name,
+ std::map<std::string, int>& acceptedValues,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::IntEnum::DynamicCastAndCheck(input);
@@ -152,64 +217,96 @@ namespace armarx::aron::type::reader
name = o->getEnumName();
acceptedValues = o->getAcceptedValueMap();
maybe = o->getMaybe();
+ defaultValue = o->getDefaultValueName();
p = o->getPath();
}
- void VariantReader::readInt(const aron::type::VariantPtr& input, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readInt(const aron::type::VariantPtr& input,
+ std::optional<int>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Int::DynamicCastAndCheck(input);
maybe = o->getMaybe();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readLong(const aron::type::VariantPtr& input, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readLong(const aron::type::VariantPtr& input,
+ std::optional<long>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Long::DynamicCastAndCheck(input);
maybe = o->getMaybe();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readFloat(const aron::type::VariantPtr& input, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readFloat(const aron::type::VariantPtr& input,
+ std::optional<float>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Float::DynamicCastAndCheck(input);
maybe = o->getMaybe();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readDouble(const aron::type::VariantPtr& input, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readDouble(const aron::type::VariantPtr& input,
+ std::optional<double>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Double::DynamicCastAndCheck(input);
maybe = o->getMaybe();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readString(const aron::type::VariantPtr& input, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readString(const aron::type::VariantPtr& input,
+ std::optional<std::string>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::String::DynamicCastAndCheck(input);
maybe = o->getMaybe();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readBool(const aron::type::VariantPtr& input, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readBool(const aron::type::VariantPtr& input,
+ std::optional<bool>& defaultValue,
+ type::Maybe& maybe,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::Bool::DynamicCastAndCheck(input);
maybe = o->getMaybe();
+ defaultValue = o->getDefaultValue();
p = o->getPath();
}
- void VariantReader::readAnyObject(const aron::type::VariantPtr& input, type::Maybe& maybe, Path& p)
+ void
+ VariantReader::readAnyObject(const aron::type::VariantPtr& input, type::Maybe& maybe, Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = type::AnyObject::DynamicCastAndCheck(input);
@@ -217,4 +314,4 @@ namespace armarx::aron::type::reader
maybe = o->getMaybe();
p = o->getPath();
}
-}
+} // namespace armarx::aron::type::reader
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h b/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h
index 5b9d592c08b4e4b43d80b3f960d90e92288b01f6..1f7c5cc8bb6a604af59b7dc557ba1ec66f12f8f6 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h
@@ -30,11 +30,9 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
-
namespace armarx::aron::type::reader
{
- class VariantReader :
- public ReaderInterface<const aron::type::VariantPtr>
+ class VariantReader : public ReaderInterface<const aron::type::VariantPtr>
{
public:
// constructors
@@ -42,27 +40,94 @@ namespace armarx::aron::type::reader
type::Descriptor getDescriptor(InputType& input) final;
- void readObject(InputType& input, std::string& name, std::vector<std::string>& templates, std::vector<std::string>& templateInstantiations, std::map<std::string, InputTypeNonConst>& memberTypes, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readList(InputType& input, InputTypeNonConst& acceptedType, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readDict(InputType& input, InputTypeNonConst& acceptedType, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readTuple(InputType& input, std::vector<InputTypeNonConst>& acceptedTypes, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readPair(InputType& input, InputTypeNonConst& acceptedType1, InputTypeNonConst& acceptedType2, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readObject(InputType& input,
+ std::string& name,
+ std::vector<std::string>& templates,
+ std::vector<std::string>& templateInstantiations,
+ std::map<std::string, InputTypeNonConst>& memberTypes,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readList(InputType& input,
+ InputTypeNonConst& acceptedType,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readDict(InputType& input,
+ InputTypeNonConst& acceptedType,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readTuple(InputType& input,
+ std::vector<InputTypeNonConst>& acceptedTypes,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readPair(InputType& input,
+ InputTypeNonConst& acceptedType1,
+ InputTypeNonConst& acceptedType2,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readNDArray(InputType& input, int& ndim, type::ndarray::ElementType& type, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readMatrix(InputType& input, int& rows, int& cols, type::matrix::ElementType& type, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readQuaternion(InputType& input, type::quaternion::ElementType& type, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readPointCloud(InputType& input, type::pointcloud::VoxelType& type, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readImage(InputType& input, type::image::PixelType& type, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readNDArray(InputType& input,
+ int& ndim,
+ type::ndarray::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readMatrix(InputType& input,
+ int& rows,
+ int& cols,
+ type::matrix::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readQuaternion(InputType& input,
+ type::quaternion::ElementType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readPointCloud(InputType& input,
+ type::pointcloud::VoxelType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readImage(InputType& input,
+ type::image::PixelType& type,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readIntEnum(InputType& input, std::string& name, std::map<std::string, int>& acceptedValues, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readIntEnum(InputType& input,
+ std::string& name,
+ std::map<std::string, int>& acceptedValues,
+ std::string& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readInt(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readLong(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readFloat(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readDouble(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readString(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readBool(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readInt(InputType& input,
+ std::optional<int>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readLong(InputType& input,
+ std::optional<long>& defaultvalue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readFloat(InputType& input,
+ std::optional<float>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readDouble(InputType& input,
+ std::optional<double>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readString(InputType& input,
+ std::optional<std::string>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readBool(InputType& input,
+ std::optional<bool>& defaultValue,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
- void readAnyObject(InputType& input, type::Maybe& maybe, Path& p = *std::unique_ptr<Path>(new Path())) override;
+ void readAnyObject(InputType& input,
+ type::Maybe& maybe,
+ Path& p = *std::unique_ptr<Path>(new Path())) override;
};
-}
+} // namespace armarx::aron::type::reader
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
index 159c604b10c0e0bb4d761cc855538240e879abbe..b1a116365b2e564ed70d1e2282a42d663a11e67b 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
@@ -25,29 +25,41 @@
#include "NlohmannJSONWriter.h"
// Constantes
-#include "../../json/Data.h"
#include <RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.h>
+#include "../../json/Data.h"
namespace armarx::aron::type::writer
{
namespace
{
/// Set important members for json object (aron meta information)
- void setupAronMetaInformationForType(nlohmann::json& json, const std::string& type, const type::Maybe& maybe, const Path& p)
+ void
+ setupAronMetaInformationForType(nlohmann::json& json,
+ const std::string& type,
+ const type::Maybe& maybe,
+ const Path& p)
{
json[rw::json::constantes::TYPE_SLUG] = type;
json[rw::json::constantes::PATH_SLUG] = p.getPath();
json[rw::json::constantes::MAYBE_SLUG] = rw::json::conversion::Maybe2String.at(maybe);
}
- }
+ } // namespace
- type::Descriptor NlohmannJSONWriter::getDescriptor(ReturnTypeConst& input)
+ type::Descriptor
+ NlohmannJSONWriter::getDescriptor(ReturnTypeConst& input)
{
return ConstNlohmannJSONVisitor::GetDescriptor(input);
}
- nlohmann::json NlohmannJSONWriter::writeObject(const std::string& name, const std::vector<std::string>& templates, const std::vector<std::string>& templateInstantiations, const std::map<std::string, nlohmann::json>& memberTypes, const std::optional<nlohmann::json>& extends, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeObject(const std::string& name,
+ const std::vector<std::string>& templates,
+ const std::vector<std::string>& templateInstantiations,
+ const std::map<std::string, nlohmann::json>& memberTypes,
+ const std::optional<nlohmann::json>& extends,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::OBJECT_TYPENAME_SLUG, maybe, p);
@@ -73,7 +85,10 @@ namespace armarx::aron::type::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeList(const nlohmann::json& acceptedType, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeList(const nlohmann::json& acceptedType,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::LIST_TYPENAME_SLUG, maybe, p);
@@ -81,7 +96,10 @@ namespace armarx::aron::type::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeDict(const nlohmann::json& acceptedType, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeDict(const nlohmann::json& acceptedType,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::DICT_TYPENAME_SLUG, maybe, p);
@@ -89,7 +107,11 @@ namespace armarx::aron::type::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writePair(const nlohmann::json& acceptedType1, const nlohmann::json& acceptedType2, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writePair(const nlohmann::json& acceptedType1,
+ const nlohmann::json& acceptedType2,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::PAIR_TYPENAME_SLUG, maybe, p);
@@ -97,7 +119,10 @@ namespace armarx::aron::type::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeTuple(const std::vector<nlohmann::json>& acceptedTypes, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeTuple(const std::vector<nlohmann::json>& acceptedTypes,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::TUPLE_TYPENAME_SLUG, maybe, p);
@@ -105,53 +130,90 @@ namespace armarx::aron::type::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeNDArray(const int ndim, const type::ndarray::ElementType type, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeNDArray(const int ndim,
+ const type::ndarray::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::NDARRAY_TYPENAME_SLUG, maybe, p);
o[rw::json::constantes::USED_TYPE_SLUG] = rw::json::conversion::NDArrayType2String.at(type);
o[rw::json::constantes::DIMENSIONS_SLUG] = ndim;
+ o[rw::json::constantes::DEFAULT_SLUG] = defaultValue;
return o;
}
- nlohmann::json NlohmannJSONWriter::writeMatrix(const int rows, const int cols, const type::matrix::ElementType type, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeMatrix(const int rows,
+ const int cols,
+ const type::matrix::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::MATRIX_TYPENAME_SLUG, maybe, p);
o[rw::json::constantes::USED_TYPE_SLUG] = rw::json::conversion::MatrixType2String.at(type);
o[rw::json::constantes::DIMENSIONS_SLUG] = {rows, cols};
+ o[rw::json::constantes::DEFAULT_SLUG] = defaultValue;
return o;
}
- nlohmann::json NlohmannJSONWriter::writeQuaternion(const type::quaternion::ElementType type, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeQuaternion(const type::quaternion::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
- setupAronMetaInformationForType(o, rw::json::constantes::QUATERNION_TYPENAME_SLUG, maybe, p);
- o[rw::json::constantes::USED_TYPE_SLUG] = rw::json::conversion::QuaternionType2String.at(type);
+ setupAronMetaInformationForType(
+ o, rw::json::constantes::QUATERNION_TYPENAME_SLUG, maybe, p);
+ o[rw::json::constantes::USED_TYPE_SLUG] =
+ rw::json::conversion::QuaternionType2String.at(type);
+ o[rw::json::constantes::DEFAULT_SLUG] = defaultValue;
return o;
}
- nlohmann::json NlohmannJSONWriter::writeImage(const type::image::PixelType type, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeImage(const type::image::PixelType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::IMAGE_TYPENAME_SLUG, maybe, p);
o[rw::json::constantes::USED_TYPE_SLUG] = rw::json::conversion::PixelType2String.at(type);
+ o[rw::json::constantes::DEFAULT_SLUG] = defaultValue;
return o;
}
- nlohmann::json NlohmannJSONWriter::writePointCloud(const type::pointcloud::VoxelType type, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writePointCloud(const type::pointcloud::VoxelType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
- setupAronMetaInformationForType(o, rw::json::constantes::POINT_CLOUD_TYPENAME_SLUG, maybe, p);
+ setupAronMetaInformationForType(
+ o, rw::json::constantes::POINT_CLOUD_TYPENAME_SLUG, maybe, p);
o[rw::json::constantes::USED_TYPE_SLUG] = rw::json::conversion::VoxelType2String.at(type);
+ o[rw::json::constantes::DEFAULT_SLUG] = defaultValue;
return o;
}
- nlohmann::json NlohmannJSONWriter::writeIntEnum(const std::string& name, const std::map<std::string, int>& acceptedValues, const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeIntEnum(const std::string& name,
+ const std::map<std::string, int>& acceptedValues,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::INT_ENUM_TYPENAME_SLUG, maybe, p);
o[rw::json::constantes::NAME_SLUG] = name;
+ o[rw::json::constantes::DEFAULT_SLUG] = defaultValue;
o[rw::json::constantes::ELEMENTS_SLUG] = nlohmann::json(nlohmann::json::value_t::object);
for (const auto& [key, value] : acceptedValues)
@@ -161,52 +223,83 @@ namespace armarx::aron::type::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeInt(const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeInt(const std::optional<int>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::INT_TYPENAME_SLUG, maybe, p);
+ o[rw::json::constantes::DEFAULT_SLUG] =
+ defaultValue.has_value() ? std::to_string(*defaultValue) : "";
return o;
}
- nlohmann::json NlohmannJSONWriter::writeLong(const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeLong(const std::optional<long>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::LONG_TYPENAME_SLUG, maybe, p);
+ o[rw::json::constantes::DEFAULT_SLUG] =
+ defaultValue.has_value() ? std::to_string(*defaultValue) : "";
return o;
}
- nlohmann::json NlohmannJSONWriter::writeFloat(const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeFloat(const std::optional<float>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::FLOAT_TYPENAME_SLUG, maybe, p);
+ o[rw::json::constantes::DEFAULT_SLUG] =
+ defaultValue.has_value() ? std::to_string(*defaultValue) : "";
return o;
}
- nlohmann::json NlohmannJSONWriter::writeDouble(const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeDouble(const std::optional<double>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::DOUBLE_TYPENAME_SLUG, maybe, p);
+ o[rw::json::constantes::DEFAULT_SLUG] =
+ defaultValue.has_value() ? std::to_string(*defaultValue) : "";
return o;
}
- nlohmann::json NlohmannJSONWriter::writeString(const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeString(const std::optional<std::string>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::STRING_TYPENAME_SLUG, maybe, p);
+ o[rw::json::constantes::DEFAULT_SLUG] = defaultValue.has_value() ? *defaultValue : "";
return o;
}
- nlohmann::json NlohmannJSONWriter::writeBool(const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeBool(const std::optional<bool>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::BOOL_TYPENAME_SLUG, maybe, p);
+ o[rw::json::constantes::DEFAULT_SLUG] =
+ defaultValue.has_value() ? std::to_string(*defaultValue) : "";
return o;
}
- nlohmann::json NlohmannJSONWriter::writeAnyObject(const type::Maybe maybe, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeAnyObject(const type::Maybe maybe, const Path& p)
{
nlohmann::json o;
- setupAronMetaInformationForType(o, rw::json::constantes::ANY_OBJECT_TYPENAME_SLUG, maybe, p);
+ setupAronMetaInformationForType(
+ o, rw::json::constantes::ANY_OBJECT_TYPENAME_SLUG, maybe, p);
return o;
}
-}
+} // namespace armarx::aron::type::writer
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h b/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
index 25e89bdf09322528c30aac0d94a41d6483497c25..ecf22f5f7ed3a0d0d76fd4c8ace2da9839b7cc8c 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
@@ -22,8 +22,8 @@
// STD / STL
#include <memory>
-#include <stack>
#include <sstream>
+#include <stack>
// Simox
#include <SimoxUtility/json.h>
@@ -33,35 +33,83 @@
namespace armarx::aron::type::writer
{
- class NlohmannJSONWriter :
- public WriterInterface<nlohmann::json>
+ class NlohmannJSONWriter : public WriterInterface<nlohmann::json>
{
public:
NlohmannJSONWriter() = default;
type::Descriptor getDescriptor(ReturnTypeConst& input) final;
- ReturnType writeObject(const std::string& name, const std::vector<std::string>& templates, const std::vector<std::string>& templateInstantiations, const std::map<std::string, ReturnType>& memberTypes, const std::optional<ReturnType>& extends, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeList(const ReturnType& acceptedType, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeDict(const ReturnType& acceptedType, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writePair(const ReturnType& acceptedType1, const ReturnType& acceptedType2, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeTuple(const std::vector<ReturnType>& acceptedTypes, const type::Maybe maybe, const Path& p = Path()) override;
+ ReturnType writeObject(const std::string& name,
+ const std::vector<std::string>& templates,
+ const std::vector<std::string>& templateInstantiations,
+ const std::map<std::string, ReturnType>& memberTypes,
+ const std::optional<ReturnType>& extends,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeList(const ReturnType& acceptedType,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeDict(const ReturnType& acceptedType,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writePair(const ReturnType& acceptedType1,
+ const ReturnType& acceptedType2,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeTuple(const std::vector<ReturnType>& acceptedTypes,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
- ReturnType writeNDArray(const int ndim, const type::ndarray::ElementType, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeMatrix(const int rows, const int cols, const type::matrix::ElementType type, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeQuaternion(const type::quaternion::ElementType, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeImage(const type::image::PixelType, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writePointCloud(const type::pointcloud::VoxelType, const type::Maybe maybe, const Path& p = Path()) override;
+ ReturnType writeNDArray(const int ndim,
+ const type::ndarray::ElementType,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeMatrix(const int rows,
+ const int cols,
+ const type::matrix::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeQuaternion(const type::quaternion::ElementType,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeImage(const type::image::PixelType,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writePointCloud(const type::pointcloud::VoxelType,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
- ReturnType writeIntEnum(const std::string& name, const std::map<std::string, int>& acceptedValues, const type::Maybe maybe, const Path& p = Path()) override;
+ ReturnType writeIntEnum(const std::string& name,
+ const std::map<std::string, int>& acceptedValues,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
- ReturnType writeInt(const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeLong(const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeFloat(const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeDouble(const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeString(const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeBool(const type::Maybe maybe, const Path& p = Path()) override;
+ ReturnType writeInt(const std::optional<int>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeLong(const std::optional<long>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeFloat(const std::optional<float>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeDouble(const std::optional<double>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeString(const std::optional<std::string>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeBool(const std::optional<bool>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
ReturnType writeAnyObject(const type::Maybe maybe, const Path& p = Path()) override;
};
-}
+} // namespace armarx::aron::type::writer
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.cpp b/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.cpp
index 87af0839552de8e5a66a3e48bb2a95763f6636cb..64993469993a2b8bf3d0bad17cdc74a8cd59659f 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.cpp
@@ -20,23 +20,35 @@
#include "VariantWriter.h"
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
-
#include <memory>
#include <numeric>
+
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
namespace armarx::aron::type::writer
{
- type::Descriptor VariantWriter::getDescriptor(ReturnTypeConst& input)
+ type::Descriptor
+ VariantWriter::getDescriptor(ReturnTypeConst& input)
{
return ConstVariantVisitor::GetDescriptor(input);
}
- aron::type::VariantPtr VariantWriter::writeObject(const std::string& name, const std::vector<std::string>& templates, const std::vector<std::string>& templateInstantiations, const std::map<std::string, aron::type::VariantPtr>& memberTypes, const std::optional<aron::type::VariantPtr>& extends, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeObject(const std::string& name,
+ const std::vector<std::string>& templates,
+ const std::vector<std::string>& templateInstantiations,
+ const std::map<std::string, aron::type::VariantPtr>& memberTypes,
+ const std::optional<aron::type::VariantPtr>& extends,
+ const type::Maybe maybe,
+ const Path& p)
{
- auto o = std::make_shared<type::Object>(name, templates, templateInstantiations, memberTypes, p);
+ auto o = std::make_shared<type::Object>(p);
+ o->setObjectName(name);
+ o->setTemplates(templates);
+ o->setTemplateInstantiations(templateInstantiations);
+ o->setMemberTypes(memberTypes);
o->setMaybe(maybe);
if (extends.has_value())
{
@@ -46,127 +58,208 @@ namespace armarx::aron::type::writer
return o;
}
- aron::type::VariantPtr VariantWriter::writeList(const aron::type::VariantPtr& acceptedType, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeList(const aron::type::VariantPtr& acceptedType,
+ const type::Maybe maybe,
+ const Path& p)
{
- auto o = std::make_shared<type::List>(acceptedType, p);
+ auto o = std::make_shared<type::List>(p);
+ o->setAcceptedType(acceptedType);
o->setMaybe(maybe);
return o;
}
- aron::type::VariantPtr VariantWriter::writeDict(const aron::type::VariantPtr& acceptedType, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeDict(const aron::type::VariantPtr& acceptedType,
+ const type::Maybe maybe,
+ const Path& p)
{
- auto o = std::make_shared<type::Dict>(acceptedType, p);
+ auto o = std::make_shared<type::Dict>(p);
+ o->setAcceptedType(acceptedType);
o->setMaybe(maybe);
return o;
}
- aron::type::VariantPtr VariantWriter::writePair(const aron::type::VariantPtr& acceptedType1, const aron::type::VariantPtr& acceptedType2, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writePair(const aron::type::VariantPtr& acceptedType1,
+ const aron::type::VariantPtr& acceptedType2,
+ const type::Maybe maybe,
+ const Path& p)
{
- auto o = std::make_shared<type::Pair>(acceptedType1, acceptedType2, p);
+ auto o = std::make_shared<type::Pair>(p);
+ o->setFirstAcceptedType(acceptedType1);
+ o->setSecondAcceptedType(acceptedType2);
o->setMaybe(maybe);
return o;
}
- aron::type::VariantPtr VariantWriter::writeTuple(const std::vector<aron::type::VariantPtr>& acceptedTypes, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeTuple(const std::vector<aron::type::VariantPtr>& acceptedTypes,
+ const type::Maybe maybe,
+ const Path& p)
{
- auto o = std::make_shared<type::Tuple>(acceptedTypes, p);
+ auto o = std::make_shared<type::Tuple>(p);
+ o->setAcceptedTypes(acceptedTypes);
o->setMaybe(maybe);
return o;
}
- aron::type::VariantPtr VariantWriter::writeNDArray(const int ndim, const type::ndarray::ElementType, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeNDArray(const int ndim,
+ const type::ndarray::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
- auto o = std::make_shared<type::NDArray>(p);
- return o;
+ auto o = std::make_shared<type::NDArray>(p);
+ o->setMaybe(maybe);
+ o->setElementType(type);
+ o->setDefaultValue(defaultValue);
+ return o;
}
- aron::type::VariantPtr VariantWriter::writeMatrix(const int rows, const int cols, const type::matrix::ElementType type, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeMatrix(const int rows,
+ const int cols,
+ const type::matrix::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
auto o = std::make_shared<type::Matrix>(p);
o->setMaybe(maybe);
o->setRows(rows);
o->setCols(cols);
o->setElementType(type);
+ o->setDefaultValue(defaultValue);
return o;
}
- aron::type::VariantPtr VariantWriter::writeQuaternion(const type::quaternion::ElementType type, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeQuaternion(const type::quaternion::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
auto o = std::make_shared<type::Quaternion>(p);
o->setMaybe(maybe);
o->setElementType(type);
+ o->setDefaultValue(defaultValue);
return o;
}
- aron::type::VariantPtr VariantWriter::writeImage(const type::image::PixelType type, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeImage(const type::image::PixelType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
auto o = std::make_shared<type::Image>(p);
o->setMaybe(maybe);
o->setPixelType(type);
+ o->setDefaultValue(defaultValue);
return o;
}
- aron::type::VariantPtr VariantWriter::writePointCloud(const type::pointcloud::VoxelType type, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writePointCloud(const type::pointcloud::VoxelType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
auto o = std::make_shared<type::PointCloud>(p);
o->setMaybe(maybe);
o->setVoxelType(type);
+ o->setDefaultValue(defaultValue);
return o;
}
- aron::type::VariantPtr VariantWriter::writeIntEnum(const std::string& name, const std::map<std::string, int>& acceptedValues, const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeIntEnum(const std::string& name,
+ const std::map<std::string, int>& acceptedValues,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
- auto o = std::make_shared<type::IntEnum>(name, acceptedValues, p);
+ auto o = std::make_shared<type::IntEnum>(p);
+ o->setEnumName(name);
+ o->setAcceptedValueMap(acceptedValues);
+ o->setDefaultValueName(defaultValue);
o->setMaybe(maybe);
return o;
}
- aron::type::VariantPtr VariantWriter::writeInt(const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeInt(const std::optional<int>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
auto o = std::make_shared<type::Int>(p);
+ o->setDefaultValue(defaultValue);
o->setMaybe(maybe);
return o;
}
- aron::type::VariantPtr VariantWriter::writeLong(const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeLong(const std::optional<long>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
auto o = std::make_shared<type::Long>(p);
+ o->setDefaultValue(defaultValue);
o->setMaybe(maybe);
return o;
}
- aron::type::VariantPtr VariantWriter::writeFloat(const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeFloat(const std::optional<float>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
auto o = std::make_shared<type::Float>(p);
+ o->setDefaultValue(defaultValue);
o->setMaybe(maybe);
return o;
}
- aron::type::VariantPtr VariantWriter::writeDouble(const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeDouble(const std::optional<double>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
auto o = std::make_shared<type::Double>(p);
+ o->setDefaultValue(defaultValue);
o->setMaybe(maybe);
return o;
}
- aron::type::VariantPtr VariantWriter::writeString(const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeString(const std::optional<std::string>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
auto o = std::make_shared<type::String>(p);
+ o->setDefaultValue(defaultValue);
o->setMaybe(maybe);
return o;
}
- aron::type::VariantPtr VariantWriter::writeBool(const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeBool(const std::optional<bool>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p)
{
auto o = std::make_shared<type::Bool>(p);
o->setMaybe(maybe);
+ o->setDefaultValue(defaultValue);
return o;
}
- aron::type::VariantPtr VariantWriter::writeAnyObject(const type::Maybe maybe, const Path& p)
+ aron::type::VariantPtr
+ VariantWriter::writeAnyObject(const type::Maybe maybe, const Path& p)
{
auto o = std::make_shared<type::AnyObject>(p);
o->setMaybe(maybe);
return o;
}
-}
+} // namespace armarx::aron::type::writer
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h b/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h
index 92fea0d713c496a4b09915dc392252a5c8c9275a..b9dcf91d60ff94544e2af333f9cc62d8cb01933f 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h
@@ -20,44 +20,91 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/type/rw/Writer.h>
-#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
-
#include <memory>
#include <stack>
+#include <RobotAPI/libraries/aron/core/type/rw/Writer.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
namespace armarx::aron::type::writer
{
- class VariantWriter :
- public type::WriterInterface<aron::type::VariantPtr>
+ class VariantWriter : public type::WriterInterface<aron::type::VariantPtr>
{
public:
VariantWriter() = default;
type::Descriptor getDescriptor(ReturnTypeConst& input) final;
- ReturnType writeObject(const std::string& name, const std::vector<std::string>& templates, const std::vector<std::string>& templateInstantiations, const std::map<std::string, ReturnType>& memberTypes, const std::optional<ReturnType>& extends, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeList(const ReturnType& acceptedType, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeDict(const ReturnType& acceptedType, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writePair(const ReturnType& acceptedType1, const ReturnType& acceptedType2, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeTuple(const std::vector<ReturnType>& acceptedTypes, const type::Maybe maybe, const Path& p = Path()) override;
+ ReturnType writeObject(const std::string& name,
+ const std::vector<std::string>& templates,
+ const std::vector<std::string>& templateInstantiations,
+ const std::map<std::string, ReturnType>& memberTypes,
+ const std::optional<ReturnType>& extends,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeList(const ReturnType& acceptedType,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeDict(const ReturnType& acceptedType,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writePair(const ReturnType& acceptedType1,
+ const ReturnType& acceptedType2,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeTuple(const std::vector<ReturnType>& acceptedTypes,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
- ReturnType writeNDArray(const int ndim, const type::ndarray::ElementType, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeMatrix(const int rows, const int cols, const type::matrix::ElementType type, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeQuaternion(const type::quaternion::ElementType, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeImage(const type::image::PixelType, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writePointCloud(const type::pointcloud::VoxelType, const type::Maybe maybe, const Path& p = Path()) override;
+ ReturnType writeNDArray(const int ndim,
+ const type::ndarray::ElementType,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeMatrix(const int rows,
+ const int cols,
+ const type::matrix::ElementType type,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeQuaternion(const type::quaternion::ElementType,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeImage(const type::image::PixelType,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writePointCloud(const type::pointcloud::VoxelType,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
- ReturnType writeIntEnum(const std::string& name, const std::map<std::string, int>& acceptedValues, const type::Maybe maybe, const Path& p = Path()) override;
+ ReturnType writeIntEnum(const std::string& name,
+ const std::map<std::string, int>& acceptedValues,
+ const std::string& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
- ReturnType writeInt(const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeLong(const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeFloat(const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeDouble(const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeString(const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeBool(const type::Maybe maybe, const Path& p = Path()) override;
+ ReturnType writeInt(const std::optional<int>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeLong(const std::optional<long>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeFloat(const std::optional<float>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeDouble(const std::optional<double>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeString(const std::optional<std::string>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
+ ReturnType writeBool(const std::optional<bool>& defaultValue,
+ const type::Maybe maybe,
+ const Path& p = Path()) override;
ReturnType writeAnyObject(const type::Maybe maybe, const Path& p = Path()) override;
};
-}
+} // namespace armarx::aron::type::writer
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
index 405a5e7487213fc8097acc09986a8012b81edfad..21ba5cd0984b9077f668886327b57e8722b1f553 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
@@ -32,32 +32,91 @@
namespace armarx::aron::type
{
// Access method
- std::unique_ptr<type::Variant> VariantFactory::create(const type::dto::GenericType& aron, const Path& path) const
+ std::unique_ptr<type::Variant>
+ VariantFactory::create(const type::dto::GenericType& aron, const Path& path) const
{
+ auto v = aron.VERSION;
+ auto v_str =
+ std::to_string(v.MAJOR) + "." + std::to_string(v.MINOR) + "." + std::to_string(v.PATCH);
+
+ auto d = aron::Version();
+ auto d_str =
+ std::to_string(d.MAJOR) + "." + std::to_string(d.MINOR) + "." + std::to_string(d.PATCH);
+
+ if (v != d)
+ {
+ ARMARX_WARNING
+ << "Found inconsistencies in aron versions. This may lead to ice marshalling "
+ "exceptions. Please make sure to update the sender and receiver packages. "
+ << v_str << " != " << d_str;
+ }
+
auto descriptor = type::Aron2Descriptor(aron);
- switch(descriptor)
+ switch (descriptor)
{
- case type::Descriptor::LIST: return std::make_unique<type::List>(dynamic_cast<const type::dto::List&>(aron), path);
- case type::Descriptor::DICT: return std::make_unique<type::Dict>(dynamic_cast<const type::dto::Dict&>(aron), path);
- case type::Descriptor::PAIR: return std::make_unique<type::Pair>(dynamic_cast<const type::dto::Pair&>(aron), path);
- case type::Descriptor::TUPLE: return std::make_unique<type::Tuple>(dynamic_cast<const type::dto::Tuple&>(aron), path);
- case type::Descriptor::OBJECT: return std::make_unique<type::Object>(dynamic_cast<const type::dto::AronObject&>(aron), path);
- case type::Descriptor::NDARRAY: return std::make_unique<type::NDArray>(dynamic_cast<const type::dto::NDArray&>(aron), path);
- case type::Descriptor::MATRIX: return std::make_unique<type::Matrix>(dynamic_cast<const type::dto::Matrix&>(aron), path);
- case type::Descriptor::IMAGE: return std::make_unique<type::Image>(dynamic_cast<const type::dto::Image&>(aron), path);
- case type::Descriptor::POINTCLOUD: return std::make_unique<type::PointCloud>(dynamic_cast<const type::dto::PointCloud&>(aron), path);
- case type::Descriptor::QUATERNION: return std::make_unique<type::Quaternion>(dynamic_cast<const type::dto::Quaternion&>(aron), path);
- case type::Descriptor::INT_ENUM: return std::make_unique<type::IntEnum>(dynamic_cast<const type::dto::IntEnum&>(aron), path);
- case type::Descriptor::INT: return std::make_unique<type::Int>(dynamic_cast<const type::dto::AronInt&>(aron), path);
- case type::Descriptor::LONG: return std::make_unique<type::Long>(dynamic_cast<const type::dto::AronLong&>(aron), path);
- case type::Descriptor::FLOAT: return std::make_unique<type::Float>(dynamic_cast<const type::dto::AronFloat&>(aron), path);
- case type::Descriptor::DOUBLE: return std::make_unique<type::Double>(dynamic_cast<const type::dto::AronDouble&>(aron), path);
- case type::Descriptor::STRING: return std::make_unique<type::String>(dynamic_cast<const type::dto::AronString&>(aron), path);
- case type::Descriptor::BOOL: return std::make_unique<type::Bool>(dynamic_cast<const type::dto::AronBool&>(aron), path);
- case type::Descriptor::ANY_OBJECT: return std::make_unique<type::AnyObject>(dynamic_cast<const type::dto::AnyObject&>(aron), path);
- case type::Descriptor::UNKNOWN: break;
+ case type::Descriptor::LIST:
+ return std::make_unique<type::List>(dynamic_cast<const type::dto::List&>(aron),
+ path);
+ case type::Descriptor::DICT:
+ return std::make_unique<type::Dict>(dynamic_cast<const type::dto::Dict&>(aron),
+ path);
+ case type::Descriptor::PAIR:
+ return std::make_unique<type::Pair>(dynamic_cast<const type::dto::Pair&>(aron),
+ path);
+ case type::Descriptor::TUPLE:
+ return std::make_unique<type::Tuple>(dynamic_cast<const type::dto::Tuple&>(aron),
+ path);
+ case type::Descriptor::OBJECT:
+ return std::make_unique<type::Object>(
+ dynamic_cast<const type::dto::AronObject&>(aron), path);
+ case type::Descriptor::NDARRAY:
+ return std::make_unique<type::NDArray>(
+ dynamic_cast<const type::dto::NDArray&>(aron), path);
+ case type::Descriptor::MATRIX:
+ return std::make_unique<type::Matrix>(dynamic_cast<const type::dto::Matrix&>(aron),
+ path);
+ case type::Descriptor::IMAGE:
+ return std::make_unique<type::Image>(dynamic_cast<const type::dto::Image&>(aron),
+ path);
+ case type::Descriptor::POINTCLOUD:
+ return std::make_unique<type::PointCloud>(
+ dynamic_cast<const type::dto::PointCloud&>(aron), path);
+ case type::Descriptor::QUATERNION:
+ return std::make_unique<type::Quaternion>(
+ dynamic_cast<const type::dto::Quaternion&>(aron), path);
+ case type::Descriptor::INT_ENUM:
+ return std::make_unique<type::IntEnum>(
+ dynamic_cast<const type::dto::IntEnum&>(aron), path);
+ case type::Descriptor::INT:
+ return std::make_unique<type::Int>(dynamic_cast<const type::dto::AronInt&>(aron),
+ path);
+ case type::Descriptor::LONG:
+ return std::make_unique<type::Long>(dynamic_cast<const type::dto::AronLong&>(aron),
+ path);
+ case type::Descriptor::FLOAT:
+ return std::make_unique<type::Float>(
+ dynamic_cast<const type::dto::AronFloat&>(aron), path);
+ case type::Descriptor::DOUBLE:
+ return std::make_unique<type::Double>(
+ dynamic_cast<const type::dto::AronDouble&>(aron), path);
+ case type::Descriptor::STRING:
+ return std::make_unique<type::String>(
+ dynamic_cast<const type::dto::AronString&>(aron), path);
+ case type::Descriptor::BOOL:
+ return std::make_unique<type::Bool>(dynamic_cast<const type::dto::AronBool&>(aron),
+ path);
+ case type::Descriptor::ANY_OBJECT:
+ return std::make_unique<type::AnyObject>(
+ dynamic_cast<const type::dto::AnyObject&>(aron), path);
+ case type::Descriptor::UNKNOWN:
+ break;
}
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Got an unknown descriptor", std::to_string((int) descriptor) + " aka " + defaultconversion::string::Descriptor2String.at(descriptor), path);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__,
+ "Got an unknown descriptor",
+ std::to_string((int)descriptor) + " aka " +
+ defaultconversion::string::Descriptor2String.at(descriptor),
+ path);
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Variant.h b/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
index ab996582c630a9083bafb45bc7ec19755e59bb67..6a1e2afa366ff9ae61bef7871d1ddaff9f1b070e 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Variant.h
@@ -24,19 +24,19 @@
#pragma once
// STD/STL
-#include <memory>
-#include <vector>
#include <map>
+#include <memory>
#include <string>
+#include <vector>
// Simox
// #include <SimoxUtility/algorithm/string.h>
// ArmarX
-#include <RobotAPI/libraries/aron/core/Path.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/Path.h>
namespace armarx::aron::type
{
@@ -64,15 +64,17 @@ namespace armarx::aron::type
public:
// constructors
Variant(const type::Descriptor& descriptor, const Path& path = Path()) :
- descriptor(descriptor),
- path(path)
+ descriptor(descriptor), path(path)
{
}
+
virtual ~Variant() = default;
// operators
virtual bool operator==(const Variant& other) const = 0;
- bool operator==(const VariantPtr& other) const
+
+ bool
+ operator==(const VariantPtr& other) const
{
if (!other)
{
@@ -87,17 +89,20 @@ namespace armarx::aron::type
static VariantPtr FromAronDTO(const type::dto::GenericType&, const Path& = Path());
// public methods
- type::Descriptor getDescriptor() const
+ type::Descriptor
+ getDescriptor() const
{
return descriptor;
}
- Path getPath() const
+ Path
+ getPath() const
{
return path;
}
- std::string pathToString() const
+ std::string
+ pathToString() const
{
return path.toString();
}
@@ -139,4 +144,4 @@ namespace armarx::aron::type
template <class T>
concept isVariant = std::is_base_of<Variant, T>::value;
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp
index edaeccf90a02ec918fdbc90733a8209616e0bc1b..18dcb97e8c0b33b16db4b076d9481bce36cd2f51 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.cpp
@@ -24,15 +24,12 @@
// Header
#include "Dict.h"
-
namespace armarx::aron::type
{
// constructors
- Dict::Dict(const VariantPtr& acceptedType, const Path& path) :
- detail::ContainerVariant<type::dto::Dict, Dict>(type::Descriptor::DICT, path),
- acceptedType(acceptedType)
+ Dict::Dict(const Path& path) :
+ detail::ContainerVariant<type::dto::Dict, Dict>(type::Descriptor::DICT, path)
{
- aron->acceptedType = acceptedType->toAronDTO();
}
Dict::Dict(const type::dto::Dict& o, const Path& path) :
@@ -41,75 +38,87 @@ namespace armarx::aron::type
{
}
- VariantPtr Dict::getAcceptedType() const
+ VariantPtr
+ Dict::getAcceptedType() const
{
ARMARX_CHECK_NOT_NULL(acceptedType);
return acceptedType;
}
- void Dict::setAcceptedType(const VariantPtr& a)
+ void
+ Dict::setAcceptedType(const VariantPtr& a)
{
ARMARX_CHECK_NOT_NULL(a);
aron->acceptedType = a->toAronDTO();
acceptedType = a;
}
- type::dto::DictPtr Dict::toDictDTO() const
+ type::dto::DictPtr
+ Dict::toDictDTO() const
{
return aron;
}
// virtual implementations
- std::vector<VariantPtr> Dict::getChildren() const
+ std::vector<VariantPtr>
+ Dict::getChildren() const
{
return {acceptedType};
}
- size_t Dict::childrenSize() const
+ size_t
+ Dict::childrenSize() const
{
return 1;
}
- std::string Dict::getShortName() const
+ std::string
+ Dict::getShortName() const
{
return "Dict<" + acceptedType->getShortName() + ">";
}
- std::string Dict::getFullName() const
+ std::string
+ Dict::getFullName() const
{
return "armarx::aron::type::Dict<" + acceptedType->getFullName() + ">";
}
- std::string Dict::GetFullNamePrefix()
+ std::string
+ Dict::GetFullNamePrefix()
{
return "armarx::aron::type::Dict";
}
- std::string Dict::GetNamePrefix()
+ std::string
+ Dict::GetNamePrefix()
{
return "Dict";
}
- VariantPtr Dict::navigateAbsolute(const Path& path) const
+ VariantPtr
+ Dict::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
}
if (path.size() != 1)
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not get more than 1 value from a dict.", path);
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "Could not get more than 1 value from a dict.", path);
}
std::string el = path.getFirstElement();
if (el != "::accepted_type")
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
}
return acceptedType;
}
-}
-
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
index 7ebbf72eaa7a7582d9b33157279b857c14756540..9ae860df56869350ba2c6e56127ce8d21d6a3b46 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Dict.h
@@ -24,8 +24,8 @@
#pragma once
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Base Class
#include "../detail/ContainerVariant.h"
@@ -37,12 +37,11 @@ namespace armarx::aron::type
* A dict only has an accepted type (a variant)
* for the corresponding data object @see data/variant/container/Dict.h
*/
- class Dict :
- public detail::ContainerVariant<type::dto::Dict, Dict>
+ class Dict : public detail::ContainerVariant<type::dto::Dict, Dict>
{
public:
// constructors
- Dict(const VariantPtr& acceptedType, const Path& path = Path());
+ Dict(const Path& path = Path());
Dict(const type::dto::Dict&, const Path& path = Path());
// public member functions
@@ -66,4 +65,4 @@ namespace armarx::aron::type
// members
VariantPtr acceptedType;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp
index 958596b7e60f201e565d7082d2e26c7107d7ce05..a5d036f35061ca17107e2effcd91795153d9f499 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/List.cpp
@@ -27,11 +27,9 @@
namespace armarx::aron::type
{
// constructors
- List::List(const VariantPtr& acceptedType, const Path& path) :
- detail::ContainerVariant<type::dto::List, List>(type::Descriptor::LIST, path),
- acceptedType(acceptedType)
+ List::List(const Path& path) :
+ detail::ContainerVariant<type::dto::List, List>(type::Descriptor::LIST, path)
{
- aron->acceptedType = acceptedType->toAronDTO();
}
List::List(const type::dto::List& o, const Path& path) :
@@ -41,12 +39,14 @@ namespace armarx::aron::type
}
// Member functions
- VariantPtr List::getAcceptedType() const
+ VariantPtr
+ List::getAcceptedType() const
{
return acceptedType;
}
- void List::setAcceptedType(const VariantPtr& a)
+ void
+ List::setAcceptedType(const VariantPtr& a)
{
ARMARX_CHECK_NOT_NULL(a);
aron->acceptedType = a->toAronDTO();
@@ -54,61 +54,72 @@ namespace armarx::aron::type
}
// static methods
- type::dto::ListPtr List::toListDTO() const
+ type::dto::ListPtr
+ List::toListDTO() const
{
return aron;
}
// virtual implementations
- std::vector<VariantPtr> List::getChildren() const
+ std::vector<VariantPtr>
+ List::getChildren() const
{
return {acceptedType};
}
- size_t List::childrenSize() const
+ size_t
+ List::childrenSize() const
{
return 1;
}
- std::string List::getShortName() const
+ std::string
+ List::getShortName() const
{
return "List<" + acceptedType->getShortName() + ">";
}
- std::string List::getFullName() const
+ std::string
+ List::getFullName() const
{
return "armarx::aron::type::List<" + acceptedType->getFullName() + ">";
}
- std::string List::GetFullNamePrefix()
+ std::string
+ List::GetFullNamePrefix()
{
return "armarx::aron::type::List";
}
- std::string List::GetNamePrefix()
+ std::string
+ List::GetNamePrefix()
{
return "List";
}
- VariantPtr List::navigateAbsolute(const Path& path) const
+ VariantPtr
+ List::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
}
if (path.size() != 1)
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not get more than 1 value from a dict.", path);
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "Could not get more than 1 value from a dict.", path);
}
std::string el = path.getFirstElement();
if (el != "::accepted_type")
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
}
return acceptedType;
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/List.h b/source/RobotAPI/libraries/aron/core/type/variant/container/List.h
index 67db861e299cc4031f560fbaa6507b20f1f3fbc5..4334468eeff2c121d5b93bc1589eca547beb964c 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/List.h
@@ -36,12 +36,11 @@ namespace armarx::aron::type
* @brief The List class. It represents the list type
* It only has an accepted type (a variant)
*/
- class List :
- public detail::ContainerVariant<type::dto::List, List>
+ class List : public detail::ContainerVariant<type::dto::List, List>
{
public:
// constructors
- List(const VariantPtr& acceptedType, const Path& path = Path());
+ List(const Path& path = Path());
List(const type::dto::List&, const Path& path = Path());
// public member functions
@@ -65,4 +64,4 @@ namespace armarx::aron::type
// members
VariantPtr acceptedType;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
index f6d7d259a33220799098537cf1320d1a965bc4f7..bdcd00aa9914823bab7214631eb6fd06c5bbc2c0 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.cpp
@@ -24,26 +24,16 @@
// Header
#include "Object.h"
-
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/string_conversion.h>
-
namespace armarx::aron::type
{
// constructors
- Object::Object(const std::string& name, const std::vector<std::string>& templates, const std::vector<std::string>& templateInstantiations, const std::map<std::string, VariantPtr>& m, const Path& path) :
- detail::ContainerVariant<type::dto::AronObject, Object>(type::Descriptor::OBJECT, path),
- memberTypes(m)
+ Object::Object(const Path& path) :
+ detail::ContainerVariant<type::dto::AronObject, Object>(type::Descriptor::OBJECT, path)
{
- aron->templates = templates;
- aron->templateInstantiations = templateInstantiations;
- setObjectName(name);
- for (const auto& [key, value] : memberTypes)
- {
- aron->elementTypes[key] = value->toAronDTO();
- }
}
Object::Object(const type::dto::AronObject& o, const Path& path) :
@@ -60,7 +50,8 @@ namespace armarx::aron::type
}
}
- ObjectPtr Object::FromAronObjectDTO(const type::dto::AronObjectPtr& aron, const aron::Path& path)
+ ObjectPtr
+ Object::FromAronObjectDTO(const type::dto::AronObjectPtr& aron, const aron::Path& path)
{
if (!aron)
{
@@ -69,7 +60,8 @@ namespace armarx::aron::type
return std::make_shared<Object>(*aron, path);
}
- type::dto::AronObjectPtr Object::ToAronObjectDTO(const ObjectPtr& aron)
+ type::dto::AronObjectPtr
+ Object::ToAronObjectDTO(const ObjectPtr& aron)
{
if (!aron)
{
@@ -78,7 +70,8 @@ namespace armarx::aron::type
return aron->toAronObjectDTO();
}
- bool Object::checkObjectName(const std::string& s) const
+ bool
+ Object::checkObjectName(const std::string& s) const
{
if (s.empty())
{
@@ -89,7 +82,30 @@ namespace armarx::aron::type
}
// public member functions
- std::map<std::string, VariantPtr> Object::getMemberTypes() const
+ void
+ Object::setMemberTypes(const std::map<std::string, VariantPtr>& memberTypes)
+ {
+ this->memberTypes = memberTypes;
+ for (const auto& [key, value] : memberTypes)
+ {
+ aron->elementTypes[key] = value->toAronDTO();
+ }
+ }
+
+ void
+ Object::setTemplates(const std::vector<std::string>& templates)
+ {
+ aron->templates = templates;
+ }
+
+ void
+ Object::setTemplateInstantiations(const std::vector<std::string>& templateInstantiations) const
+ {
+ aron->templateInstantiations = templateInstantiations;
+ }
+
+ std::map<std::string, VariantPtr>
+ Object::getMemberTypes() const
{
std::map<std::string, VariantPtr> ret = memberTypes;
if (extends)
@@ -102,16 +118,23 @@ namespace armarx::aron::type
return ret;
}
- std::map<std::string, VariantPtr> Object::getDirectMemberTypes() const
+ std::map<std::string, VariantPtr>
+ Object::getDirectMemberTypes() const
{
return memberTypes;
}
- VariantPtr Object::getMemberType(const std::string& s) const
+ VariantPtr
+ Object::getMemberType(const std::string& s) const
{
- if (memberTypes.find(s) == memberTypes.end() and not (extends and extends->hasMemberType(s)))
+ if (memberTypes.find(s) == memberTypes.end() and not(extends and extends->hasMemberType(s)))
{
- throw error::ValueNotValidException("ObjectNavigator", "getMemberType", "Member not set. The list of all members is: " + simox::alg::to_string(simox::alg::get_keys(memberTypes)), s);
+ throw error::ValueNotValidException(
+ "ObjectNavigator",
+ "getMemberType",
+ "Member not set. The list of all members is: " +
+ simox::alg::to_string(simox::alg::get_keys(memberTypes)),
+ s);
}
if (memberTypes.find(s) == memberTypes.end())
{
@@ -120,11 +143,13 @@ namespace armarx::aron::type
return memberTypes.at(s);
}
- void Object::addMemberType(const std::string& k, const VariantPtr& v)
+ void
+ Object::addMemberType(const std::string& k, const VariantPtr& v)
{
if (k.empty())
{
- throw error::AronException(__PRETTY_FUNCTION__, "Cannot set an element with an empty key.", getPath());
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "Cannot set an element with an empty key.", getPath());
}
ARMARX_CHECK_NOT_NULL(v);
@@ -133,14 +158,16 @@ namespace armarx::aron::type
memberTypes[k] = v;
}
- void Object::setObjectName(const std::string& n)
+ void
+ Object::setObjectName(const std::string& n)
{
checkObjectName(n);
//path.setRootIdentifier(n);
this->aron->objectName = n;
}
- void Object::setExtends(const std::shared_ptr<Object>& p)
+ void
+ Object::setExtends(const std::shared_ptr<Object>& p)
{
ARMARX_CHECK_NOT_NULL(p);
type::dto::AronObjectPtr ex = p->toAronObjectDTO();
@@ -149,18 +176,21 @@ namespace armarx::aron::type
extends = p;
}
- bool Object::hasMemberType(const std::string& k) const
+ bool
+ Object::hasMemberType(const std::string& k) const
{
return memberTypes.count(k) > 0 or (extends && extends->hasMemberType(k));
}
- std::string Object::getObjectNameWithoutNamespace() const
+ std::string
+ Object::getObjectNameWithoutNamespace() const
{
std::vector<std::string> split = simox::alg::split(aron->objectName, "::");
- return split[split.size() -1];
+ return split[split.size() - 1];
}
- std::string Object::getObjectNameWithTemplateInstantiations() const
+ std::string
+ Object::getObjectNameWithTemplateInstantiations() const
{
if (aron->templateInstantiations.empty())
{
@@ -169,7 +199,8 @@ namespace armarx::aron::type
return getObjectName() + "<" + simox::alg::join(aron->templateInstantiations, ", ") + ">";
}
- std::string Object::getObjectNameWithTemplates() const
+ std::string
+ Object::getObjectNameWithTemplates() const
{
if (aron->templates.empty())
{
@@ -178,45 +209,56 @@ namespace armarx::aron::type
return getObjectName() + "<" + simox::alg::join(aron->templates, ", ") + ">";
}
- void Object::addTemplate(const std::string& s) const
+ void
+ Object::addTemplate(const std::string& s) const
{
if (std::find(aron->templates.begin(), aron->templates.end(), s) != aron->templates.end())
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The template already exists!", s);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "The template already exists!", s);
}
aron->templates.push_back(s);
}
- void Object::addTemplateInstantiation(const std::string& s) const
+ void
+ Object::addTemplateInstantiation(const std::string& s) const
{
- if (std::find(aron->templateInstantiations.begin(), aron->templateInstantiations.end(), s) != aron->templateInstantiations.end())
+ if (std::find(aron->templateInstantiations.begin(),
+ aron->templateInstantiations.end(),
+ s) != aron->templateInstantiations.end())
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The template arg already exists!", s);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "The template arg already exists!", s);
}
aron->templateInstantiations.push_back(s);
}
- std::vector<std::string> Object::getTemplates() const
+ std::vector<std::string>
+ Object::getTemplates() const
{
return aron->templates;
}
- std::vector<std::string> Object::getTemplateInstantiations() const
+ std::vector<std::string>
+ Object::getTemplateInstantiations() const
{
return aron->templateInstantiations;
}
- std::string Object::getObjectName() const
+ std::string
+ Object::getObjectName() const
{
return this->aron->objectName;
}
- std::shared_ptr<Object> Object::getExtends() const
+ std::shared_ptr<Object>
+ Object::getExtends() const
{
return extends;
}
- std::vector<std::string> Object::getAllKeys() const
+ std::vector<std::string>
+ Object::getAllKeys() const
{
std::vector<std::string> ret;
for (const auto& [k, _] : memberTypes)
@@ -233,13 +275,15 @@ namespace armarx::aron::type
return ret;
}
- type::dto::AronObjectPtr Object::toAronObjectDTO() const
+ type::dto::AronObjectPtr
+ Object::toAronObjectDTO() const
{
return this->aron;
}
// virtual implementations
- std::vector<VariantPtr> Object::getChildren() const
+ std::vector<VariantPtr>
+ Object::getChildren() const
{
std::vector<VariantPtr> ret;
for (const auto& [k, t] : memberTypes)
@@ -256,42 +300,52 @@ namespace armarx::aron::type
return ret;
}
- size_t Object::childrenSize() const
+ size_t
+ Object::childrenSize() const
{
return memberTypes.size();
}
- std::string Object::getShortName() const
+ std::string
+ Object::getShortName() const
{
- return "Object<" + this->aron->objectName + (extends ? (" : " + extends->getShortName()) : "") + ">";
+ return "Object<" + this->aron->objectName +
+ (extends ? (" : " + extends->getShortName()) : "") + ">";
}
- std::string Object::getFullName() const
+ std::string
+ Object::getFullName() const
{
- return "armarx::aron::type::Object<" + this->aron->objectName + (extends ? (" : " + extends->getFullName()) : "") + ">";
+ return "armarx::aron::type::Object<" + this->aron->objectName +
+ (extends ? (" : " + extends->getFullName()) : "") + ">";
}
- std::string Object::GetFullNamePrefix()
+ std::string
+ Object::GetFullNamePrefix()
{
return "armarx::aron::type::Object";
}
- std::string Object::GetNamePrefix()
+ std::string
+ Object::GetNamePrefix()
{
return "Object";
}
- VariantPtr Object::navigateAbsolute(const Path& path) const
+ VariantPtr
+ Object::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
}
std::string el = path.getFirstElement();
if (!hasMemberType(el))
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
}
if (path.size() == 1)
@@ -303,10 +357,10 @@ namespace armarx::aron::type
Path next = path.withDetachedFirstElement();
if (!memberTypes.at(el))
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate into a NULL member.", next);
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "Could not navigate into a NULL member.", next);
}
return memberTypes.at(el)->navigateAbsolute(next);
}
}
-}
-
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Object.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.h
index 0f452d4b8070424af81d7d371e65acec54a4fdae..9a8417ace771bb659721006ad83d100477744991 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Object.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Object.h
@@ -24,13 +24,12 @@
#pragma once
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Base Class
#include "../detail/ContainerVariant.h"
-
namespace armarx::aron::type
{
class Object;
@@ -40,12 +39,11 @@ namespace armarx::aron::type
* @brief The Object class. It represents the object type
* An object may have different types for each member, here represented as a map, mapping from the member name (as string) to the type (as a variant)
*/
- class Object :
- public detail::ContainerVariant<type::dto::AronObject, Object>
+ class Object : public detail::ContainerVariant<type::dto::AronObject, Object>
{
public:
// constructors
- Object(const std::string&, const std::vector<std::string>& templates = {}, const std::vector<std::string>& templateInstantiations = {}, const std::map<std::string, VariantPtr>& = {}, const Path& = Path());
+ Object(const Path& = Path());
Object(const type::dto::AronObject&, const Path& = Path());
static ObjectPtr FromAronObjectDTO(const type::dto::AronObjectPtr&,
@@ -65,8 +63,12 @@ namespace armarx::aron::type
void setObjectName(const std::string&);
void setExtends(const std::shared_ptr<Object>&);
+
+ void setMemberTypes(const std::map<std::string, VariantPtr>&);
void addMemberType(const std::string&, const VariantPtr&);
+ void setTemplates(const std::vector<std::string>&);
void addTemplate(const std::string&) const;
+ void setTemplateInstantiations(const std::vector<std::string>&) const;
void addTemplateInstantiation(const std::string&) const;
bool hasMemberType(const std::string&) const;
@@ -95,4 +97,4 @@ namespace armarx::aron::type
std::shared_ptr<Object> extends;
std::map<std::string, VariantPtr> memberTypes;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp
index c147bd3dd3ab17a66b9e0068e6d3e044c663e67d..8753a121ab0e744f564ef4441425676c40cf9172 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.cpp
@@ -24,17 +24,12 @@
// Header
#include "Pair.h"
-
namespace armarx::aron::type
{
// constructors
- Pair::Pair(const VariantPtr& acceptedType1, const VariantPtr& acceptedType2, const Path& path) :
- detail::ContainerVariant<type::dto::Pair, Pair>(type::Descriptor::PAIR, path),
- acceptedType1(acceptedType1),
- acceptedType2(acceptedType2)
+ Pair::Pair(const Path& path) :
+ detail::ContainerVariant<type::dto::Pair, Pair>(type::Descriptor::PAIR, path)
{
- aron->acceptedType1 = acceptedType1->toAronDTO();
- aron->acceptedType2 = acceptedType2->toAronDTO();
}
Pair::Pair(const type::dto::Pair& o, const Path& path) :
@@ -45,22 +40,26 @@ namespace armarx::aron::type
}
// Member functions
- std::pair<VariantPtr, VariantPtr> Pair::getAcceptedTypes() const
+ std::pair<VariantPtr, VariantPtr>
+ Pair::getAcceptedTypes() const
{
return {acceptedType1, acceptedType2};
}
- VariantPtr Pair::getFirstAcceptedType() const
+ VariantPtr
+ Pair::getFirstAcceptedType() const
{
return acceptedType1;
}
- VariantPtr Pair::getSecondAcceptedType() const
+ VariantPtr
+ Pair::getSecondAcceptedType() const
{
return acceptedType2;
}
- void Pair::addAcceptedType(const VariantPtr& n)
+ void
+ Pair::addAcceptedType(const VariantPtr& n)
{
ARMARX_CHECK_NOT_NULL(n);
@@ -77,59 +76,71 @@ namespace armarx::aron::type
throw error::AronException(__PRETTY_FUNCTION__, "Both types are already set", getPath());
}
- void Pair::setFirstAcceptedType(const VariantPtr& n)
+ void
+ Pair::setFirstAcceptedType(const VariantPtr& n)
{
ARMARX_CHECK_NOT_NULL(n);
acceptedType1 = n;
}
- void Pair::setSecondAcceptedType(const VariantPtr& n)
+ void
+ Pair::setSecondAcceptedType(const VariantPtr& n)
{
ARMARX_CHECK_NOT_NULL(n);
acceptedType2 = n;
}
- type::dto::PairPtr Pair::toPairDTO() const
+ type::dto::PairPtr
+ Pair::toPairDTO() const
{
return this->aron;
}
// virtual implementations
- std::vector<VariantPtr> Pair::getChildren() const
+ std::vector<VariantPtr>
+ Pair::getChildren() const
{
return {acceptedType1, acceptedType2};
}
- size_t Pair::childrenSize() const
+ size_t
+ Pair::childrenSize() const
{
return 2;
}
- std::string Pair::getShortName() const
+ std::string
+ Pair::getShortName() const
{
return "Pair<" + acceptedType1->getShortName() + ", " + acceptedType2->getShortName() + ">";
}
- std::string Pair::getFullName() const
+ std::string
+ Pair::getFullName() const
{
- return "armarx::aron::type::Pair<" + acceptedType1->getFullName() + ", " + acceptedType2->getFullName() + ">";
+ return "armarx::aron::type::Pair<" + acceptedType1->getFullName() + ", " +
+ acceptedType2->getFullName() + ">";
}
- std::string Pair::GetFullNamePrefix()
+ std::string
+ Pair::GetFullNamePrefix()
{
return "armarx::aron::type::Pair";
}
- std::string Pair::GetNamePrefix()
+ std::string
+ Pair::GetNamePrefix()
{
return "Pair";
}
- VariantPtr Pair::navigateAbsolute(const Path& path) const
+ VariantPtr
+ Pair::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
}
std::string el = path.getFirstElement();
@@ -159,8 +170,8 @@ namespace armarx::aron::type
}
else
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
}
}
-}
-
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
index e8bb11cd6fa82d711d87b39cf50b1dcfbd6f71fd..ce88e8d2e9b7cd03127e431c9e2b81e305a04aa4 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Pair.h
@@ -24,8 +24,8 @@
#pragma once
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Base Class
#include "../detail/ContainerVariant.h"
@@ -36,12 +36,11 @@ namespace armarx::aron::type
* @brief The Pair class. It represents the pair type
* A pair has two accepted types
*/
- class Pair :
- public detail::ContainerVariant<type::dto::Pair, Pair>
+ class Pair : public detail::ContainerVariant<type::dto::Pair, Pair>
{
public:
// constructors
- Pair(const VariantPtr& acceptedType1, const VariantPtr& acceptedType2, const Path& path = Path());
+ Pair(const Path& path = Path());
Pair(const type::dto::Pair&, const Path& path = Path());
// public member functions
@@ -69,6 +68,5 @@ namespace armarx::aron::type
// members
VariantPtr acceptedType1;
VariantPtr acceptedType2;
-
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
index cdcd605f2d475d4e8e83ab1d25a25914cc32e99a..ce4e66261f9c5e8de9e36ebaaf387c62ca0ef64c 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
@@ -29,14 +29,9 @@
namespace armarx::aron::type
{
// constructors
- Tuple::Tuple(const std::vector<VariantPtr>& acceptedTypes, const Path& path) :
- detail::ContainerVariant<type::dto::Tuple, Tuple>(type::Descriptor::TUPLE, path),
- acceptedTypes(acceptedTypes)
+ Tuple::Tuple(const Path& path) :
+ detail::ContainerVariant<type::dto::Tuple, Tuple>(type::Descriptor::TUPLE, path)
{
- for (const auto& acceptedType : acceptedTypes)
- {
- aron->elementTypes.push_back(acceptedType->toAronDTO());
- }
}
Tuple::Tuple(const type::dto::Tuple& o, const Path& path) :
@@ -49,7 +44,8 @@ namespace armarx::aron::type
}
}
- type::dto::TuplePtr Tuple::toTupleDTO() const
+ type::dto::TuplePtr
+ Tuple::toTupleDTO() const
{
if (acceptedTypes.empty())
{
@@ -59,22 +55,36 @@ namespace armarx::aron::type
}
// public member functions
- std::vector<VariantPtr> Tuple::getAcceptedTypes() const
+ std::vector<VariantPtr>
+ Tuple::getAcceptedTypes() const
{
return acceptedTypes;
}
- bool Tuple::hasAcceptedType(unsigned int i) const
+ bool
+ Tuple::hasAcceptedType(unsigned int i) const
{
return i < acceptedTypes.size();
}
- VariantPtr Tuple::getAcceptedType(unsigned int i) const
+ VariantPtr
+ Tuple::getAcceptedType(unsigned int i) const
{
return acceptedTypes[i];
}
- void Tuple::addAcceptedType(const VariantPtr& v)
+ void
+ Tuple::setAcceptedTypes(const std::vector<VariantPtr>& acceptedTypes)
+ {
+ this->acceptedTypes = acceptedTypes;
+ for (const auto& acceptedType : acceptedTypes)
+ {
+ aron->elementTypes.push_back(acceptedType->toAronDTO());
+ }
+ }
+
+ void
+ Tuple::addAcceptedType(const VariantPtr& v)
{
ARMARX_CHECK_NOT_NULL(v);
this->aron->elementTypes.push_back(v->toAronDTO());
@@ -82,17 +92,20 @@ namespace armarx::aron::type
}
// virtual implementations
- std::vector<VariantPtr> Tuple::getChildren() const
+ std::vector<VariantPtr>
+ Tuple::getChildren() const
{
return acceptedTypes;
}
- size_t Tuple::childrenSize() const
+ size_t
+ Tuple::childrenSize() const
{
return acceptedTypes.size();
}
- std::string Tuple::getShortName() const
+ std::string
+ Tuple::getShortName() const
{
std::vector<std::string> names;
for (const auto& n : acceptedTypes)
@@ -102,7 +115,8 @@ namespace armarx::aron::type
return "Tuple<" + simox::alg::to_string(names, ", ") + ">";
}
- std::string Tuple::getFullName() const
+ std::string
+ Tuple::getFullName() const
{
std::vector<std::string> names;
for (const auto& n : acceptedTypes)
@@ -112,21 +126,25 @@ namespace armarx::aron::type
return "armarx::aron::type::Tuple<" + simox::alg::to_string(names, ", ") + ">";
}
- std::string Tuple::GetFullNamePrefix()
+ std::string
+ Tuple::GetFullNamePrefix()
{
return "armarx::aron::type::Tuple";
}
- std::string Tuple::GetNamePrefix()
+ std::string
+ Tuple::GetNamePrefix()
{
return "Tuple";
}
- VariantPtr Tuple::navigateAbsolute(const Path& path) const
+ VariantPtr
+ Tuple::navigateAbsolute(const Path& path) const
{
if (!path.hasElement())
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
}
std::string el = path.getFirstElement();
@@ -145,7 +163,7 @@ namespace armarx::aron::type
}
}
}
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
}
-}
-
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
index 070bed6264c8a2d9c19751d1b7866794e6e3bfb8..e382cc526eb18e3859c4531b4d7814a17dfd7461 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
@@ -24,8 +24,8 @@
#pragma once
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Base Class
#include "../detail/ContainerVariant.h"
@@ -36,18 +36,19 @@ namespace armarx::aron::type
* @brief The Tuple class. It represents the tuple type
* A tuple has a list of accepted types
*/
- class Tuple :
- public detail::ContainerVariant<type::dto::Tuple, Tuple>
+ class Tuple : public detail::ContainerVariant<type::dto::Tuple, Tuple>
{
public:
// constructors
- Tuple(const std::vector<VariantPtr>& acceptedTypes, const Path& path = Path());
+ Tuple(const Path& path = Path());
Tuple(const type::dto::Tuple&, const Path& path = Path());
// public member functions
std::vector<VariantPtr> getAcceptedTypes() const;
bool hasAcceptedType(unsigned int i) const;
VariantPtr getAcceptedType(unsigned int i) const;
+
+ void setAcceptedTypes(const std::vector<VariantPtr>&);
void addAcceptedType(const VariantPtr&);
type::dto::TuplePtr toTupleDTO() const;
@@ -66,4 +67,4 @@ namespace armarx::aron::type
private:
std::vector<VariantPtr> acceptedTypes;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h
index a9b390b8db6383d63fbc594f23841351861def4f..84b1bf7821f6518498ca40554a26a16fcfaaf019 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/EnumVariant.h
@@ -35,29 +35,154 @@
namespace armarx::aron::type::detail
{
- template<typename AronTypeT, typename DerivedT>
- class EnumVariant :
- public SpecializedVariantBase<AronTypeT, DerivedT>
+ template <typename AronTypeT, typename DerivedT, typename ValueT>
+ class EnumVariant : public SpecializedVariantBase<AronTypeT, DerivedT>
{
public:
- using SpecializedVariantBase<AronTypeT, DerivedT>::SpecializedVariantBase;
+ using ValueType = ValueT;
+ using Base = SpecializedVariantBase<AronTypeT, DerivedT>;
+
+ using Base::SpecializedVariantBase;
virtual ~EnumVariant() = default;
/* virtual implementations */
- VariantPtr navigateAbsolute(const Path &path) const override
+ VariantPtr
+ navigateAbsolute(const Path& path) const override
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through a non container navigator. The input path was: " + path.toString(), Variant::getPath());
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Could not navigate through a non container navigator. The input path was: " +
+ path.toString(),
+ Variant::getPath());
}
- std::vector<VariantPtr> getChildren() const override
+ std::vector<VariantPtr>
+ getChildren() const override
{
return {};
}
- size_t childrenSize() const override
+ size_t
+ childrenSize() const override
{
return 0;
}
+
+ void
+ setAcceptedValueMap(const std::map<std::string, ValueType>& valueMap)
+ {
+ this->aron->acceptedValues = valueMap;
+ }
+
+ std::map<std::string, int>
+ getAcceptedValueMap() const
+ {
+ return this->aron->acceptedValues;
+ }
+
+ std::vector<std::string>
+ getAcceptedValueNames() const
+ {
+ std::vector<std::string> names;
+ for (const auto& [k, _] : this->aron->acceptedValues)
+ {
+ names.push_back(k);
+ }
+ return names;
+ }
+
+ std::vector<ValueType>
+ getAcceptedValues() const
+ {
+ std::vector<ValueType> vals;
+ for (const auto& [_, i] : this->aron->acceptedValues)
+ {
+ vals.push_back(i);
+ }
+ return vals;
+ }
+
+ std::string
+ getValueName(ValueType i) const
+ {
+ for (const auto& [k, v] : this->aron->acceptedValues)
+ {
+ if (v == i)
+ {
+ return k;
+ }
+ }
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "Enum could not be resolved. Input was: " +
+ std::to_string(i),
+ this->getPath());
+ }
+
+ int
+ getValue(const std::string& s) const
+ {
+ return this->aron->acceptedValues[s];
+ }
+
+ std::string
+ getEnumName() const
+ {
+ return this->aron->enumName;
+ }
+
+ void
+ setEnumName(const std::string& s)
+ {
+ this->aron->enumName = s;
+ }
+
+ void
+ addAcceptedValue(const std::string& s, ValueType i)
+ {
+ this->aron->acceptedValues[s] = i;
+ }
+
+ std::string
+ getDefaultValueName() const
+ {
+ return this->aron->defaultValue;
+ }
+
+ ValueType
+ getDefaultValue() const
+ {
+ // ARMARX_CHECK(std::find(this->aron->acceptedValues.begin(),
+ // this->aron->acceptedValues.end(),
+ // this->aron->defaultValue) != this->aron->acceptedValues.end())
+ // << "An aron enum type has an invalid default value set. Got: "
+ // << this->aron->defaultValue;
+ return this->aron->acceptedValues[this->aron->defaultValue];
+ }
+
+ void
+ setDefaultValue(const ValueType& v)
+ {
+ for (const auto& [key, value] : this->aron->acceptedValues)
+ {
+ if (value == v)
+ {
+ this->aron->defaultValue = key;
+ }
+ }
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__, "Got invalid input for enum value", std::to_string(v));
+ }
+
+ void
+ setDefaultValueName(const std::string& v)
+ {
+ // ARMARX_CHECK(std::find(this->aron->acceptedValues.begin(),
+ // this->aron->acceptedValues.end(),
+ // this->aron->defaultValue) != this->aron->acceptedValues.end())
+ // << "Trying to set an invalid default value for aron enum type. Got: "
+ // << this->aron->defaultValue;
+ this->aron->defaultValue = v;
+ }
};
-}
+} // namespace armarx::aron::type::detail
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h
index 283b11a6383593e389e905c6b27e5fec4716a72a..168db8b1073aee4ea767940b27933109800bd025 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/NDArrayVariant.h
@@ -35,9 +35,8 @@
namespace armarx::aron::type::detail
{
- template<typename AronTypeT, typename DerivedT>
- class NDArrayVariant :
- public SpecializedVariantBase<AronTypeT, DerivedT>
+ template <typename AronTypeT, typename DerivedT>
+ class NDArrayVariant : public SpecializedVariantBase<AronTypeT, DerivedT>
{
public:
using SpecializedVariantBase<AronTypeT, DerivedT>::SpecializedVariantBase;
@@ -45,19 +44,38 @@ namespace armarx::aron::type::detail
virtual ~NDArrayVariant() = default;
/* virtual implementations */
- VariantPtr navigateAbsolute(const Path &path) const override
+ VariantPtr
+ navigateAbsolute(const Path& path) const override
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through a non container navigator. The input path was: " + path.toString(), Variant::getPath());
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Could not navigate through a non container navigator. The input path was: " +
+ path.toString(),
+ Variant::getPath());
}
- std::vector<VariantPtr> getChildren() const override
+ std::vector<VariantPtr>
+ getChildren() const override
{
return {};
}
- size_t childrenSize() const override
+ size_t
+ childrenSize() const override
{
return 0;
}
+
+ std::string
+ getDefaultValue() const
+ {
+ return this->aron->defaultValue;
+ }
+
+ void
+ setDefaultValue(const std::string& v)
+ {
+ this->aron->defaultValue = v;
+ }
};
-}
+} // namespace armarx::aron::type::detail
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
index c7c6eb0b9c00e9ab1cd40800d5917929b8e9e5c3..24dd9171d771e9957600196cf9b9171d5940f455 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
@@ -35,29 +35,64 @@
namespace armarx::aron::type::detail
{
- template<typename AronTypeT, typename DerivedT>
- class PrimitiveVariant :
- public SpecializedVariantBase<AronTypeT, DerivedT>
+ template <typename AronTypeT, typename DerivedT, typename ValueT>
+ class PrimitiveVariant : public SpecializedVariantBase<AronTypeT, DerivedT>
{
public:
- using SpecializedVariantBase<AronTypeT, DerivedT>::SpecializedVariantBase;
+ using Base = SpecializedVariantBase<AronTypeT, DerivedT>;
+
+ using ValueType = ValueT;
+
+ using Base::SpecializedVariantBase;
virtual ~PrimitiveVariant() = default;
/* virtual implementations */
- VariantPtr navigateAbsolute(const Path &path) const override
+ VariantPtr
+ navigateAbsolute(const Path& path) const override
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through a non container navigator. The input path was: " + path.toString(), Variant::getPath());
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Could not navigate through a non container navigator. The input path was: " +
+ path.toString(),
+ Variant::getPath());
}
- std::vector<VariantPtr> getChildren() const override
+ std::vector<VariantPtr>
+ getChildren() const override
{
return {};
}
- size_t childrenSize() const override
+ size_t
+ childrenSize() const override
{
return 0;
}
+
+ std::optional<ValueType>
+ getDefaultValue() const
+ {
+ if (!this->aron->defaultValue)
+ {
+ return std::nullopt;
+ }
+ return *this->aron->defaultValue;
+ }
+
+ void
+ setDefaultValue(const std::optional<ValueType>& v)
+ {
+ if (!v)
+ {
+ this->aron->defaultValue = IceUtil::None;
+ }
+ else
+ {
+ this->aron->defaultValue = *v;
+ }
+ }
+
+ protected:
};
-}
+} // namespace armarx::aron::type::detail
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
index a34a215308e265d844b10d2e6d1359e1b31fd111..a984daed2357434ceae4650c96ad8ff4a10a2356 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
@@ -25,6 +25,7 @@
// STD/STL
#include <memory>
+#include <optional>
#include <string>
#include <unordered_map>
@@ -35,21 +36,20 @@
namespace armarx::aron::type::detail
{
- template<typename AronTypeT, typename DerivedT>
- class SpecializedVariantBase :
- public type::Variant
+ template <typename AronTypeT, typename DerivedT>
+ class SpecializedVariantBase : public type::Variant
{
public:
SpecializedVariantBase(const type::Descriptor& descriptor, const Path& path) :
- Variant(descriptor, path),
- aron(new AronTypeT())
+ Variant(descriptor, path), aron(new AronTypeT())
{
}
- SpecializedVariantBase(const AronTypeT& o, const type::Descriptor& descriptor, const Path& path) :
- Variant(descriptor, path),
- aron(new AronTypeT(o))
+ SpecializedVariantBase(const AronTypeT& o,
+ const type::Descriptor& descriptor,
+ const Path& path) :
+ Variant(descriptor, path), aron(new AronTypeT(o))
{
}
@@ -61,57 +61,67 @@ namespace armarx::aron::type::detail
return *aron;
}
- bool operator==(const Variant& other) const override
+ bool
+ operator==(const Variant& other) const override
{
const auto& n = dynamic_cast<const DerivedT&>(other);
return *this == n;
}
- bool operator==(const DerivedT& other) const
+ bool
+ operator==(const DerivedT& other) const
{
return *(this->aron) == *(other.aron);
}
// virtual implementations
- type::dto::GenericTypePtr toAronDTO() const override
+ type::dto::GenericTypePtr
+ toAronDTO() const override
{
ARMARX_CHECK_NOT_NULL(aron);
return aron;
}
- void setMaybe(const type::Maybe m) override
+
+ void
+ setMaybe(const type::Maybe m) override
{
aron->maybe = m;
}
- type::Maybe getMaybe() const override
+ type::Maybe
+ getMaybe() const override
{
return aron->maybe;
}
- VariantPtr navigateRelative(const Path& path) const override
+ VariantPtr
+ navigateRelative(const Path& path) const override
{
Path absoluteFromHere = path.getWithoutPrefix(getPath());
return navigateAbsolute(absoluteFromHere);
}
-
// static methods
- static DerivedT& DynamicCast(Variant& n)
+ static DerivedT&
+ DynamicCast(Variant& n)
{
return dynamic_cast<DerivedT&>(n);
}
- static const DerivedT& DynamicCast(const Variant& n)
+ static const DerivedT&
+ DynamicCast(const Variant& n)
{
return dynamic_cast<const DerivedT&>(n);
}
- static std::shared_ptr<DerivedT> DynamicCast(const VariantPtr& n)
+ static std::shared_ptr<DerivedT>
+ DynamicCast(const VariantPtr& n)
{
return std::dynamic_pointer_cast<DerivedT>(n);
}
- static std::shared_ptr<DerivedT> DynamicCastAndCheck(const VariantPtr& n)
+ static std::shared_ptr<DerivedT>
+ DynamicCastAndCheck(const VariantPtr& n)
{
if (!n)
{
@@ -126,4 +136,4 @@ namespace armarx::aron::type::detail
protected:
typename AronTypeT::PointerType aron;
};
-}
+} // namespace armarx::aron::type::detail
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp
index 5ccace22723cbd9eb530064614339827cfcdd3ae..f6afbfb36c87c88c4bb2ba25532dab32e591ee56 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.cpp
@@ -27,89 +27,31 @@
namespace armarx::aron::type
{
// constructors
- IntEnum::IntEnum(const std::string& name, const std::map<std::string, int>& valueMap, const Path& path) :
- detail::EnumVariant<type::dto::IntEnum, IntEnum>(type::Descriptor::INT_ENUM, path)
+ IntEnum::IntEnum(const Path& path) : Base(type::Descriptor::INT_ENUM, path)
{
- aron->enumName = name;
- aron->acceptedValues = valueMap;
}
IntEnum::IntEnum(const type::dto::IntEnum& o, const Path& path) :
- detail::EnumVariant<type::dto::IntEnum, IntEnum>(o, type::Descriptor::INT_ENUM, path)
+ Base(o, type::Descriptor::INT_ENUM, path)
{
}
- std::map<std::string, int> IntEnum::getAcceptedValueMap() const
- {
- return this->aron->acceptedValues;
- }
-
- std::vector<std::string> IntEnum::getAcceptedValueNames() const
- {
- std::vector<std::string> names;
- for (const auto& [k, _] : aron->acceptedValues)
- {
- names.push_back(k);
- }
- return names;
- }
-
- std::vector<int> IntEnum::getAcceptedValues() const
- {
- std::vector<int> vals;
- for (const auto& [_, i] : aron->acceptedValues)
- {
- vals.push_back(i);
- }
- return vals;
- }
-
- std::string IntEnum::getValueName(int i) const
- {
- for (const auto& [k, v] : this->aron->acceptedValues)
- {
- if (v == i)
- {
- return k;
- }
- }
- throw error::AronException(__PRETTY_FUNCTION__, "Enum could not be resolved. Input was: " + std::to_string(i), getPath());
- }
-
- int IntEnum::getValue(const std::string& s) const
- {
- return this->aron->acceptedValues[s];
- }
-
- std::string IntEnum::getEnumName() const
- {
- return this->aron->enumName;
- }
-
- void IntEnum::setEnumName(const std::string& s)
- {
- this->aron->enumName = s;
- }
-
- void IntEnum::addAcceptedValue(const std::string& s, int i)
- {
- this->aron->acceptedValues[s] = i;
- }
-
- type::dto::IntEnumPtr IntEnum::toIntEnumDTO() const
+ type::dto::IntEnumPtr
+ IntEnum::toIntEnumDTO() const
{
return this->aron;
}
// virtual implementations
- std::string IntEnum::getShortName() const
+ std::string
+ IntEnum::getShortName() const
{
return "IntEnum";
}
- std::string IntEnum::getFullName() const
+ std::string
+ IntEnum::getFullName() const
{
return "armarx::aron::type::IntEnum";
}
-}
-
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h
index cf652d23cfb225d1ba4efde9c7d48af7c0e34966..7e38527ba6e82a19c4572fba2c8614183001a7ec 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/enum/IntEnum.h
@@ -24,8 +24,8 @@
#pragma once
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Base class
#include "../detail/EnumVariant.h"
@@ -39,30 +39,19 @@ namespace armarx::aron::type
* @brief The IntEnum class. It represents the int enum type
* An int enum contains a map from strings to int values
*/
- class IntEnum :
- public detail::EnumVariant<type::dto::IntEnum, IntEnum>
+ class IntEnum : public detail::EnumVariant<type::dto::IntEnum, IntEnum, int>
{
public:
+ using Base = detail::EnumVariant<type::dto::IntEnum, IntEnum, int>;
+
// constructors
- IntEnum(const std::string& name, const std::map<std::string, int>& valueMap, const Path& path = Path());
+ IntEnum(const Path& path = Path());
IntEnum(const type::dto::IntEnum&, const Path& path = Path());
- // public member functions
- std::map<std::string, int> getAcceptedValueMap() const;
- std::vector<std::string> getAcceptedValueNames() const;
- std::vector<int> getAcceptedValues() const;
-
- std::string getValueName(int) const;
- int getValue(const std::string&) const;
- std::string getEnumName() const;
-
- void setEnumName(const std::string&);
- void addAcceptedValue(const std::string&, int);
-
type::dto::IntEnumPtr toIntEnumDTO() const;
// virtual implementations
std::string getShortName() const override;
std::string getFullName() const override;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp
index f5eed5e4ed7ff4d2fa0575563a8428ce58225d0e..86e66b911e1762abdf2fd2445cde2d4e787d3756 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.cpp
@@ -23,21 +23,18 @@
#include "Image.h"
-#include <RobotAPI/libraries/aron/core/Exception.h>
-
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/meta/EnumNames.hpp>
+#include <RobotAPI/libraries/aron/core/Exception.h>
namespace armarx::aron::type
{
- const std::map<image::PixelType, std::string> Image::Pixeltype2String
- {
+ const std::map<image::PixelType, std::string> Image::Pixeltype2String{
{image::PixelType::RGB24, "RGB24"},
- {image::PixelType::DEPTH32, "DEPTH32"}
- };
- const std::map<std::string, image::PixelType> Image::String2Pixeltype = conversion::util::InvertMap(Pixeltype2String);
-
+ {image::PixelType::DEPTH32, "DEPTH32"}};
+ const std::map<std::string, image::PixelType> Image::String2Pixeltype =
+ conversion::util::InvertMap(Pixeltype2String);
// constructors
Image::Image(const Path& path) :
@@ -45,37 +42,39 @@ namespace armarx::aron::type
{
}
-
Image::Image(const type::dto::Image& o, const Path& path) :
detail::NDArrayVariant<type::dto::Image, Image>(o, type::Descriptor::IMAGE, path)
{
}
- type::dto::ImagePtr Image::toImageDTO() const
+ type::dto::ImagePtr
+ Image::toImageDTO() const
{
return this->aron;
}
-
- image::PixelType Image::getPixelType() const
+ image::PixelType
+ Image::getPixelType() const
{
return aron->pixelType;
}
- void Image::setPixelType(const image::PixelType pixelType) const
+ void
+ Image::setPixelType(const image::PixelType pixelType) const
{
aron->pixelType = pixelType;
}
// virtual implementations
- std::string Image::getShortName() const
+ std::string
+ Image::getShortName() const
{
return "Image";
}
- std::string Image::getFullName() const
+ std::string
+ Image::getFullName() const
{
return "armarx::aron::type::Image<" + Pixeltype2String.at(this->aron->pixelType) + ">";
}
-}
-
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
index 0b9f9dd0992c79187368a7136b7fa957a42cacae..941687c4861b504a6a57ad7350910af609b61034 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Image.h
@@ -36,8 +36,7 @@ namespace armarx::aron::type
* @brief The Image class. It represents the image type.
* The code generation manages, which code will be generated for this type object, e.g. c++ generates opencv code.
*/
- class Image :
- public detail::NDArrayVariant<type::dto::Image, Image>
+ class Image : public detail::NDArrayVariant<type::dto::Image, Image>
{
public:
// constructors
@@ -57,4 +56,4 @@ namespace armarx::aron::type
static const std::map<image::PixelType, std::string> Pixeltype2String;
static const std::map<std::string, image::PixelType> String2Pixeltype;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
index b7657a8719be955f833ef8658891a39ae23ca05e..e3bcbf57344c50fe915a6177c1f60df0ead2d239 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
@@ -29,16 +29,15 @@
namespace armarx::aron::type
{
- const std::map<matrix::ElementType, std::string> Matrix::Elementtype2String
- {
+ const std::map<matrix::ElementType, std::string> Matrix::Elementtype2String{
{matrix::ElementType::INT16, "INT16"},
{matrix::ElementType::INT32, "INT32"},
{matrix::ElementType::INT64, "INT64"},
{matrix::ElementType::FLOAT32, "FLOAT32"},
- {matrix::ElementType::FLOAT64, "FLOAT64"}
- };
+ {matrix::ElementType::FLOAT64, "FLOAT64"}};
- const std::map<std::string, matrix::ElementType> Matrix::String2Elementtype = conversion::util::InvertMap(Elementtype2String);
+ const std::map<std::string, matrix::ElementType> Matrix::String2Elementtype =
+ conversion::util::InvertMap(Elementtype2String);
// constructors
Matrix::Matrix(const Path& path) :
@@ -51,56 +50,67 @@ namespace armarx::aron::type
{
}
- int Matrix::getRows() const
+ int
+ Matrix::getRows() const
{
return this->aron->rows;
}
- int Matrix::getCols() const
+ int
+ Matrix::getCols() const
{
return this->aron->cols;
}
- void Matrix::setRows(const int w)
+ void
+ Matrix::setRows(const int w)
{
if (w == 0 || w < -1)
{
- throw error::AronException(__PRETTY_FUNCTION__, "The rows cannot be 0 or < -1", getPath());
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "The rows cannot be 0 or < -1", getPath());
}
this->aron->rows = w;
}
- void Matrix::setCols(const int h)
+ void
+ Matrix::setCols(const int h)
{
if (h == 0 || h < -1)
{
- throw error::AronException(__PRETTY_FUNCTION__, "The cols cannot be 0 or < -1", getPath());
+ throw error::AronException(
+ __PRETTY_FUNCTION__, "The cols cannot be 0 or < -1", getPath());
}
this->aron->cols = h;
}
- type::matrix::ElementType Matrix::getElementType() const
+ type::matrix::ElementType
+ Matrix::getElementType() const
{
return this->aron->elementType;
}
- void Matrix::setElementType(const type::matrix::ElementType u)
+ void
+ Matrix::setElementType(const type::matrix::ElementType u)
{
this->aron->elementType = u;
}
- type::dto::MatrixPtr Matrix::toMatrixDTO() const
+ type::dto::MatrixPtr
+ Matrix::toMatrixDTO() const
{
return this->aron;
}
// virtual implementations
- std::string Matrix::getShortName() const
+ std::string
+ Matrix::getShortName() const
{
return "Matrix";
}
- std::string Matrix::getFullName() const
+ std::string
+ Matrix::getFullName() const
{
std::string rows = std::to_string(aron->rows);
std::string cols = std::to_string(aron->cols);
@@ -115,7 +125,7 @@ namespace armarx::aron::type
cols = "Dynamic";
}
- return "armarx::aron::type::Matrix<" + rows + ", " + cols + ", " + Elementtype2String.at(this->aron->elementType) + ">";
+ return "armarx::aron::type::Matrix<" + rows + ", " + cols + ", " +
+ Elementtype2String.at(this->aron->elementType) + ">";
}
-}
-
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h
index 41dc008d34f29e0cabfc567416af6cbcc0588a5b..b28fc260ddb8110bbb54d9a8fbb10aed052abb66 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h
@@ -24,8 +24,8 @@
#pragma once
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Base class
#include "../detail/NDArrayVariant.h"
@@ -36,8 +36,7 @@ namespace armarx::aron::type
* @brief The Matrix class. It represents the matrix type
* A Matrix is defined through the number of rows, cols and the element type
*/
- class Matrix :
- public detail::NDArrayVariant<type::dto::Matrix, Matrix>
+ class Matrix : public detail::NDArrayVariant<type::dto::Matrix, Matrix>
{
public:
// constructors
@@ -61,4 +60,4 @@ namespace armarx::aron::type
static const std::map<matrix::ElementType, std::string> Elementtype2String;
static const std::map<std::string, matrix::ElementType> String2Elementtype;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp
index bddbcf91cf100a0d60b49f30fc0bc075837756d5..8c188f92225331c7166d0f2fa6e5293326bc7283 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.cpp
@@ -37,45 +37,52 @@ namespace armarx::aron::type
{
}
- type::dto::NDArrayPtr NDArray::toNDArrayDTO() const
+ type::dto::NDArrayPtr
+ NDArray::toNDArrayDTO() const
{
return this->aron;
}
- int NDArray::getNumberDimensions() const
+ int
+ NDArray::getNumberDimensions() const
{
return this->aron->ndimensions;
}
- type::ndarray::ElementType NDArray::getElementType() const
+ type::ndarray::ElementType
+ NDArray::getElementType() const
{
return this->aron->elementType;
}
- void NDArray::setElementType(type::ndarray::ElementType s)
+ void
+ NDArray::setElementType(type::ndarray::ElementType s)
{
this->aron->elementType = s;
}
- void NDArray::setNumberDimensions(int v)
+ void
+ NDArray::setNumberDimensions(int v)
{
this->aron->ndimensions = v;
}
- void NDArray::addDimension()
+ void
+ NDArray::addDimension()
{
this->aron->ndimensions++;
}
// virtual implementations
- std::string NDArray::getShortName() const
+ std::string
+ NDArray::getShortName() const
{
return "NDArray";
}
- std::string NDArray::getFullName() const
+ std::string
+ NDArray::getFullName() const
{
return "armarx::aron::type::NDArray";
}
-}
-
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
index d04d218eb5ce19a1d53b8d1851b00d3a9ba7925a..b8df436d15f816c47820bed0e6b58bbe94a68dff 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/NDArray.h
@@ -24,8 +24,8 @@
#pragma once
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Base class
#include "../detail/NDArrayVariant.h"
@@ -35,8 +35,7 @@ namespace armarx::aron::type
/**
* @brief The NDArray class. It represents an arbitrary array type
*/
- class NDArray :
- public detail::NDArrayVariant<type::dto::NDArray, NDArray>
+ class NDArray : public detail::NDArrayVariant<type::dto::NDArray, NDArray>
{
public:
// constructors
@@ -57,4 +56,4 @@ namespace armarx::aron::type
std::string getShortName() const override;
std::string getFullName() const override;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp
index c28a750e26f5e7acf86bf0ebdc8fb9ad4fe48cc3..5d0609803a1f684738272ebe367ff53d7065b1e8 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.cpp
@@ -26,54 +26,60 @@
namespace armarx::aron::type
{
- const std::map<pointcloud::VoxelType, std::string> PointCloud::Voxeltype2String
- {
+ const std::map<pointcloud::VoxelType, std::string> PointCloud::Voxeltype2String{
{pointcloud::VoxelType::POINT_XYZ, "POINT_XYZ"},
{pointcloud::VoxelType::POINT_XYZI, "POINT_XYZI"},
{pointcloud::VoxelType::POINT_XYZL, "POINT_XYZL"},
{pointcloud::VoxelType::POINT_XYZRGB, "POINT_XYZRGB"},
{pointcloud::VoxelType::POINT_XYZRGBL, "POINT_XYZRGBL"},
{pointcloud::VoxelType::POINT_XYZRGBA, "POINT_XYZRGBA"},
- {pointcloud::VoxelType::POINT_XYZHSV, "POINT_XYZHSV"}
- };
+ {pointcloud::VoxelType::POINT_XYZHSV, "POINT_XYZHSV"}};
- const std::map<std::string, pointcloud::VoxelType> PointCloud::String2Voxeltype = conversion::util::InvertMap(Voxeltype2String);
+ const std::map<std::string, pointcloud::VoxelType> PointCloud::String2Voxeltype =
+ conversion::util::InvertMap(Voxeltype2String);
// constructors
PointCloud::PointCloud(const Path& path) :
- detail::NDArrayVariant<type::dto::PointCloud, PointCloud>(type::Descriptor::POINTCLOUD, path)
+ detail::NDArrayVariant<type::dto::PointCloud, PointCloud>(type::Descriptor::POINTCLOUD,
+ path)
{
}
PointCloud::PointCloud(const type::dto::PointCloud& o, const Path& path) :
- detail::NDArrayVariant<type::dto::PointCloud, PointCloud>(o, type::Descriptor::POINTCLOUD, path)
+ detail::NDArrayVariant<type::dto::PointCloud, PointCloud>(o,
+ type::Descriptor::POINTCLOUD,
+ path)
{
}
- type::dto::PointCloudPtr PointCloud::toPointCloudDTO() const
+ type::dto::PointCloudPtr
+ PointCloud::toPointCloudDTO() const
{
return this->aron;
}
- type::pointcloud::VoxelType PointCloud::getVoxelType() const
+ type::pointcloud::VoxelType
+ PointCloud::getVoxelType() const
{
return this->aron->voxelType;
}
- void PointCloud::setVoxelType(type::pointcloud::VoxelType u)
+ void
+ PointCloud::setVoxelType(type::pointcloud::VoxelType u)
{
this->aron->voxelType = u;
}
// virtual implementations
- std::string PointCloud::getShortName() const
+ std::string
+ PointCloud::getShortName() const
{
return "PointCloud";
}
- std::string PointCloud::getFullName() const
+ std::string
+ PointCloud::getFullName() const
{
return "armarx::aron::type::PointCloud<" + Voxeltype2String.at(this->aron->voxelType) + ">";
}
-}
-
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h
index 37d3aacdbed11c396cdc7351f8e7077705f2e2a6..9358e4b59fee7421da8b500990d5e5646b8c8bbb 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/PointCloud.h
@@ -24,8 +24,8 @@
#pragma once
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Base class
#include "../detail/NDArrayVariant.h"
@@ -36,8 +36,7 @@ namespace armarx::aron::type
* @brief The PointCloud class. It represents the pointcloud type.
* A pointcloud is defined through a type. The dimension is NOT part of the type description
*/
- class PointCloud :
- public detail::NDArrayVariant<type::dto::PointCloud, PointCloud>
+ class PointCloud : public detail::NDArrayVariant<type::dto::PointCloud, PointCloud>
{
public:
// constructors
@@ -58,4 +57,4 @@ namespace armarx::aron::type
static const std::map<pointcloud::VoxelType, std::string> Voxeltype2String;
static const std::map<std::string, pointcloud::VoxelType> String2Voxeltype;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp
index 0e1ff22fc13e08f1bdcc3d15722feeb92d17fa61..ab85bdf94860e106b3dfb222d09a663da8c5482d 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.cpp
@@ -28,48 +28,55 @@
namespace armarx::aron::type
{
- const std::map<quaternion::ElementType, std::string> Quaternion::Elementtype2String =
- {
+ const std::map<quaternion::ElementType, std::string> Quaternion::Elementtype2String = {
{quaternion::ElementType::FLOAT32, "FLOAT32"},
- {quaternion::ElementType::FLOAT64, "FLOAT64"}
- };
- const std::map<std::string, quaternion::ElementType> Quaternion::String2Elementtype = conversion::util::InvertMap(Elementtype2String);
+ {quaternion::ElementType::FLOAT64, "FLOAT64"}};
+ const std::map<std::string, quaternion::ElementType> Quaternion::String2Elementtype =
+ conversion::util::InvertMap(Elementtype2String);
// constructors
Quaternion::Quaternion(const Path& path) :
- detail::NDArrayVariant<type::dto::Quaternion, Quaternion>(type::Descriptor::QUATERNION, path)
+ detail::NDArrayVariant<type::dto::Quaternion, Quaternion>(type::Descriptor::QUATERNION,
+ path)
{
}
Quaternion::Quaternion(const type::dto::Quaternion& o, const Path& path) :
- detail::NDArrayVariant<type::dto::Quaternion, Quaternion>(o, type::Descriptor::QUATERNION, path)
+ detail::NDArrayVariant<type::dto::Quaternion, Quaternion>(o,
+ type::Descriptor::QUATERNION,
+ path)
{
}
- type::dto::QuaternionPtr Quaternion::toQuaternionDTO() const
+ type::dto::QuaternionPtr
+ Quaternion::toQuaternionDTO() const
{
return this->aron;
}
- type::quaternion::ElementType Quaternion::getElementType() const
+ type::quaternion::ElementType
+ Quaternion::getElementType() const
{
return this->aron->elementType;
}
- void Quaternion::setElementType(type::quaternion::ElementType t)
+ void
+ Quaternion::setElementType(type::quaternion::ElementType t)
{
this->aron->elementType = t;
}
// virtual implementations
- std::string Quaternion::getShortName() const
+ std::string
+ Quaternion::getShortName() const
{
return "Quaternion";
}
- std::string Quaternion::getFullName() const
+ std::string
+ Quaternion::getFullName() const
{
- return "armarx::aron::type::Quaternion<" + Elementtype2String.at(this->aron->elementType) + ">";
+ return "armarx::aron::type::Quaternion<" + Elementtype2String.at(this->aron->elementType) +
+ ">";
}
-}
-
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h
index f660702be8c84613877d164763914bcf4cd57bc9..e8140a726731f3f4a9b63fc3dda380a1104d8542 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Quaternion.h
@@ -24,8 +24,8 @@
#pragma once
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Base class
#include "../detail/NDArrayVariant.h"
@@ -36,8 +36,7 @@ namespace armarx::aron::type
* @brief The Qaternion class. It represents the quaternion type
* A Quaternion has dimension 1x4 and a varying type
*/
- class Quaternion :
- public detail::NDArrayVariant<type::dto::Quaternion, Quaternion>
+ class Quaternion : public detail::NDArrayVariant<type::dto::Quaternion, Quaternion>
{
public:
// constructors
@@ -57,4 +56,4 @@ namespace armarx::aron::type
static const std::map<quaternion::ElementType, std::string> Elementtype2String;
static const std::map<std::string, quaternion::ElementType> String2Elementtype;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp
index fc5cdb5cb403e4de3605b82ad9f7a755c925c5d2..d9d87c0a97088e419ed6cfbc890e2e1c1d004bdd 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.cpp
@@ -23,8 +23,8 @@
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Header
#include "Bool.h"
@@ -32,29 +32,31 @@
namespace armarx::aron::type
{
/* constructors */
- Bool::Bool(const Path& path) :
- detail::PrimitiveVariant<type::dto::AronBool, Bool>(type::Descriptor::BOOL, path)
+ Bool::Bool(const Path& path) : Base(type::Descriptor::BOOL, path)
{
}
- Bool::Bool(const type::dto::AronBool&o, const Path& path) :
- detail::PrimitiveVariant<type::dto::AronBool, Bool>(o, type::Descriptor::BOOL, path)
+ Bool::Bool(const type::dto::AronBool& o, const Path& path) :
+ Base(o, type::Descriptor::BOOL, path)
{
}
- type::dto::AronBoolPtr Bool::toBoolDTO() const
+ type::dto::AronBoolPtr
+ Bool::toBoolDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string Bool::getShortName() const
+ std::string
+ Bool::getShortName() const
{
return "Bool";
}
- std::string Bool::getFullName() const
+ std::string
+ Bool::getFullName() const
{
return "armarx::aron::type::Bool";
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h
index eac4aeaea068387ba065a98376a64212a2fc2307..3225dde43e9b2293e947000394e619b6019f53b9 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Bool.h
@@ -34,10 +34,11 @@ namespace armarx::aron::type
/**
* @brief The Bool class. It represents the bool type
*/
- class Bool :
- public detail::PrimitiveVariant<type::dto::AronBool, Bool>
+ class Bool : public detail::PrimitiveVariant<type::dto::AronBool, Bool, bool>
{
public:
+ using Base = detail::PrimitiveVariant<type::dto::AronBool, Bool, bool>;
+
/* constructors */
Bool(const Path& = Path());
Bool(const type::dto::AronBool&, const Path& = Path());
@@ -48,4 +49,4 @@ namespace armarx::aron::type
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp
index e4fd415fe7c54b34d6e60314810a336856e845c0..d5baa23743e6ec8cea2259aa2f0049acc9df63c5 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.cpp
@@ -23,8 +23,8 @@
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Header
#include "Double.h"
@@ -32,29 +32,31 @@
namespace armarx::aron::type
{
/* constructors */
- Double::Double(const Path& path) :
- detail::PrimitiveVariant<type::dto::AronDouble, Double>(type::Descriptor::DOUBLE, path)
+ Double::Double(const Path& path) : Base(type::Descriptor::DOUBLE, path)
{
}
- Double::Double(const type::dto::AronDouble&o, const Path& path) :
- detail::PrimitiveVariant<type::dto::AronDouble, Double>(o, type::Descriptor::DOUBLE, path)
+ Double::Double(const type::dto::AronDouble& o, const Path& path) :
+ Base(o, type::Descriptor::DOUBLE, path)
{
}
- type::dto::AronDoublePtr Double::toDoubleDTO() const
+ type::dto::AronDoublePtr
+ Double::toDoubleDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string Double::getShortName() const
+ std::string
+ Double::getShortName() const
{
return "Double";
}
- std::string Double::getFullName() const
+ std::string
+ Double::getFullName() const
{
return "armarx::aron::type::Double";
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h
index 17aa98ad764ac8081cfcd2e23aad38f388749d18..f63251434e1c945c9e8d7b79b10e3a48cecdabd6 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Double.h
@@ -34,10 +34,11 @@ namespace armarx::aron::type
/**
* @brief The Double class. It represents the double type
*/
- class Double :
- public detail::PrimitiveVariant<type::dto::AronDouble, Double>
+ class Double : public detail::PrimitiveVariant<type::dto::AronDouble, Double, double>
{
public:
+ using Base = detail::PrimitiveVariant<type::dto::AronDouble, Double, double>;
+
/* constructors */
Double(const Path& = Path());
Double(const type::dto::AronDouble&, const Path& = Path());
@@ -48,4 +49,4 @@ namespace armarx::aron::type
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp
index 45bbad7dec390c876ff1033c650e187f29a4788e..ef7eb6e7a3791ecd82a20732dc36ae5d9c4df716 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.cpp
@@ -23,8 +23,8 @@
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Header
#include "Float.h"
@@ -32,29 +32,31 @@
namespace armarx::aron::type
{
/* constructors */
- Float::Float(const Path& path) :
- detail::PrimitiveVariant<type::dto::AronFloat, Float>(type::Descriptor::FLOAT, path)
+ Float::Float(const Path& path) : Base(type::Descriptor::FLOAT, path)
{
}
- Float::Float(const type::dto::AronFloat&o, const Path& path) :
- detail::PrimitiveVariant<type::dto::AronFloat, Float>(o, type::Descriptor::FLOAT, path)
+ Float::Float(const type::dto::AronFloat& o, const Path& path) :
+ Base(o, type::Descriptor::FLOAT, path)
{
}
- type::dto::AronFloatPtr Float::toFloatDTO() const
+ type::dto::AronFloatPtr
+ Float::toFloatDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string Float::getShortName() const
+ std::string
+ Float::getShortName() const
{
return "Float";
}
- std::string Float::getFullName() const
+ std::string
+ Float::getFullName() const
{
return "armarx::aron::type::Float";
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h
index 2dedfb1739b6f000baddc8bb13306781a3d34a99..0185727d6a1250630de0567e33cddb271a1eaeb3 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Float.h
@@ -34,10 +34,11 @@ namespace armarx::aron::type
/**
* @brief The Float class. It represents the float type
*/
- class Float :
- public detail::PrimitiveVariant<type::dto::AronFloat, Float>
+ class Float : public detail::PrimitiveVariant<type::dto::AronFloat, Float, float>
{
public:
+ using Base = detail::PrimitiveVariant<type::dto::AronFloat, Float, float>;
+
/* constructors */
Float(const Path& = Path());
Float(const type::dto::AronFloat&, const Path& = Path());
@@ -48,4 +49,4 @@ namespace armarx::aron::type
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp
index 6abe9850c96afcdf34785f0226cd22f11e067be8..b4ab9eb61afa58f31950b6d0d5208709ead3ca34 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.cpp
@@ -23,8 +23,8 @@
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Header
#include "Int.h"
@@ -32,29 +32,30 @@
namespace armarx::aron::type
{
/* constructors */
- Int::Int(const Path& path) :
- detail::PrimitiveVariant<type::dto::AronInt, Int>(type::Descriptor::INT, path)
+ Int::Int(const Path& path) : Base(type::Descriptor::INT, path)
{
}
- Int::Int(const type::dto::AronInt&o, const Path& path) :
- detail::PrimitiveVariant<type::dto::AronInt, Int>(o, type::Descriptor::INT, path)
+ Int::Int(const type::dto::AronInt& o, const Path& path) : Base(o, type::Descriptor::INT, path)
{
}
- type::dto::AronIntPtr Int::toIntDTO() const
+ type::dto::AronIntPtr
+ Int::toIntDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string Int::getShortName() const
+ std::string
+ Int::getShortName() const
{
return "Int";
}
- std::string Int::getFullName() const
+ std::string
+ Int::getFullName() const
{
return "armarx::aron::type::Int";
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h
index 240cd4a50df8687f8cdba2ff15c040d21ec27fbe..10f222b79eb0432cc03e906c30ff5b588c6786ad 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Int.h
@@ -34,10 +34,11 @@ namespace armarx::aron::type
/**
* @brief The Int class. It represents the int type
*/
- class Int :
- public detail::PrimitiveVariant<type::dto::AronInt, Int>
+ class Int : public detail::PrimitiveVariant<type::dto::AronInt, Int, int>
{
public:
+ using Base = detail::PrimitiveVariant<type::dto::AronInt, Int, int>;
+
/* constructors */
Int(const Path& = Path());
Int(const type::dto::AronInt&, const Path& = Path());
@@ -48,4 +49,4 @@ namespace armarx::aron::type
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp
index f7898bcf2247ddbffea6fedba1c51699571030ba..5abee29f0049d0a5c2e52c6604628c922a6f8490 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.cpp
@@ -23,8 +23,8 @@
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Header
#include "Long.h"
@@ -32,29 +32,31 @@
namespace armarx::aron::type
{
/* constructors */
- Long::Long(const Path& path) :
- detail::PrimitiveVariant<type::dto::AronLong, Long>(type::Descriptor::LONG, path)
+ Long::Long(const Path& path) : Base(type::Descriptor::LONG, path)
{
}
- Long::Long(const type::dto::AronLong&o, const Path& path) :
- detail::PrimitiveVariant<type::dto::AronLong, Long>(o, type::Descriptor::LONG, path)
+ Long::Long(const type::dto::AronLong& o, const Path& path) :
+ Base(o, type::Descriptor::LONG, path)
{
}
- type::dto::AronLongPtr Long::toLongDTO() const
+ type::dto::AronLongPtr
+ Long::toLongDTO() const
{
return this->aron;
}
/* virtual implementations */
- std::string Long::getShortName() const
+ std::string
+ Long::getShortName() const
{
return "Long";
}
- std::string Long::getFullName() const
+ std::string
+ Long::getFullName() const
{
return "armarx::aron::type::Long";
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h
index 710df1a421e5ff684ac85b70839ff7cff513378e..f64ca0a3492afcbcda9a4d023f2b61b3ede8075d 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/Long.h
@@ -34,10 +34,11 @@ namespace armarx::aron::type
/**
* @brief The Long class. It represents the long type
*/
- class Long :
- public detail::PrimitiveVariant<type::dto::AronLong, Long>
+ class Long : public detail::PrimitiveVariant<type::dto::AronLong, Long, long>
{
public:
+ using Base = detail::PrimitiveVariant<type::dto::AronLong, Long, long>;
+
/* constructors */
Long(const Path& = Path());
Long(const type::dto::AronLong&, const Path& = Path());
@@ -48,4 +49,4 @@ namespace armarx::aron::type
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp
index 0ede75062e53745057b0c181db65681656365d7d..8c350084c1a710300e41783c8fec91fa8e666cc8 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.cpp
@@ -23,8 +23,8 @@
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Header
#include "String.h"
@@ -32,30 +32,32 @@
namespace armarx::aron::type
{
/* constructors */
- String::String(const Path& path) :
- detail::PrimitiveVariant<type::dto::AronString, String>(type::Descriptor::STRING, path)
+ String::String(const Path& path) : Base(type::Descriptor::STRING, path)
{
}
- String::String(const type::dto::AronString&o, const Path& path) :
- detail::PrimitiveVariant<type::dto::AronString, String>(o, type::Descriptor::STRING, path)
+ String::String(const type::dto::AronString& o, const Path& path) :
+ Base(o, type::Descriptor::STRING, path)
{
}
/* public member functions */
- type::dto::AronStringPtr String::toStringDTO() const
+ type::dto::AronStringPtr
+ String::toStringDTO() const
{
return aron;
}
/* virtual implementations */
- std::string String::getShortName() const
+ std::string
+ String::getShortName() const
{
return "String";
}
- std::string String::getFullName() const
+ std::string
+ String::getFullName() const
{
return "armarx::aron::type::String";
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h
index 54dedab7b228d2262097d0cbe59675c4f721eb39..a0b9433305f455e52f2c4ddee1c3392aa1b6a20e 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/String.h
@@ -34,10 +34,11 @@ namespace armarx::aron::type
/**
* @brief The String class. It represents the string type
*/
- class String :
- public detail::PrimitiveVariant<type::dto::AronString, String>
+ class String : public detail::PrimitiveVariant<type::dto::AronString, String, std::string>
{
public:
+ using Base = detail::PrimitiveVariant<type::dto::AronString, String, std::string>;
+
/* constructors */
String(const Path& = Path());
String(const type::dto::AronString&, const Path& = Path());
@@ -48,4 +49,4 @@ namespace armarx::aron::type
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
index 03b4c0f270a1b552034022b4fed16b341085be0f..9ed6259492303586191d67d5fdcc3aebee525bf8 100644
--- a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
+++ b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/application/properties/PropertyUser.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron_component_config/VariantHelperFactory.h>
@@ -50,7 +51,7 @@ namespace armarx::aron::component_config
{
void
- PropertyDefinitionGetterVisitor::visitInt(DataInput& i, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitInt(DataInput& i, TypeInput& /*elementType*/)
{
INPUT_GUARD(i);
auto value = data::Int::DynamicCastAndCheck(i);
@@ -59,7 +60,7 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitFloat(DataInput& f, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitFloat(DataInput& f, TypeInput& /*elementType*/)
{
INPUT_GUARD(f);
auto value = data::Float::DynamicCastAndCheck(f);
@@ -68,7 +69,7 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitDouble(DataInput& d, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitDouble(DataInput& d, TypeInput& /*elementType*/)
{
INPUT_GUARD(d);
auto value = data::Double::DynamicCastAndCheck(d);
@@ -77,7 +78,7 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitBool(DataInput& b, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitBool(DataInput& b, TypeInput& /*elementType*/)
{
INPUT_GUARD(b);
auto value = data::Bool::DynamicCastAndCheck(b);
@@ -86,7 +87,7 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitString(DataInput& string, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitString(DataInput& string, TypeInput& /*elementType*/)
{
INPUT_GUARD(string);
auto value = data::String::DynamicCastAndCheck(string);
@@ -135,13 +136,13 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitListOnExit(DataInput& o, TypeInput& t)
+ PropertyDefinitionGetterVisitor::visitListOnExit(DataInput& /*o*/, TypeInput& /*t*/)
{
in_list_ = false;
}
void
- PropertyDefinitionGetterVisitor::visitIntEnum(DataInput& enumData, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitIntEnum(DataInput& enumData, TypeInput& /*elementType*/)
{
ARMARX_TRACE;
auto data = data::Int::DynamicCastAndCheck(enumData);
@@ -150,21 +151,23 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitUnknown(DataInput& unknown, TypeInput& elementType)
+ PropertyDefinitionGetterVisitor::visitUnknown(DataInput& unknown, TypeInput& /*elementType*/)
{
ARMARX_WARNING << "Unknown data encountered: "
<< (unknown ? unknown->getFullName() : "nullptr");
}
PropertyDefinitionGetterVisitor::PropertyDefinitionGetterVisitor(
- const armarx::PropertyUser& defs, const std::string& global_namespace):
- property_user_(std::experimental::make_observer(&defs)), global_namespace_(global_namespace), in_list_(false)
+ const armarx::PropertyUser& defs,
+ const std::string& globalNamespace) :
+ property_user_(std::experimental::make_observer(&defs)),
+ global_namespace_(globalNamespace),
+ in_list_(false)
{
}
void
- PropertyDefinitionGetterVisitor::visitDictOnEnter(std::shared_ptr<data::Variant>& o,
- const std::shared_ptr<type::Variant>& t)
+ PropertyDefinitionGetterVisitor::visitDictOnEnter(DataInput& o, TypeInput& t)
{
ARMARX_TRACE;
in_list_ = true;
@@ -198,9 +201,8 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitDictOnExit(
- std::shared_ptr<data::Variant>& elementData,
- const std::shared_ptr<type::Variant>& elementType)
+ PropertyDefinitionGetterVisitor::visitDictOnExit(DataInput& /*elementData*/,
+ TypeInput& /*elementType*/)
{
in_list_ = false;
}
@@ -242,8 +244,34 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionGetterVisitor::visitObjectOnEnter(std::shared_ptr<data::Variant>& i,
- const std::shared_ptr<type::Variant>& j)
+ PropertyDefinitionGetterVisitor::visitMatrix(DataInput& matrix, TypeInput& /*type*/)
+ {
+ INPUT_GUARD(matrix);
+ auto data = data::NDArray::DynamicCastAndCheck(matrix);
+ const auto name = global_namespace_ + pathToName(matrix);
+ const std::string property = property_user_->getProperty<std::string>(name).getValue();
+ const auto vec = simox::alg::to_eigen_vec(property, ", ");
+ ARMARX_CHECK_EQUAL(sizeof(float) * vec.size(), data->getDataAsVector().size())
+ << "Matrix recovered from properties does not match the aron type";
+
+ data->setData(data->getDataAsVector().size(),
+ reinterpret_cast<const unsigned char*>(vec.data()));
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitQuaternion(DataInput& quaternion, TypeInput& /*type*/)
+ {
+ INPUT_GUARD(quaternion);
+ auto value = data::NDArray::DynamicCastAndCheck(quaternion);
+ auto name = global_namespace_ + pathToName(quaternion);
+ const std::string property = property_user_->getProperty<std::string>(name).getValue();
+ const Eigen::Quaternion quat(simox::alg::to_eigen_vec(property, ", ").data());
+ const auto& newQuat = data::converter::AronEigenConverter::ConvertFromQuaternion(quat);
+ value->setData(newQuat->getDataAsVector().size(), newQuat->getData());
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitObjectOnEnter(DataInput& i, TypeInput& j)
{
INPUT_GUARD(i);
if (global_namespace_.empty())
@@ -254,13 +282,16 @@ namespace armarx::aron::component_config
}
PropertyDefinitionSetterVisitor::PropertyDefinitionSetterVisitor(
- const PropertyDefinitionsPtr& defs, const std::string& global_namespace) :
- property_definitions_(std::experimental::make_observer(defs.get())), global_namespace_(global_namespace)
+ const PropertyDefinitionsPtr& defs,
+ const std::string& globalNamespace) :
+ property_definitions_(std::experimental::make_observer(defs.get())),
+ global_namespace_(globalNamespace)
{
}
void
- PropertyDefinitionSetterVisitor::visitAronVariant(const data::IntPtr& i, const type::IntPtr&)
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::IntPtr& i,
+ const type::IntPtr& /*unused*/)
{
INPUT_GUARD(i);
auto name = global_namespace_ + pathToName(i);
@@ -269,7 +300,7 @@ namespace armarx::aron::component_config
void
PropertyDefinitionSetterVisitor::visitAronVariant(const data::FloatPtr& f,
- const type::FloatPtr&)
+ const type::FloatPtr& /*unused*/)
{
INPUT_GUARD(f);
auto name = global_namespace_ + pathToName(f);
@@ -278,7 +309,7 @@ namespace armarx::aron::component_config
void
PropertyDefinitionSetterVisitor::visitAronVariant(const data::DoublePtr& d,
- const type::DoublePtr&)
+ const type::DoublePtr& /*unused*/)
{
INPUT_GUARD(d);
auto name = global_namespace_ + pathToName(d);
@@ -286,7 +317,8 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionSetterVisitor::visitAronVariant(const data::BoolPtr& b, const type::BoolPtr&)
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::BoolPtr& b,
+ const type::BoolPtr& /*unused*/)
{
INPUT_GUARD(b);
auto name = global_namespace_ + pathToName(b);
@@ -295,7 +327,7 @@ namespace armarx::aron::component_config
void
PropertyDefinitionSetterVisitor::visitAronVariant(const data::StringPtr& string,
- const type::StringPtr& type)
+ const type::StringPtr& /*type*/)
{
INPUT_GUARD(string);
auto name = global_namespace_ + pathToName(string);
@@ -339,8 +371,8 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::ListPtr&,
- const type::ListPtr&)
+ PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::ListPtr& /*unused*/,
+ const type::ListPtr& /*unused*/)
{
in_list_ = false;
}
@@ -375,8 +407,8 @@ namespace armarx::aron::component_config
}
void
- PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::DictPtr&,
- const type::DictPtr&)
+ PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::DictPtr& /*unused*/,
+ const type::DictPtr& /*unused*/)
{
in_list_ = false;
}
@@ -391,6 +423,48 @@ namespace armarx::aron::component_config
global_namespace_ = type->getObjectNameWithoutNamespace() + ".";
}
}
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::NDArrayPtr& quaternion,
+ const type::QuaternionPtr& /*type*/)
+ {
+ INPUT_GUARD(quaternion);
+ auto name = global_namespace_ + pathToName(quaternion);
+ const auto quat =
+ data::converter::AronEigenConverter::ConvertToQuaternion<float>(quaternion);
+ property_definitions_->defineOptionalProperty<std::string>(
+ name, simox::alg::to_string(quat.coeffs(), ", "));
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::NDArrayPtr& matrix,
+ const type::MatrixPtr& /*type*/)
+ {
+ INPUT_GUARD(matrix);
+ auto name = global_namespace_ + pathToName(matrix);
+ auto shape = matrix->getShape();
+ // TODO: why is shape 3 for a matrix?
+ if (shape.size() != 3 or (shape[0] != 1 and shape[1] != 1))
+ {
+ ARMARX_WARNING << "Matrix is not a vector, ignoring";
+ return;
+ }
+ const auto mat = data::converter::AronEigenConverter::ConvertToMatrix<float>(*matrix);
+ std::string value = "";
+ for (int i = 0; i < shape[0]; i++)
+ {
+ for (int j = 0; j < shape[1]; j++)
+ {
+ value += std::to_string(mat(i, j)) + ", ";
+ }
+ }
+ if (not value.empty())
+ {
+ value.pop_back();
+ value.pop_back();
+ }
+ property_definitions_->defineOptionalProperty<std::string>(name, value);
+ }
} // namespace armarx::aron::component_config
#undef INPUT_GUARD
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h
index a9c1d5823bd8589acaea27b4f27928b4668ef853..e00f96e75d2ea44b44d08674fc599528e59e7a12 100644
--- a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h
+++ b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h
@@ -34,7 +34,8 @@ namespace armarx::aron::component_config
class PropertyDefinitionSetterVisitor : public aron::data::RecursiveConstTypedVariantVisitor
{
public:
- explicit PropertyDefinitionSetterVisitor(const armarx::PropertyDefinitionsPtr& defs, const std::string& global_namespace = "");
+ explicit PropertyDefinitionSetterVisitor(const armarx::PropertyDefinitionsPtr& defs,
+ const std::string& global_namespace = "");
void visitAronVariant(const data::IntPtr&, const type::IntEnumPtr&) override;
@@ -48,6 +49,10 @@ namespace armarx::aron::component_config
void visitAronVariant(const data::StringPtr&, const type::StringPtr&) override;
+ void visitAronVariant(const data::NDArrayPtr&, const type::QuaternionPtr&) override;
+
+ void visitAronVariant(const data::NDArrayPtr&, const type::MatrixPtr&) override;
+
void visitAronVariantOnEnter(const data::ListPtr&, const type::ListPtr&) override;
void visitAronVariantOnExit(const data::ListPtr&, const type::ListPtr&) override;
@@ -69,7 +74,8 @@ namespace armarx::aron::component_config
public aron::data::RecursiveTypedVisitor<data::VariantPtr, const type::VariantPtr>
{
public:
- explicit PropertyDefinitionGetterVisitor(const armarx::PropertyUser& defs, const std::string& global_namespace = "");
+ explicit PropertyDefinitionGetterVisitor(const armarx::PropertyUser& defs,
+ const std::string& global_namespace = "");
type::Descriptor getDescriptor(DataInput& o, TypeInput& t) override;
@@ -95,6 +101,10 @@ namespace armarx::aron::component_config
void visitUnknown(DataInput& elementData, TypeInput& elementType) override;
+ void visitQuaternion(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitMatrix(DataInput& elementData, TypeInput& elementType) override;
+
void visitListOnEnter(DataInput& elementData, TypeInput& elementType) override;
void visitListOnExit(DataInput& elementData, TypeInput& elementType) override;
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index 4849196ad868ebc11c5247c780e9dd6ea41814a6..324ac1afa5153251ae397de56903a53b11481d75 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -24,17 +24,17 @@
#define EIGEN_NO_STATIC_ASSERT
#include "Pose.h"
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/LinkedCoordinate.h>
-#include <VirtualRobot/LinkedCoordinate.h>
-#include <VirtualRobot/VirtualRobot.h>
-#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include <sstream>
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include <sstream>
+#include <VirtualRobot/LinkedCoordinate.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
template class ::IceInternal::Handle<::armarx::Pose>;
template class ::IceInternal::Handle<::armarx::Vector2>;
@@ -61,27 +61,31 @@ namespace armarx
this->y = y;
}
- Eigen::Vector2f Vector2::toEigen() const
+ Eigen::Vector2f
+ Vector2::toEigen() const
{
Eigen::Vector2f v;
v << this->x, this->y;
return v;
}
- void Vector2::operator=(const Eigen::Vector2f& vec)
+ void
+ Vector2::operator=(const Eigen::Vector2f& vec)
{
x = vec[0];
y = vec[1];
}
- std::string Vector2::output(const Ice::Current&) const
+ std::string
+ Vector2::output(const Ice::Current&) const
{
std::stringstream s;
s << toEigen();
return s.str();
}
- void Vector2::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
+ void
+ Vector2::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -89,7 +93,8 @@ namespace armarx
obj->setFloat("y", y);
}
- void Vector2::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&)
+ void
+ Vector2::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -125,28 +130,32 @@ namespace armarx
this->z = z;
}
- Eigen::Vector3f Vector3::toEigen() const
+ Eigen::Vector3f
+ Vector3::toEigen() const
{
Eigen::Vector3f v;
v << this->x, this->y, this->z;
return v;
}
- void Vector3::operator=(const Eigen::Vector3f& vec)
+ void
+ Vector3::operator=(const Eigen::Vector3f& vec)
{
x = vec[0];
y = vec[1];
z = vec[2];
}
- std::string Vector3::output(const Ice::Current&) const
+ std::string
+ Vector3::output(const Ice::Current&) const
{
std::stringstream s;
s << toEigen().format(ArmarXEigenFormat);
return s.str();
}
- void Vector3::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
+ void
+ Vector3::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -155,7 +164,8 @@ namespace armarx
obj->setFloat("z", z);
}
- void Vector3::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ void
+ Vector3::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -164,9 +174,7 @@ namespace armarx
z = obj->getFloat("z");
}
-
- Quaternion::Quaternion()
- = default;
+ Quaternion::Quaternion() = default;
Quaternion::Quaternion(const Eigen::Matrix4f& m4)
{
@@ -192,24 +200,28 @@ namespace armarx
this->qz = qz;
}
- Eigen::Matrix3f Quaternion::toEigen() const
+ Eigen::Matrix3f
+ Quaternion::toEigen() const
{
return toEigenQuaternion().toRotationMatrix();
}
- Eigen::Quaternionf Quaternion::toEigenQuaternion() const
+ Eigen::Quaternionf
+ Quaternion::toEigenQuaternion() const
{
return {this->qw, this->qx, this->qy, this->qz};
}
- void Quaternion::init(const Eigen::Matrix3f& m3)
+ void
+ Quaternion::init(const Eigen::Matrix3f& m3)
{
Eigen::Quaternionf q;
q = m3;
init(q);
}
- void Quaternion::init(const Eigen::Quaternionf& q)
+ void
+ Quaternion::init(const Eigen::Quaternionf& q)
{
this->qw = q.w();
this->qx = q.x();
@@ -217,12 +229,14 @@ namespace armarx
this->qz = q.z();
}
- Eigen::Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f& m)
+ Eigen::Matrix3f
+ Quaternion::slerp(float alpha, const Eigen::Matrix3f& m)
{
return Quaternion::slerp(alpha, this->toEigen(), m);
}
- Eigen::Matrix3f Quaternion::slerp(float alpha, const Eigen::Matrix3f& m1, const Eigen::Matrix3f& m2)
+ Eigen::Matrix3f
+ Quaternion::slerp(float alpha, const Eigen::Matrix3f& m1, const Eigen::Matrix3f& m2)
{
Eigen::Matrix3f result;
Eigen::Quaternionf q1, q2;
@@ -232,14 +246,16 @@ namespace armarx
return result;
}
- std::string Quaternion::output(const Ice::Current& c) const
+ std::string
+ Quaternion::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen()/*.format(ArmarXEigenFormat)*/;
+ s << toEigen() /*.format(ArmarXEigenFormat)*/;
return s.str();
}
- void Quaternion::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
+ void
+ Quaternion::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -249,7 +265,8 @@ namespace armarx
obj->setFloat("qw", qw);
}
- void Quaternion::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ void
+ Quaternion::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -298,14 +315,16 @@ namespace armarx
init();
}
- void Pose::operator=(const Eigen::Matrix4f& matrix)
+ void
+ Pose::operator=(const Eigen::Matrix4f& matrix)
{
this->orientation = new Quaternion(matrix);
this->position = new Vector3(matrix);
init();
}
- Eigen::Matrix4f Pose::toEigen() const
+ Eigen::Matrix4f
+ Pose::toEigen() const
{
Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
ARMARX_CHECK_EXPRESSION(c_orientation);
@@ -315,21 +334,23 @@ namespace armarx
return m;
}
- std::string Pose::output(const Ice::Current&) const
+ std::string
+ Pose::output(const Ice::Current&) const
{
std::stringstream s;
- s << toEigen()/*.format(ArmarXEigenFormat)*/;
+ s << toEigen() /*.format(ArmarXEigenFormat)*/;
return s.str();
}
-
- void Pose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
+ void
+ Pose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
{
position->serialize(serializer, c);
orientation->serialize(serializer, c);
}
- void Pose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ void
+ Pose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -337,103 +358,163 @@ namespace armarx
orientation->deserialize(obj);
}
- void Pose::init()
+ void
+ Pose::init()
{
this->c_position = Vector3Ptr::dynamicCast(this->position);
this->c_orientation = QuaternionPtr::dynamicCast(this->orientation);
}
- void Pose::ice_postUnmarshal()
+ void
+ Pose::ice_postUnmarshal()
{
init();
}
-}
-
+} // namespace armarx
-void armarx::fromIce(const Vector3BasePtr& ice, Eigen::Vector3f& vector)
+void
+armarx::fromIce(const Vector3BasePtr& ice, Eigen::Vector3f& vector)
{
vector = fromIce(ice);
}
-void armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Quaternionf& quaternion)
+void
+armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Quaternionf& quaternion)
{
quaternion = fromIce(ice);
}
-void armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Matrix3f& rotation)
+void
+armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Matrix3f& rotation)
{
rotation = fromIce(ice).toRotationMatrix();
}
-void armarx::fromIce(const PoseBasePtr& ice, Eigen::Matrix4f& pose)
+void
+armarx::fromIce(const QuaternionBasePtr& ice, Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation)
+{
+ Eigen::Matrix3f helper;
+ fromIce(ice, helper);
+ rotation = helper;
+}
+
+void
+armarx::fromIce(const PoseBasePtr& ice, Eigen::Matrix4f& pose)
{
pose = fromIce(ice);
}
+void
+armarx::fromIce(const PoseBasePtr& ice, Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose)
+{
+ Eigen::Matrix4f helper;
+ fromIce(ice, helper);
+ pose = helper;
+}
-Eigen::Vector3f armarx::fromIce(const Vector3BasePtr& pose)
+Eigen::Vector3f
+armarx::fromIce(const Vector3BasePtr& pose)
{
auto cast = Vector3Ptr::dynamicCast(pose);
ARMARX_CHECK_NOT_NULL(cast) << "Vector3BasePtr must be a Vector3Ptr.";
return cast->toEigen();
}
-Eigen::Quaternionf armarx::fromIce(const QuaternionBasePtr& pose)
+
+Eigen::Quaternionf
+armarx::fromIce(const QuaternionBasePtr& pose)
{
auto cast = QuaternionPtr::dynamicCast(pose);
ARMARX_CHECK_NOT_NULL(cast) << "QuaternionBasePtr must be a QuaternionPtr.";
return cast->toEigenQuaternion();
}
-Eigen::Matrix4f armarx::fromIce(const PoseBasePtr& pose)
+
+Eigen::Matrix4f
+armarx::fromIce(const PoseBasePtr& pose)
{
auto cast = PosePtr::dynamicCast(pose);
ARMARX_CHECK_NOT_NULL(cast) << "PoseBasePtr must be a PosePtr.";
return cast->toEigen();
}
-
-void armarx::toIce(Vector3BasePtr& ice, const Eigen::Vector3f& vector)
+void
+armarx::toIce(Vector3BasePtr& ice, const Eigen::Vector3f& vector)
{
ice = new Vector3(vector);
}
-void armarx::toIce(QuaternionBasePtr& ice, const Eigen::Matrix3f& rotation)
+void
+armarx::toIce(QuaternionBasePtr& ice, const Eigen::Matrix3f& rotation)
{
ice = new Quaternion(rotation);
}
-void armarx::toIce(QuaternionBasePtr& ice, const Eigen::Quaternionf& quaternion)
+void
+armarx::toIce(QuaternionBasePtr& ice, const Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation)
+{
+ Eigen::Matrix3f helper = rotation;
+ toIce(ice, helper);
+}
+
+void
+armarx::toIce(QuaternionBasePtr& ice, const Eigen::Quaternionf& quaternion)
{
ice = new Quaternion(quaternion);
}
-void armarx::toIce(PoseBasePtr& ice, const Eigen::Matrix4f& pose)
+void
+armarx::toIce(PoseBasePtr& ice, const Eigen::Matrix4f& pose)
{
ice = new Pose(pose);
}
+void
+armarx::toIce(PoseBasePtr& ice, const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose)
+{
+ Eigen::Matrix4f helper = pose;
+ toIce(ice, helper);
+}
-armarx::Vector3Ptr armarx::toIce(const Eigen::Vector3f& vector)
+armarx::Vector3Ptr
+armarx::toIce(const Eigen::Vector3f& vector)
{
return new Vector3(vector);
}
-armarx::QuaternionPtr armarx::toIce(const Eigen::Matrix3f& rotation)
+armarx::QuaternionPtr
+armarx::toIce(const Eigen::Matrix3f& rotation)
{
return new Quaternion(rotation);
}
-armarx::QuaternionPtr armarx::toIce(const Eigen::Quaternionf& quaternion)
+armarx::QuaternionPtr
+armarx::toIce(const Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation)
+{
+ Eigen::Matrix3f helper = rotation;
+ return toIce(helper);
+}
+
+armarx::QuaternionPtr
+armarx::toIce(const Eigen::Quaternionf& quaternion)
{
return new Quaternion(quaternion);
}
-armarx::PosePtr armarx::toIce(const Eigen::Matrix4f& pose)
+armarx::PosePtr
+armarx::toIce(const Eigen::Matrix4f& pose)
{
return new Pose(pose);
}
-armarx::PosePtr armarx::toIce(const Eigen::Isometry3f& pose)
+armarx::PosePtr
+armarx::toIce(const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose)
+{
+ Eigen::Matrix4f helper = pose;
+ return toIce(helper);
+}
+
+armarx::PosePtr
+armarx::toIce(const Eigen::Isometry3f& pose)
{
return armarx::toIce(pose.matrix());
}
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index 2748a0ded0a959633c3a44a1c8dd3f1c789c6ff6..18332395d12d9992118c32e98823974dded3b402 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -24,12 +24,12 @@
#pragma once
-#include <RobotAPI/interface/core/PoseBase.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
#include <ArmarXCore/observers/variant/Variant.h>
-#include <Eigen/Core>
-#include <Eigen/Geometry>
+#include <RobotAPI/interface/core/PoseBase.h>
namespace armarx::VariantType
{
@@ -38,11 +38,12 @@ namespace armarx::VariantType
const VariantTypeId Vector3 = Variant::addTypeName("::armarx::Vector3Base");
const VariantTypeId Quaternion = Variant::addTypeName("::armarx::QuaternionBase");
const VariantTypeId Pose = Variant::addTypeName("::armarx::PoseBase");
-}
+} // namespace armarx::VariantType
namespace armarx
{
- const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "");
+ const Eigen::IOFormat
+ ArmarXEigenFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "");
/**
* @brief The Vector2 class
@@ -60,34 +61,44 @@ namespace armarx
virtual Eigen::Vector2f toEigen() const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
+ Ice::ObjectPtr
+ ice_clone() const override
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& = Ice::emptyCurrent) const override
+
+ VariantDataClassPtr
+ clone(const Ice::Current& = Ice::emptyCurrent) const override
{
return new Vector2(*this);
}
+
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
+
+ VariantTypeId
+ getType(const Ice::Current& = Ice::emptyCurrent) const override
{
return VariantType::Vector2;
}
- bool validate(const Ice::Current& = Ice::emptyCurrent) override
+
+ bool
+ validate(const Ice::Current& = Ice::emptyCurrent) override
{
return true;
}
- friend std::ostream& operator<<(std::ostream& stream, const Vector2& rhs)
+ friend std::ostream&
+ operator<<(std::ostream& stream, const Vector2& rhs)
{
stream << "Vector2: " << std::endl << rhs.output() << std::endl;
return stream;
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
-
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
};
using Vector2Ptr = IceInternal::Handle<Vector2>;
@@ -98,8 +109,7 @@ namespace armarx
* @ingroup RobotAPI-FramedPose
* @brief The Vector3 class
*/
- class Vector3 :
- virtual public Vector3Base
+ class Vector3 : virtual public Vector3Base
{
public:
Vector3();
@@ -112,50 +122,57 @@ namespace armarx
virtual Eigen::Vector3f toEigen() const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
+ Ice::ObjectPtr
+ ice_clone() const override
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
+
+ VariantDataClassPtr
+ clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new Vector3(*this);
}
+
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
+
+ VariantTypeId
+ getType(const Ice::Current& = Ice::emptyCurrent) const override
{
return VariantType::Vector3;
}
- bool validate(const Ice::Current& = Ice::emptyCurrent) override
+
+ bool
+ validate(const Ice::Current& = Ice::emptyCurrent) override
{
return true;
}
-
- friend std::ostream& operator<<(std::ostream& stream, const Vector3& rhs)
+ friend std::ostream&
+ operator<<(std::ostream& stream, const Vector3& rhs)
{
stream << "Vector3: " << std::endl << rhs.output() << std::endl;
return stream;
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
};
using Vector3Ptr = IceInternal::Handle<Vector3>;
-
/**
* @class Quaternion
* @ingroup VariantsGrp
* @ingroup RobotAPI-FramedPose
* @brief The Quaternion class
*/
- class Quaternion :
- virtual public QuaternionBase
+ class Quaternion : virtual public QuaternionBase
{
public:
-
/// Construct an identity quaternion.
Quaternion();
Quaternion(const Eigen::Matrix4f&);
@@ -170,34 +187,44 @@ namespace armarx
static Eigen::Matrix3f slerp(float, const Eigen::Matrix3f&, const Eigen::Matrix3f&);
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
+ Ice::ObjectPtr
+ ice_clone() const override
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& = Ice::emptyCurrent) const override
+
+ VariantDataClassPtr
+ clone(const Ice::Current& = Ice::emptyCurrent) const override
{
return new Quaternion(*this);
}
+
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
+
+ VariantTypeId
+ getType(const Ice::Current& = Ice::emptyCurrent) const override
{
return VariantType::Quaternion;
}
- bool validate(const Ice::Current& = Ice::emptyCurrent) override
+
+ bool
+ validate(const Ice::Current& = Ice::emptyCurrent) override
{
return true;
}
-
- friend std::ostream& operator<<(std::ostream& stream, const Quaternion& rhs)
+ friend std::ostream&
+ operator<<(std::ostream& stream, const Quaternion& rhs)
{
stream << "Quaternion: " << std::endl << rhs.output() << std::endl;
return stream;
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
private:
void init(const Eigen::Matrix3f&);
@@ -206,15 +233,13 @@ namespace armarx
using QuaternionPtr = IceInternal::Handle<Quaternion>;
-
/**
* @class Pose
* @ingroup VariantsGrp
* @ingroup RobotAPI-FramedPose
* @brief The Pose class
*/
- class Pose :
- virtual public PoseBase
+ class Pose : virtual public PoseBase
{
public:
Pose();
@@ -228,42 +253,53 @@ namespace armarx
virtual Eigen::Matrix4f toEigen() const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
+ Ice::ObjectPtr
+ ice_clone() const override
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& = Ice::emptyCurrent) const override
+
+ VariantDataClassPtr
+ clone(const Ice::Current& = Ice::emptyCurrent) const override
{
return new Pose(*this);
}
+
std::string output(const Ice::Current& = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& = Ice::emptyCurrent) const override
+
+ VariantTypeId
+ getType(const Ice::Current& = Ice::emptyCurrent) const override
{
return VariantType::Pose;
}
- bool validate(const Ice::Current& = Ice::emptyCurrent) override
+
+ bool
+ validate(const Ice::Current& = Ice::emptyCurrent) override
{
return true;
}
- friend std::ostream& operator<<(std::ostream& stream, const Pose& rhs)
+ friend std::ostream&
+ operator<<(std::ostream& stream, const Pose& rhs)
{
stream << "Pose: " << std::endl << rhs.output() << std::endl;
return stream;
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
protected:
void init();
void ice_postUnmarshal() override;
+
private:
//! To void unnecessary upcasts
QuaternionPtr c_orientation;
Vector3Ptr c_position;
-
};
using PosePtr = IceInternal::Handle<Pose>;
@@ -275,6 +311,9 @@ namespace armarx
void fromIce(const QuaternionBasePtr& ice, Eigen::Quaternionf& quaternion);
void fromIce(const QuaternionBasePtr& ice, Eigen::Matrix3f& rotation);
void fromIce(const PoseBasePtr& ice, Eigen::Matrix4f& pose);
+ void fromIce(const QuaternionBasePtr& ice,
+ Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation);
+ void fromIce(const PoseBasePtr& ice, Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose);
Eigen::Vector3f fromIce(const Vector3BasePtr& position);
Eigen::Quaternionf fromIce(const QuaternionBasePtr& rotation);
@@ -282,18 +321,22 @@ namespace armarx
void toIce(Vector3BasePtr& ice, const Eigen::Vector3f& vector);
void toIce(QuaternionBasePtr& ice, const Eigen::Matrix3f& rotation);
+ void toIce(QuaternionBasePtr& ice, const Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation);
void toIce(QuaternionBasePtr& ice, const Eigen::Quaternionf& quaternion);
void toIce(PoseBasePtr& ice, const Eigen::Matrix4f& pose);
+ void toIce(PoseBasePtr& ice, const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose);
Vector3Ptr toIce(const Eigen::Vector3f& vector);
QuaternionPtr toIce(const Eigen::Matrix3f& rotation);
+ QuaternionPtr toIce(const Eigen::Matrix<float, 3, 3, Eigen::RowMajor>& rotation);
QuaternionPtr toIce(const Eigen::Quaternionf& quaternion);
PosePtr toIce(const Eigen::Matrix4f& pose);
+ PosePtr toIce(const Eigen::Matrix<float, 4, 4, Eigen::RowMajor>& pose);
PosePtr toIce(const Eigen::Isometry3f& pose);
-}
+} // namespace armarx
-extern template class ::IceInternal::Handle< ::armarx::Pose>;
-extern template class ::IceInternal::Handle< ::armarx::Vector2>;
-extern template class ::IceInternal::Handle< ::armarx::Vector3>;
-extern template class ::IceInternal::Handle< ::armarx::Quaternion>;
+extern template class ::IceInternal::Handle<::armarx::Pose>;
+extern template class ::IceInternal::Handle<::armarx::Vector2>;
+extern template class ::IceInternal::Handle<::armarx::Vector3>;
+extern template class ::IceInternal::Handle<::armarx::Quaternion>;
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
index d53a8dd6a89f0cd37f609c9a4742d97d86f4deba..0f1f8047f3f47976a7ce52a928bac858d665b053 100644
--- a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -62,6 +62,7 @@ namespace armarx::obstacle_avoidance
if (auto obstacle = finder.loadManipulationObject(objectPose))
{
obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ obstacle->setName(objectPose.objectID.instanceName());
sceneObjects->addSceneObject(obstacle);
}
}
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 60a38b465ddb8bd9d6cbdac5ec4952606624880c..3999c956b3ec3d85be444e7b27d56c27fbc2d98e 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -1,198 +1,701 @@
#include "SkillManagerComponentPlugin.h"
#include <ArmarXCore/core/Component.h>
-#include "../error/Exception.h"
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.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
{
- void SkillManagerComponentPlugin::preOnInitComponent()
- {}
-
- void SkillManagerComponentPlugin::preOnConnectComponent()
- {}
+ void
+ SkillManagerComponentPlugin::preOnInitComponent()
+ {
+ }
- void SkillManagerComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
- {}
-}
+ void
+ SkillManagerComponentPlugin::preOnConnectComponent()
+ {
+ auto& p = parent<SkillManagerComponentPluginUser>();
+ p.getProxy(myPrx, -1);
+ }
+ void
+ SkillManagerComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ {
+ }
-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&)
+ void
+ 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 << "' but the provider already exists. "
+ 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&)
+ void
+ 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 << "' but it couldn't be found.";
+ 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)
+ provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId);
+ }
+
+
+ if (auto it = skillProviderMap.find(provderId); it != skillProviderMap.end())
+ {
+ const auto& provider = it->second;
+
+ if (!provider)
{
- if (currentSkillName == skillName)
- {
- return providerName;
- }
+ 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.");
}
}
- return "INVALID PROVIDER NAME";
+ 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.");
+ }
}
- skills::provider::dto::SkillStatusUpdate SkillManagerComponentPluginUser::executeSkill(const skills::manager::dto::SkillExecutionRequest& info, const Ice::Current&)
+ skills::SkillExecutionID
+ SkillManagerComponentPlugin::executeSkillAsync(
+ const skills::SkillExecutionRequest& executionRequest)
{
- std::string providerName = "INVALID PROVIDER NAME";
- if (info.skillId.providerName == "*")
+ 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 == "*")
{
- providerName = getFirstProviderNameThatHasSkill(info.skillId.skillName);
+ provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId);
+ }
+
+
+ if (auto it = skillProviderMap.find(provderId); it != skillProviderMap.end())
+ {
+ 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_executeSkillAsync(provider_executionRequest.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto provider_executionID_ice = provider->end_executeSkillAsync(async);
+
+ // convert to manager view
+ auto executionId =
+ skills::SkillExecutionID::FromIce(provider_executionID_ice, provderId);
+ executionId.skillId.providerId = provderId;
+ return executionId;
+ }
+ 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.");
}
+ }
- bool remove = false;
- if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+ bool
+ SkillManagerComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
+ const aron::data::DictPtr& data)
+ {
+ 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 (s)
+ if (!provider)
{
- skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
- getProxy(myPrx, -1);
-
- skills::provider::dto::SkillExecutionRequest exInfo;
- exInfo.skillName = info.skillId.skillName;
- exInfo.executorName = info.executorName;
- exInfo.callbackInterface = myPrx;
- exInfo.params = info.params;
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+ return false;
+ }
- return s->executeSkill(exInfo);
+ 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;
}
- else
+ catch (...)
{
- remove = true;
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills on next execute.";
+ return false;
}
+ }
+ else
+ {
+ return false;
+ }
+ }
- if (remove)
+ bool
+ SkillManagerComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
+ {
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
+ it != skillProviderMap.end())
+ {
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
{
- std::scoped_lock l(skillProviderMapMutex);
- if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
- {
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected 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_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
{
- 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 ¤t)
+ bool
+ SkillManagerComponentPlugin::abortSkillAsync(const skills::SkillExecutionID& executionId)
{
- std::scoped_lock l(skillProviderMapMutex);
- if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+ 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;
- if (s)
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
{
- s->abortSkill(skillName);
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+ return false;
}
- else
+
+ try
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
- it = skillProviderMap.erase(it);
+ auto async = provider->begin_abortSkill(executionId.toProviderIce());
+ return true;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found buggy skill provider '"
+ << providerId << "'. Removing it from skills on next execute.";
+ return false;
}
}
+ else
+ {
+ return false;
+ }
}
- 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 ¤t)
+ 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;
- if (s)
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
{
- skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
- ret.insert({n, m});
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ continue;
+ }
+
+ try
+ {
+ skills::provider::dto::SkillDescriptionMap m = provider->getSkillDescriptions();
+
+ for (const auto& [provider_skillId_ice, skillDescription_ice] : m)
+ {
+ ret.insert(
+ {skills::SkillID::FromIce(provider_skillId_ice, providerId),
+ skills::SkillDescription::FromIce(skillDescription_ice, providerId)});
+ }
+
++it;
}
- else
+ catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected 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 ¤t)
+ std::optional<skills::SkillStatusUpdate>
+ SkillManagerComponentPlugin::getSkillExecutionStatus(
+ const skills::SkillExecutionID& executionId)
+ {
+ ARMARX_CHECK(executionId.skillId.isFullySpecified());
+
+ std::unique_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
+ it != skillProviderMap.end())
+ {
+ const auto& providerId = it->first;
+ const auto& provider = it->second;
+
+ if (!provider)
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
+ << providerId << "'. Removing it from skills.";
+ skillProviderMap.erase(it);
+
+ 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_getSkillExecutionStatus(executionId.toProviderIce());
+ l.unlock(); // allow parallel e.g. stopping
+ auto provider_statusUpdate_ice = provider->end_getSkillExecutionStatus(async);
+
+ if (not provider_statusUpdate_ice)
+ {
+ return std::nullopt;
+ }
+
+ // 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()
{
- skills::manager::dto::SkillStatusUpdateMapMap ret;
+ std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> ret;
std::scoped_lock l(skillProviderMapMutex);
for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
{
- const auto& n = it->first;
- const auto& s = it->second;
- if (s)
+ 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
{
- skills::provider::dto::SkillStatusUpdateMap m = s->getSkillExecutionStatuses();
- ret.insert({n, m});
+ auto m = provider->getSkillExecutionStatuses();
+
+ for (const auto& [provider_executionId_ice, provider_statusUpdate_ice] : m)
+ {
+ ret.insert(
+ {skills::SkillExecutionID::FromIce(provider_executionId_ice, providerId),
+ skills::SkillStatusUpdate::FromIce(provider_statusUpdate_ice,
+ providerId)});
+ }
it++;
}
- else
+ catch (...)
{
- ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected 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 21b32da1a75488fde1efea01b3b48e42b7fa77b9..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/Skill.cpp
+++ /dev/null
@@ -1,195 +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();
- }
-
- 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 0522191bfb57d272785380502217f23006a9e254..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/Skill.h
+++ /dev/null
@@ -1,174 +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:
- // 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 6b74dd1e2479068ca920dde2b8f2ec102b0d7646..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,42 +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>&&);
+
+ skills::SkillBlueprint* addSkillFactory(const skills::SkillDescription&,
+ const skills::LambdaSkill::FunctionType&);
+
+ template <class SkillT, typename... Args>
+
+ requires skills::isSkill<SkillT> skills::SkillBlueprint*
+ addSkillFactory(Args&&... args)
+ {
+ 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>
- template <typename T>
- T*
- addSkill()
+ requires skills::isSkillBlueprint<FactoryT> FactoryT*
+ addCustomSkillFactory(Args&&... args)
{
- static_assert(std::is_base_of<skills::Skill, T>::value,
- "T must be derived from skills::Skill!");
- static_assert(std::is_default_constructible<T>::value,
- "T must be default constructible!");
-
- auto skill = std::make_unique<T>();
- auto* skillPtr = skill.get();
- addSkill(std::move(skill));
- return static_cast<T*>(skillPtr);
+ 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,
@@ -87,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>
- T*
- addSkill()
+ requires skills::isSkillBlueprint<FacT> FacT*
+ addCustomSkillFactory(Args&&... args)
{
- return plugin->addSkill<T>();
+ 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 e0ebdb3b653a1be1d59d9b945ac6b35c58962595..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.cpp
+++ /dev/null
@@ -1,47 +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;
- }
- }
-}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProxy.h b/source/RobotAPI/libraries/skills/provider/SkillProxy.h
deleted file mode 100644
index 6e8ddc5c74ec5e3d36423984d67807ae12da0680..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/skills/provider/SkillProxy.h
+++ /dev/null
@@ -1,26 +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);
-
- 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/provider/mixins/ObjectReadingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/ObjectReadingSkillMixin.h
index de5ef34b1dc448edc4a8ba634ad74600c7d33da4..ecf3d2a64ef0a5d3f01b5b9b23839b1db14cb921 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/ObjectReadingSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/ObjectReadingSkillMixin.h
@@ -10,7 +10,7 @@ namespace armarx::skills::mixin
{
armem::obj::instance::Reader objectReader;
- ObjectReadingSkillMixin(armem::client::MemoryNameSystem& mns, const objpose::ObjectPoseProviderPrx& o) : objectReader(mns, o)
+ ObjectReadingSkillMixin(armem::client::MemoryNameSystem& mns) : objectReader(mns)
{}
};
@@ -19,7 +19,7 @@ namespace armarx::skills::mixin
std::string objectEntityId;
armem::obj::instance::Reader objectReader;
- SpecificObjectReadingSkillMixin(const std::string& n, armem::client::MemoryNameSystem& mns, const objpose::ObjectPoseProviderPrx& o) : objectEntityId(n), objectReader(mns, o)
+ SpecificObjectReadingSkillMixin(const std::string& n, armem::client::MemoryNameSystem& mns) : objectEntityId(n), objectReader(mns)
{}
};
}
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/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
index 006016c5daa24d555c8a5d9537b8bfbbd0c96229..33340476f9568dce65fbd43bcd3f745f80df32f6 100644
--- a/source/RobotAPI/statecharts/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -13,3 +13,5 @@ add_subdirectory(StatechartExecutionGroup)
add_subdirectory(ProsthesisKinestheticTeachIn)
add_subdirectory(DebugDrawerToArVizGroup)
+
+add_subdirectory(ObjectMemoryGroup)
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt b/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a751e89b2566ed34fadcfa9c04943088955c1d5e
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/CMakeLists.txt
@@ -0,0 +1,31 @@
+# Modern CMake way:
+#armarx_add_statechart(ObjectMemoryGroup
+# SOURCES
+# ObjectMemoryGroupRemoteStateOfferer.cpp
+# HEADERS
+# ObjectMemoryGroupRemoteStateOfferer.h
+# DEPENDENCIES
+#)
+
+# Legacy CMake way:
+
+armarx_component_set_name("ObjectMemoryGroup")
+
+set(COMPONENT_LIBS
+ ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers
+ # RobotAPI
+ RobotAPIArmarXObjects
+ armem_objects
+)
+
+set(SOURCES
+ ObjectMemoryGroupRemoteStateOfferer.cpp
+)
+
+set(HEADERS
+ ObjectMemoryGroupRemoteStateOfferer.h
+ ObjectMemoryGroup.scgxml
+)
+
+armarx_generate_statechart_cmake_lists()
+armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml
new file mode 100644
index 0000000000000000000000000000000000000000..8f7d00053ef37819464e9d1b7bc4171cf4caf72b
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroup.scgxml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="utf-8"?>
+<StatechartGroup name="ObjectMemoryGroup" package="RobotAPI" generateContext="true">
+ <Proxies>
+ <Proxy value="RobotAPIInterfaces.MemoryNameSystem"/>
+ </Proxies>
+ <Folder basename="test">
+ <State filename="RequestObjectsTest.xml" visibility="public"/>
+ </Folder>
+ <State filename="RequestObjects.xml" visibility="public"/>
+</StatechartGroup>
+
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1324dfff62f4a6ec3df04e61d451f062125a50c8
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp
@@ -0,0 +1,29 @@
+#include "ObjectMemoryGroupRemoteStateOfferer.h"
+
+namespace armarx::ObjectMemoryGroup
+{
+ // DO NOT EDIT NEXT LINE
+ ObjectMemoryGroupRemoteStateOfferer::SubClassRegistry ObjectMemoryGroupRemoteStateOfferer::Registry(ObjectMemoryGroupRemoteStateOfferer::GetName(), &ObjectMemoryGroupRemoteStateOfferer::CreateInstance);
+
+ ObjectMemoryGroupRemoteStateOfferer::ObjectMemoryGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer < ObjectMemoryGroupStatechartContext > (reader)
+ {}
+
+ void ObjectMemoryGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer() {}
+
+ void ObjectMemoryGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer() {}
+
+ void ObjectMemoryGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer() {}
+
+ // DO NOT EDIT NEXT FUNCTION
+ std::string ObjectMemoryGroupRemoteStateOfferer::GetName()
+ {
+ return "ObjectMemoryGroupRemoteStateOfferer";
+ }
+
+ // DO NOT EDIT NEXT FUNCTION
+ XMLStateOffererFactoryBasePtr ObjectMemoryGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+ {
+ return XMLStateOffererFactoryBasePtr(new ObjectMemoryGroupRemoteStateOfferer(reader));
+ }
+}
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h
new file mode 100644
index 0000000000000000000000000000000000000000..882627aec09b55c16b4b0f07fe313c0138392368
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+#include "ObjectMemoryGroupStatechartContext.generated.h"
+
+namespace armarx::ObjectMemoryGroup
+{
+ class ObjectMemoryGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < ObjectMemoryGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ {
+ public:
+ ObjectMemoryGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
+
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
+
+
+ };
+}
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..56dc86697810e10870f6b409b3f6f810ee9b0729
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
@@ -0,0 +1,142 @@
+#include "RequestObjects.h"
+
+//#include <ArmarXCore/core/time/TimeUtil.h>
+//#include <ArmarXCore/observers/variant/DatafieldRef.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::RequestObjects(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<RequestObjects>(stateData),
+ RequestObjectsGeneratedBase<RequestObjects>(stateData)
+ {
+ }
+
+ void
+ RequestObjects::onEnter()
+ {
+
+ // put your user code for the enter-point here
+ // execution time should be short (<100ms)
+ }
+
+ void
+ RequestObjects::run()
+ {
+ if (not in.getEnable())
+ {
+ emitSuccess();
+ return;
+ }
+
+ using Reader = armarx::armem::obj::instance::Reader;
+
+ 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());
+
+ std::stringstream info;
+ std::stringstream warn;
+
+ std::vector<armarx::ObjectID> objectIds;
+ for (const std::string& idString : objectIdsString)
+ {
+ try
+ {
+ 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();
+ }
+ }
+
+ auto context = getContext<ObjectMemoryGroupStatechartContext>();
+ armarx::armem::client::MemoryNameSystem mns(getMemoryNameSystem(), context);
+
+ Reader reader;
+ reader.connect(mns);
+ objpose::ObjectPoseStorageInterfacePrx storage = reader.getObjectPoseStorage();
+
+ armarx::objpose::observer::RequestObjectsInput input;
+ input.provider = provider;
+ for (const armarx::ObjectID& id : objectIds)
+ {
+ armarx::toIce(input.request.objectIDs.emplace_back(), id);
+ }
+ input.request.relativeTimeoutMS = relativeTimeout.toMilliSeconds();
+
+
+ objpose::observer::RequestObjectsOutput output;
+ try
+ {
+ output = storage->requestObjects(input);
+ }
+ catch (const Ice::Exception& e)
+ {
+ ARMARX_WARNING << "Failed to request object localization: " << e.what();
+ emitFailure();
+ return;
+ }
+
+ for (const auto& [id, result] : output.results)
+ {
+ if (result.result.success)
+ {
+ info << "Requested object " << id << " via provider '" << result.providerName
+ << "'.\n";
+ }
+ else
+ {
+ warn << "\nFailed to request the object " << id << " for localization.";
+ }
+ }
+
+ if (not info.str().empty())
+ {
+ ARMARX_INFO << info.str();
+ }
+ if (not warn.str().empty())
+ {
+ ARMARX_WARNING
+ << "The following issues occurred whhen requesting objects for localization:"
+ << warn.str();
+ }
+
+ emitSuccess();
+ }
+
+ //void RequestObjects::onBreak()
+ //{
+ // // put your user code for the breaking point here
+ // // execution time should be short (<100ms)
+ //}
+
+ 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)
+ {
+ return XMLStateFactoryBasePtr(new RequestObjects(stateData));
+ }
+} // namespace armarx::ObjectMemoryGroup
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h
new file mode 100644
index 0000000000000000000000000000000000000000..4390865434749d591d6082df003c55e3b8560cd6
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.generated.h>
+
+namespace armarx::ObjectMemoryGroup
+{
+ class RequestObjects :
+ public RequestObjectsGeneratedBase < RequestObjects >
+ {
+ public:
+ RequestObjects(const XMLStateConstructorParams& stateData);
+
+ // inherited from StateBase
+ void onEnter() override;
+ void run() override;
+ // void onBreak() override;
+ void onExit() override;
+
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
+
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
+}
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8b32c20109a1bee77bcd3ee1203716d3abb02f0d
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="RequestObjects" uuid="9F09FB2F-CC0C-4B8A-A716-F130E04A7230" width="800" height="600" type="Normal State">
+ <InputParameters>
+ <Parameter name="Enable" type="::armarx::BoolVariantData" docType="bool" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::BoolVariantData","value":true}}' docValue="True"/>
+ </Parameter>
+ <Parameter name="ObjectIds" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no"/>
+ <Parameter name="Provider" type="::armarx::StringVariantData" docType="string" optional="yes"/>
+ <Parameter name="RelativeTimeoutMilliseconds" type="::armarx::IntVariantData" docType="int" optional="no"/>
+ </InputParameters>
+ <OutputParameters/>
+ <LocalParameters/>
+ <Substates/>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ <Event name="Success"/>
+ </Events>
+ <Transitions/>
+</State>
+
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/test/RequestObjectsTest.xml b/source/RobotAPI/statecharts/ObjectMemoryGroup/test/RequestObjectsTest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..edb16e98297feb19d1769ca6c54ab28901da7ad1
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/test/RequestObjectsTest.xml
@@ -0,0 +1,67 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="RequestObjectsTest" uuid="E74E1D8D-2E97-4846-9282-73AC4F49FAEE" width="493.167" height="355.278" type="Normal State">
+ <InputParameters>
+ <Parameter name="ObjectIds" type="::armarx::SingleTypeVariantListBase(::armarx::StringVariantData)" docType="List(string)" optional="no">
+ <DefaultValue value='{"array":[{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/spraybottle"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/handwash-paste"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/mounting-adhesive"}},{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"Maintenance/multipurpose-grease"}}],"type":"::armarx::SingleTypeVariantListBase"}' docValue="Maintenance/spraybottle\nMaintenance/handwash-paste\nMaintenance/mounting-adhesive\nMaintenance/multipurpose-grease"/>
+ </Parameter>
+ <Parameter name="Provider" type="::armarx::StringVariantData" docType="string" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::StringVariantData","value":"sim_track"}}' docValue="sim_track"/>
+ </Parameter>
+ <Parameter name="RelativeTimeoutMilliseconds" type="::armarx::IntVariantData" docType="int" optional="no">
+ <DefaultValue value='{"type":"::armarx::SingleVariantBase","variant":{"typeName":"::armarx::IntVariantData","value":10000}}' docValue="10000"/>
+ </Parameter>
+ </InputParameters>
+ <OutputParameters/>
+ <LocalParameters/>
+ <Substates>
+ <EndState name="Failure" event="Failure" left="363.167" top="140.056" boundingSquareSize="99.6636"/>
+ <LocalState name="RequestObjects" refuuid="9F09FB2F-CC0C-4B8A-A716-F130E04A7230" left="114.167" top="196.278" boundingSquareSize="99.6636"/>
+ <EndState name="Success" event="Success" left="363.167" top="250.278" boundingSquareSize="99.6636"/>
+ </Substates>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ </Events>
+ <StartState substateName="RequestObjects">
+ <ParameterMappings>
+ <ParameterMapping sourceType="Parent" from="ObjectIds" to="ObjectIds"/>
+ <ParameterMapping sourceType="Parent" from="Provider" to="Provider"/>
+ <ParameterMapping sourceType="Parent" from="RelativeTimeoutMilliseconds" to="RelativeTimeoutMilliseconds"/>
+ </ParameterMappings>
+ <SupportPoints>
+ <SupportPoint posX="67.1067" posY="233.278"/>
+ <SupportPoint posX="76.5839" posY="233.278"/>
+ <SupportPoint posX="88.7241" posY="233.278"/>
+ <SupportPoint posX="101.186" posY="233.278"/>
+ </SupportPoints>
+ </StartState>
+ <Transitions>
+ <Transition from="RequestObjects" to="Failure" eventName="Failure">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="214.588" posY="221.9"/>
+ <SupportPoint posX="253.892" posY="213.031"/>
+ <SupportPoint posX="308.683" posY="200.669"/>
+ <SupportPoint posX="350.786" posY="191.175"/>
+ </SupportPoints>
+ </Transition>
+ <Transition from="RequestObjects" to="Success" eventName="Success">
+ <ParameterMappings/>
+ <ParameterMappingsToParentsLocal/>
+ <ParameterMappingsToParentsOutput/>
+ <SupportPoints>
+ <SupportPoint posX="214.358" posY="252.153"/>
+ <SupportPoint posX="221.884" posY="254.613"/>
+ <SupportPoint posX="229.602" posY="256.935"/>
+ <SupportPoint posX="237" posY="258.833"/>
+ <SupportPoint posX="274.158" posY="268.372"/>
+ <SupportPoint posX="316.529" posY="275.551"/>
+ <SupportPoint posX="350.428" posY="280.424"/>
+ </SupportPoints>
+ </Transition>
+ </Transitions>
+</State>
+