diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
new file mode 100644
index 0000000000000000000000000000000000000000..73976608226406fb3172844aba6097810a89d584
--- /dev/null
+++ b/.gitlab-ci.yml
@@ -0,0 +1,103 @@
+
+stages:
+ - build-and-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
+
+ # Ice configuration to run in Docker.
+ - export ICE_DEFAULT_HOST="127.0.0.1"
+ - export ICE_RUN_AS_ROOT="true"
+
+ # Activate Axii.
+ - source /axii/scripts/install_axii.sh
+
+ script:
+ # Create workspace.
+ - axii workspace create ~/workspace workspace
+ - axii workspace activate workspace
+ - _axii_auto_env_refresh
+
+ # 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
+
+ # Try starting Ice.
+ - armarx switch docker_test --ice-host 127.0.0.1 --ice-port 10000 --ice-default-host 127.0.0.1 --mongo-host 127.0.0.1 --mongo-port 10001
+ - armarx profile
+ - armarx status || true
+ #- armarx start
+ # armarx status
+ #- armarx stop
+
+ # Test.
+ # ToDo: Add and use `axii ws test -m "$PROJECT_MODULE"`
+ - cd "$PROJECT_PATH_IN_WORKSPACE/build"
+ - 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/meta/axii:latest-bionic
+
+
+build-and-test-jammy:
+ stage: build-and-test
+ extends: .build-and-test
+
+ image: git.h2t.iar.kit.edu:5050/sw/armarx/meta/axii:latest-jammy
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/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/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/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index a01c4b37c08698a77fea6588d46963fee6e6c6df..4722e9c5ce8deaabfc33a917c9b0c744d1a781fd 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -22,7 +22,7 @@ namespace armarx::viz::coin
if (filename.empty())
{
ARMARX_INFO << deactivateSpam() << "No filename provided for robot.";
- return result;
+ return nullptr;
}
std::string fullFilename;
@@ -41,7 +41,7 @@ namespace armarx::viz::coin
{
ARMARX_INFO << deactivateSpam()
<< "Unable to find readable file for name: " << filename;
- return result;
+ return nullptr;
}
try
@@ -226,15 +226,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
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 f0e3077422c3d81e044806ddb4e12391424ab67e..e5762482e69b710fa361bf26dd5705849b4b0efb 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -76,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
@@ -111,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;
}
@@ -135,9 +135,8 @@ namespace armarx::articulated_object
return nullptr;
}
- 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)
{
@@ -150,7 +149,6 @@ namespace armarx::articulated_object
return obj;
}
-
void
ArticulatedObjectLocalizerExample::run()
{
@@ -164,8 +162,11 @@ namespace armarx::articulated_object
}
}
- const auto state = articulatedObjectReaderPlugin->get().queryState(articulatedObject->getType() + "/" + articulatedObject->getName(), Clock::Now());
-
+ 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());
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/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index 0daef1ab73ee13ed22d59090270ffe94a6736c44..8c14b9de7a9d80bcf2b09d45ae3253b94a2c8b5b 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -24,6 +24,8 @@
#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>
@@ -33,6 +35,7 @@
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <mutex>
#include <optional>
namespace armarx
@@ -69,17 +72,15 @@ namespace armarx
void
RobotHealth::monitorHealthUpdateTaskClb()
{
- std::shared_lock lock(updateMutex);
+ const std::scoped_lock lock(updateMutex);
ARMARX_TRACE;
auto now = armarx::core::time::DateTime::Now();
- for (size_t i = 0; i < updateEntries.size(); i++)
+ for (auto & e : updateEntries)
{
ARMARX_TRACE;
- UpdateEntry& e = updateEntries.at(i);
-
- std::unique_lock elock(e.mutex);
+ const std::scoped_lock elock(e.mutex);
if (!e.enabled)
{
@@ -91,11 +92,11 @@ namespace armarx
continue;
}
- auto delta = now - e.history.back();
+ auto deltaToArrival = now - e.history.back().arrivalTime;
ARMARX_TRACE;
- if (delta > e.maximumCycleTimeErr)
+ if (deltaToArrival > e.maximumCycleTimeErr)
{
ARMARX_TRACE;
@@ -107,7 +108,7 @@ namespace armarx
e.state = HealthError;
}
}
- else if (delta > e.maximumCycleTimeWarn)
+ else if (deltaToArrival > e.maximumCycleTimeWarn)
{
ARMARX_TRACE;
ARMARX_WARNING << deactivateSpam(0.1, e.name) << "Component " << e.name
@@ -127,12 +128,10 @@ namespace armarx
// get aggregated status
- for (size_t i = 0; i < updateEntries.size(); i++)
+ for (auto & e : updateEntries)
{
ARMARX_TRACE;
- UpdateEntry& e = updateEntries.at(i);
-
- std::shared_lock elock(e.mutex);
+ std::scoped_lock elock(e.mutex);
// trim history
while (e.history.size() > 20)
@@ -163,7 +162,7 @@ namespace armarx
if (overallHealthState == HealthError)
{
ARMARX_TRACE;
- ARMARX_INFO << "Requesting emergency stop";
+ ARMARX_INFO << deactivateSpam(3) << "Requesting emergency stop";
ARMARX_CHECK_NOT_NULL(p.emergencyStopTopicPrx);
p.emergencyStopTopicPrx->reportEmergencyStopState(
EmergencyStopState::eEmergencyStopActive);
@@ -216,11 +215,11 @@ namespace armarx
{
ARMARX_TRACE;
- for (size_t i = 0; i < updateEntries.size(); i++)
+ for (auto & updateEntrie : updateEntries)
{
- if (updateEntries.at(i).name == name)
+ if (updateEntrie.name == name)
{
- return &updateEntries.at(i);
+ return &updateEntrie;
}
}
@@ -232,11 +231,11 @@ namespace armarx
{
ARMARX_TRACE;
{
- for (size_t i = 0; i < updateEntries.size(); i++)
+ for (auto & updateEntrie : updateEntries)
{
- if (updateEntries.at(i).name == name)
+ if (updateEntrie.name == name)
{
- return {false, updateEntries.at(i)};
+ return {false, updateEntrie};
}
}
}
@@ -252,12 +251,12 @@ namespace armarx
RobotHealth::signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current)
{
ARMARX_TRACE;
- std::unique_lock lockU(updateMutex);
+ std::scoped_lock lockU(updateMutex);
auto e = findOrCreateUpdateEntry(args.identifier);
{
- std::unique_lock lock(e.second.mutex);
+ std::scoped_lock lock(e.second.mutex);
if (e.first)
{
@@ -281,27 +280,30 @@ namespace armarx
void
RobotHealth::heartbeat(const std::string& identifier,
- const core::time::dto::DateTime& /*referenceTime*/,
+ 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::shared_lock lockU(updateMutex);
+ std::scoped_lock lockU(updateMutex);
auto* const entry = findUpdateEntry(identifier);
if (entry == nullptr)
{
- ARMARX_WARNING << "Attention. Component `" << identifier
+ ARMARX_WARNING << deactivateSpam() << "Attention. Component `" << identifier
<< "` was not signed up for heartbeat. Ignoring heartbeat for now...";
return;
}
ARMARX_CHECK_NOT_NULL(entry);
- std::unique_lock lock(entry->mutex);
+ std::scoped_lock lock(entry->mutex);
if (not entry->enabled)
{
@@ -314,28 +316,24 @@ namespace armarx
// auto now = armarx::core::time::DateTime::Now();
- // armarx::core::time::DateTime timestamp;
- // fromIce(referenceTime, timestamp);
- const DateTime timestamp = Clock::Now();
- entry->history.push_back(timestamp);
+ armarx::core::time::DateTime refTime;
+ fromIce(referenceTime, refTime);
+ entry->history.push_back(UpdateEntry::TimeInfo{.referenceTime = refTime, .arrivalTime = arrivalTimestamp});
}
void
- armarx::RobotHealth::unregister(const std::string& componentName, const Ice::Current&)
+ armarx::RobotHealth::unregister(const std::string& identifier, const Ice::Current&)
{
ARMARX_TRACE;
- std::shared_lock lock(updateMutex);
+ std::scoped_lock lock(updateMutex);
bool found = false;
{
- std::shared_lock lock(updateMutex);
-
- for (size_t i = 0; i < updateEntries.size(); i++)
+ for (auto & e : updateEntries)
{
- auto& e = updateEntries.at(i);
- if (e.name == componentName)
+ if (e.name == identifier)
{
- std::unique_lock elock(e.mutex);
+ std::scoped_lock elock(e.mutex);
e.required = false;
e.enabled = false;
found = true;
@@ -347,11 +345,11 @@ namespace armarx
if (found)
{
- ARMARX_INFO << "Component " << componentName << " disabled from heartbeat";
+ ARMARX_INFO << "Component " << identifier << " disabled from heartbeat";
}
else
{
- ARMARX_INFO << "Could not unregister component " << componentName << ". Not found.";
+ ARMARX_INFO << "Could not unregister component " << identifier << ". Not found.";
}
updateRequiredElements();
@@ -362,7 +360,7 @@ namespace armarx
const std::vector<std::string>& tags,
const Ice::Current& current)
{
- std::shared_lock lock(updateMutex);
+ std::scoped_lock lock(updateMutex);
// add newly requested tags
for(const auto& tag : tags)
@@ -425,7 +423,7 @@ namespace armarx
const std::vector<std::string>& tags,
const Ice::Current& current)
{
- std::shared_lock lock(updateMutex);
+ std::scoped_lock lock(updateMutex);
// update the required tags list / map
for(const auto& tag : tags)
@@ -439,7 +437,7 @@ namespace armarx
void
armarx::RobotHealth::resetRequiredTags(const Ice::Current& current)
{
- std::shared_lock lock(updateMutex);
+ std::scoped_lock lock(updateMutex);
// treat the special provider (see onInitComponent()) appropriately
ARMARX_CHECK(tagsPerRequester.count(PROPERTY_REQUESTER_ID) > 0);
@@ -453,7 +451,7 @@ namespace armarx
RobotHealthInfo
RobotHealth::getSummary(const Ice::Current&)
{
- std::shared_lock lock(updateMutex);
+ std::scoped_lock lock(updateMutex);
ARMARX_TRACE;
RobotHealthInfo ret;
@@ -472,7 +470,7 @@ namespace armarx
auto later = e.history[i];
auto pre = e.history[i - 1];
- auto delta = later - pre;
+ auto delta = later.arrivalTime - pre.arrivalTime;
if (minDelta > delta)
{
@@ -485,7 +483,12 @@ namespace armarx
}
}
- const Duration timeSinceLastUpdate = e.history.empty() ? Duration() : Clock::Now() - e.history.back();
+ 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;
healthEntry.identifier = e.name;
healthEntry.state = e.state;
@@ -493,7 +496,10 @@ namespace armarx
healthEntry.required = e.required;
toIce(healthEntry.minDelta, minDelta);
toIce(healthEntry.maxDelta,maxDelta);
- toIce(healthEntry.timeSinceLastUpdate, timeSinceLastUpdate);
+ 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;
@@ -522,14 +528,20 @@ namespace armarx
{
auto later = entry.history[entry.history.size() - 1];
auto pre = entry.history[entry.history.size() - 2];
- const Duration delta = later - pre;
+ const Duration delta = later.arrivalTime - pre.arrivalTime;
- const Duration deltaToNow = Clock::Now() - entry.history.back();
+ 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 + "_deltaToNow",
- deltaToNow.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",
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index 899c237647f8c381bab41dd4db35d96dbe6499a4..1e45f31116382a9761b879eaffb80375541fdaf7 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -70,6 +70,29 @@ namespace armarx
return "RobotHealth";
}
+ // 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;
+
+
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
@@ -116,8 +139,18 @@ namespace armarx
bool required = false;
bool enabled = false;
- mutable std::shared_mutex mutex;
- std::deque<armarx::core::time::DateTime> history;
+ 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;
@@ -130,36 +163,13 @@ namespace armarx
void reportDebugObserver();
- // RobotHealthInterface interface
- public:
- 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;
-
- // RobotHealthComponentInterface interface
- public:
- RobotHealthInfo getSummary(const Ice::Current& current) override;
-
- private:
+
void updateRequiredElements();
std::set<std::string> requestedTags() const;
- mutable std::shared_mutex updateMutex;
+ // 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;
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
index e028bc55648f02446b5980cc00124950d4564219..34894b6ea9c449d13f47af00d2c012a47f764f1d 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
@@ -80,7 +80,7 @@ namespace armarx
}
void
- RobotHealthDummy::busydwait(long microseconds)
+ RobotHealthDummy::busywait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
@@ -95,51 +95,60 @@ namespace armarx
{
auto args = RobotHealthHeartbeatArgs();
args.identifier = getName();
+ armarx::core::time::toIce(args.maximumCycleTimeError, armarx::core::time::Duration::MilliSeconds(1000));
+ armarx::core::time::toIce(args.maximumCycleTimeWarning, armarx::core::time::Duration::MilliSeconds(500));
robotHealthTopicPrx->signUp(args);
ARMARX_INFO << "starting rinning task";
while (!dummyTask->isStopped())
{
- long beforeTopicCall = TimeUtil::GetTime().toMicroSeconds();
+ auto beforeTopicCall = armarx::core::time::DateTime::Now();
//ARMARX_INFO << "send heartbeat";
armarx::core::time::dto::DateTime now;
armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
robotHealthTopicPrx->heartbeat(getName(), now);
- long afterTopicCall = TimeUtil::GetTime().toMicroSeconds();
+ auto afterTopicCall = armarx::core::time::DateTime::Now();
+
if (sleepmode == "nanosleep")
{
- NanoSleep(10 * 1000 * 1000);
+ NanoSleep(10 * 1000 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "sleepwait")
{
- sleepwait(10 * 1000);
+ sleepwait(10 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "yieldwait")
{
- yieldwait(10 * 1000);
+ yieldwait(10 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "busywait")
{
- busydwait(10 * 1000);
+ busywait(10 * 1000); // wait 10 milliseconds
+ }
+ else if (sleepmode == "std::this_thread::sleep_for")
+ {
+ std::this_thread::sleep_for(std::chrono::microseconds(10 * 1000));
}
else
{
throw LocalException("Unknown sleepmode.");
}
- long afterSleep = TimeUtil::GetTime().toMicroSeconds();
- long topicCallDelay = afterTopicCall - beforeTopicCall;
- long sleepDelay = afterSleep - afterTopicCall;
- if (sleepDelay > 20000)
+ auto afterSleep = armarx::core::time::DateTime::Now();
+ auto topicCallDelay = afterTopicCall - beforeTopicCall;
+ auto sleepDelay = afterSleep - afterTopicCall;
+ if (sleepDelay.toMicroSeconds() > 20000)
{
- ARMARX_IMPORTANT << sleepmode << ": " << sleepDelay << "us";
+ ARMARX_IMPORTANT << sleepmode << " took long: " << sleepDelay << "us";
}
- if (topicCallDelay > 1000)
+ if (topicCallDelay.toMicroSeconds() > 1000)
{
- ARMARX_IMPORTANT << "topic: " << topicCallDelay << "us";
+ ARMARX_IMPORTANT << "topic took long: " << topicCallDelay << "us";
}
}
+
+ robotHealthTopicPrx->unregister(args.identifier);
}
void
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
index cfd8d369d0335768c2e85dd735f1d31d1e30e60d..a841720145b96f925c427443f3998475620509bb 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
@@ -78,7 +78,7 @@ namespace armarx
int NanoSleep(long nanosec);
void yieldwait(long microseconds);
- void busydwait(long microseconds);
+ void busywait(long microseconds);
void sleepwait(long microseconds);
protected:
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
index d204c95fac16b40f110555729049a5da632d8846..6ba767713bb6481f5c406b4dccf37f3a375e4c44 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml
@@ -33,7 +33,7 @@ An example data containing different native ARON types.
<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/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/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/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..0a11b09754f6a47f7b52e3fe0958f66aeeb034bd 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,54 +76,46 @@ 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,
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..697b087cc7f0b53edc3196b499c0f76dd3dec312 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);
@@ -134,7 +134,8 @@ namespace armarx
}
}
- 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)
@@ -281,16 +283,21 @@ namespace armarx
_tripRt2NonRt.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 +306,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 +318,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 +330,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 +344,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 +359,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 +376,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 +386,47 @@ 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();
}
- 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();
}
- void NJointCartesianWaypointController::stopMovement(const Ice::Current&)
+ void
+ NJointCartesianWaypointController::stopMovement(const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -408,10 +437,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,13 +468,10 @@ 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};
@@ -460,11 +486,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 +503,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..dce6aed44f8bb931baf2a598c06f127cb9b20d66 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,53 @@ 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;
+ mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
+ TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
//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..e9aea0a9858d85e24b9e49ee088c18405af0668a 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;
}
@@ -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..8cb11c331040c5dfd12fea456c0d2391a36c9f2b 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;
@@ -528,11 +528,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 +542,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 +587,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 +603,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 +650,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 +670,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 +913,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 +1148,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 +1190,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..a7083b662e54b0a0c02ad847c3e0f197bce74d58 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -20,66 +20,78 @@
* 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
+ 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 +99,20 @@ 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();
+ robotPlatformInstanceName = getProperty<std::string>("PlatformInstanceName").getValue();
//load robot
{
@@ -117,13 +128,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 +154,10 @@ 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..7b5defcbfbec169a83e7185f688a5016851c24ab 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,30 +37,34 @@ namespace armarx
namespace armarx::RobotUnitModule
{
- class RobotDataPropertyDefinitions: public ModuleBasePropertyDefinitions
+ class RobotDataPropertyDefinitions : public ModuleBasePropertyDefinitions
{
public:
- RobotDataPropertyDefinitions(std::string prefix): ModuleBasePropertyDefinitions(prefix)
+ 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)");
+ 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", "",
+ "RobotName",
+ "",
"Override robot name if you want to load multiple robots of the same type");
defineOptionalProperty<std::string>(
- "RobotNodeSetName", "Robot",
+ "RobotNodeSetName",
+ "Robot",
"Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
defineOptionalProperty<std::string>(
- "PlatformName", "Platform",
+ "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')");
-
+ defineOptionalProperty<std::string>("PlatformInstanceName",
+ "Platform",
+ "Name of the platform instance (will publish "
+ "values on PlatformInstanceName + 'State')");
}
};
@@ -73,15 +77,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 //////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -152,5 +159,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..a4d9867488a2a2f8e9cb176158ba18fc01a8c538 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -20,33 +20,36 @@
* GNU General Public License
*/
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Obstacle.h>
+#include "RobotUnitModuleSelfCollisionChecker.h"
+
+#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/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 +75,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 +168,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 +197,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);
@@ -202,75 +222,85 @@ namespace armarx::RobotUnitModule
//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);
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);
nodePairsToCheck.emplace_back(first, second);
}
ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), nodePairsToCheck.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();
@@ -280,19 +310,22 @@ namespace armarx::RobotUnitModule
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_INFO << 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{1000});
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
{
@@ -315,12 +348,16 @@ 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))
+ 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 +365,19 @@ 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)});
+ std::this_thread::sleep_until(
+ startT + std::chrono::microseconds{static_cast<int64_t>(1000000 / freq)});
}
}
- void SelfCollisionChecker::_preFinishRunning()
+ void
+ SelfCollisionChecker::_preFinishRunning()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
ARMARX_INFO << "Stopping Self Collision Avoidance.";
@@ -345,5 +386,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..ad3cc0f97680eabc2519b58598823294c770733f 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,13 +153,15 @@ 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()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
auto beg = TimeUtil::GetTime(true);
@@ -171,12 +186,13 @@ namespace armarx::RobotUnitModule
ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
initializeTcpControllerUnit();
ARMARX_DEBUG << "TcpControllerUnit initialized";
-
}
- 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 +212,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 +227,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 +238,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 +272,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 +281,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,33 +334,38 @@ 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;
@@ -355,76 +384,91 @@ namespace armarx::RobotUnitModule
ARMARX_INFO << "no platform unit created since no platform name was given";
return;
}
- if (!_module<Devices>().getControlDevices().has(_module<RobotData>().getRobotPlatformName()))
+ 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);
+ 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_CHECK_EXPRESSION(jointVel);
- ARMARX_CHECK_EXPRESSION(sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
+ ARMARX_CHECK_EXPRESSION(
+ sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
//add it
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("");
+ IceInternal::Handle<UnitT> unit =
+ Component::create<UnitT>(properties, configName, getConfigDomain());
//config
- NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config = new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
+ 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_CHECK_EXPRESSION(ctrl);
unit->pt = ctrl;
- 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_CHECK_EXPRESSION(ctrlGlobalPosition);
unit->pt = ctrl;
unit->globalPosCtrl = ctrlGlobalPosition;
- 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_DEBUG << "initializeLocalizationUnit";
@@ -441,8 +485,10 @@ namespace armarx::RobotUnitModule
}
ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition";
- const SensorDevicePtr& sensorDeviceRelativePosition = _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
- ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueHolonomicPlatformRelativePosition>());
+ const SensorDevicePtr& sensorDeviceRelativePosition =
+ _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
+ ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()
+ ->asA<SensorValueHolonomicPlatformRelativePosition>());
// const SensorDevicePtr& sensorDevicePoseCorrection = _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName());
// ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueGlobalPoseCorrection>());
@@ -455,26 +501,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 +555,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 +586,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 +623,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 +633,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 +659,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 +683,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 +706,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 +715,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 +751,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 +818,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 f230d88c16da317777eebc3667d5d1bbff3f9f15..e1c717c58e00be94df3035916b5d3d4f0a50a589 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_VERBOSE << VAROUT(entries.size());
+ const size_t writePosition_local = writePosition.load(); // copy to prevent external changes
+ if (writePosition_local - onePastLoggingReadPosition >= numEntries)
{
- ARMARX_VERBOSE << "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_VERBOSE << " 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_VERBOSE << 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_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;
+ 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_VERBOSE << " 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_VERBOSE << "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,79 @@ 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 +323,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 +353,27 @@ 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..b247f12167424f45a2e87c690461d5aaab5516d0 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,15 +102,16 @@ 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));
@@ -111,18 +119,22 @@ namespace armarx::introspection
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/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 2a1f02718bb8b1fa9465445086238a4e04ba92dd..a89af6168a5f66f366a4df3a36b59c01906a0695 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -24,6 +24,7 @@
#include <SimoxUtility/algorithm/string/string_tools.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>
@@ -45,9 +46,9 @@ 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();
@@ -92,7 +93,7 @@ HokuyoLaserUnit::onConnectComponent()
{
ARMARX_TRACE;
- topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
+ getProxyFromProperty(listenerPrx, "LaserScannerListenerName");
debugObserver = getTopic<DebugObserverInterfacePrx>(
getProperty<std::string>("DebugObserverName").getValue());
@@ -133,7 +134,7 @@ HokuyoLaserUnit::onConnectComponent()
info.stepSize = (info.maxAngle - info.minAngle) / (maxStep - minStep);
device.lengthData.resize(lengthDataSize);
- device.scanTopic = topic;
+ device.listenerPrx = listenerPrx;
device.robotHealthPlugin = heartbeat;
device.debugObserver = debugObserver;
@@ -181,7 +182,7 @@ HokuyoLaserUnit::createPropertyDefinitions()
std::string
HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
{
- return topicName;
+ return laserScannerListenerProxyName;
}
LaserScannerInfoSeq
@@ -230,6 +231,8 @@ 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)
@@ -272,15 +275,16 @@ 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();
if (robotHealthPlugin != nullptr)
{
@@ -302,10 +306,10 @@ HokuyoLaserScanDevice::run()
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["proxy_report_ms"] =
+ new Variant((time_proxyReport - time_update).toMilliSecondsDouble());
durations["topic_health_ms"] =
- new Variant((time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble());
+ new Variant((time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble());
debugObserver->setDebugChannel(
"LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
@@ -318,9 +322,9 @@ HokuyoLaserScanDevice::run()
<< "Update: "
<< (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
<< "TopicSensor: "
- << (time_topicSensor - time_update).toMilliSecondsDouble() << "ms\n"
+ << (time_proxyReport - time_update).toMilliSecondsDouble() << "ms\n"
<< "TopicHeart: "
- << (time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble()
+ << (time_topicHeartbeat - time_proxyReport).toMilliSecondsDouble()
<< "ms\n";
}
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
index 0b8ad3985e9e3b1825054fe1339cb41a5c59f4cd..98da02fce21736694803272e4e76e046080d2cd4 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -47,7 +47,7 @@ namespace armarx
armarx::ComponentPropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>(
- "LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
+ "LaserScannerListenerName", "CartographerMappingAndLocalization", "Name of the laser scan listener.");
defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans");
defineOptionalProperty<float>("AngleOffset",
-2.3561944902,
@@ -87,7 +87,7 @@ namespace armarx
LaserScan scan;
std::string componentName;
- LaserScannerUnitListenerPrx scanTopic;
+ LaserScannerUnitListenerPrx listenerPrx;
armarx::plugins::HeartbeatComponentPlugin* robotHealthPlugin;
DebugObserverInterfacePrx debugObserver;
};
@@ -150,8 +150,8 @@ 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;
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
index aa795eb22ff8c96dfcb82b77f2f4f46f1512471a..4868ebda8519f9d571c0a62a17ccbecd66e5a18d 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
@@ -25,8 +25,10 @@
#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>
@@ -109,6 +111,24 @@ namespace armarx
robotHealthTopicPrx->heartbeat(getName(), now);
}
+
+ std::string to_string_rounded(float value, int decimals = 100)
+ {
+ std::stringstream ss;
+ ss << std::fixed << std::setprecision(3) << value;
+ return ss.str();
+ }
+
+
+ QTableWidgetItem* make_item(const std::string& text,
+ Qt::AlignmentFlag horAlignment = Qt::AlignLeft)
+ {
+ auto* item = new QTableWidgetItem(QString::fromStdString(text));
+ item->setTextAlignment(horAlignment | Qt::AlignVCenter);
+ return item;
+ }
+
+
void
GuiHealthClientWidgetController::updateSummaryTimerClb()
{
@@ -120,7 +140,7 @@ namespace armarx
{
auto summary = robotHealthComponentPrx->getSummary();
- const std::size_t nCols = 7;
+ const std::size_t nCols = 10;
auto& tableWidget = widget.tableHealthState;
tableWidget->setRowCount(summary.entries.size());
@@ -129,7 +149,7 @@ namespace armarx
const auto summaryVals = simox::alg::get_values(summary.entries);
tableWidget->setHorizontalHeaderLabels({
- "identifier", "required", "keeps frequency", "tags", "time since\nlast update [ms]", "warning\nthreshold [ms]", "error\nthreshold [ms]"
+ "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);
@@ -139,48 +159,71 @@ namespace armarx
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 color;
+ QColor stateColor;
switch(entry.state)
{
case HealthOK:
stateRepr = "yes";
- color.setRgb(0, 255, 0); // green
+ stateColor.setRgb(0, 255, 0); // green
break;
case HealthWarning:
stateRepr = "yes";
- color.setRgb(255, 165, 0); // orange
+ stateColor.setRgb(255, 165, 0); // orange
break;
case HealthError:
stateRepr = "no";
- color.setRgb(255, 0, 0); // red
+ stateColor.setRgb(255, 0, 0); // red
break;
}
- const std::string timeSinceLastRepr = std::to_string(entry.timeSinceLastUpdate.microSeconds / 1000);
+ 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)));
-
- auto* requiredItem = new QTableWidgetItem(QString::fromStdString(entry.required ? "yes" : "no"));
- requiredItem->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
- tableWidget->setItem(i, 1, requiredItem);
-
- auto* stateItem = new QTableWidgetItem(QString::fromStdString(stateRepr));
- stateItem->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
- tableWidget->setItem(i, 2, stateItem);
- tableWidget->item(i, 2)->setBackgroundColor(color);
+ tableWidget->setItem(i, 1, make_item(entry.required ? "yes" : "no", Qt::AlignHCenter));
+
+ {
+ auto* item = make_item(stateRepr, Qt::AlignHCenter);
+ item->setBackgroundColor(stateColor);
+ tableWidget->setItem(i, 2, item);
+ }
tableWidget->setItem(i, 3, new QTableWidgetItem(QString::fromStdString(tagsRepr)));
+ tableWidget->setItem(i, 4, 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, 4, new QTableWidgetItem(QString::fromStdString(timeSinceLastRepr)));
- tableWidget->setItem(i, 5, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeWarning.microSeconds / 1000))));
- tableWidget->setItem(i, 6, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeError.microSeconds / 1000))));
+ tableWidget->setItem(i, 7, make_item(to_string_rounded(entry.maximumCycleTimeWarning.microSeconds / 1000.), Qt::AlignRight));
+ tableWidget->setItem(i, 8, make_item(to_string_rounded(entry.maximumCycleTimeError.microSeconds / 1000.), Qt::AlignRight));
+ tableWidget->setItem(i, 9, new QTableWidgetItem(QString::fromStdString(hostname)));
}
std::string tagsText = "Active tags: [";
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 5437d499db20e1c4a29a59efe98f2c1b92a341fa..fa8011d66e9fb45de290395eab222e9299888620 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -22,59 +22,59 @@
* 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 <string>
//#define KINEMATIC_UNIT_FILE_DEFAULT std::string("RobotAPI/robots/Armar3/ArmarIII.xml")
@@ -84,268 +84,277 @@
#define SLIDER_POS_DEG_MULTIPLIER 5
#define SLIDER_POS_RAD_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();
}
+ }
+
+ void
+ KinematicUnitWidgetController::onExitComponent()
+ {
+ kinematicUnitInterfacePrx = nullptr;
- if (debugLayerVisu)
+ 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 +362,848 @@ 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)
- {
- debugDrawer->enableAllLayers();
- }
- else
+ if (!dialog)
{
- 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
- {
- kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointVelocities(vels);
- }
- catch (...)
+ void
+ KinematicUnitWidgetController::updateKinematicUnitListInDialog()
{
}
- const auto selectedControlMode = getSelectedControlMode();
- if (selectedControlMode == eVelocityControl)
+ void
+ KinematicUnitWidgetController::modelUpdateCB()
{
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
}
-}
-
-void KinematicUnitWidgetController::resetSlider()
-{
- const auto selectedControlMode = getSelectedControlMode();
-
- if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
- {
- resetSliderToZeroPosition();
- }
- else if (selectedControlMode == ePositionControl)
+ SoNode*
+ KinematicUnitWidgetController::getScene()
{
- if (currentNode)
+ if (viewerEnabled)
{
- 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));
+ ARMARX_INFO << "Returning scene ";
+ return rootVisu;
+ }
+ else
+ {
+ ARMARX_INFO << "viewer disabled - returning null scene";
+ return NULL;
}
}
-}
-
-void KinematicUnitWidgetController::resetSliderToZeroPosition()
-{
- const auto selectedControlMode = getSelectedControlMode();
+ 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);
- if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
- {
- ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
- ui.lcdNumberKinematicUnitJointValue->display(SLIDER_ZERO_POSITION);
- }
-}
-
-void KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
-{
- ARMARX_VERBOSE << "Setting control mode of radio button group to " << controlMode;
-
- 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;
+ fetchData();
}
-}
-
-void KinematicUnitWidgetController::setControlModePosition()
-{
- if (!ui.radioButtonPositionControl->isChecked())
+ void
+ KinematicUnitWidgetController::kinematicUnitZeroVelocity()
{
- return;
- }
- NameControlModeMap jointModes;
- // selectedControlMode = ePositionControl;
- ui.widgetSliderFactor->setVisible(false);
+ if (!robotNodeSet)
+ {
+ return;
+ }
- // FIXME currentNode should be passed to this function!
+ std::unique_lock lock(mutexNodeSet);
+ std::vector<VirtualRobot::RobotNodePtr> rn = robotNodeSet->getAllRobotNodes();
+ NameValueMap vels;
+ NameControlModeMap jointModes;
- if (currentNode)
- {
- 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;
+ for (unsigned int i = 0; i < rn.size(); i++)
+ {
+ jointModes[rn[i]->getName()] = eVelocityControl;
+ vels[rn[i]->getName()] = 0.0f;
+ }
- if (kinematicUnitInterfacePrx)
+ try
{
kinematicUnitInterfacePrx->switchControlMode(jointModes);
+ kinematicUnitInterfacePrx->setJointVelocities(vels);
}
-
- float lo = currentNode->getJointLimitLo() * conversionFactor;
- float hi = currentNode->getJointLimitHi() * conversionFactor;
-
- if (hi - lo <= 0.0f)
+ catch (...)
{
- return;
}
+ const auto selectedControlMode = getSelectedControlMode();
+ if (selectedControlMode == eVelocityControl)
{
- // 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();
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
}
+ }
- float pos = currentNode->getJointValue() * conversionFactor;
- ARMARX_INFO << "Setting position control for current node "
- << "(name '" << currentNode->getName() << "' with current value " << pos << ")";
+ void
+ KinematicUnitWidgetController::resetSlider()
+ {
+ const auto selectedControlMode = getSelectedControlMode();
- // 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 (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
+ {
+ resetSliderToZeroPosition();
+ }
+ else if (selectedControlMode == ePositionControl)
+ {
+ if (currentNode)
+ {
+ const bool isDeg = ui.checkBoxUseDegree->isChecked();
+ const bool isRot = currentNode->isRotationalJoint();
+ const auto factor =
+ isRot && !isDeg ? SLIDER_POS_RAD_MULTIPLIER : SLIDER_POS_DEG_MULTIPLIER;
+ float conversionFactor = isRot && isDeg ? 180.0 / M_PI : 1.0f;
+ float pos = currentNode->getJointValue() * conversionFactor;
+
+ ui.lcdNumberKinematicUnitJointValue->display((int)pos);
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition((int)(pos * factor));
+ }
+ }
+ }
- ui.horizontalSliderKinematicUnitPos->setMaximum(hi * factor);
- ui.horizontalSliderKinematicUnitPos->setMinimum(lo * factor);
- ui.lcdNumberKinematicUnitJointValue->display(pos);
+ void
+ KinematicUnitWidgetController::resetSliderToZeroPosition()
+ {
+ const auto selectedControlMode = getSelectedControlMode();
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
+ if (selectedControlMode == eVelocityControl || selectedControlMode == eTorqueControl)
+ {
+ ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
+ ui.lcdNumberKinematicUnitJointValue->display(SLIDER_ZERO_POSITION);
+ }
}
-}
-void KinematicUnitWidgetController::setControlModeVelocity()
-{
- if (!ui.radioButtonVelocityControl->isChecked())
+ void
+ KinematicUnitWidgetController::setControlModeRadioButtonGroup(const ControlMode& controlMode)
{
- return;
+ ARMARX_VERBOSE << "Setting control mode of radio button group to " << controlMode;
+
+ 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;
+ }
}
- NameControlModeMap jointModes;
- NameValueMap jointVelocities;
- if (currentNode)
+ void
+ KinematicUnitWidgetController::setControlModePosition()
{
- jointModes[currentNode->getName()] = eVelocityControl;
-
- // set the velocity to zero to stop any previous controller (e.g. torque controller)
- jointVelocities[currentNode->getName()] = 0;
+ if (!ui.radioButtonPositionControl->isChecked())
+ {
+ return;
+ }
+ NameControlModeMap jointModes;
+ // selectedControlMode = ePositionControl;
+ ui.widgetSliderFactor->setVisible(false);
- 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;
+ // FIXME currentNode should be passed to this function!
- try
+ if (currentNode)
{
+ 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;
+
if (kinematicUnitInterfacePrx)
{
kinematicUnitInterfacePrx->switchControlMode(jointModes);
- kinematicUnitInterfacePrx->setJointVelocities(jointVelocities);
}
- }
- catch (...)
- {
- }
-
- ui.widgetSliderFactor->setVisible(true);
+ float lo = currentNode->getJointLimitLo() * conversionFactor;
+ float hi = currentNode->getJointLimitHi() * conversionFactor;
- ui.horizontalSliderKinematicUnitPos->blockSignals(true);
- ui.horizontalSliderKinematicUnitPos->setMaximum(hi);
- ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
- ui.horizontalSliderKinematicUnitPos->blockSignals(false);
- resetSlider();
- }
-}
-
-ControlMode KinematicUnitWidgetController::getSelectedControlMode() const
-{
- if(ui.radioButtonPositionControl->isChecked())
- {
- return ControlMode::ePositionControl;
- }
+ if (hi - lo <= 0.0f)
+ {
+ return;
+ }
- if (ui.radioButtonVelocityControl->isChecked())
- {
- return ControlMode::eVelocityControl;
- }
+ {
+ // 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();
+ }
- if (ui.radioButtonTorqueControl->isChecked())
- {
- return ControlMode::eTorqueControl;
- }
+ float pos = currentNode->getJointValue() * conversionFactor;
+ ARMARX_INFO << "Setting position control for current node "
+ << "(name '" << currentNode->getName() << "' with current value " << pos
+ << ")";
- // 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;
+ // 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);
-
-}
+ ui.horizontalSliderKinematicUnitPos->setMaximum(hi * factor);
+ ui.horizontalSliderKinematicUnitPos->setMinimum(lo * factor);
+ ui.lcdNumberKinematicUnitJointValue->display(pos);
-void KinematicUnitWidgetController::setControlModeTorque()
-{
- if (!ui.radioButtonTorqueControl->isChecked())
- {
- return;
+ 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 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;
+
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);
+
+ ui.horizontalSliderKinematicUnitPos->blockSignals(false);
+ resetSlider();
+ }
+ }
- if (robot != NULL)
+ 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;
+ }
- if (robot)
+ return 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;
+ }
+ }
- if (robotNodeSet)
+ return nodeSetPtr;
+ }
+
+ 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();
- 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 +1211,563 @@ 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() ? 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())
+ {
+ 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/interface/aron/Aron.ice b/source/RobotAPI/interface/aron/Aron.ice
index 0bfb4fa816c80a1a5cc5768750b2017392aebac8..aae664fa5f4d79e7a59f94bebc1e74823d06dfc5 100644
--- a/source/RobotAPI/interface/aron/Aron.ice
+++ b/source/RobotAPI/interface/aron/Aron.ice
@@ -3,32 +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>
// Those macros stay defined
-#define ARON_MAJOR "2" // ice interface changes
-#define ARON_MINOR "0" // aron header changes
-#define ARON_PATCH "0" // aron cpp changes
-#define ARON_VERSION "2.0.0"
+#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
{
// also make version number available to other languages without a preprocessor
- const string MAJOR = ARON_MAJOR;
- const string MINOR = ARON_MINOR;
- const string PATCH = ARON_PATCH;
- const string VERSION = ARON_VERSION;
+ struct Version
+ {
+ int MAJOR = ARON_MAJOR;
+ int MINOR = ARON_MINOR;
+ int PATCH = ARON_PATCH;
+ };
/*************************
* Aron Types ************
************************/
module type
+
{
enum Maybe
{
@@ -148,7 +155,7 @@ module armarx
{
class GenericType
{
- string VERSION = ARON_VERSION;
+ Version VERSION;
Maybe maybe = Maybe::NONE;
};
@@ -248,32 +255,33 @@ module armarx
/* ***** Primitive Types ***** */
class AronInt extends GenericType
{
- int defaultValue = 0; // <value>
+ optional(0) int defaultValue; // <value>
};
class AronLong extends GenericType
{
- long defaultValue = 0; // <value>
+
+ optional(1) long defaultValue; // <value>
};
class AronDouble extends GenericType
{
- double defaultValue = 0; // <value>
+ optional(2) double defaultValue; // <value>
};
class AronFloat extends GenericType
{
- float defaultValue = 0; // <value>
+ optional(3) float defaultValue; // <value>
};
class AronString extends GenericType
{
- string defaultValue = ""; // <value>
+ optional(4) string defaultValue; // <value>
};
class AronBool extends GenericType
{
- bool defaultValue = false; // <value>
+ optional(4) bool defaultValue; // <value>
};
};
};
@@ -288,7 +296,7 @@ module armarx
{
class GenericData
{
- string VERSION = ARON_VERSION;
+ Version VERSION;
};
sequence<GenericData> GenericDataSeq;
diff --git a/source/RobotAPI/interface/components/RobotHealthInterface.ice b/source/RobotAPI/interface/components/RobotHealthInterface.ice
index a42072ae33903969d0de84e5bd7de708d50532a0..0832146d934a45cf35dd2d42591a4b4949f8d15d 100644
--- a/source/RobotAPI/interface/components/RobotHealthInterface.ice
+++ b/source/RobotAPI/interface/components/RobotHealthInterface.ice
@@ -72,7 +72,16 @@ module armarx
// string message;
armarx::core::time::dto::Duration minDelta;
armarx::core::time::dto::Duration maxDelta;
- armarx::core::time::dto::Duration timeSinceLastUpdate;
+
+ 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;
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/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..4e14baacb6040f7bf97a4855d3da7e4d252bce03 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -1,25 +1,26 @@
#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(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
{
}
-
- void GraspCandidateReader::connect(bool use)
+ void
+ GraspCandidateReader::connect(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 +35,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 +49,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 +62,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 +76,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 +85,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 +105,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 +114,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 +126,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 +141,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 +196,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 +232,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 +285,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 +298,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 +311,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 +324,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..384f5cdb0c1158b714ba8b3038e09f4707dd74bb 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
{
@@ -17,35 +16,43 @@ namespace armarx::armem
void connect(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,9 @@ 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/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..73984b1d1c3f6539e384e02668e76e264f30565c 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
+++ b/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
@@ -1,5 +1,11 @@
add_subdirectory(core)
+
+add_subdirectory(util/LocationLoader)
+add_subdirectory(util/AffordanceLoader)
+
add_subdirectory(motions)
add_subdirectory(objects)
+add_subdirectory(robots)
+add_subdirectory(navigation_graphs)
#add_subdirectory(subjects)
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..7283ac4abae3f8a64d659c9b92430c9b610715f7
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp
@@ -0,0 +1,50 @@
+#include "AffordanceLoader.h"
+
+#include <ArmarXCore/core/eigen/ice_conversions.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+namespace armarx::priorknowledge::util
+{
+ std::vector<StaticAffordance>
+ AffordanceLoader::LoadStaticAffordances(const std::string& sourceId, const nlohmann::json& js)
+ {
+ std::vector<StaticAffordance> ret;
+ if (not js.contains("static_affordances"))
+ {
+ ARMARX_WARNING << "The affordances file has the wrong structure. Missing key "
+ "'static_affordances'.";
+ return ret;
+ }
+
+ for (const auto& affordance : js["static_affordances"].get<std::vector<std::string>>())
+ {
+ StaticAffordance a(sourceId, affordance);
+ ret.push_back(a);
+ }
+ return ret;
+ }
+
+ std::vector<LocatableAffordance>
+ AffordanceLoader::LoadLocatableAffordances(const std::string& sourceId,
+ const nlohmann::json& js)
+ {
+ std::vector<LocatableAffordance> ret;
+ if (not js.contains("locatable_affordances"))
+ {
+ ARMARX_WARNING << "The affordances file has the wrong structure. Missing key "
+ "'locatable_affordances'.";
+ return ret;
+ }
+
+ for (const auto& [affordance, locationIds] :
+ js["locatable_affordances"].get<std::map<std::string, std::vector<std::string>>>())
+ {
+ for (const auto& locationId : locationIds)
+ {
+ LocatableAffordance a(sourceId, affordance, locationId);
+ 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..71696abc97db46be35c9475d9ccc7506ed0933dd
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
@@ -0,0 +1,26 @@
+#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";
+
+ AffordanceLoader() = delete;
+
+ static std::vector<StaticAffordance> LoadStaticAffordances(const std::string& sourceId,
+ const nlohmann::json&);
+
+ static std::vector<LocatableAffordance>
+ LoadLocatableAffordances(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..621ce74f0e41668382bf65ecc992e92890e8d3f5
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp
@@ -0,0 +1,33 @@
+#include "Affordance.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+namespace armarx::priorknowledge::util
+{
+
+ AffordanceId::AffordanceId(const std::string& dataset, const std::string& name) :
+ sourceId(dataset), name(name)
+ {
+ }
+
+ std::string
+ AffordanceId::toString() const
+ {
+ return sourceId + "/" + name;
+ }
+
+ StaticAffordance::StaticAffordance(const std::string& dataset, const std::string& name) :
+ id(dataset, name)
+ {
+ }
+
+ LocatableAffordance::LocatableAffordance(const std::string& dataset,
+ const std::string& name,
+ const std::string& locationId) :
+ id(dataset, name), locationId(locationId)
+ {
+ }
+
+} // 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..ce030b534a4471c1ff33d89478972b3477fe71e0
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <string>
+
+namespace armarx::priorknowledge::util
+{
+ struct AffordanceId
+ {
+ std::string sourceId;
+ std::string name;
+
+ AffordanceId(const std::string& dataset, const std::string& name);
+
+ std::string toString() const;
+ };
+
+ struct StaticAffordance
+ {
+ AffordanceId id;
+
+ StaticAffordance(const std::string& dataset, const std::string& name);
+ };
+
+ struct LocatableAffordance
+ {
+ AffordanceId id;
+ std::string locationId;
+
+ LocatableAffordance(const std::string& dataset,
+ const std::string& name,
+ const std::string& locationId);
+ };
+} // 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..47b8601280e5191e99a7f1a4d56fec5aeaf75ab5
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
@@ -0,0 +1,82 @@
+#include "LocationLoader.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/eigen/ice_conversions.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+namespace armarx::priorknowledge::util
+{
+ std::vector<Location>
+ LocationLoader::LoadLocations(const std::string& dataset, const nlohmann::json& js)
+ {
+ std::vector<Location> ret;
+ if (not js.contains("locations"))
+ {
+ ARMARX_WARNING
+ << "The locations file has the wrong structure. Missing key 'locations'.";
+ return ret;
+ }
+
+ for (const auto& [locationIdStr, j] :
+ js["locations"].get<std::map<std::string, nlohmann::json>>())
+ {
+ if (j.find("framedPose") == j.end())
+ {
+ ARMARX_WARNING << "The element '" << locationIdStr
+ << "' has no 'framedPose' member. Skipping "
+ "this entity.";
+ continue;
+ }
+
+ 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 '" + locationIdStr +
+ "'. Sanitizing it to '" + GlobalFrame + "'.";
+ frame = GlobalFrame;
+ }
+ else
+ {
+ ARMARX_WARNING << "Got empty frame for location '" + locationIdStr +
+ "'. Sanitizing it to 'root' because "
+ "an agent name '" +
+ agent + "' was set.";
+ frame = "root";
+ }
+ }
+
+ if (frame != GlobalFrame && agent.empty())
+ {
+ ARMARX_WARNING << "Got an empty agent name but a set frame '" + frame +
+ "' for location '" + locationIdStr +
+ "'. This may lead to problems...";
+ // TODO: What to do in that case?
+ }
+
+ // Utilize ice structure of eigen
+ armarx::core::eigen::fromIce(
+ pose,
+ framedPose.at("pose")
+ .get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
+
+ armarx::FramedPose fp(pose, frame, agent);
+
+ // escape locationIdStr
+ auto locationIdStrSplitted = simox::alg::split(locationIdStr, "#");
+ std::string locationIdStrEscaped = simox::alg::trim_copy(locationIdStrSplitted[0]);
+
+ Location loc{{dataset, fp.agent, locationIdStrEscaped}, fp};
+ ret.push_back(loc);
+ }
+ 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..70f66236bc4d8c4f286a669c6199caf64cf2ae97
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h
@@ -0,0 +1,23 @@
+#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<Location> LoadLocations(const std::string& dataset,
+ const nlohmann::json&);
+ };
+} // 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..6935d58403c62e1b74dd9361da955f6236519554
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
@@ -0,0 +1,24 @@
+#include "Visu.h"
+
+namespace armarx::priorknowledge::util::location
+{
+ armarx::viz::Pose
+ Visu::getLocationVertex(const std::string& id, const Eigen::Matrix4f& locationGlobalPose) const
+ {
+ // Add global location to layer
+ return armarx::viz::Pose(id).pose(locationGlobalPose).scale(settings.vertexScale);
+ }
+
+ viz::Layer
+ Visu::locationsToLayer(const std::string& layerName,
+ const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const
+ {
+ auto layer = arviz.layer(layerName);
+ for (auto& [id, pose] : locationGlobalPoses)
+ {
+ // Add global location to layer
+ layer.add(getLocationVertex(id, pose));
+ }
+ 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..33abc90e7d3e8560f93b96f226f8542692b33a7c
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
@@ -0,0 +1,34 @@
+#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:
+ Visu(viz::Client& arviz) : arviz(arviz)
+ {
+ }
+
+ ~Visu() = default;
+
+ armarx::viz::Pose getLocationVertex(const std::string& id,
+ const Eigen::Matrix4f& locationGlobalPose) const;
+
+ viz::Layer
+ locationsToLayer(const std::string& layerName,
+ const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const;
+
+ struct Settings
+ {
+ float vertexScale = 1.0;
+ } 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..ea08e20229bb09989f2b4239a529d9b107a3e0f2
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
@@ -0,0 +1,25 @@
+#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 sourceId + "/" + toStringWithoutSourceId();
+ }
+
+ std::string
+ LocationId::toStringWithoutSourceId() const
+ {
+ if (! prefix.empty())
+ {
+ return prefix + "/" + name;
+ }
+ return 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..202b7734a2b7dea90e37d1df1f4a6886ba162b18
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+namespace armarx::priorknowledge::util
+{
+ struct LocationId
+ {
+ std::string sourceId;
+ std::string prefix;
+ std::string name;
+
+ std::string toString() const;
+ std::string toStringWithoutSourceId() const;
+ };
+
+ struct Location
+ {
+ LocationId id;
+ armarx::FramedPose pose;
+ };
+} // 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 f5e65cd47f11601d37c0b6a15ec1e7fb5a931222..9c1d995dee640e413d178d900df769068f1746df 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -59,50 +59,50 @@ namespace armarx::plugins
HeartbeatComponentPlugin::signUp(const RobotHealthHeartbeatArgs& args)
{
ARMARX_TRACE;
- ARMARX_CHECK_NOT_NULL(rhprx);
+ ARMARX_CHECK_NOT_NULL(robotHealthComponentPrx);
if (args.identifier.empty())
{
RobotHealthHeartbeatArgs argsCopy = args;
argsCopy.identifier = parent().getName();
- rhprx->signUp(argsCopy);
+ robotHealthComponentPrx->signUp(argsCopy);
}
else
{
// add component name prefix to identifier
RobotHealthHeartbeatArgs argsCopy = args;
argsCopy.identifier = parent().getName() + "_" + argsCopy.identifier;
- rhprx->signUp(argsCopy);
+ robotHealthComponentPrx->signUp(argsCopy);
}
}
void
HeartbeatComponentPlugin::heartbeat()
{
- if (robotHealthTopic)
+ if (robotHealthComponentPrx)
{
armarx::core::time::dto::DateTime now;
armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
- robotHealthTopic->heartbeat(parent().getName(), now);
+ robotHealthComponentPrx->heartbeat(parent().getName(), now);
}
else
{
- ARMARX_WARNING << "No robot health topic available!";
+ ARMARX_WARNING << "No robot health proxy available!";
}
}
void
HeartbeatComponentPlugin::heartbeatOnChannel(const std::string& channelName)
{
- if (robotHealthTopic)
+ if (robotHealthComponentPrx)
{
armarx::core::time::dto::DateTime now;
armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
- robotHealthTopic->heartbeat(parent().getName() + "_" + channelName, now);
+ robotHealthComponentPrx->heartbeat(parent().getName() + "_" + channelName, now);
}
else
{
- ARMARX_WARNING << "No robot health topic available!";
+ ARMARX_WARNING << "No robot health proxy available!";
}
}
@@ -131,9 +131,7 @@ namespace armarx::plugins
void
HeartbeatComponentPlugin::postOnConnectComponent()
{
- ARMARX_CHECK_NOT_NULL(rhprx);
- topicName = rhprx->getTopicName();
- robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName);
+ ARMARX_CHECK_NOT_NULL(robotHealthComponentPrx);
}
void
@@ -142,19 +140,19 @@ namespace armarx::plugins
if (!properties->hasDefinition(makePropertyName(healthPropertyName)))
{
properties->component(
- rhprx, "RobotHealth", healthPropertyName, "Name of the robot health component.");
+ robotHealthComponentPrx, "RobotHealth", healthPropertyName, "Name of the robot health component.");
}
if (not properties->hasDefinition(makePropertyName(maximumCycleTimeWarningMSPropertyName)))
{
- properties->required(p.maximumCycleTimeWarningMS,
+ properties->optional(p.maximumCycleTimeWarningMS,
maximumCycleTimeWarningMSPropertyName,
"maximum cycle time before warning is emitted");
}
if (not properties->hasDefinition(makePropertyName(maximumCycleTimeErrorMSPropertyName)))
{
- properties->required(p.maximumCycleTimeErrorMS,
+ 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 7b3654e84562fdbc5386eedb70e86424701592af..8a7f4a42ca0e8a603945535445a0f35abf3110c6 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
@@ -88,11 +88,7 @@ namespace armarx::plugins
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
private:
- //! heartbeat topic name (outgoing)
- RobotHealthInterfacePrx robotHealthTopic;
- std::string topicName{"RobotHealthTopic"};
-
- RobotHealthComponentInterfacePrx rhprx;
+ RobotHealthComponentInterfacePrx robotHealthComponentPrx;
//
static constexpr auto healthPropertyName = "heartbeat.ComponentName";
@@ -103,8 +99,8 @@ namespace armarx::plugins
struct Properties
{
- long maximumCycleTimeWarningMS = 50;
- long maximumCycleTimeErrorMS = 100;
+ long maximumCycleTimeWarningMS = 100; // [ms]
+ long maximumCycleTimeErrorMS = 200; // [ms]
} p;
//! default config used in heartbeat(), set via properties
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/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/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
index e567b76b8d36d8cf41c00118a49d3e041d0607bc..45b26e7c9e8c4dd124b762d297d2fb0dca29a317 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -38,7 +38,14 @@ namespace armarx::armem::client::util
class SimpleReaderBase
{
+
public:
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = "";
+ std::string coreSegmentName = "";
+ };
SimpleReaderBase(MemoryNameSystem& memoryNameSystem);
virtual ~SimpleReaderBase() = default;
@@ -46,17 +53,13 @@ namespace armarx::armem::client::util
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
virtual void connect();
-
- protected:
-
- // Properties
- struct Properties
+ const Properties& properties() const;
+ void setProperties(const Properties& p)
{
- std::string memoryName = "";
- std::string coreSegmentName = "";
- };
+ props = p;
+ }
- const Properties& properties() const;
+ protected:
virtual std::string propertyPrefix() const = 0;
virtual Properties defaultProperties() const = 0;
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..efe2d01c13731c5c32ba876a3837b2e1816ea41a 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
@@ -67,7 +67,7 @@ namespace armarx::armem::grasping::known_grasps
{ 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());
@@ -88,7 +88,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]] */
{
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_laser_scans/client/common/Writer.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
index 9a7aa7e5aa8a97032bd5ab9dcbe245b693a52cc3..8b43cef166b729c2bb7a0bb3d64df74268d1975d 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
@@ -80,7 +80,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/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..0fd0c6a1b5258d7960e9179c649ab0286b26be5a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
@@ -0,0 +1,108 @@
+#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
+{
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem),
+ objReader(memoryNameSystem),
+ robotReader(memoryNameSystem)
+ {
+ }
+
+ 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()
+ {
+ // 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();
+ robotReader.connect();
+ }
+
+ 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..ad895c269e52fd2e4ec1b6e6bbda037fbefedb04
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.h
@@ -0,0 +1,54 @@
+#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(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Reader() = default;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
+ 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";
+
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ 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..ac01fb3b87a1dc4bc53b40982a38002a80cc0d23 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -6,6 +6,7 @@
#include <Eigen/Geometry>
#include <ArmarXCore/core/exceptions/local/ExpressionException.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>
@@ -17,6 +18,7 @@
#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/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>
@@ -33,33 +35,15 @@ namespace armarx::armem::articulated_object
{
}
- 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()
{
// 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 +52,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 +96,7 @@ 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,38 @@ 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 +148,14 @@ 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 +173,33 @@ 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 +215,7 @@ 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 +224,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();
@@ -278,7 +262,7 @@ 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 +272,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;
@@ -309,7 +293,7 @@ namespace armarx::armem::articulated_object
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;
@@ -329,7 +313,7 @@ namespace armarx::armem::articulated_object
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..fc8f1da389f443a90c12aa0c0fce70c24b53c3f1 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -42,28 +42,37 @@ namespace armarx::armem::articulated_object
Reader(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Reader() = default;
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def){}
void connect();
- 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,14 +86,6 @@ 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;
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..b2b740b735e933245a44bbfd3ce8d1c437ff30b2 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -3,50 +3,53 @@
#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)
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
{
- ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
+ }
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
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()
{
// 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 +58,95 @@ 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 false;
+
- return queryObject(qResult.memory, timestamp);
}
- /// 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..81a0fc03092bbbe67c476d3e5f2146a28d62517a 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(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Reader() = default;
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
void connect();
- // 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/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..a5822667f61138a40a9f581de8dc68fb7fd812d6 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;
- object.position = simox::math::position(objectInstance->pose.objectPoseGlobal);
- object.orientation = simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+ SceneObject& object = stamped.object;
+ object.className = objectID.getClassID().str();
+ object.instanceName = objectID.instanceName();
+ object.collection = "";
- object.isStatic = objectInstance->pose.isStatic;
- object.jointValues = objectInstance->pose.objectJointValues;
+ 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;
+ }
}
}
- }
- });
+ });
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;
@@ -1117,11 +1127,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 +1150,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 +1164,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 +1172,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 +1187,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 +1197,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 +1236,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 +1259,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 +1320,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 +1346,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 +1354,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/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..bfb14cb23d8dd6a169b5b9bd916723af9cbd6a2a 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,13 +257,13 @@
<ObjectChild key="extraFloats">
<Dict>
- <Float />
+ <float32 />
</Dict>
</ObjectChild>
<ObjectChild key="extraLongs">
<Dict>
- <Long />
+ <int64 />
</Dict>
</ObjectChild>
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..5fb5004c926f4997a9b3e252e8a882c92d0462ea 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>
@@ -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,7 +79,7 @@ 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):
@@ -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..6f90a9b249286efb7d1db277ef3df492e0dd8319 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -51,23 +51,23 @@ namespace armarx::armem::robot_state
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,
+ 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 +134,8 @@ 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;
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..f971c88c3e41bf962500a1ac49e0b5ad7e5a39be 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>
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..0e972f79138fbfc973dbab543206d94965227f03 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -32,7 +32,7 @@ namespace armarx::armem::robot_state
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());
@@ -117,5 +117,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..7a2f39c17939d30b26ef75057e1c9d302b0f68a8 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -47,7 +47,7 @@ namespace armarx::armem::robot_state
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..fc3aed1eea251e199077d92ccb615249b1777570 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -70,7 +70,7 @@ namespace armarx::armem::robot_state
const robot::RobotState robotState{.timestamp = timestamp,
.globalPose =
robot::RobotState::Pose(robot.getGlobalPose()),
- .jointMap = robot.getJointValues()};
+ .jointMap = robot.getJointValues(), .proprioception=std::nullopt};
return RobotWriter::storeState(robotState,
robot.getType(),
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_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_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/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/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
index be7974fb2ba04d07814a468eec39f982d55aa7c5..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,7 @@ 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>
@@ -103,12 +111,13 @@ 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(static_cast<int>( " + std::to_string(type.getRows()) + "), " + "static_cast<int>( " +
- std::to_string(type.getCols()) + "), " +
+ ".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);
+ type.getDefaultValue() + "\", " +
+ conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
+ ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
+ "})); // of " + cppAccessor);
return b;
}
@@ -120,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({static_cast<int>( " + cppAccessor +
- nextEl() + "rows() ), " + "static_cast<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);
}
@@ -139,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);
-
- if ((this->type.getRows() == -1 or this->type.getRows() == Eigen::Dynamic) and
- this->type.getCols() != -1)
- {
- block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + ".at(0));");
- }
+ std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
- if (this->type.getRows() != -1 and
- (this->type.getCols() == -1 or this->type.getCols() == Eigen::Dynamic))
+ if (const auto reset = resolveMaybeGeneratorWithSetter(cppAccessor); !reset.empty())
{
- block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + ".at(1));");
+ block_if_data->addLine(reset);
}
- 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/PointCloud.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
index 66b3c77d68c4cdf598b4712819c618249e49cdb8..2c5a6d1d30bd297e35a4b5592a8bf7e32ef8abcb 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
@@ -115,8 +115,8 @@ namespace armarx::aron::codegenerator::cpp::generator
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
block_if_data->addLine(
- variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({" + cppAccessor +
- nextEl() + "width, " + cppAccessor + nextEl() + "height, " +
+ variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({ static_cast<int>(" + cppAccessor +
+ nextEl() + "width), static_cast<int>(" + cppAccessor + nextEl() + "height), " +
std::to_string(std::get<1>(VoxelType2Cpp.at(type.getVoxelType()))) + "}, " + "\"" +
std::get<0>(VoxelType2Cpp.at(type.getVoxelType())) + "\", " +
"reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() +
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
index 5e5632e45ca43b52befe4eea73703ceee340a2ac..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"}}};
+ 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) :
@@ -49,7 +54,7 @@ namespace armarx::aron::codegenerator::cpp::generator
std::vector<std::string>
Quaternion::getRequiredIncludes() const
{
- return {"<Eigen/Core>", "<Eigen/Geometry>"};
+ return {"<RobotAPI/libraries/aron/common/rw/eigen.h>"};
}
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
@@ -109,15 +114,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({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);
}
@@ -128,18 +131,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);
- 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);
}
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 0dad3b9e12a65c73d41d390db1029e50c70f8a90..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
@@ -42,7 +42,11 @@ namespace armarx::aron::codegenerator::cpp::generator
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
Bool::getCtorInitializers(const std::string& name) const
{
- return {{{name, std::to_string(type.getDefaultValue())}}, true};
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, std::to_string(*def)}}, true};
+ }
+ return {{}, false};
}
CppBlockPtr
@@ -55,11 +59,14 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? std::to_string(*type.getDefaultValue())
+ : "std::nullopt";
+
b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeBool(" +
- std::to_string(type.getDefaultValue()) + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
- ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
- "})); // of " + typeAccessor);
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
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 149050164efeae441132e2e2608f57321d3ef3e8..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
@@ -42,7 +42,11 @@ namespace armarx::aron::codegenerator::cpp::generator
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
Double::getCtorInitializers(const std::string& name) const
{
- return {{{name, std::to_string(type.getDefaultValue())}}, true};
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, std::to_string(*def)}}, true};
+ }
+ return {{}, false};
}
CppBlockPtr
@@ -55,11 +59,14 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? std::to_string(*type.getDefaultValue())
+ : "std::nullopt";
+
b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeDouble(" +
- std::to_string(type.getDefaultValue()) + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
- ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
- "})); // of " + typeAccessor);
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
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 76c71a6431ca9368811c11ed516d6486c636a9ee..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
@@ -42,7 +42,11 @@ namespace armarx::aron::codegenerator::cpp::generator
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
Float::getCtorInitializers(const std::string& name) const
{
- return {{{name, std::to_string(type.getDefaultValue())}}, true};
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, std::to_string(*def)}}, true};
+ }
+ return {{}, false};
}
CppBlockPtr
@@ -55,11 +59,14 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? std::to_string(*type.getDefaultValue())
+ : "std::nullopt";
+
b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeFloat(" +
- std::to_string(type.getDefaultValue()) + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
- ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
- "})); // of " + typeAccessor);
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
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 635cab4dee34bfd7f26232e43c80a0d0b98c73c1..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
@@ -41,7 +41,11 @@ namespace armarx::aron::codegenerator::cpp::generator
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
Int::getCtorInitializers(const std::string& name) const
{
- return {{{name, std::to_string(type.getDefaultValue())}}, true};
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, std::to_string(*def)}}, true};
+ }
+ return {{}, false};
}
CppBlockPtr
@@ -54,11 +58,14 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? std::to_string(*type.getDefaultValue())
+ : "std::nullopt";
+
b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeInt(" +
- std::to_string(type.getDefaultValue()) + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
- ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
- "})); // of " + typeAccessor);
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
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 9b61ee5f607b3cc1f4816252b40582bfdb92fae8..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
@@ -42,7 +42,11 @@ namespace armarx::aron::codegenerator::cpp::generator
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
Long::getCtorInitializers(const std::string& name) const
{
- return {{{name, std::to_string(type.getDefaultValue())}}, true};
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, std::to_string(*def)}}, true};
+ }
+ return {{}, false};
}
CppBlockPtr
@@ -55,11 +59,14 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
+ std::string defaultValue = type.getDefaultValue().has_value()
+ ? std::to_string(*type.getDefaultValue())
+ : "std::nullopt";
+
b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeLong(" +
- std::to_string(type.getDefaultValue()) + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " + "armarx::aron::Path(" +
- ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
- "})); // of " + typeAccessor);
+ defaultValue + ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
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 e1649b982a053830e7b65e3f763298f9b7c01b61..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
@@ -42,7 +42,11 @@ namespace armarx::aron::codegenerator::cpp::generator
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
String::getCtorInitializers(const std::string& name) const
{
- return {{{name, "\"" + type.getDefaultValue() + "\""}}, true};
+ if (auto def = type.getDefaultValue(); def.has_value())
+ {
+ return {{{name, "\"" + *def + "\""}}, true};
+ }
+ return {{}, false};
}
CppBlockPtr
@@ -55,11 +59,14 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeString(\"" +
- type.getDefaultValue() + "\", " +
- 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;
}
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 db2d4e30c4a44df1d250b7a14c1b051b00a2c3f1..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,7 +46,7 @@
</List>
</ObjectChild>
- <ObjectChild key='some_eigen_matrix'>
+ <ObjectChild key='some_eigen_matrix'> <!-- not optional -->
<Matrix rows="25" cols="10" type="int64" />
</ObjectChild>
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 35ad458c89f4531934a8b77d24aa7f8f5e356e8c..549e34181085626b9aec8958964a2cdcedf247c7 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aron/PrimitiveTest.xml
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aron/PrimitiveTest.xml
@@ -19,7 +19,7 @@
<string default="lalala" />
</ObjectChild>
<objectchild key='the_bool'>
- <bool default="true"/>
+ <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.h b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.h
index 03473264cfee8732ae8bb96fb60021bbdccb5cfb..cb4a9bad4ce82f4d07ac1655cfb2edd3c0c15157 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Data.h
@@ -180,7 +180,7 @@ namespace armarx::aron::typereader::xml
{"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",
{"RobotAPI", "libraries/aron/common/aron/time.xml"},
@@ -193,6 +193,13 @@ namespace armarx::aron::typereader::xml
{"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",
{"RobotAPI", "libraries/aron/common/aron/framed.xml"},
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp
index da43a7631f26585412947d053131b948a2652f65..4f77481df505023025c4315481b98e78b47fb49e 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp
@@ -41,9 +41,11 @@ namespace armarx::aron::typereader::xml
void
printDeprecationWarning(const std::string& warning)
{
+ 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" + armarx::aron::VERSION + " deprecation warning**]: "
- << warning
+ << "\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;
}
@@ -740,80 +742,89 @@ namespace armarx::aron::typereader::xml
type::VariantPtr
ReaderFactory::createInt(const RapidXmlReaderNode& node, const Path& path) const
{
- const std::string def =
- util::GetAttributeWithDefault(node, constantes::DEFAULT_ATTRIBUTE_NAME, "0");
+ auto o = std::make_shared<type::Int>(path);
- util::EnforceStrIsInt(def);
+ 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));
+ }
- auto o = std::make_shared<type::Int>(path);
- o->setDefaultValue(simox::alg::to_<int>(def));
return o;
}
type::VariantPtr
ReaderFactory::createLong(const RapidXmlReaderNode& node, const Path& path) const
{
- const std::string def =
- util::GetAttributeWithDefault(node, constantes::DEFAULT_ATTRIBUTE_NAME, "0");
+ auto o = std::make_shared<type::Long>(path);
- util::EnforceStrIsInt(def);
+ 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));
+ }
- auto o = std::make_shared<type::Long>(path);
- o->setDefaultValue(simox::alg::to_<long>(def));
return o;
}
type::VariantPtr
ReaderFactory::createFloat(const RapidXmlReaderNode& node, const Path& path) const
{
- const std::string def =
- util::GetAttributeWithDefault(node, constantes::DEFAULT_ATTRIBUTE_NAME, "0");
+ auto o = std::make_shared<type::Float>(path);
- util::EnforceStrIsFloat(def);
+ 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));
+ }
- auto o = std::make_shared<type::Float>(path);
- o->setDefaultValue(simox::alg::to_<float>(def));
return o;
}
type::VariantPtr
ReaderFactory::createDouble(const RapidXmlReaderNode& node, const Path& path) const
{
- const std::string def =
- util::GetAttributeWithDefault(node, constantes::DEFAULT_ATTRIBUTE_NAME, "0");
-
- util::EnforceStrIsFloat(def);
+ 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));
+ }
- auto o = std::make_shared<type::Double>(path);
- o->setDefaultValue(simox::alg::to_<double>(def));
return o;
}
type::VariantPtr
ReaderFactory::createString(const RapidXmlReaderNode& node, const Path& path) const
{
- const std::string def =
- util::GetAttributeWithDefault(node, constantes::DEFAULT_ATTRIBUTE_NAME, "");
-
auto o = std::make_shared<type::String>(path);
- o->setDefaultValue(def);
+
+ 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
{
- const std::string def =
- util::GetAttributeWithDefault(node, constantes::DEFAULT_ATTRIBUTE_NAME, "false");
-
- util::EnforceStrIsBool(def);
-
auto o = std::make_shared<type::Bool>(path);
- util::EnforceStrIsBool(def);
+ 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));
+ }
- o->setDefaultValue(simox::alg::to_<bool>(def));
return o;
}
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..be011068dffcabda87b34bc3da51d6e8b432a2e1 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,22 +46,24 @@ 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;
@@ -70,7 +76,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/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/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
index 3b7418fe505dc5a82e54285404ab44708f4d33c5..c03b3c37fec376834168d1795dbc6fc390874bea 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
@@ -43,13 +43,20 @@ namespace armarx::aron::data
};
// compare versions
- auto aronVersion = aron->VERSION;
- if (aronVersion != armarx::aron::VERSION)
+ 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. "
- << aronVersion << " != " << armarx::aron::VERSION;
+ << v_str << " != " << d_str;
}
auto descriptor = data::Aron2Descriptor(*aron);
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 dbe828b0e92797b427b6f6f10ef16b5d46eb551d..dfad132e5f6a45f828dd00942b8c5de4c1f427f3 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -53,7 +53,7 @@ namespace armarx::aron::data
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(data::dto::NDArrayPtr(new data::dto::NDArray(aron::Version(), dim, t, data)), path)
{
}
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 74d33da9af8fb260a9fe098221a1188a52d01cdd..df103ffd694f5e5f7773ef16e358747058eb976b 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.h
@@ -74,6 +74,7 @@ namespace armarx::aron::data
void clear();
// virtual implementations
+ using detail::ContainerVariant<data::dto::List, List>::clone;
ListPtr clone(const Path& p) const override;
std::string getShortName() const override;
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
index 69cabb5ba160f3abef84d331b971c4146d866c29..b5444266d3a2bfbc09e2cc151755eb90402e511a 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
@@ -45,7 +45,7 @@ namespace armarx::aron::data::detail
using ValueType = ValueT;
public:
- using Base::SpecializedVariantBase;
+ using typename Base::SpecializedVariantBase;
PrimitiveVariant(const ValueT& v,
const data::Descriptor descriptor,
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
index 1ce3a07fae4061b8fa99f3777c4b4ab321803fed..031d58ae16d06a2cdfefcfa180f36d753bb5030c 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/SpecializedVariant.h
@@ -49,7 +49,6 @@ namespace armarx::aron::data::detail
SpecializedVariantBase(const data::Descriptor descriptor, const Path& path = Path()) :
Variant(descriptor, path), aron(new AronDataType())
{
- aron->VERSION = aron::VERSION;
}
SpecializedVariantBase(const typename AronDataType::PointerType& o,
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 2355fb7f8a8786da18c861012bdd5a92b36cae1a..7e6f0c84dd3d9503f8e0f0d14429395d31867ace 100644
--- a/source/RobotAPI/libraries/aron/core/type/converter/Converter.h
+++ b/source/RobotAPI/libraries/aron/core/type/converter/Converter.h
@@ -225,7 +225,7 @@ namespace armarx::aron::type
{
type::Maybe maybe;
Path p;
- int defaultValue;
+ std::optional<int> defaultValue;
r.readInt(o, defaultValue, maybe, p);
last_returned = w.writeInt(defaultValue, maybe, p);
};
@@ -235,7 +235,7 @@ namespace armarx::aron::type
{
type::Maybe maybe;
Path p;
- long defaultValue;
+ std::optional<long> defaultValue;
r.readLong(o, defaultValue, maybe, p);
last_returned = w.writeLong(defaultValue, maybe, p);
};
@@ -245,7 +245,7 @@ namespace armarx::aron::type
{
type::Maybe maybe;
Path p;
- float defaultValue;
+ std::optional<float> defaultValue;
r.readFloat(o, defaultValue, maybe, p);
last_returned = w.writeFloat(defaultValue, maybe, p);
};
@@ -255,7 +255,7 @@ namespace armarx::aron::type
{
type::Maybe maybe;
Path p;
- double defaultValue;
+ std::optional<double> defaultValue;
r.readDouble(o, defaultValue, maybe, p);
last_returned = w.writeDouble(defaultValue, maybe, p);
};
@@ -265,7 +265,7 @@ namespace armarx::aron::type
{
type::Maybe maybe;
Path p;
- bool defaultValue;
+ std::optional<bool> defaultValue;
r.readBool(o, defaultValue, maybe, p);
last_returned = w.writeBool(defaultValue, maybe, p);
};
@@ -275,7 +275,7 @@ namespace armarx::aron::type
{
type::Maybe maybe;
Path p;
- std::string defaultValue;
+ std::optional<std::string> defaultValue;
r.readString(o, defaultValue, maybe, p);
last_returned = w.writeString(defaultValue, maybe, p);
};
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Reader.h b/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
index 5a736c0ada20388f3ccaba62489e28b7e75b9770..9b9b5cf97723bd1b79f53775ebb7e485627c013f 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Reader.h
@@ -22,6 +22,7 @@
// STD/STL
#include <memory>
+#include <optional>
#include <string>
// ArmarX
@@ -125,30 +126,40 @@ namespace armarx::aron::type
Path& p) = 0;
/// Extract information from an int type
- virtual void
- readInt(const InputType& input, int& defaultValue, 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, long& defaultValue, 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, float& defaultValue, 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, double& defaultValue, 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,
- std::string& defaultValue,
+ std::optional<std::string>& defaultValue,
type::Maybe& maybe,
Path& p) = 0;
/// Extract information from an bool type
- virtual void
- readBool(const InputType& input, bool& defaultValue, 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;
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
index 8ff7b1ce295e1c3ddbb2f28a350677db6b7344d2..27bcfcd0da522ce9defa148017ec57f6ea28e9bb 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/Writer.h
@@ -114,25 +114,34 @@ namespace armarx::aron::type
const Path& p) = 0;
/// Construct a int from the params
- virtual ReturnType writeInt(int defaultValue, 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(long defaultValue, 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(float defaultValue, 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(double defaultValue, 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 std::string& defaultValue, 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(bool defaultValue, 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;
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 2e9b1e46aac49f71f4b14ebd524c9cf49026eb54..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
@@ -250,7 +250,7 @@ namespace armarx::aron::type::reader
void
NlohmannJSONReader::readInt(const nlohmann::json& input,
- int& defaultValue,
+ std::optional<int>& defaultValue,
type::Maybe& maybe,
Path& p)
{
@@ -262,7 +262,7 @@ namespace armarx::aron::type::reader
void
NlohmannJSONReader::readLong(const nlohmann::json& input,
- long& defaultValue,
+ std::optional<long>& defaultValue,
type::Maybe& maybe,
Path& p)
{
@@ -274,7 +274,7 @@ namespace armarx::aron::type::reader
void
NlohmannJSONReader::readFloat(const nlohmann::json& input,
- float& defaultValue,
+ std::optional<float>& defaultValue,
type::Maybe& maybe,
Path& p)
{
@@ -286,7 +286,7 @@ namespace armarx::aron::type::reader
void
NlohmannJSONReader::readDouble(const nlohmann::json& input,
- double& defaultValue,
+ std::optional<double>& defaultValue,
type::Maybe& maybe,
Path& p)
{
@@ -298,7 +298,7 @@ namespace armarx::aron::type::reader
void
NlohmannJSONReader::readString(const nlohmann::json& input,
- std::string& defaultValue,
+ std::optional<std::string>& defaultValue,
type::Maybe& maybe,
Path& p)
{
@@ -310,7 +310,7 @@ namespace armarx::aron::type::reader
void
NlohmannJSONReader::readBool(const nlohmann::json& input,
- bool& defaultValue,
+ std::optional<bool>& defaultValue,
type::Maybe& maybe,
Path& p)
{
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 fea83d4c8178db7ee73be00660039c9ce0f19b23..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
@@ -102,27 +102,27 @@ namespace armarx::aron::type::reader
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readInt(InputType& input,
- int& defaultValue,
+ std::optional<int>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readLong(InputType& input,
- long& defaultValue,
+ std::optional<long>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readFloat(InputType& input,
- float& defaultValue,
+ std::optional<float>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readDouble(InputType& input,
- double& defaultValue,
+ std::optional<double>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readString(InputType& input,
- std::string& defaultValue,
+ std::optional<std::string>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readBool(InputType& input,
- bool& defaultValue,
+ std::optional<bool>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
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 90dd15d49d84c6e330b1f2756bb4959e8daefb89..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
@@ -223,7 +223,7 @@ namespace armarx::aron::type::reader
void
VariantReader::readInt(const aron::type::VariantPtr& input,
- int& defaultValue,
+ std::optional<int>& defaultValue,
type::Maybe& maybe,
Path& p)
{
@@ -237,7 +237,7 @@ namespace armarx::aron::type::reader
void
VariantReader::readLong(const aron::type::VariantPtr& input,
- long& defaultValue,
+ std::optional<long>& defaultValue,
type::Maybe& maybe,
Path& p)
{
@@ -251,7 +251,7 @@ namespace armarx::aron::type::reader
void
VariantReader::readFloat(const aron::type::VariantPtr& input,
- float& defaultValue,
+ std::optional<float>& defaultValue,
type::Maybe& maybe,
Path& p)
{
@@ -265,7 +265,7 @@ namespace armarx::aron::type::reader
void
VariantReader::readDouble(const aron::type::VariantPtr& input,
- double& defaultValue,
+ std::optional<double>& defaultValue,
type::Maybe& maybe,
Path& p)
{
@@ -279,7 +279,7 @@ namespace armarx::aron::type::reader
void
VariantReader::readString(const aron::type::VariantPtr& input,
- std::string& defaultValue,
+ std::optional<std::string>& defaultValue,
type::Maybe& maybe,
Path& p)
{
@@ -293,7 +293,7 @@ namespace armarx::aron::type::reader
void
VariantReader::readBool(const aron::type::VariantPtr& input,
- bool& defaultValue,
+ std::optional<bool>& defaultValue,
type::Maybe& maybe,
Path& p)
{
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 5a9e8cb87d5745d1df3cc27350c13d46b3ac5725..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
@@ -102,27 +102,27 @@ namespace armarx::aron::type::reader
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readInt(InputType& input,
- int& defaultValue,
+ std::optional<int>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readLong(InputType& input,
- long& defaultvalue,
+ std::optional<long>& defaultvalue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readFloat(InputType& input,
- float& defaultValue,
+ std::optional<float>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readDouble(InputType& input,
- double& defaultValue,
+ std::optional<double>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readString(InputType& input,
- std::string& defaultValue,
+ std::optional<std::string>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
void readBool(InputType& input,
- bool& defaultValue,
+ std::optional<bool>& defaultValue,
type::Maybe& maybe,
Path& p = *std::unique_ptr<Path>(new Path())) override;
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 960d781ce3e7d438779fd304ac518562f63b9c4d..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
@@ -224,58 +224,73 @@ namespace armarx::aron::type::writer
}
nlohmann::json
- NlohmannJSONWriter::writeInt(int defaultValue, const type::Maybe maybe, const Path& p)
+ 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;
+ o[rw::json::constantes::DEFAULT_SLUG] =
+ defaultValue.has_value() ? std::to_string(*defaultValue) : "";
return o;
}
nlohmann::json
- NlohmannJSONWriter::writeLong(long defaultValue, const type::Maybe maybe, const Path& p)
+ 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;
+ o[rw::json::constantes::DEFAULT_SLUG] =
+ defaultValue.has_value() ? std::to_string(*defaultValue) : "";
return o;
}
nlohmann::json
- NlohmannJSONWriter::writeFloat(float defaultValue, const type::Maybe maybe, const Path& p)
+ 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;
+ o[rw::json::constantes::DEFAULT_SLUG] =
+ defaultValue.has_value() ? std::to_string(*defaultValue) : "";
return o;
}
nlohmann::json
- NlohmannJSONWriter::writeDouble(double defaultValue, const type::Maybe maybe, const Path& p)
+ 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;
+ o[rw::json::constantes::DEFAULT_SLUG] =
+ defaultValue.has_value() ? std::to_string(*defaultValue) : "";
return o;
}
nlohmann::json
- NlohmannJSONWriter::writeString(const std::string& defaultValue,
+ 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;
+ o[rw::json::constantes::DEFAULT_SLUG] = defaultValue.has_value() ? *defaultValue : "";
return o;
}
nlohmann::json
- NlohmannJSONWriter::writeBool(bool defaultValue, const type::Maybe maybe, const Path& p)
+ 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;
+ o[rw::json::constantes::DEFAULT_SLUG] =
+ defaultValue.has_value() ? std::to_string(*defaultValue) : "";
return o;
}
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 6020bb54cdf1e71efb65ce933194c0aef674e115..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
@@ -91,19 +91,24 @@ namespace armarx::aron::type::writer
const type::Maybe maybe,
const Path& p = Path()) override;
- ReturnType
- writeInt(int defaultValue, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType
- writeLong(long defaultValue, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType
- writeFloat(float defaultValue, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType
- writeDouble(double defaultValue, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeString(const std::string& defaultValue,
+ 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
- writeBool(bool 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;
};
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 40f5987949d14c905e3b54030b609918f2cb2949..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
@@ -190,7 +190,9 @@ namespace armarx::aron::type::writer
}
aron::type::VariantPtr
- VariantWriter::writeInt(int defaultValue, const type::Maybe maybe, const Path& p)
+ 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);
@@ -199,7 +201,9 @@ namespace armarx::aron::type::writer
}
aron::type::VariantPtr
- VariantWriter::writeLong(long defaultValue, const type::Maybe maybe, const Path& p)
+ 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);
@@ -208,7 +212,9 @@ namespace armarx::aron::type::writer
}
aron::type::VariantPtr
- VariantWriter::writeFloat(float defaultValue, const type::Maybe maybe, const Path& p)
+ 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);
@@ -217,7 +223,9 @@ namespace armarx::aron::type::writer
}
aron::type::VariantPtr
- VariantWriter::writeDouble(double defaultValue, const type::Maybe maybe, const Path& p)
+ 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);
@@ -226,7 +234,7 @@ namespace armarx::aron::type::writer
}
aron::type::VariantPtr
- VariantWriter::writeString(const std::string& defaultValue,
+ VariantWriter::writeString(const std::optional<std::string>& defaultValue,
const type::Maybe maybe,
const Path& p)
{
@@ -237,7 +245,9 @@ namespace armarx::aron::type::writer
}
aron::type::VariantPtr
- VariantWriter::writeBool(bool defaultValue, const type::Maybe maybe, const Path& p)
+ 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);
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 78accf7c8390313d1410cfe6e276489235da3b25..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
@@ -86,19 +86,24 @@ namespace armarx::aron::type::writer
const type::Maybe maybe,
const Path& p = Path()) override;
- ReturnType
- writeInt(int defaultValue, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType
- writeLong(long defaultValue, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType
- writeFloat(float defaultValue, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType
- writeDouble(double defaultValue, const type::Maybe maybe, const Path& p = Path()) override;
- ReturnType writeString(const std::string& defaultValue,
+ 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
- writeBool(bool 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;
};
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
index fccc4e1306af41cc3498c700f8ed448e83d42b7b..21ba5cd0984b9077f668886327b57e8722b1f553 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
@@ -35,14 +35,20 @@ namespace armarx::aron::type
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 aronVersion = aron.VERSION;
- if (aronVersion != armarx::aron::VERSION)
+ 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. "
- << aronVersion << " != " << armarx::aron::VERSION;
+ << v_str << " != " << d_str;
}
auto descriptor = type::Aron2Descriptor(aron);
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 5bfa7fe14d8c4af284e8af977bbc61463e0540f5..24dd9171d771e9957600196cf9b9171d5940f455 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
@@ -70,16 +70,27 @@ namespace armarx::aron::type::detail
return 0;
}
- ValueType
+ std::optional<ValueType>
getDefaultValue() const
{
- return this->aron->defaultValue;
+ if (!this->aron->defaultValue)
+ {
+ return std::nullopt;
+ }
+ return *this->aron->defaultValue;
}
void
- setDefaultValue(const ValueType& v)
+ setDefaultValue(const std::optional<ValueType>& v)
{
- this->aron->defaultValue = v;
+ if (!v)
+ {
+ this->aron->defaultValue = IceUtil::None;
+ }
+ else
+ {
+ this->aron->defaultValue = *v;
+ }
}
protected:
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/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/skills/core/mixins/ObjectReadingSkillMixin.h b/source/RobotAPI/libraries/skills/core/mixins/ObjectReadingSkillMixin.h
index de5ef34b1dc448edc4a8ba634ad74600c7d33da4..ecf3d2a64ef0a5d3f01b5b9b23839b1db14cb921 100644
--- a/source/RobotAPI/libraries/skills/core/mixins/ObjectReadingSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/core/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/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index d6bfe6e059f799a89a2a611220812c798b1f4cd6..f4d3489fe9c9f235e9ad94a4d73d028b57a4d6e6 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -48,16 +48,14 @@ namespace armarx::plugins
void addSkill(const skills::LambdaSkill::FunT&, const skills::SkillDescription&);
void addSkill(std::unique_ptr<skills::Skill>&&);
- template <typename T>
+ template <typename T, typename... Args>
T*
- addSkill()
+ addSkill(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 skill = std::make_unique<T>(std::forward<Args>(args)...);
auto* skillPtr = skill.get();
addSkill(std::move(skill));
return static_cast<T*>(skillPtr);
@@ -114,11 +112,11 @@ namespace armarx
void addSkill(const skills::LambdaSkill::FunT&, const skills::SkillDescription&);
void addSkill(std::unique_ptr<skills::Skill>&&);
- template <typename T>
+ template <typename T, typename... Args>
T*
- addSkill()
+ addSkill(Args&&... args)
{
- return plugin->addSkill<T>();
+ return plugin->addSkill<T>(std::forward<Args>(args)...);
}
private:
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..562ddabd56b6d07428f8ce2b386fa682ac95dd1f
--- /dev/null
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.cpp
@@ -0,0 +1,133 @@
+#include "RequestObjects.h"
+
+//#include <ArmarXCore/core/time/TimeUtil.h>
+//#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include "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{mns};
+ reader.connect();
+ 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));
+ }
+}
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>
+