diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 98450f59a9acce6bf2788644c34f49d40392eb6e..2388c61e39a8e0582f63b11dec0ca45ec51c8d24 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -280,7 +280,7 @@
propertyName="ObstacleDetectionName"
propertyIsOptional="true"
propertyDefaultValue="PlatformObstacleAvoidance" />
- <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h"
+ <Proxy include="RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h"
humanName="Dynamic obstacle manager interface"
typeName="DynamicObstacleManagerInterfacePrx"
memberName="dynamicObstacleManager"
@@ -320,13 +320,13 @@
propertyName="DebugDrawerToArVizLayerBlackWhitelistTopicName"
propertyIsOptional="true"
propertyDefaultValue="DebugDrawerToArVizLayerBlackWhitelistUpdates" />
- <Proxy include="RobotAPI/interface/objectpose/ObjectPoseObserver.h"
- humanName="ObjectPoseObserver"
- typeName="objpose::ObjectPoseObserverInterfacePrx"
- memberName="ObjectPoseObserver"
- getterName="getObjectPoseObserver"
- propertyName="ObjectPoseObserverName"
+ <Proxy include="RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h"
+ humanName="ObjectPoseStorage"
+ typeName="objpose::ObjectPoseStorageInterfacePrx"
+ memberName="ObjectPoseStorage"
+ getterName="getObjectPoseStorage"
+ propertyName="ObjectPoseStorageName"
propertyIsOptional="true"
- propertyDefaultValue="ObjectPoseObserver" />
+ propertyDefaultValue="ObjectMemory" />
</Lib>
</VariantInfo>
diff --git a/scenarios/ObjectPoseObserverExample/ObjectPoseObserverExample.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
similarity index 55%
rename from scenarios/ObjectPoseObserverExample/ObjectPoseObserverExample.scx
rename to scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
index 1aa5238bfbb09c201635d6504393e499e7e21593..95ec0bddab3697b5270df9c454318d38b63e6467 100644
--- a/scenarios/ObjectPoseObserverExample/ObjectPoseObserverExample.scx
+++ b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
@@ -1,11 +1,13 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="ObjectPoseObserverExample" creation="2020-08-03.10:09:13" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
- <application name="ObjectPoseObserver" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ObjectPoseProviderExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+<scenario name="ArMemObjectMemory" creation="2021-04-22.11:29:22" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ObjectPoseProviderExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="ObjectPoseClientExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ObjectPoseObserverExample/config/ArVizStorage.cfg b/scenarios/ArMemObjectMemory/config/ArVizStorage.cfg
similarity index 100%
rename from scenarios/ObjectPoseObserverExample/config/ArVizStorage.cfg
rename to scenarios/ArMemObjectMemory/config/ArVizStorage.cfg
diff --git a/scenarios/ObjectPoseObserverExample/config/DebugObserver.cfg b/scenarios/ArMemObjectMemory/config/DebugObserver.cfg
similarity index 100%
rename from scenarios/ObjectPoseObserverExample/config/DebugObserver.cfg
rename to scenarios/ArMemObjectMemory/config/DebugObserver.cfg
diff --git a/scenarios/ArMemObjectMemory/config/MemoryNameSystem.cfg b/scenarios/ArMemObjectMemory/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..7dd22218243ca4f9e67e843da8b42916f3b8568a
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/MemoryNameSystem.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# MemoryNameSystem properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_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)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.MemoryNameSystem.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MemoryNameSystem.EnableProfiling = false
+
+
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryNameSystem.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.ObjectName = ""
+
+
+# ArmarX.MemoryNameSystem.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
similarity index 56%
rename from scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg
rename to scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index 279193cc2f6feef27a705bf30dbae8451f7bfe14..22981bc341684bcac781c01b26249ba6376c1ad2 100644
--- a/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -1,5 +1,5 @@
# ==================================================================
-# ObjectPoseObserver properties
+# ObjectMemory 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.
@@ -92,262 +92,330 @@
# ArmarX.LoggingGroup = ""
-# ArmarX.ObjectPoseObserver.ArVizTopicName: Name of the ArViz topic
+# ArmarX.ObjectMemory.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.ArVizTopicName = ArVizTopic
+# ArmarX.ObjectMemory.ArVizTopicName = ArVizTopic
-# ArmarX.ObjectPoseObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# ArmarX.ObjectMemory.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.ObjectPoseObserver.CreateUpdateFrequenciesChannel = false
+# ArmarX.ObjectMemory.EnableProfiling = false
-# ArmarX.ObjectPoseObserver.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ObjectMemory.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: false
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.EnableProfiling = false
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ObjectMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ObjectName = ""
-# ArmarX.ObjectPoseObserver.KinematicUnitObserverName: Name of the kinematic unit observer.
+# ArmarX.ObjectMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.ObjectMemory.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
# Attributes:
# - Default: KinematicUnitObserver
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.KinematicUnitObserverName = KinematicUnitObserver
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
-# ArmarX.ObjectPoseObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# ArmarX.ObjectMemory.mem.MemoryName: Name of this memory server.
# Attributes:
-# - Default: 50
+# - Default: Object
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.MaxHistoryRecordFrequency = 50
+# ArmarX.ObjectMemory.mem.MemoryName = Object
-# ArmarX.ObjectPoseObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# ArmarX.ObjectMemory.mem.cls.CoreSegmentName: Name of the object clazz core segment.
# Attributes:
-# - Default: 5000
+# - Default: Class
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.MaxHistorySize = 5000
+# ArmarX.ObjectMemory.mem.cls.CoreSegmentName = Class
-# ArmarX.ObjectPoseObserver.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
# Attributes:
-# - Default: Undefined
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.ObjectPoseObserver.MinimumLoggingLevel = Undefined
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage = true
-# ArmarX.ObjectPoseObserver.ObjectName: Name of IceGrid well-known object
+# ArmarX.ObjectMemory.mem.cls.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
# Attributes:
-# - Default: ""
+# - Default: -1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.ObjectName = ""
+# ArmarX.ObjectMemory.mem.cls.MaxHistorySize = -1
-# ArmarX.ObjectPoseObserver.ObjectPoseTopicName: Name of the Object Pose Topic.
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage: Name of the objects package to load from.
# Attributes:
-# - Default: ObjectPoseTopic
+# - Default: ArmarXObjects
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.ObjectPoseTopicName = ObjectPoseTopic
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage = ArmarXObjects
-# ArmarX.ObjectPoseObserver.RemoteGuiName: Name of the remote gui provider
+# ArmarX.ObjectMemory.mem.inst.CoreSegmentName: Name of the object instance core segment.
# Attributes:
-# - Default: RemoteGuiProvider
+# - Default: Instance
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.RemoteGuiName = RemoteGuiProvider
+# ArmarX.ObjectMemory.mem.inst.CoreSegmentName = Instance
-# ArmarX.ObjectPoseObserver.RemoteStateComponentName: Name of the robot state component
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
+# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
# Attributes:
-# - Default: RobotStateComponent
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached = true
+
+
+# ArmarX.ObjectMemory.mem.inst.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.RemoteStateComponentName = RobotStateComponent
+# ArmarX.ObjectMemory.mem.inst.MaxHistorySize = -1
-# ArmarX.ObjectPoseObserver.calibration.offset: Offset for the node to be calibrated.
+# ArmarX.ObjectMemory.mem.inst.calibration.offset: Offset for the node to be calibrated.
# Attributes:
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.calibration.offset = 0
+# ArmarX.ObjectMemory.mem.inst.calibration.offset = 0
-# ArmarX.ObjectPoseObserver.calibration.robotNode: Robot node which can be calibrated.
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode: Robot node which can be calibrated.
# Attributes:
# - Default: Neck_2_Pitch
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.calibration.robotNode = Neck_2_Pitch
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode = Neck_2_Pitch
-# ArmarX.ObjectPoseObserver.decay.delaySeconds: Duration after latest localization before decay starts.
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds: Duration after latest localization before decay starts.
# Attributes:
# - Default: 5
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.decay.delaySeconds = 5
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds = 5
-# ArmarX.ObjectPoseObserver.decay.durationSeconds: How long to reach minimal confidence.
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds: How long to reach minimal confidence.
# Attributes:
# - Default: 20
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.decay.durationSeconds = 20
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds = 20
-# ArmarX.ObjectPoseObserver.decay.enabled: If true, object poses decay over time when not localized anymore.
+# ArmarX.ObjectMemory.mem.inst.decay.enabled: If true, object poses decay over time when not localized anymore.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.decay.enabled = false
+# ArmarX.ObjectMemory.mem.inst.decay.enabled = false
-# ArmarX.ObjectPoseObserver.decay.maxConfidence: Confidence when decay starts.
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence: Confidence when decay starts.
# Attributes:
# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.decay.maxConfidence = 1
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence = 1
-# ArmarX.ObjectPoseObserver.decay.minConfidence: Confidence after decay duration.
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence: Confidence after decay duration.
# Attributes:
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.decay.minConfidence = 0
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence = 0
-# ArmarX.ObjectPoseObserver.decay.removeObjectsBelowConfidence: Remove objects whose confidence is lower than this value.
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence: Remove objects whose confidence is lower than this value.
# Attributes:
# - Default: 0.100000001
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.decay.removeObjectsBelowConfidence = 0.100000001
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence = 0.100000001
-# ArmarX.ObjectPoseObserver.head.checkHeadVelocity: If true, check whether the head is moving and discard updates in the meantime.
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity: If true, check whether the head is moving and discard updates in the meantime.
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.head.checkHeadVelocity = true
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity = true
-# ArmarX.ObjectPoseObserver.head.discardIntervalAfterMoveMS: For how long new updates are ignored after moving the head.
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS: For how long new updates are ignored after moving the head.
# Attributes:
# - Default: 100
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.head.discardIntervalAfterMoveMS = 100
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS = 100
-# ArmarX.ObjectPoseObserver.head.maxJointVelocity: If a head joint's velocity is higher, the head is considered moving.
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity: If a head joint's velocity is higher, the head is considered moving.
# Attributes:
# - Default: 0.0500000007
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.head.maxJointVelocity = 0.0500000007
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity = 0.0500000007
-# ArmarX.ObjectPoseObserver.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
-# Attributes:
-# - Default: DebugObserver
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectPoseObserver.tpc.pub.DebugObserver = DebugObserver
-
-
-# ArmarX.ObjectPoseObserver.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
+# ArmarX.ObjectMemory.mem.inst.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
# Attributes:
# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.visu.alpha = 1
+# ArmarX.ObjectMemory.mem.inst.visu.alpha = 1
-# ArmarX.ObjectPoseObserver.visu.alphaByConfidence: If true, use the pose confidence as alpha (if < 1.0).
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence: If true, use the pose confidence as alpha (if < 1.0).
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.visu.alphaByConfidence = false
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence = false
-# ArmarX.ObjectPoseObserver.visu.enabled: Enable or disable visualization of objects.
+# ArmarX.ObjectMemory.mem.inst.visu.enabled: Enable or disable visualization of objects.
# Attributes:
-# - Default: false
+# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-ArmarX.ObjectPoseObserver.visu.enabled = true
+# ArmarX.ObjectMemory.mem.inst.visu.enabled = true
-# ArmarX.ObjectPoseObserver.visu.frequenzyHz: Frequency of visualization.
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz: Frequency of visualization.
# Attributes:
# - Default: 25
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.visu.frequenzyHz = 25
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz = 25
-# ArmarX.ObjectPoseObserver.visu.inGlobalFrame: If true, show global poses. If false, show poses in robot frame.
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame: If true, show global poses. If false, show poses in robot frame.
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.visu.inGlobalFrame = true
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame = true
-# ArmarX.ObjectPoseObserver.visu.objectFrames: Enable showing object frames.
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames: Enable showing object frames.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.visu.objectFrames = false
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames = false
-# ArmarX.ObjectPoseObserver.visu.objectFramesScale: Scaling of object frames.
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale: Scaling of object frames.
# Attributes:
# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectPoseObserver.visu.objectFramesScale = 1
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale = 1
-# ArmarX.ObjectPoseObserver.visu.oobbs: Enable showing oriented bounding boxes.
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs: Enable showing oriented bounding boxes.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectPoseObserver.visu.oobbs = false
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ObjectMemory.tpc.pub.MemoryListener: Name of the `MemoryListener` topic to publish data to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.pub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
+# Attributes:
+# - Default: ObjectPoseTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic = ObjectPoseTopic
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
diff --git a/scenarios/ArMemObjectMemory/config/ObjectPoseClientExample.cfg b/scenarios/ArMemObjectMemory/config/ObjectPoseClientExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f75bad15e903fd399e990eb38dce28887c537d98
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ObjectPoseClientExample.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# ObjectPoseClientExample 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_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)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.ObjectPoseClientExample.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseClientExample.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ObjectPoseClientExample.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseClientExample.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.ObjectPoseClientExample.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.ObjectPoseClientExample.EnableProfiling = false
+
+
+# ArmarX.ObjectPoseClientExample.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.ObjectPoseClientExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectPoseClientExample.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseClientExample.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.ObjectPoseClientExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseClientExample.ObjectName = ""
+
+
+# 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/ObjectPoseObserverExample/config/ObjectPoseProviderExample.cfg b/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
similarity index 98%
rename from scenarios/ObjectPoseObserverExample/config/ObjectPoseProviderExample.cfg
rename to scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
index 53ccf9cb429c6f9b94ebf0aed72f3a72a9c368a9..45075e6114eae82fe89f50317c0bced824c96b46 100644
--- a/scenarios/ObjectPoseObserverExample/config/ObjectPoseProviderExample.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectPoseProviderExample.cfg
@@ -132,7 +132,7 @@
# - Case sensitivity: yes
# - Required: no
# - Possible values: {KIT/Amicelli, KIT/YellowSaltCylinder}
-ArmarX.ObjectPoseProviderExample.Objects = KIT/Amicelli, YCB/002_master_chef_can
+# ArmarX.ObjectPoseProviderExample.Objects = KIT/Amicelli, KIT/YellowSaltCylinder
# ArmarX.ObjectPoseProviderExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
diff --git a/scenarios/ObjectPoseObserverExample/config/RemoteGuiProviderApp.cfg b/scenarios/ArMemObjectMemory/config/RemoteGuiProviderApp.cfg
similarity index 100%
rename from scenarios/ObjectPoseObserverExample/config/RemoteGuiProviderApp.cfg
rename to scenarios/ArMemObjectMemory/config/RemoteGuiProviderApp.cfg
diff --git a/scenarios/ObjectPoseObserverExample/config/RobotStateComponent.cfg b/scenarios/ArMemObjectMemory/config/RobotStateComponent.cfg
similarity index 100%
rename from scenarios/ObjectPoseObserverExample/config/RobotStateComponent.cfg
rename to scenarios/ArMemObjectMemory/config/RobotStateComponent.cfg
diff --git a/scenarios/ObjectPoseObserverExample/config/RobotToArVizApp.cfg b/scenarios/ArMemObjectMemory/config/RobotToArVizApp.cfg
similarity index 100%
rename from scenarios/ObjectPoseObserverExample/config/RobotToArVizApp.cfg
rename to scenarios/ArMemObjectMemory/config/RobotToArVizApp.cfg
diff --git a/scenarios/ObjectPoseObserverExample/config/global.cfg b/scenarios/ArMemObjectMemory/config/global.cfg
similarity index 96%
rename from scenarios/ObjectPoseObserverExample/config/global.cfg
rename to scenarios/ArMemObjectMemory/config/global.cfg
index 7c603bf9905a566cbf240054770023ad225aebbc..39e1d4a1aac92cf8ffdb032a033f2d88d9534601 100644
--- a/scenarios/ObjectPoseObserverExample/config/global.cfg
+++ b/scenarios/ArMemObjectMemory/config/global.cfg
@@ -1,5 +1,5 @@
# ==================================================================
-# Global Config from Scenario ObjectPoseObserverExample
+# Global Config from Scenario ArMemObjectMemory
# ==================================================================
# RobotConfig.AgentName: Custom Property
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 7e1ec4053f83a4d33bb323bd129ff87f0d7b703a..5a21a1cd4e02935799bdd4788aa7e2cac5ca12c8 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -89,6 +89,9 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
+add_library(RobotAPI::ArViz ALIAS ArViz)
+
+
armarx_component_set_name("ArVizStorage")
set(COMPONENT_LIBS
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index 57e7a5c74665b112f0999c37be4bd2ff37be5687..3ce3ea6f48c69658788abc3ba2ed00c6a81e1002 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -21,6 +21,15 @@ namespace armarx::viz
armarx::PackageFileLocation file = info.simoxXML();
return this->file(file.package, file.relativePath);
}
+
+ Object& Object::alpha(float alpha)
+ {
+ if (alpha < 1)
+ {
+ overrideColor(simox::Color::white().with_alpha(alpha));
+ }
+ return *this;
+ }
}
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index b6eadc5bd0cea09050d4dc6f8ddb79dc84bd9227..297a22469d52be8b7cda3c71438f6ee0617c0091 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -38,7 +38,7 @@ namespace Eigen
namespace armarx
{
- ///@see <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+ // <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
class ObjectID;
}
@@ -479,6 +479,7 @@ namespace armarx::viz
Object& fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
Object& fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
+ Object& alpha(float alpha);
Object& useCollisionModel()
{
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index cec5e53f76ea3d9d9a52745e3e50923e7ca01b2d..d19343cee661810483206528bfccaa62c614bff4 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -18,7 +18,7 @@ add_subdirectory(KITHandUnit)
add_subdirectory(KITProstheticHandUnit)
add_subdirectory(MultiHandUnit)
add_subdirectory(NaturalIKTest)
-add_subdirectory(ObjectPoseObserver)
+add_subdirectory(ObjectPoseClientExample)
add_subdirectory(ObjectPoseProviderExample)
add_subdirectory(RobotHealth)
add_subdirectory(RobotNameService)
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseClientExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1ff92199fbec6f531cb69daf5ffa9b8266abbd59
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseClientExample/CMakeLists.txt
@@ -0,0 +1,25 @@
+armarx_component_set_name("ObjectPoseClientExample")
+
+
+set(COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCoreComponentPlugins
+ # RobotAPI
+ RobotAPI::ArmarXObjects
+ RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+)
+
+set(SOURCES
+ ObjectPoseClientExample.cpp
+)
+set(HEADERS
+ ObjectPoseClientExample.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+#generate the application
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6d5c668ecd0ad66bf47036d1b300efa9e814803
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
@@ -0,0 +1,98 @@
+/*
+ * 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::ObjectPoseClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ObjectPoseClientExample.h"
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+
+namespace armarx
+{
+
+ armarx::PropertyDefinitionsPtr ObjectPoseClientExample::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ return defs;
+ }
+
+ std::string ObjectPoseClientExample::getDefaultName() const
+ {
+ return "ObjectPoseClientExample";
+ }
+
+ void ObjectPoseClientExample::onInitComponent()
+ {
+ }
+
+ void ObjectPoseClientExample::onConnectComponent()
+ {
+ setDebugObserverBatchModeEnabled(true);
+
+ objectProcessingTask = new SimpleRunningTask<>([this]()
+ {
+ this->objectProcessingTaskRun();
+ });
+ objectProcessingTask->start();
+ }
+
+ void ObjectPoseClientExample::onDisconnectComponent()
+ {
+ }
+
+ void ObjectPoseClientExample::onExitComponent()
+ {
+ }
+
+
+ void ObjectPoseClientExample::objectProcessingTaskRun()
+ {
+ CycleUtil cycle(50);
+
+ while (objectProcessingTask && !objectProcessingTask->isStopped())
+ {
+ const objpose::ObjectPoseSeq objectPoses = ObjectPoseClient::getObjectPoses();
+
+ ARMARX_VERBOSE << "Received poses of " << objectPoses.size() << " objects.";
+
+ {
+ setDebugObserverDatafield("NumObjectPoses", objectPoses.size());
+ sendDebugObserverBatch();
+ }
+
+ {
+ // Visualize the objects.
+ viz::Layer layer = arviz.layer("Objects");
+ for (const objpose::ObjectPose& objectPose : objectPoses)
+ {
+ layer.add(viz::Object(objectPose.objectID.str())
+ .pose(objectPose.objectPoseGlobal)
+ .fileByObjectFinder(objectPose.objectID)
+ .alpha(objectPose.confidence));
+ }
+ arviz.commit(layer);
+ }
+
+ cycle.waitForCycleDuration();
+ }
+ }
+}
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
new file mode 100644
index 0000000000000000000000000000000000000000..c49e28f3518755cc492ffa31b67710b005abf7a3
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
@@ -0,0 +1,84 @@
+/*
+ * 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::ObjectPoseClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+// Include the ClientPlugin
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+
+
+namespace armarx
+{
+
+ /**
+ * @defgroup Component-ObjectPoseClientExample ObjectPoseClientExample
+ * @ingroup RobotAPI-Components
+ *
+ * An example showing how to get object poses from the ObjectPoseStorage.
+ *
+ * @class ObjectPoseClientExample
+ * @ingroup Component-ObjectPoseClientExample
+ * @brief Brief description of class ObjectPoseClientExample.
+ *
+ * Gets and visualizes object poses from the ObjectPoseStorage.
+ */
+ class ObjectPoseClientExample :
+ virtual public armarx::Component
+ , virtual public armarx::DebugObserverComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
+ // Derive from the client plugin.
+ , virtual public armarx::ObjectPoseClientPluginUser
+
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ protected:
+
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+
+ private:
+
+ void objectProcessingTaskRun();
+
+
+ private:
+
+ armarx::SimpleRunningTask<>::pointer_type objectProcessingTask;
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
deleted file mode 100644
index 74da78adf1af125b3ccab6669b1b47a2a9d0d9e8..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ /dev/null
@@ -1,45 +0,0 @@
-armarx_component_set_name("ObjectPoseObserver")
-
-
-set(COMPONENT_LIBS
- ArmarXCore ArmarXCoreInterfaces
- ArmarXGuiComponentPlugins
- RobotAPIArmarXObjects RobotAPIComponentPlugins
- ArViz
-
- ${PROJECT_NAME}Interfaces
-)
-
-set(SOURCES
- ObjectPoseObserver.cpp
-
- detail/Data.cpp
- detail/Decay.cpp
- detail/RobotHeadMovement.cpp
- detail/Visu.cpp
-
- plugins/ObjectPoseProviderPlugin.cpp
- plugins/ObjectPoseClientPlugin.cpp
- plugins/RequestedObjects.cpp
-)
-set(HEADERS
- ObjectPoseObserver.h
-
- detail/Data.h
- detail/Decay.h
- detail/RobotHeadMovement.h
- detail/Visu.h
-
- plugins/ObjectPoseProviderPlugin.h
- plugins/ObjectPoseClientPlugin.h
- plugins/RequestedObjects.h
-)
-
-armarx_add_component("${SOURCES}" "${HEADERS}")
-
-
-# add unit tests
-add_subdirectory(test)
-
-# generate the application
-armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
deleted file mode 100644
index 62a56ebf248e9601ea4e281f84a0d043bef68a61..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ /dev/null
@@ -1,600 +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::ObjectPoseObserver
- * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "ObjectPoseObserver.h"
-
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/observers/variant/Variant.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotConfig.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/meta/EnumNames.hpp>
-
-
-namespace armarx
-{
-
- ObjectPoseObserverPropertyDefinitions::ObjectPoseObserverPropertyDefinitions(std::string prefix) :
- armarx::ObserverPropertyDefinitions(prefix)
- {
- }
-
- armarx::PropertyDefinitionsPtr ObjectPoseObserver::createPropertyDefinitions()
- {
- armarx::PropertyDefinitionsPtr defs(new ObjectPoseObserverPropertyDefinitions(getConfigIdentifier()));
-
- defs->defineOptionalProperty<std::string>("ObjectPoseTopicName", "ObjectPoseTopic", "Name of the Object Pose Topic.");
- defs->defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer.");
- defs->topic(debugObserver);
-
- calibration.defineProperties(defs, "calibration.");
- data.defineProperties(defs);
- robotHead.defineProperties(defs, "head.");
- visu.defineProperties(defs, "visu.");
-
- return defs;
- }
-
- std::string ObjectPoseObserver::getDefaultName() const
- {
- return "ObjectPoseObserver";
- }
-
- void ObjectPoseObserver::onInitObserver()
- {
- data.setTag(getName());
- data.decay.setTag(getName());
- robotHead.setTag(getName());
- visu.setTag(getName());
-
- usingTopicFromProperty("ObjectPoseTopicName");
- }
-
- void ObjectPoseObserver::onConnectObserver()
- {
- // onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
- // So we need to always make sure to guard a call to addRobot
- if (!RobotState::hasRobot("robot"))
- {
- data.robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
- }
- data.robotStateComponent = getRobotStateComponent();
-
- getProxyFromProperty(robotHead.kinematicUnitObserver, "KinematicUnitObserverName", false, "", false);
- robotHead.debugObserver = debugObserver;
- robotHead.fetchDatafields();
-
- visu.arviz = arviz;
- if (!visu.updateTask)
- {
- visu.updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
- visu.updateTask->start();
- }
-
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
- }
-
- void ObjectPoseObserver::onDisconnectComponent()
- {
- }
-
- void ObjectPoseObserver::onExitObserver()
- {
- }
-
-
- void ObjectPoseObserver::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
- {
- updateProviderInfo(providerName, info);
- }
-
-
- void ObjectPoseObserver::reportObjectPoses(
- const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
- {
- ARMARX_VERBOSE << "Received object " << providedPoses.size() << " poses from provider '" << providerName << "'.";
- updateObjectPoses(providerName, providedPoses);
- }
-
-
- void ObjectPoseObserver::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
- {
- if (!info.proxy)
- {
- ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
- << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
- return;
- }
- {
- std::scoped_lock lock(dataMutex);
- std::stringstream ss;
- for (const auto& id : info.supportedObjects)
- {
- ss << "- " << id << "\n";
- }
- ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
- << "Supported objects: \n" << ss.str();
- data.providers[providerName] = info;
- }
-
- if (!existsChannel(providerName))
- {
- offerChannel(providerName, "Channel of provider '" + providerName + "'.");
- }
- offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType),
- "The object type (known or unknown)");
- offerOrUpdateDataField(providerName, "numSupportedObjects", int(info.supportedObjects.size()),
- "Number of requestable objects.");
- }
-
-
- void ObjectPoseObserver::updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses)
- {
- TIMING_START(ReportObjectPoses);
-
- std::optional<IceUtil::Time> discardUpdatesUntil;
- bool discardAll = false;
- {
- std::scoped_lock lock(robotHeadMutex);
- if (robotHead.checkHeadVelocity)
- {
- if (robotHead.isMoving())
- {
- robotHead.movementStarts(robotHead.discardIntervalAfterMoveMS);
- // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moving! until " << robotHead.discardUpdatesUntil;
- discardAll = true;
- }
- else if (TimeUtil::GetTime() < robotHead.discardUpdatesUntil)
- {
- discardAll = true;
- // ARMARX_IMPORTANT << "Ignoring pose update because robot head has moved until: " << robotHead.discardUpdatesUntil;
- }
- else
- {
- discardUpdatesUntil = robotHead.discardUpdatesUntil;
- }
- }
- }
- if (debugObserver)
- {
- StringVariantBaseMap map;
- map["Discarding All Updates"] = new Variant(discardAll ? 1.f : 0.f);
- if (discardAll)
- {
- map["Proportion Updated Poses"] = new Variant(0.f);
- }
- debugObserver->setDebugChannel(getName(), map);
- }
-
- if (discardAll)
- {
- return;
- }
-
- {
- std::scoped_lock lock(dataMutex);
- RobotState::synchronizeLocalClone(data.robot);
-
- if (data.robot->hasRobotNode(calibration.robotNode))
- {
- VirtualRobot::RobotNodePtr robotNode = data.robot->getRobotNode(calibration.robotNode);
- float value = robotNode->getJointValue();
- robotNode->setJointValue(value + calibration.offset);
- }
-
- // This stays empty on the first report.
- objpose::ObjectPoseSeq previousPoses;
- if (auto it = data.objectPoses.find(providerName); it != data.objectPoses.end())
- {
- previousPoses = it->second;
- }
-
- // Build new poses.
- objpose::ObjectPoseSeq newObjectPoses;
- int numUpdated = 0;
- for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
- {
- IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
-
- // Check whether we have an old pose for this object.
- std::optional<objpose::ObjectPose> previousPose;
- for (const objpose::ObjectPose& prev : previousPoses)
- {
- if (prev.objectID == fromIce(provided.objectID))
- {
- previousPose = prev;
- }
- }
-
- if (discardUpdatesUntil && timestamp < *discardUpdatesUntil)
- {
- if (previousPose)
- {
- // Keep the old one
- newObjectPoses.push_back(*previousPose);
- }
- else
- {
- // Discard the new pose.
- // ARMARX_IMPORTANT << "Ignoring update of object " << provided.objectID << " because robot head is moved.\n"
- // << "timestamp " << timestamp << " < " << robotHead.discardUpdatesUntil;
- }
- }
- else if (previousPose && timestamp == previousPose->timestamp)
- {
- // Keep the old one.
- newObjectPoses.push_back(*previousPose);
- }
- else
- {
- numUpdated++;
- objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
- newPose.fromProvidedPose(provided, data.robot);
- if (newPose.objectID.dataset().empty())
- {
- // Try to find the data set. (It might be good to cache this.)
- if (std::optional<ObjectInfo> objectInfo = data.objectFinder.findObject(newPose.objectID))
- {
- newPose.objectID = { objectInfo->dataset(), newPose.objectID.className(), newPose.objectID.instanceName() };
- }
- }
- if (!provided.localOOBB)
- {
- // Try to load oobb from disk.
- newPose.localOOBB = data.getObjectOOBB(newPose.objectID);
- }
- }
- }
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "Discarding All Updates", new Variant(discardAll ? 1 : 0) },
- { "Proportion Updated Poses", new Variant(static_cast<float>(numUpdated) / providedPoses.size()) }
- });
- }
-
- data.objectPoses[providerName] = newObjectPoses;
- handleProviderUpdate(providerName);
-
- TIMING_END_STREAM(ReportObjectPoses, ARMARX_VERBOSE);
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "ReportObjectPoses [ms]", new Variant(ReportObjectPoses.toMilliSecondsDouble()) },
- });
- }
- }
- }
-
-
- void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
- {
- // Initialized to 0 on first access.
- if (data.providers.count(providerName) == 0)
- {
- data.providers[providerName] = objpose::ProviderInfo();
- }
-
- if (!existsChannel(providerName))
- {
- offerChannel(providerName, "Channel of provider '" + providerName + "'.");
- }
- offerOrUpdateDataField(providerName, "objectCount", Variant(int(data.objectPoses.at(providerName).size())), "Number of provided object poses.");
- }
-
-
- objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
- {
- TIMING_START(GetObjectPoses);
-
- TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(dataMutex);
- TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(data.getObjectPoses(now));
-
- TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "getObjectPoses() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- { "getObjectPoses() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
- });
- }
-
- return result;
- }
-
- objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
- {
- TIMING_START(GetObjectPoses);
-
- TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(dataMutex);
- TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- const objpose::data::ObjectPoseSeq result = objpose::toIce(data.getObjectPosesByProvider(providerName, now));
-
- TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "getObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- { "getObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
- });
- }
-
- return result;
- }
-
-
- objpose::observer::RequestObjectsOutput ObjectPoseObserver::requestObjects(
- const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
- {
- std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
- std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
-
- objpose::observer::RequestObjectsOutput output;
-
- auto updateProxy = [&](const std::string & providerName)
- {
- if (proxies.count(providerName) == 0)
- {
- if (auto it = data.providers.find(providerName); it != data.providers.end())
- {
- proxies[providerName] = it->second.proxy;
- }
- else
- {
- ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
- proxies[providerName] = nullptr;
- }
- }
- };
-
- if (input.provider.size() > 0)
- {
- providerRequests[input.provider] = input.request;
- updateProxy(input.provider);
- }
- else
- {
- std::scoped_lock lock(dataMutex);
- for (const auto& objectID : input.request.objectIDs)
- {
- bool found = true;
- for (const auto& [providerName, info] : data.providers)
- {
- // ToDo: optimize look up.
- if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
- {
- providerRequests[providerName].objectIDs.push_back(objectID);
- updateProxy(providerName);
- break;
- }
- }
- if (!found)
- {
- ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
- output.results[objectID].providerName = "";
- }
- }
- }
-
- for (const auto& [providerName, request] : providerRequests)
- {
- if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
- {
- ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
- << providerName << "' for " << request.relativeTimeoutMS << " ms.";
- objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
-
- int successful = 0;
- for (const auto& [objectID, result] : providerOutput.results)
- {
- objpose::observer::ObjectRequestResult& res = output.results[objectID];
- res.providerName = providerName;
- res.result = result;
- successful += int(result.success);
- }
- ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
- }
- }
- return output;
- }
-
- objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersInfo(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.providers;
- }
-
- Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return simox::alg::get_keys(data.providers);
- }
-
- objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.getProviderInfo(providerName);
- }
-
- bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.providers.count(providerName) > 0;
- }
-
-
- objpose::AttachObjectToRobotNodeOutput ObjectPoseObserver::attachObjectToRobotNode(
- const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.attachObjectToRobotNode(input);
- }
-
- objpose::DetachObjectFromRobotNodeOutput ObjectPoseObserver::detachObjectFromRobotNode(
- const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.detachObjectFromRobotNode(input);
- }
-
- objpose::DetachAllObjectsFromRobotNodesOutput ObjectPoseObserver::detachAllObjectsFromRobotNodes(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
- return data.detachAllObjectsFromRobotNodes();
- }
-
-
- objpose::AgentFramesSeq ObjectPoseObserver::getAttachableFrames(const Ice::Current&)
- {
- std::scoped_lock lock(dataMutex);
-
- objpose::AgentFramesSeq output;
- std::vector<VirtualRobot::RobotPtr> agents = { data.robot };
- for (VirtualRobot::RobotPtr agent : agents)
- {
- objpose::AgentFrames& frames = output.emplace_back();
- frames.agent = agent->getName();
- frames.frames = agent->getRobotNodeNames();
- }
- return output;
- }
-
- objpose::SignalHeadMovementOutput
- ObjectPoseObserver::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
- {
- std::scoped_lock lock(robotHeadMutex);
- return robotHead.signalHeadMovement(input);
- }
-
-
- void ObjectPoseObserver::visualizeRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
- while (visu.updateTask && !visu.updateTask->isStopped())
- {
- {
- std::scoped_lock lock(visuMutex);
-
- if (visu.enabled)
- {
- TIMING_START(Visu);
-
- std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
- ObjectFinder objectFinder;
- float minConfidence = -1;
- {
- std::scoped_lock lock(dataMutex);
-
- const IceUtil::Time now = TimeUtil::GetTime();
- data.updateObjectPoses(now);
- objectPoses = data.objectPoses;
- objectFinder = data.objectFinder;
- if (data.decay.enabled)
- {
- minConfidence = data.decay.removeObjectsBelowConfidence;
- }
- }
- const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, minConfidence, objectFinder);
- arviz.commit(layers);
-
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
- {
- { "Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- });
- }
- }
- }
- cycle.waitForCycleDuration();
- }
- }
-
-
- objpose::ObjectPoseProviderPrx ObjectPoseObserver::getProviderProxy(const std::string& providerName)
- {
- return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
- }
-
- void ObjectPoseObserver::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
-
- tab.visu.setup(this->visu);
- tab.decay.setup(this->data.decay);
- tab.robotHead.setup(this->robotHead);
-
- VBoxLayout root = {tab.visu.group, tab.decay.group, tab.robotHead.group, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
- }
-
- void ObjectPoseObserver::RemoteGui_update()
- {
- // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
- {
- std::scoped_lock lock(visuMutex);
- tab.visu.update(this->visu);
- }
- {
- std::scoped_lock lock(robotHeadMutex);
- tab.robotHead.update(this->robotHead);
- }
- {
- std::scoped_lock lock(dataMutex);
- tab.decay.update(this->data.decay);
- }
- }
-
- void ObjectPoseObserver::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
- defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
- }
-
-}
-
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
deleted file mode 100644
index 8fb3369ceaed1f4880037c741bd74f8e2eb5625e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ /dev/null
@@ -1,192 +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::ObjectPoseObserver
- * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-#include <mutex>
-
-#include <ArmarXCore/observers/Observer.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-
-#include <RobotAPI/components/ObjectPoseObserver/detail/Data.h>
-#include <RobotAPI/components/ObjectPoseObserver/detail/Decay.h>
-#include <RobotAPI/components/ObjectPoseObserver/detail/Visu.h>
-#include <RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h>
-
-#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-
-
-namespace armarx
-{
- /**
- * @class ObjectPoseObserverPropertyDefinitions
- * @brief Property definitions of `ObjectPoseObserver`.
- */
- class ObjectPoseObserverPropertyDefinitions :
- public ObserverPropertyDefinitions
- {
- public:
- ObjectPoseObserverPropertyDefinitions(std::string prefix);
- };
-
-
-
- /**
- * @defgroup Component-ObjectPoseObserver ObjectPoseObserver
- * @ingroup RobotAPI-Components
- * A description of the component ObjectPoseObserver.
- *
- * @class ObjectPoseObserver
- * @ingroup Component-ObjectPoseObserver
- * @brief Brief description of class ObjectPoseObserver.
- *
- * Detailed description of class ObjectPoseObserver.
- */
- class ObjectPoseObserver :
- virtual public Observer
- , virtual public objpose::ObjectPoseObserverInterface
- , virtual public armarx::RobotStateComponentPluginUser
- , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- , virtual public armarx::ArVizComponentPluginUser
- {
- class Data;
-
- public:
- using RobotState = armarx::RobotStateComponentPluginUser;
-
- /// @see armarx::ManagedIceObject::getDefaultName()
- std::string getDefaultName() const override;
-
-
- // ObjectPoseTopic interface
- public:
- void reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
- void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
-
- // ObjectPoseObserverInterface interface
- public:
-
- // OBJECT POSES
-
- objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
- objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
-
- // PROVIDER INFORMATION
-
- bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
- objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
- Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
- objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
-
-
- // REQUESTING
-
- objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
-
- // ATTACHING
-
- objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
- objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
- objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(ICE_CURRENT_ARG) override;
-
- objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
-
- // HEAD MOVEMENT SIGNALS
-
- objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
-
-
-
- // Remote GUI
- void createRemoteGuiTab();
- void RemoteGui_update() override;
-
-
- protected:
-
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
- void onInitObserver() override;
- void onConnectObserver() override;
-
- void onDisconnectComponent() override;
- void onExitObserver() override;
-
-
- private:
-
- void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
-
- void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
- void handleProviderUpdate(const std::string& providerName);
-
- objpose::ObjectPoseProviderPrx getProviderProxy(const std::string& providerName);
-
-
-
- // Visualization
-
- void visualizeRun();
-
-
- private:
-
- DebugObserverInterfacePrx debugObserver;
-
- objpose::observer::Data data;
- std::mutex dataMutex;
-
- objpose::observer::RobotHeadMovement robotHead;
- std::mutex robotHeadMutex;
-
- objpose::observer::Visu visu;
- std::mutex visuMutex;
-
-
- struct Calibration
- {
- std::string robotNode = "Neck_2_Pitch";
- float offset = 0.0f;
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
- };
- Calibration calibration;
-
-
- struct RemoteGuiTab : RemoteGui::Client::Tab
- {
- objpose::observer::Visu::RemoteGui visu;
- objpose::observer::Decay::RemoteGui decay;
- objpose::observer::RobotHeadMovement::RemoteGui robotHead;
- };
- RemoteGuiTab tab;
-
- };
-
-}
-
-#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
deleted file mode 100644
index be89f745d995ff82431d4b2fb285181a607730dc..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
+++ /dev/null
@@ -1,359 +0,0 @@
-#include "Data.h"
-
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
-
-#include <sstream>
-
-
-namespace armarx::objpose::observer
-{
-
- void Data::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- decay.defineProperties(defs, prefix + "decay.");
- }
-
- ObjectPoseSeq Data::getObjectPoses(IceUtil::Time now)
- {
- bool synchronized = false;
- ObjectPoseSeq result;
-
- for (auto& [providerName, objectPoses] : objectPoses)
- {
- // Update data.
- updateObjectPoses(objectPoses, now, robot, synchronized);
-
- // Collect results.
- for (const ObjectPose& objectPose : objectPoses)
- {
- if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
- {
- result.push_back(objectPose);
- }
- }
- }
- return result;
- }
-
-
- ObjectPoseSeq Data::getObjectPosesByProvider(
- const std::string& providerName,
- IceUtil::Time now)
- {
- bool synchronized = false;
-
- // Update data.
- ObjectPoseSeq& objectPoses = this->objectPoses.at(providerName);
- updateObjectPoses(objectPoses, now, robot, synchronized);
-
- // Collect results.
- ObjectPoseSeq result;
- for (const ObjectPose& objectPose : objectPoses)
- {
- if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
- {
- result.push_back(objectPose);
- }
- }
- return result;
- }
-
-
- void Data::updateObjectPoses(IceUtil::Time now)
- {
- bool synchronized = false;
- for (auto& [providerName, objectPoses] : objectPoses)
- {
- updateObjectPoses(objectPoses, now, robot, synchronized);
- }
- }
-
-
- void Data::updateObjectPoses(
- ObjectPoseSeq& objectPoses,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
- {
- for (ObjectPose& pose : objectPoses)
- {
- updateObjectPose(pose, now, agent, agentSynchronized);
- }
- }
-
-
- void Data::updateObjectPose(
- ObjectPose& objectPose,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
- {
- updateAttachement(objectPose, agent, agentSynchronized);
-
- if (decay.enabled)
- {
- decay.updateConfidence(objectPose, now);
- }
- }
-
-
- void Data::updateAttachement(
- ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
- {
- if (!objectPose.attachment)
- {
- // Fetch attachment info from internal data structure.
- auto it = attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
- if (it != attachments.end())
- {
- // Store it in the objectPose.
- objectPose.attachment = it->second;
- }
- else
- {
- // No attachment, nothing to do.
- return;
- }
- }
- ARMARX_CHECK(objectPose.attachment);
-
- if (!synchronized) // Synchronize only once.
- {
- RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
- synchronized = true;
- }
- objectPose.updateAttached(agent);
- }
-
-
- ObjectPose* Data::findObjectPose(const ObjectID& objectID, const std::string& providerName)
- {
- ObjectPose* pose = nullptr;
- if (!providerName.empty())
- {
- pose = findObjectPoseByID(objectPoses.at(providerName), objectID);
- }
- else
- {
- for (auto& [_, poses] : objectPoses)
- {
- pose = findObjectPoseByID(poses, objectID);
- if (pose)
- {
- break;
- }
- }
- }
- return pose;
- }
-
-
- std::optional<simox::OrientedBoxf> Data::getObjectOOBB(const ObjectID& id)
- {
- return oobbCache.get(id, [this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
- {
- // Try to get OOBB from repository.
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
- {
- try
- {
- 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;
- }
- }
- else
- {
- return std::nullopt;
- }
- });
- }
-
- ProviderInfo Data::getProviderInfo(const std::string& providerName)
- {
- try
- {
- return providers.at(providerName);
- }
- catch (const std::out_of_range&)
- {
- std::stringstream ss;
- ss << "No provider with name '" << providerName << "' available.\n";
- ss << "Available are:\n";
- for (const auto& [name, _] : providers)
- {
- ss << "- '" << name << "'\n";
- }
- throw std::out_of_range(ss.str());
- }
- }
-
-
-
- AttachObjectToRobotNodeOutput
- Data::attachObjectToRobotNode(const AttachObjectToRobotNodeInput& input)
- {
- AttachObjectToRobotNodeOutput output;
- output.success = false; // We are not successful until proven otherwise.
-
- ObjectID objectID = armarx::fromIce(input.objectID);
-
- if (input.agentName != "" && input.agentName != this->robot->getName())
- {
- ARMARX_WARNING << "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: " << std::vector<std::string> {this->robot->getName()};
- return output;
- }
- VirtualRobot::RobotPtr agent = this->robot;
-
- if (!agent->hasRobotNode(input.frameName))
- {
- ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
- << "' of agent '" << agent->getName() << "'.";
- return output;
- }
- std::string frameName = input.frameName;
-
-
- // Find object pose provider name can be empty.
- ObjectPose* currentObjectPose = this->findObjectPose(objectID, input.providerName);
- if (!currentObjectPose)
- {
- ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
- << "' of agent '" << agent->getName() << "', but object is currently not provided.";
- return output;
- }
-
- ObjectAttachmentInfo info;
- info.agentName = agent->getName();
- info.frameName = frameName;
-
- if (input.poseInFrame)
- {
- info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
- }
- else
- {
- RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
-
- armarx::FramedPose framed(currentObjectPose->objectPoseGlobal, armarx::GlobalFrame, agent->getName());
- if (frameName == armarx::GlobalFrame)
- {
- info.poseInFrame = framed.toGlobalEigen(this->robot);
- }
- else
- {
- framed.changeFrame(this->robot, info.frameName);
- info.poseInFrame = framed.toEigen();
- }
- }
- this->attachments[std::make_pair(currentObjectPose->providerName, objectID)] = info;
-
- ARMARX_INFO << "Attached object " << objectID << " by provider '" << currentObjectPose->providerName << "' "
- << "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
- << "Object pose in frame: \n" << info.poseInFrame;
-
- output.success = true;
- output.attachment = new data::ObjectAttachmentInfo();
- output.attachment->frameName = info.frameName;
- output.attachment->agentName = info.agentName;
- output.attachment->poseInFrame = new Pose(info.poseInFrame);
-
- return output;
- }
-
- DetachObjectFromRobotNodeOutput Data::detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& input)
- {
- ObjectID objectID = armarx::fromIce(input.objectID);
- std::string providerName = input.providerName;
-
- std::optional<ObjectAttachmentInfo> attachment;
- {
- // Remove from latest pose (if it was cached).
- ObjectPose* objectPose = this->findObjectPose(objectID, input.providerName);
- if (objectPose)
- {
- objectPose->attachment = std::nullopt;
- }
-
- if (providerName.empty() && objectPose)
- {
- providerName = objectPose->providerName;
- }
- // Remove from attachment map.
- if (input.providerName.size() > 0)
- {
- auto it = this->attachments.find(std::make_pair(input.providerName, objectID));
- if (it != this->attachments.end())
- {
- attachment = it->second;
- this->attachments.erase(it);
- }
- }
- else
- {
- // Search for entry with matching object ID.
- for (auto it = this->attachments.begin(); it != this->attachments.end(); ++it)
- {
- const ObjectID& id = it->first.second;
- if (id == objectID)
- {
- attachment = it->second;
- this->attachments.erase(it);
- break;
- }
- }
- }
- }
-
- DetachObjectFromRobotNodeOutput output;
- output.wasAttached = bool(attachment);
- if (attachment)
- {
- 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 << "' "
- << "from robot node, but it was not attached.";
- }
-
- return output;
- }
-
- DetachAllObjectsFromRobotNodesOutput Data::detachAllObjectsFromRobotNodes()
- {
- DetachAllObjectsFromRobotNodesOutput output;
- output.numDetached = int(this->attachments.size());
-
- // Clear attachment map.
- this->attachments.clear();
-
- // Remove from poses (if it was cached).
- for (auto& [prov, poses] : this->objectPoses)
- {
- for (auto& pose : poses)
- {
- pose.attachment = std::nullopt;
- }
- }
-
- ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
-
- return output;
- }
-
-
-}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
deleted file mode 100644
index e825a95340f71e4357392433ae16f8c3fec1ee30..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
+++ /dev/null
@@ -1,98 +0,0 @@
-#pragma once
-
-#include <map>
-#include <string>
-#include <optional>
-
-#include <SimoxUtility/caching/CacheMap.h>
-#include <SimoxUtility/shapes/OrientedBox.h>
-
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-#include "Decay.h"
-
-
-namespace armarx::objpose::observer
-{
-
- /**
- * @brief Models decay of object localizations by decreasing the confidence
- * the longer the object was not localized.
- */
- class Data : public armarx::Logging
- {
- public:
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
- ObjectPoseSeq getObjectPoses(IceUtil::Time now);
- ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
-
- ObjectPose* findObjectPose(const ObjectID& objectID, const std::string& providerName = "");
- std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
-
- ProviderInfo getProviderInfo(const std::string& providerName);
-
-
- AttachObjectToRobotNodeOutput attachObjectToRobotNode(const AttachObjectToRobotNodeInput& input);
- DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& input);
- DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
-
-
- void updateObjectPoses(IceUtil::Time now);
- void updateObjectPoses(
- ObjectPoseSeq& objectPoses,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
- void updateObjectPose(
- ObjectPose& objectPose,
- IceUtil::Time now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
-
- /**
- * @brief If the object is attached to a robot node, update it according to the current robot state.
- *
- * If there is no attachement info in `objectPose` itself, the internal data
- * structure `attachments` is queried. If an attachment is found there,
- * it is written into the given `objectPose` (thus, it is "cached" in the
- * info `objectPose`).
- *
- * @param synchronized Indicates whether the agent is already synchronized to the current time.
- */
- void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
- bool& synchronized) const;
-
-
- public:
-
- RobotStateComponentInterfacePrx robotStateComponent;
- VirtualRobot::RobotPtr robot;
-
- ProviderInfoMap providers;
-
- std::map<std::string, ObjectPoseSeq> objectPoses;
-
- std::map<std::pair<std::string, ObjectID>, ObjectAttachmentInfo> attachments;
-
-
- ObjectFinder objectFinder;
- /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
- simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
-
- /// Decay model.
- Decay decay;
-
- };
-
-}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
index 738482cc1c41cb2c8a040ec0aa53206e88c2d427..9eb2a0dac0aadc72946c163c9d79148f799e5aaf 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
@@ -1,76 +1,5 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#pragma message("This header is deprecated. Use <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h> instead.")
-#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
-namespace armarx::plugins
-{
- class ObjectPoseClientPlugin : public ComponentPlugin
- {
- public:
- using ComponentPlugin::ComponentPlugin;
-
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
- objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
-
- template<class...Ts>
- std::optional<ObjectInfo> findObject(Ts&& ...ts) const
- {
- return _finder.findObject(std::forward<Ts>(ts)...);
- }
-
- template<class...Ts>
- VirtualRobot::ManipulationObjectPtr
- findAndLoadObject(Ts&& ...ts) const
- {
- return findAndLoadObject(findObject(std::forward<Ts>(ts)...));
- }
- VirtualRobot::ManipulationObjectPtr
- findAndLoadObject(const std::optional<ObjectInfo>& ts) const
- {
- return _finder.loadManipulationObject(ts);
- }
-
- const ObjectFinder& setObjectFinderPath(const std::string& path);
- const ObjectFinder& getObjectFinder() const;
-
- private:
- void preOnInitComponent() override;
- void preOnConnectComponent() override;
-
- static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
-
- ObjectFinder _finder;
- };
-}
-
-namespace armarx
-{
- /**
- * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
- */
- class ObjectPoseClientPluginUser :
- virtual public ManagedIceObject
- {
- public:
- /// Allow usage like: ObjectPoseClient::getObjects()
- using ObjectPoseClient = ObjectPoseClientPluginUser;
-
- ObjectPoseClientPluginUser();
-
- objpose::ObjectPoseObserverInterfacePrx createObjectPoseObserver();
- objpose::ObjectPoseObserverInterfacePrx objectPoseObserver;
-
- objpose::ObjectPoseSeq getObjectPoses();
-
- plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin();
- const plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin() const;
-
- const ObjectFinder& getObjectFinder() const;
- private:
- armarx::plugins::ObjectPoseClientPlugin* plugin = nullptr;
- };
-}
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
index 332064711b688d0e81969f3d1cc2ced472d817b1..55f6366e85ef20f50833f0889c6cf4b88c3614d2 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
@@ -1,60 +1,5 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#pragma message("This header is deprecated. Use <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h> instead.")
-#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
-
-
-namespace armarx::plugins
-{
-
- class ObjectPoseProviderPlugin : public ComponentPlugin
- {
- public:
- using ComponentPlugin::ComponentPlugin;
-
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
-
- void preOnInitComponent() override;
- void preOnConnectComponent() override;
- void postOnConnectComponent() override;
-
- objpose::ObjectPoseTopicPrx createObjectPoseTopic();
-
-
- private:
-
- static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
-
- };
-
-}
-
-
-namespace armarx
-{
-
- /**
- * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
- */
- class ObjectPoseProviderPluginUser :
- virtual public ManagedIceObject
- , virtual public objpose::ObjectPoseProvider
- {
- public:
-
- ObjectPoseProviderPluginUser();
-
- /// Implement to process object requests (empty default implementation).
- objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
-
- objpose::ObjectPoseTopicPrx createObjectPoseTopic();
-
- objpose::ObjectPoseTopicPrx objectPoseTopic;
-
- private:
-
- armarx::plugins::ObjectPoseProviderPlugin* plugin = nullptr;
-
- };
-}
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
diff --git a/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt
deleted file mode 100644
index 878921745f8b8cfd10cc67cbcc594d2199c84ea3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore ObjectPoseObserver)
-
-armarx_add_test(ObjectPoseObserverTest ObjectPoseObserverTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp b/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
deleted file mode 100644
index 903d51bfb830771b2b70b0ef4bdacc414ecee3ed..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseObserver/test/ObjectPoseObserverTest.cpp
+++ /dev/null
@@ -1,68 +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::ObjectPoseObserver
- * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ObjectPoseObserver
-
-#define ARMARX_BOOST_TEST
-
-#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/core/Pose.h>
-
-#include <iostream>
-
-using namespace armarx;
-
-
-BOOST_AUTO_TEST_CASE(test_from_to_OOBB)
-{
- Eigen::Vector3f pos(-100, -200, -300);
- Eigen::Matrix3f ori = Eigen::AngleAxisf(1.0, Eigen::Vector3f(1, 2, 3).normalized()).toRotationMatrix();
- Eigen::Vector3f extents(40, 50, 60);
-
- armarx::objpose::Box box;
- box.position = new Vector3(pos);
- box.orientation = new Quaternion(ori);
- box.extents = new Vector3(extents);
-
-
- const float prec = 1e-3;
-
- simox::OrientedBoxf oobb;
- armarx::objpose::fromIce(box, oobb);
- ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec);
- ARMARX_CHECK(oobb.rotation().isApprox(ori, prec));
- ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec);
-
- armarx::objpose::Box boxOut;
- armarx::objpose::toIce(boxOut, oobb);
-
- Eigen::Vector3f posOut = Vector3Ptr::dynamicCast(boxOut.position)->toEigen();
- Eigen::Matrix3f oriOut = QuaternionPtr::dynamicCast(boxOut.orientation)->toEigen();
- Eigen::Vector3f extentsOut = Vector3Ptr::dynamicCast(boxOut.extents)->toEigen();
-
- ARMARX_CHECK_LESS_EQUAL((posOut - pos).norm(), prec);
- ARMARX_CHECK(oriOut.isApprox(ori, prec));
- ARMARX_CHECK_LESS_EQUAL((extentsOut - extents).norm(), prec);
-}
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
index fae15ee0afc1b1577bf808383a75faaa2d814ec3..1395fc02f564792a7ffe7e0963264f5c0ac6fb9e 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/CMakeLists.txt
@@ -2,10 +2,10 @@ armarx_component_set_name("ObjectPoseProviderExample")
set(COMPONENT_LIBS
- ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
- # RobotAPICore RobotAPIInterfaces
- # RobotAPIComponentPlugins # for ArViz and other plugins
- RobotAPIArmarXObjects ObjectPoseObserver
+ # ArmarXCore
+ ArmarXCore ArmarXCoreInterfaces
+ # RobotAPI
+ RobotAPI::ArmarXObjects
)
set(SOURCES
@@ -18,16 +18,6 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-# all target_include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# target_include_directories(ObjectPoseProviderExample PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
-
-# add unit tests
-add_subdirectory(test)
#generate the application
armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index 590bad8d596e06666405a3a631b78a790263cd10..f759697348f61927af7df5c9297e2081b0c1db91 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -30,16 +30,13 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
namespace armarx
{
- ObjectPoseProviderExamplePropertyDefinitions::ObjectPoseProviderExamplePropertyDefinitions(std::string prefix) :
- armarx::ComponentPropertyDefinitions(prefix)
- {
- }
armarx::PropertyDefinitionsPtr ObjectPoseProviderExample::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ObjectPoseProviderExamplePropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
index aac11a5f1c9fd6d8a920a85166495e29346e6772..7ed814eb06c219f3280f1d5d4e9473ef56344fb0 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
@@ -31,26 +31,14 @@
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
// Include the ProviderPlugin
-#include <RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h>
-#include <RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
namespace armarx
{
- /**
- * @class ObjectPoseProviderExamplePropertyDefinitions
- * @brief Property definitions of `ObjectPoseProviderExample`.
- */
- class ObjectPoseProviderExamplePropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
- {
- public:
- ObjectPoseProviderExamplePropertyDefinitions(std::string prefix);
- };
-
-
/**
* @defgroup Component-ObjectPoseProviderExample ObjectPoseProviderExample
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt
deleted file mode 100644
index e65a2f31bc9150164b5e5ed6fc0e93227f5176ce..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/ObjectPoseProviderExample/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore ${ARMARX_COMPONENT_LIB_NAME})
-
-armarx_add_test(ObjectPoseProviderExampleTest ObjectPoseProviderExampleTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/armem/CMakeLists.txt b/source/RobotAPI/components/armem/CMakeLists.txt
index b80992aeaf1f03968a7b229d600def905796d1f7..526448e132f6e282b3d3f14fd39132fe32017ebb 100644
--- a/source/RobotAPI/components/armem/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/CMakeLists.txt
@@ -1,8 +1,5 @@
# memory servers
-add_subdirectory(server/ExampleMemory)
-add_subdirectory(server/GeneralPurposeMemory)
-add_subdirectory(server/RobotSensorMemory)
-add_subdirectory(server/SkillsMemory)
+add_subdirectory(server)
# memory server addons
diff --git a/source/RobotAPI/components/armem/server/CMakeLists.txt b/source/RobotAPI/components/armem/server/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..83fd40e2dee27f8459c028474b01f178218f752f
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/CMakeLists.txt
@@ -0,0 +1,5 @@
+add_subdirectory(ExampleMemory)
+add_subdirectory(GeneralPurposeMemory)
+add_subdirectory(ObjectMemory)
+add_subdirectory(RobotSensorMemory)
+add_subdirectory(SkillsMemory)
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index a3c7f7bc8c31e0c6c8ff2af96ebd36412521ee00..8bd030bb8883e73910b182d7aad66feda0dab96f 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -60,16 +60,16 @@ namespace armarx
void ExampleMemory::onInitComponent()
{
- workingmemory.name() = p.memoryName;
+ workingMemory.name() = p.memoryName;
// Usually, the memory server will specify a number of core segments with a specific aron type.
- workingmemory.addCoreSegment("ExampleData", armem::example::ExampleData::toInitialAronType());
+ workingMemory.addCoreSegment("ExampleData", armem::example::ExampleData::toInitialAronType());
// For illustration purposes, we add more segments (without types).
bool trim = true;
p.core.defaultCoreSegments = simox::alg::split(p.core._defaultSegmentsStr, ",", trim);
p.core._defaultSegmentsStr.clear();
- workingmemory.addCoreSegments(p.core.defaultCoreSegments);
+ workingMemory.addCoreSegments(p.core.defaultCoreSegments);
}
void ExampleMemory::onConnectComponent()
@@ -121,8 +121,8 @@ namespace armarx
using namespace armarx::RemoteGui::Client;
{
- std::scoped_lock lock(workingmemoryMutex);
- tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingmemory);
+ std::scoped_lock lock(workingMemoryMutex);
+ tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory);
}
VBoxLayout root = {tab.memoryGroup, VSpacer()};
diff --git a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
index 8247ab6f9d1ff22baecefb6146a14ea6866c8d93..3b17f4fc879dfb49fd1b816f8e4b0e450e3f926f 100644
--- a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
@@ -50,7 +50,7 @@ namespace armarx
void GeneralPurposeMemory::onInitComponent()
{
- workingmemory.name() = memoryName;
+ workingMemory.name() = memoryName;
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt
index 28ade93141eef153f7ed6dd8972cc0adda2b9f3b..1c90c244bf872fdc4931e0b9067a92c189d9b694 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/CMakeLists.txt
@@ -2,12 +2,16 @@ armarx_component_set_name("ObjectMemory")
set(COMPONENT_LIBS
- ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ # ArmarXCore
+ ArmarXCore ArmarXCoreInterfaces
+ # ArmarXGui
ArmarXGuiComponentPlugins
- RobotAPICore RobotAPIInterfaces armem
- # RobotAPIComponentPlugins # for ArViz and other plugins
+ # RobotAPI
+ RobotAPI::ComponentPlugins
+ RobotAPI::armem_objects
- ${IVT_LIBRARIES}
+ # This project
+ ${PROJECT_NAME}Interfaces
)
set(SOURCES
@@ -19,8 +23,11 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
-#generate the application
-armarx_generate_and_add_component_executable()
-
+# add unit tests
+# add_subdirectory(test)
+# generate the application
+armarx_generate_and_add_component_executable(
+ COMPONENT_NAMESPACE ::armarx::armem::server::obj
+)
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 93977f9d9a04181f9b22c24d0c6639571d591412..18ccb6875d1611e05cf20a8fa714dd293df995e6 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -15,78 +15,146 @@
*
* @package RobotAPI::ArmarXObjects::ObjectMemory
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
+ * @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#include "ObjectMemory.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <SimoxUtility/algorithm/string.h>
+namespace armarx::armem::server::obj
+{
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+ const std::string ObjectMemory::defaultMemoryName = "Object";
-namespace armarx
-{
- ObjectMemory::ObjectMemory()
- {
- }
armarx::PropertyDefinitionsPtr ObjectMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
+
+ // Offer
+ defs->topic(debugObserver);
+
+ // Subscribe
+ defs->topic<objpose::ObjectPoseTopic>(); // "ObjectPoseTopic", "ObjectPoseTopicName", "Name of the Object Pose Topic.");
+
+ // Use
+ // defs->component(kinematicUnitObserver); // Optional dependency.
+ defs->defineOptionalProperty<std::string>("cmp.KinematicUnitObserverName", "KinematicUnitObserver",
+ "Name of the kinematic unit observer.");
+
+ const std::string prefix = "mem.";
+
+ workingMemory.name() = defaultMemoryName;
+ defs->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
+
+ classSegment.defineProperties(defs, prefix + "cls.");
+ instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
+
return defs;
}
+ ObjectMemory::ObjectMemory() :
+ server::ComponentPluginUser(),
+ instance::SegmentAdapter(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex),
+ classSegment(server::ComponentPluginUser::iceMemory,
+ server::ComponentPluginUser::workingMemoryMutex)
+ {
+ }
std::string ObjectMemory::getDefaultName() const
{
return "ObjectMemory";
}
-
void ObjectMemory::onInitComponent()
{
- memory.name() = memoryName;
+ workingMemory.name() = defaultMemoryName;
+
+ instance::SegmentAdapter::init();
+
+ try
+ {
+ classSegment.init();
+ }
+ catch (const LocalException& e)
+ {
+ ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Failed to init class segment for unknown reason.";
+ }
}
-
void ObjectMemory::onConnectComponent()
{
+ // onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
+ // So we need to always make sure to guard a call to addRobot
+ const std::string robotKey = "robot";
+ VirtualRobot::RobotPtr robot = RobotState::hasRobot(robotKey)
+ ? RobotState::getRobot(robotKey)
+ : RobotState::addRobot(robotKey, VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ robotStateComponent = RobotState::getRobotStateComponent();
+
+ getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
+
+ instance::SegmentAdapter::connect(
+ robotStateComponent, robot,
+ kinematicUnitObserver,
+ ArVizComponentPluginUser::arviz,
+ debugObserver
+ );
+ classSegment.connect(
+ ArVizComponentPluginUser::arviz
+ );
+
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
}
-
void ObjectMemory::onDisconnectComponent()
{
}
-
void ObjectMemory::onExitComponent()
{
}
-
- // WRITING
- armem::data::AddSegmentsResult ObjectMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
+ void ObjectMemory::createRemoteGuiTab()
{
- armem::data::AddSegmentsResult result = ComponentPluginUser::addSegments(input, addCoreSegmentOnUsage);
- return result;
+ using namespace armarx::RemoteGui::Client;
+
+ tab.instance.setup(*this);
+ tab.clazz.setup(classSegment);
+
+ HBoxLayout segments =
+ {
+ tab.instance.group,
+ tab.clazz.group
+ };
+ VBoxLayout root =
+ {
+ segments,
+ VSpacer()
+ };
+ RemoteGui_createTab(Component::getName(), root, &tab);
}
-
- armem::data::CommitResult ObjectMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
+ void ObjectMemory::RemoteGui_update()
{
- armem::data::CommitResult result = ComponentPluginUser::commit(commit);
- return result;
+ // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
+ tab.instance.update(*this);
+ tab.clazz.update(classSegment);
}
-
- // READING
- // Inherited from Plugin
-
-
}
+
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index ca194dfbf6c2f0c3b527a2170aa1308f883780ae..65bd9c472828faa2b8b7f58679c4922c1aef389d 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -15,25 +15,37 @@
*
* @package RobotAPI::ArmarXObjects::ObjectMemory
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2020
+ * @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#pragma once
+#include <memory>
+#include <mutex>
-#include <ArmarXCore/core/Component.h>
+#include <VirtualRobot/VirtualRobot.h>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
+
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
+
+
+#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-namespace armarx
+
+namespace armarx::armem::server::obj
{
+
/**
* @defgroup Component-ObjectMemory ObjectMemory
* @ingroup RobotAPI-Components
@@ -46,41 +58,66 @@ namespace armarx
* Detailed description of class ObjectMemory.
*/
class ObjectMemory :
- virtual public armarx::Component
- , virtual public armem::server::ComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
+ virtual public Component
+
+ , virtual public armarx::armem::server::ObjectMemoryInterface
+ , virtual public armarx::armem::server::ComponentPluginUser
+ , virtual public armarx::armem::server::obj::instance::SegmentAdapter
+
+ , virtual public armarx::RobotStateComponentPluginUser
+ , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
{
public:
+
+ using RobotState = armarx::RobotStateComponentPluginUser;
+
+ static const std::string defaultMemoryName;
+
+
+ public:
+
ObjectMemory();
+
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
- // WritingInterface interface
+
public:
- armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- protected:
- /// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
-
- /// @see armarx::ManagedIceObject::onConnectComponent()
void onConnectComponent() override;
- /// @see armarx::ManagedIceObject::onDisconnectComponent()
void onDisconnectComponent() override;
-
- /// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ // Remote GUI
+ void createRemoteGuiTab();
+ void RemoteGui_update() override;
private:
- std::string memoryName = "ObjectMemory";
- bool addCoreSegmentOnUsage = true;
+
+ DebugObserverInterfacePrx debugObserver;
+ RobotStateComponentInterfacePrx robotStateComponent;
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver;
+
+ clazz::Segment classSegment;
+
+
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ instance::SegmentAdapter::RemoteGui instance;
+ clazz::Segment::RemoteGui clazz;
+ };
+ RemoteGuiTab tab;
+
};
+
}
+
+#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/test/CMakeLists.txt b/source/RobotAPI/components/armem/server/ObjectMemory/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5f5ecdf18b6c4abf1aa08213ee23c920491cf01b
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ObjectMemory)
+
+armarx_add_test(ObjectMemoryTest ObjectMemoryTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
similarity index 71%
rename from source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
rename to source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
index d173c24a7a110c93d94a95d3df6b7c1d1a6d5e78..40329162efb831da65f1dcd1671dc458e5d6d1b0 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/test/ObjectPoseProviderExampleTest.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
@@ -13,25 +13,28 @@
* 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::ObjectPoseProviderExample
+ * @package RobotAPI::ArmarXObjects::ObjectMemory
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
* @date 2020
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ObjectPoseProviderExample
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::ObjectMemory
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h>
+#include <RobotAPI/components/ObjectMemory/ObjectMemory.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include <iostream>
-BOOST_AUTO_TEST_CASE(testExample)
-{
- armarx::ObjectPoseProviderExample instance;
+using namespace armarx;
- BOOST_CHECK_EQUAL(true, true);
+
+BOOST_AUTO_TEST_CASE(test_ObjectMemory)
+{
}
diff --git a/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp b/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp
index 2293364c6cf55555b40c86b3b9d1490783112e09..a719b42bf112d6d7a58f06066dee3b96efabae83 100644
--- a/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotSensorMemory/RobotSensorMemory.cpp
@@ -71,7 +71,7 @@ namespace armarx
robotStateComponentMemoryBatchSize = std::max((unsigned int) 1, robotStateComponentMemoryBatchSize);
robotStateComponentPollFrequency = std::clamp(robotStateComponentPollFrequency, 1, ROBOT_STATE_COMPONENT_MAXIMUM_FREQUENCY);
- workingmemory.name() = workingMemoryName;
+ workingMemory.name() = workingMemoryName;
}
@@ -114,7 +114,7 @@ namespace armarx
void RobotSensorMemory::setupRobotUnitSegment()
{
ARMARX_INFO << "Adding core segment " << robotUnitCoreSegmentName;
- workingmemory.addCoreSegments({robotUnitCoreSegmentName});
+ workingMemory.addCoreSegments({robotUnitCoreSegmentName});
ARMARX_INFO << "Adding provider segment " << robotUnitCoreSegmentName << "/" << robotUnitProviderSegmentName;
armem::data::AddSegmentInput input;
@@ -379,7 +379,7 @@ namespace armarx
void RobotSensorMemory::setupRobotStateComponentSegment()
{
ARMARX_INFO << "Adding core segment " << robotStateComponentCoreSegmentName;
- workingmemory.addCoreSegments({robotStateComponentCoreSegmentName});
+ workingMemory.addCoreSegments({robotStateComponentCoreSegmentName});
ARMARX_INFO << "Adding provider segment " << robotStateComponentCoreSegmentName << "/" << robotStateComponentProviderSegmentName;
armem::data::AddSegmentInput input;
diff --git a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
index 4efdcbff7b43ca8a168ba17d0970b1b6a5781d10..295e2c6aee2010162c6919af97141f28989b6040 100644
--- a/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SkillsMemory/SkillsMemory.cpp
@@ -50,7 +50,7 @@ namespace armarx
void SkillsMemory::onInitComponent()
{
- workingmemory.name() = memoryName;
+ workingMemory.name() = memoryName;
}
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
index f1ccdd08058d73791e7a832a8dcb8ee645a3c75b..855c88fb33c089de09965fd69b34c0537ec8f956 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/CMakeLists.txt
@@ -27,7 +27,7 @@ set(GUI_UIS
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
set(COMPONENT_LIBS
SimpleConfigDialog
- ObjectPoseObserver
+ RobotAPI::armem_objects
)
if(ArmarXGui_FOUND)
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index 80404797fa4176aff345a9acc6946024ff04574b..dffe7711debc728d596f5ecd7cd851df2527e120 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -99,14 +99,14 @@ namespace armarx
return "MemoryX.ObjectPoseGui";
}
- static const std::string CONFIG_KEY_OBJECT_POSE_OBSERVER = "ObjectPoseObserver";
+ static const std::string CONFIG_KEY_OBJECT_POSE_OBSERVER = "ObjectPoseStorage";
QPointer<QDialog> ObjectPoseGuiWidgetController::getConfigDialog(QWidget* parent)
{
if (!configDialog)
{
configDialog = new SimpleConfigDialog(parent);
- configDialog->addProxyFinder<armarx::objpose::ObjectPoseObserverInterfacePrx>({CONFIG_KEY_OBJECT_POSE_OBSERVER, "Object pose observer.", "ObjectPoseObserver"});
+ configDialog->addProxyFinder<armarx::objpose::ObjectPoseStorageInterfacePrx>({CONFIG_KEY_OBJECT_POSE_OBSERVER, "Object pose observer.", "*"});
}
return qobject_cast<QDialog*>(configDialog);
}
@@ -115,26 +115,26 @@ namespace armarx
{
if (configDialog)
{
- objectPoseObserverName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_OBSERVER);
+ ObjectPoseStorageName = configDialog->getProxyName(CONFIG_KEY_OBJECT_POSE_OBSERVER);
}
}
void ObjectPoseGuiWidgetController::onInitComponent()
{
- if (!objectPoseObserverName.empty())
+ if (!ObjectPoseStorageName.empty())
{
- usingProxy(objectPoseObserverName);
+ usingProxy(ObjectPoseStorageName);
}
}
void ObjectPoseGuiWidgetController::onConnectComponent()
{
- if (!objectPoseObserverName.empty())
+ if (!ObjectPoseStorageName.empty())
{
- getProxy(objectPoseObserver, objectPoseObserverName);
+ getProxy(ObjectPoseStorage, ObjectPoseStorageName);
}
- this->attachableFrames = objectPoseObserver->getAttachableFrames();
+ this->attachableFrames = ObjectPoseStorage->getAttachableFrames();
std::sort(attachableFrames.begin(), attachableFrames.end(), [](const auto & lhs, const auto & rhs)
{
return lhs.agent < rhs.agent;
@@ -147,7 +147,7 @@ namespace armarx
void ObjectPoseGuiWidgetController::onDisconnectComponent()
{
- objectPoseObserver = nullptr;
+ ObjectPoseStorage = nullptr;
}
void ObjectPoseGuiWidgetController::updateTab()
@@ -166,7 +166,7 @@ namespace armarx
void ObjectPoseGuiWidgetController::updateObjectsTab()
{
- if (!objectPoseObserver)
+ if (!ObjectPoseStorage)
{
// Probably disconnected.
ARMARX_VERBOSE << "No object pose observer.";
@@ -175,7 +175,7 @@ namespace armarx
IceUtil::Time start = IceUtil::Time::now();
ARMARX_VERBOSE << "Getting object poses...";
- const objpose::data::ObjectPoseSeq objectPosesIce = objectPoseObserver->getObjectPoses();
+ const objpose::data::ObjectPoseSeq objectPosesIce = ObjectPoseStorage->getObjectPoses();
ARMARX_VERBOSE << "Got " << objectPosesIce.size() << " object poses. "
<< "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
@@ -262,7 +262,7 @@ namespace armarx
void ObjectPoseGuiWidgetController::updateRequestTab()
{
- if (!objectPoseObserver)
+ if (!ObjectPoseStorage)
{
// Probably disconnected.
ARMARX_VERBOSE << "No object pose observer.";
@@ -270,7 +270,7 @@ namespace armarx
}
IceUtil::Time start = IceUtil::Time::now();
- objpose::ProviderInfoMap availableProvidersInfo = objectPoseObserver->getAvailableProvidersInfo();
+ objpose::ProviderInfoMap availableProvidersInfo = ObjectPoseStorage->getAvailableProvidersInfo();
ARMARX_VERBOSE << "Got infos of " << availableProvidersInfo.size() << " object pose providers. "
<< "(Took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
@@ -393,8 +393,17 @@ namespace armarx
input.agentName = agentName;
input.frameName = frameName;
- objpose::AttachObjectToRobotNodeOutput output = objectPoseObserver->attachObjectToRobotNode(input);
- ARMARX_VERBOSE << "Success of attaching: " << output.success;
+ try
+ {
+ objpose::AttachObjectToRobotNodeOutput output = ObjectPoseStorage->attachObjectToRobotNode(input);
+ ARMARX_VERBOSE << "Success of attaching: " << output.success;
+ }
+ catch (const IceUtil::Exception& e)
+ {
+ ARMARX_WARNING << "Failed to attach object '" << input.objectID << "' to robot node '"
+ << input.frameName << "' of agent '" << input.agentName << "'."
+ << "\nReason: " << e.what();
+ }
}
void ObjectPoseGuiWidgetController::detachObjectFromRobotNode(QString providerName, QString objectID)
@@ -405,8 +414,16 @@ namespace armarx
input.providerName = providerName.toStdString();
input.objectID = armarx::toIce(armarx::ObjectID(objectID.toStdString()));
- objpose::DetachObjectFromRobotNodeOutput output = objectPoseObserver->detachObjectFromRobotNode(input);
- ARMARX_VERBOSE << "Was attached: " << output.wasAttached;
+ try
+ {
+ 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."
+ << "\nReason: " << e.what();
+ }
}
void ObjectPoseGuiWidgetController::requestSelectedObjects()
@@ -446,7 +463,7 @@ namespace armarx
ARMARX_INFO << "Requesting " << request.request.objectIDs.size() << " objects for "
<< request.request.relativeTimeoutMS << " ms.";
- objpose::observer::RequestObjectsOutput output = objectPoseObserver->requestObjects(request);
+ objpose::observer::RequestObjectsOutput output = ObjectPoseStorage->requestObjects(request);
int successful = 0;
for (const auto& [id, result] : output.results)
{
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
index 84a0e6e964ffdd2cead4f144627f478f41c94d10..5e9910bd8185cb13adbd54435758ee871dbd8460 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.h
@@ -29,7 +29,7 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
namespace armarx
@@ -115,8 +115,8 @@ namespace armarx
QPointer<SimpleConfigDialog> configDialog;
- std::string objectPoseObserverName;
- armarx::objpose::ObjectPoseObserverInterfacePrx objectPoseObserver;
+ std::string ObjectPoseStorageName;
+ armarx::objpose::ObjectPoseStorageInterfacePrx ObjectPoseStorage;
objpose::AgentFramesSeq attachableFrames;
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 3db350c9e2d32e79214795948a5c42670f7b5eb6..b2913fb4c0f39035f94b284a52ac7f32295f73b8 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -36,7 +36,7 @@ set(SLICE_FILES
ArmarXObjects/ArmarXObjectsTypes.ice
objectpose/object_pose_types.ice
- objectpose/ObjectPoseObserver.ice
+ objectpose/ObjectPoseStorageInterface.ice
objectpose/ObjectPoseProvider.ice
units/MultiHandUnitInterface.ice
@@ -122,6 +122,9 @@ set(SLICE_FILES
armem/server/ReadingMemoryInterface.ice
armem/server/WritingMemoryInterface.ice
+ # Special Servers
+ armem/server/ObjectMemoryInterface.ice
+
armem/mns.ice
armem/mns/MemoryNameSystemInterface.ice
diff --git a/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice b/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..32e120ae8fdabfbb7b49545a86623f281b6ee159
--- /dev/null
+++ b/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
@@ -0,0 +1,54 @@
+/**
+* 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 as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser 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
+* @author Rainer Kartmann
+* @copyright 2020 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice>
+
+
+module armarx
+{
+ module armem
+ {
+ module server
+ {
+
+ interface ObjectInstanceSegmentInterface extends
+ armarx::objpose::ObjectPoseStorageInterface
+ {
+
+ };
+
+ interface ObjectMemoryInterface extends
+ MemoryInterface
+ , ObjectInstanceSegmentInterface
+ {
+
+ };
+
+ };
+ };
+};
+
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice b/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
similarity index 89%
rename from source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
rename to source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
index 068cba470558e175be65190fb8666c80d6cc2d5a..3097f6c7596fd3fdf82aedd9d974712081cf503c 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
@@ -27,6 +27,8 @@
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
+
#include <RobotAPI/interface/objectpose/object_pose_types.ice>
#include <RobotAPI/interface/objectpose/ObjectPoseProvider.ice>
@@ -87,12 +89,27 @@ module armarx
{
string providerName;
armarx::data::ObjectID objectID;
+
+ /**
+ * @brief If true, the object will stay at the position before
+ * detaching until it is provided again.
+ */
+ bool commitAttachedPose = true;
};
struct DetachObjectFromRobotNodeOutput
{
/// Whether the object was attached before.
bool wasAttached;
};
+
+ struct DetachAllObjectsFromRobotNodesInput
+ {
+ /**
+ * @brief If true, the objects will stay at the position before
+ * detaching until they are provided again.
+ */
+ bool commitAttachedPose = true;
+ }
struct DetachAllObjectsFromRobotNodesOutput
{
/// Number of objects that have been detached.
@@ -135,7 +152,8 @@ module armarx
long discardUpdatesUntilMilliSeconds = -1;
};
- interface ObjectPoseObserverInterface extends ObserverInterface, ObjectPoseTopic
+ interface ObjectPoseStorageInterface extends
+ ObjectPoseTopic
{
// Object poses
@@ -160,7 +178,7 @@ module armarx
/// Detach an attached object from a robot node.
DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(DetachObjectFromRobotNodeInput input);
/// Detach all objects from robot nodes.
- DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
+ DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(DetachAllObjectsFromRobotNodesInput input);
AgentFramesSeq getAttachableFrames();
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index 66618485d73ea91819fca88d6951875fba892bcd..385da35e387d04b21aaea7e4845b073f9c977ee4 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -85,7 +85,7 @@ module armarx
class ObjectAttachmentInfo;
- /// An object pose as stored by the ObjectPoseObserver.
+ /// An object pose as stored by the ObjectPoseStorage.
struct ObjectPose
{
/// Name of the providing component.
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 57878bbbdea5f9724bc16002ffaf570aa7659298..31fd00c7edcc840eb0a629580f3b28058698bae9 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -4,7 +4,11 @@ armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
- RobotAPICore
+ # ArmarXGui
+ RemoteGui
+ # RobotAPI
+ RobotAPI::Core
+ aroncommon
)
set(LIB_FILES
@@ -15,7 +19,12 @@ set(LIB_FILES
json_conversions.cpp
ice_conversions.cpp
- aron_conversions.cpp
+ aron_conversions/armarx.cpp
+ aron_conversions/objpose.cpp
+
+ plugins/ObjectPoseProviderPlugin.cpp
+ plugins/ObjectPoseClientPlugin.cpp
+ plugins/RequestedObjects.cpp
)
set(LIB_HEADERS
ArmarXObjects.h
@@ -27,17 +36,29 @@ set(LIB_HEADERS
json_conversions.h
ice_conversions.h
+
aron_conversions.h
+ aron_conversions/armarx.h
+ aron_conversions/objpose.h
+
+ plugins/ObjectPoseProviderPlugin.h
+ plugins/ObjectPoseClientPlugin.h
+ plugins/RequestedObjects.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+add_library(${PROJECT_NAME}::ArmarXObjects ALIAS ${PROJECT_NAME}ArmarXObjects)
+
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
ARON_FILES
+ aron/ObjectID.xml
+ aron/ObjectNames.xml
aron/ObjectPose.xml
+ aron/ObjectType.xml
)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index ed8d14d913785406841f24602ffdb17496d4a2ef..ebcf8c9766a5ca1d8204a1aac7c0f317c2173371 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -24,6 +24,11 @@ namespace armarx
packageDataDir.clear();
}
+ std::string ObjectFinder::getPackageName() const
+ {
+ return packageName;
+ }
+
void ObjectFinder::init() const
{
if (packageDataDir.empty())
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index f6e9831593f4a8714eb4f3c461016bce063cadaa..6e50a9d9cbeb1c82b7b928b2289f4b74799d70c7 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -32,6 +32,8 @@ namespace armarx
void setPath(const std::string& path);
+ std::string getPackageName() const;
+
std::optional<ObjectInfo> findObject(const std::string& dataset, const std::string& name) const;
std::optional<ObjectInfo> findObject(const std::string& nameOrID) const;
std::optional<ObjectInfo> findObject(const ObjectID& id) const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 8c70a93aac07dae51aa5b82014038e009f2a8cd1..348a5b25419813a22717f71c92d0c3484042361c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -25,6 +25,11 @@ namespace armarx
{
}
+ void ObjectInfo::setLogError(bool enabled)
+ {
+ this->_logError = enabled;
+ }
+
std::string ObjectInfo::package() const
{
return _packageName;
@@ -100,7 +105,10 @@ namespace armarx
}
catch (const std::exception& e)
{
- ARMARX_ERROR << e.what();
+ if (_logError)
+ {
+ ARMARX_ERROR << e.what();
+ }
return std::nullopt;
}
@@ -113,10 +121,10 @@ namespace armarx
simox::AxisAlignedBoundingBox aabb(min, max);
static const float prec = 1e-4f;
- ARMARX_CHECK_LESS_EQUAL((aabb.center() - center).norm(), prec) << aabb.center().transpose() << "\n" << center.transpose();
- ARMARX_CHECK_LESS_EQUAL((aabb.extents() - extents).norm(), prec) << aabb.extents().transpose() << "\n" << extents.transpose();
- ARMARX_CHECK_LESS_EQUAL((aabb.min() - min).norm(), prec) << aabb.min().transpose() << "\n" << min.transpose();
- ARMARX_CHECK_LESS_EQUAL((aabb.max() - max).norm(), prec) << aabb.max().transpose() << "\n" << max.transpose();
+ ARMARX_CHECK_LESS_EQUAL((aabb.center() - center).norm(), prec) << aabb.center().transpose() << "\n" << center.transpose() << "\n" << id();
+ ARMARX_CHECK_LESS_EQUAL((aabb.extents() - extents).norm(), prec) << aabb.extents().transpose() << "\n" << extents.transpose() << "\n" << id();
+ ARMARX_CHECK_LESS_EQUAL((aabb.min() - min).norm(), prec) << aabb.min().transpose() << "\n" << min.transpose() << "\n" << id();
+ ARMARX_CHECK_LESS_EQUAL((aabb.max() - max).norm(), prec) << aabb.max().transpose() << "\n" << max.transpose() << "\n" << id();
return aabb;
}
@@ -130,7 +138,10 @@ namespace armarx
}
catch (const std::exception& e)
{
- ARMARX_ERROR << e.what();
+ if (_logError)
+ {
+ ARMARX_ERROR << e.what();
+ }
return std::nullopt;
}
@@ -147,9 +158,21 @@ namespace armarx
ori.col(2) * extents(2));
static const float prec = 1e-3f;
- ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec) << oobb.center().transpose() << "\n" << pos.transpose();
- ARMARX_CHECK(oobb.rotation().isApprox(ori, prec)) << oobb.rotation() << "\n" << ori;
- ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec) << oobb.dimensions().transpose() << "\n" << extents.transpose();
+ ARMARX_CHECK(oobb.rotation().isApprox(ori, prec)) << oobb.rotation() << "\n" << ori << "\n" << id();
+ // If the object is too large, the above precision will trigger a false positive.
+ if (extents.squaredNorm() < 1e5f * 1e5f)
+ {
+ ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec)
+ << VAROUT(oobb.center().transpose())
+ << "\n" << VAROUT(pos.transpose())
+ << "\n" << VAROUT(extents.norm())
+ << "\n" << VAROUT(id());
+ ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec)
+ << VAROUT(oobb.dimensions().transpose())
+ << "\n" << VAROUT(extents.transpose())
+ << "\n" << VAROUT(extents.norm())
+ << "\n" << VAROUT(id());
+ }
return oobb;
}
@@ -176,12 +199,18 @@ namespace armarx
}
catch (const nlohmann::json::exception& e)
{
- ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n" << e.what();
+ if (_logError)
+ {
+ ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n" << e.what();
+ }
return std::nullopt;
}
catch (const std::exception& e)
{
- ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n" << e.what();
+ if (_logError)
+ {
+ ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n" << e.what();
+ }
return std::nullopt;
}
@@ -201,12 +230,18 @@ namespace armarx
if (!fs::is_regular_file(simoxXML().absolutePath))
{
- ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath;
+ if (_logError)
+ {
+ ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath;
+ }
result = false;
}
if (!fs::is_regular_file(wavefrontObj().absolutePath))
{
- ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath;
+ if (_logError)
+ {
+ ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath;
+ }
result = false;
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 10dbd5a08fd04c710cec3c41030357cc70c2243d..634944e4cc52379977f784c370f48af1e84ed28d 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -50,6 +50,9 @@ namespace armarx
virtual ~ObjectInfo() = default;
+ void setLogError(bool enabled);
+
+
std::string package() const;
std::string dataset() const;
@@ -114,6 +117,8 @@ namespace armarx
ObjectID _id;
+ bool _logError = true;
+
};
std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 36379f97d0d3d047058dbb9a9d57e9517a0caa71..556ac04be8c43a686e9d8f0e839fdb5bb13f7daa 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -20,12 +20,12 @@ namespace armarx::objpose
{
std::string frameName;
std::string agentName;
- Eigen::Matrix4f poseInFrame;
+ Eigen::Matrix4f poseInFrame = Eigen::Matrix4f::Identity();
};
/**
- * @brief An object pose as stored by the ObjectPoseObserver.
+ * @brief An object pose as stored by the ObjectPoseStorage.
*/
struct ObjectPose
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml
new file mode 100644
index 0000000000000000000000000000000000000000..dac70d6259875d8bc393180b415c2e4e8bb13451
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml
@@ -0,0 +1,22 @@
+<!--
+The ARON DTO of armarx::ObjectID.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::ObjectID">
+ <ObjectChild key='dataset'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='className'>
+ <string />
+ </ObjectChild>
+ <ObjectChild key='instanceName'>
+ <string />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.xml
new file mode 100644
index 0000000000000000000000000000000000000000..eb99bea9cd5a6cef00552cb17239ed8b715f5b8d
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.xml
@@ -0,0 +1,23 @@
+<!--
+Recognized and spoken names of a known object.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::ObjectNames">
+ <ObjectChild key="recognizedNames">
+ <List>
+ <String/>
+ </List>
+ </ObjectChild>
+ <ObjectChild key="spokenNames">
+ <List>
+ <String/>
+ </List>
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
index 441ec9b60ac7cd6cee5be2be62cf3e391b117862..e9701dc24c9a891449c9025a514322eaa615b52f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml
@@ -1,30 +1,22 @@
-<!--This class contains the data structure for ObjectPose -->
+<!--
+ARON DTO of armarx::objpose::ObjectPose.
+-->
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
<Include include="<Eigen/Core>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>" />
</CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.xml>" />
+ </AronIncludes>
<GenerateTypes>
- <IntEnum name="armarx::objpose::aron::ObjectTypes">
- <EnumValue key="ANY_OBJECT" value="0" />
- <EnumValue key="KNOWN_OBJECT" value="1" />
- <EnumValue key="UNKNOWN_OBJECT" value="2" />
- </IntEnum>
-
- <Object name="armarx::objpose::aron::ObjectID">
- <ObjectChild key='dataset'>
- <string />
- </ObjectChild>
- <ObjectChild key='className'>
- <string />
- </ObjectChild>
- <ObjectChild key='instanceName'>
- <string />
- </ObjectChild>
- </Object>
-
- <Object name="armarx::objpose::aron::ObjectAttachmentInfo">
+ <Object name="armarx::objpose::arondto::ObjectAttachmentInfo">
<ObjectChild key='frameName'>
<string />
</ObjectChild>
@@ -36,27 +28,18 @@
</ObjectChild>
</Object>
- <Object name="armarx::objpose::aron::OrientedBoundingBox">
- <ObjectChild key='centerPose'>
- <Pose />
- </ObjectChild>
- <ObjectChild key='extends'>
- <Position />
- </ObjectChild>
- </Object>
-
- <Object name='armarx::objpose::aron::ObjectPose'>
+ <Object name='armarx::objpose::arondto::ObjectPose'>
<ObjectChild key='providerName'>
<string />
</ObjectChild>
<ObjectChild key='objectType'>
- <armarx::objpose::aron::ObjectTypes />
+ <armarx::objpose::arondto::ObjectType />
</ObjectChild>
<ObjectChild key='objectID'>
- <armarx::objpose::aron::ObjectID />
+ <armarx::arondto::ObjectID />
</ObjectChild>
<ObjectChild key='objectPoseRobot'>
@@ -78,15 +61,18 @@
<ObjectChild key='robotConfig'>
<Dict>
<Float />
- </Dict>
+ </Dict>
</ObjectChild>
<ObjectChild key='robotPose'>
<Pose />
</ObjectChild>
+ <ObjectChild key='attachmentValid'>
+ <bool />
+ </ObjectChild>
<ObjectChild key='attachment'>
- <armarx::objpose::aron::ObjectAttachmentInfo />
+ <armarx::objpose::arondto::ObjectAttachmentInfo />
</ObjectChild>
<ObjectChild key='confidence'>
@@ -96,9 +82,12 @@
<ObjectChild key='timestamp'>
<Time />
</ObjectChild>
-
+
+ <ObjectChild key='localOOBBValid'>
+ <bool />
+ </ObjectChild>
<ObjectChild key='localOOBB'>
- <armarx::objpose::aron::OrientedBoundingBox />
+ <simox::arondto::OrientedBox />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0a40adf4027fca325d01ac02e13deb1cf0911d5d
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron/ObjectType.xml
@@ -0,0 +1,16 @@
+<!--
+ARON DTO of armarx::objpose::ObjectTypeEnum.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <IntEnum name="armarx::objpose::arondto::ObjectType">
+ <EnumValue key="AnyObject" value="0" />
+ <EnumValue key="KnownObject" value="1" />
+ <EnumValue key="UnknownObject" value="2" />
+ </IntEnum>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp
deleted file mode 100644
index 71398d59dc8d00018bcc0d4fb5c1c7cfb6010465..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-#include "aron_conversions.h"
-
-// STL
-#include <stdexcept>
-#include <string>
-
-// Ice
-#include <IceUtil/Time.h>
-
-// Simox
-#include <SimoxUtility/shapes/OrientedBox.h>
-
-// RobotAPI
-// TODO: ice dependency! header should be removed.
-#include <RobotAPI/interface/objectpose/object_pose_types.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
-
-
-namespace armarx::objpose
-{
-
- ObjectID fromAron(const aron::ObjectID& id)
- {
- return ObjectID(id.dataset, id.className, id.instanceName);
- }
-
- ObjectTypeEnum fromAron(const aron::ObjectTypes& objectType)
- {
- using AronObjectType = aron::ObjectTypes::__ImplEnum;
-
- ObjectTypeEnum e{};
-
- switch (objectType.value)
- {
- case AronObjectType::ANY_OBJECT:
- e = ObjectTypeEnum::AnyObject;
- break;
- case AronObjectType::KNOWN_OBJECT:
- e = ObjectTypeEnum::KnownObject;
- break;
- case AronObjectType::UNKNOWN_OBJECT:
- e = ObjectTypeEnum::UnknownObject;
- break;
- }
-
- return e;
- }
-
- simox::OrientedBoxf fromAron(const aron::OrientedBoundingBox& obb)
- {
- return simox::OrientedBoxf(obb.centerPose, obb.extends);
- }
-
- ObjectAttachmentInfo fromAron(const aron::ObjectAttachmentInfo& info)
- {
- return
- {
- .frameName = info.frameName,
- .agentName = info.agentName,
- .poseInFrame = info.poseInFrame
- };
- }
-
-
- void fromAron(const aron::ObjectPose& aronObjectPose, ObjectPose& objectPose)
- {
- objectPose.providerName = aronObjectPose.providerName;
-
- objectPose.objectType = fromAron(aronObjectPose.objectType);
-
- objectPose.objectID = fromAron(aronObjectPose.objectID);
-
- objectPose.objectPoseRobot = aronObjectPose.objectPoseRobot;
- objectPose.objectPoseGlobal = aronObjectPose.objectPoseGlobal;
- objectPose.objectPoseOriginal = aronObjectPose.objectPoseOriginal;
- objectPose.objectPoseOriginalFrame = aronObjectPose.objectPoseOriginalFrame;
-
- objectPose.robotConfig = aronObjectPose.robotConfig;
- objectPose.robotPose = aronObjectPose.robotPose;
-
- objectPose.attachment = fromAron(aronObjectPose.attachment);
-
- objectPose.confidence = aronObjectPose.confidence;
-
- objectPose.timestamp = IceUtil::Time::microSeconds(aronObjectPose.timestamp);
-
- objectPose.localOOBB = fromAron(aronObjectPose.localOOBB);
-
- }
-
-
- aron::ObjectID toAron(const ObjectID& id)
- {
- aron::ObjectID aronId;
-
- aronId.className = id.className();
- aronId.dataset = id.dataset();
- aronId.instanceName = id.instanceName();
-
- return aronId;
- }
-
- aron::ObjectTypes toAron(const ObjectTypeEnum& objectType)
- {
- aron::ObjectTypes ot{};
-
- switch (objectType)
- {
- case ObjectTypeEnum::AnyObject:
- ot = aron::ObjectTypes::ANY_OBJECT;
- break;
- case ObjectTypeEnum::KnownObject:
- ot = aron::ObjectTypes::KNOWN_OBJECT;
- break;
- case ObjectTypeEnum::UnknownObject:
- ot = aron::ObjectTypes::UNKNOWN_OBJECT;
- break;
- }
-
- return ot;
- }
-
- aron::ObjectAttachmentInfo toAron(const ObjectAttachmentInfo& info)
- {
- aron::ObjectAttachmentInfo aronInfo;
-
- aronInfo.agentName = info.agentName;
- aronInfo.frameName = info.frameName;
- aronInfo.poseInFrame = info.poseInFrame;
-
- return aronInfo;
- }
-
- aron::OrientedBoundingBox toAron(const simox::OrientedBoxf& box)
- {
- aron::OrientedBoundingBox aronBox;
- aronBox.centerPose = box.transformation();
- aronBox.extends = box.dimensions();
- return aronBox;
- }
-
- template <typename T>
- auto toAron(const std::optional<T>& p)
- {
- if (p)
- {
- return toAron(p.value());
- }
-
- // TODO(fabian.reister): handle optional properly
- // current fallback: default c'tor
- return decltype(toAron(p.value()))();
- }
-
-
- void toAron(const ObjectPose& objectPose, aron::ObjectPose& aronObjectPose)
- {
- aronObjectPose.providerName = objectPose.providerName;
-
- aronObjectPose.objectType = toAron(objectPose.objectType);
-
- aronObjectPose.objectID = toAron(objectPose.objectID);
-
- aronObjectPose.objectPoseRobot = objectPose.objectPoseRobot;
- aronObjectPose.objectPoseGlobal = objectPose.objectPoseGlobal;
- aronObjectPose.objectPoseOriginal = objectPose.objectPoseOriginal;
- aronObjectPose.objectPoseOriginalFrame = objectPose.objectPoseOriginalFrame;
-
- aronObjectPose.robotConfig = objectPose.robotConfig;
- aronObjectPose.robotPose = objectPose.robotPose;
-
- aronObjectPose.attachment = toAron(objectPose.attachment);
-
- aronObjectPose.confidence = objectPose.confidence;
-
- aronObjectPose.timestamp = objectPose.timestamp.toMicroSeconds();
-
- aronObjectPose.localOOBB = toAron(objectPose.localOOBB);
- }
-
-} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
index 50e7fc9412d1a30ac166386a29723547ef04cbda..f9862b6bbd1bc1f3d61771e92ef77b35dde1df83 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions.h
@@ -1,16 +1,4 @@
#pragma once
-
-namespace armarx::objpose
-{
- struct ObjectPose;
-
- namespace aron
- {
- struct ObjectPose;
- }
-
- void fromAron(const aron::ObjectPose& aronObjectPose, ObjectPose& objectPose);
- void toAron(const ObjectPose& objectPose, aron::ObjectPose& aronObjectPose);
-
-} // namespace armarx::objpose
\ No newline at end of file
+#include "aron_conversions/armarx.h"
+#include "aron_conversions/objpose.h"
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd081a06376a3ada55ea163fc20af229c3cc92b2
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.cpp
@@ -0,0 +1,27 @@
+#include "armarx.h"
+
+
+void armarx::fromAron(const arondto::ObjectID& dto, ObjectID& bo)
+{
+ bo = ObjectID(dto.dataset, dto.className, dto.instanceName);
+}
+void armarx::toAron(arondto::ObjectID& dto, const ObjectID& bo)
+{
+ dto.dataset = bo.dataset();
+ dto.className = bo.className();
+ dto.instanceName = bo.instanceName();
+}
+
+
+void armarx::fromAron(const armarx::arondto::PackagePath& dto, armarx::PackageFileLocation& bo)
+{
+ bo.package = dto.package;
+ bo.relativePath = dto.path;
+ bo.absolutePath = "";
+}
+
+void armarx::toAron(armarx::arondto::PackagePath& dto, const armarx::PackageFileLocation& bo)
+{
+ dto.package = bo.package;
+ dto.path = bo.relativePath;
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h
new file mode 100644
index 0000000000000000000000000000000000000000..08c60f45a3e27d1372f77d620a3e5efffd5c7400
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h> // For PackageFileLocation
+#include <RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
+
+
+namespace armarx
+{
+ void fromAron(const arondto::PackagePath& dto, PackageFileLocation& bo);
+ void toAron(arondto::PackagePath& dto, const PackageFileLocation& bo);
+
+ void fromAron(const arondto::ObjectID& dto, ObjectID& bo);
+ void toAron(arondto::ObjectID& dto, const ObjectID& bo);
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2de8a3da0096d0c2edf7d103c944edbb681eb56d
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
@@ -0,0 +1,138 @@
+#include "objpose.h"
+
+#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+
+
+void armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
+{
+ bo.frameName = dto.frameName;
+ bo.agentName = dto.agentName;
+ bo.poseInFrame = dto.poseInFrame;
+}
+void armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo)
+{
+ dto.frameName = bo.frameName;
+ dto.agentName = bo.agentName;
+ dto.poseInFrame = bo.poseInFrame;
+}
+
+void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectTypeEnum& bo)
+{
+ switch (dto.value)
+ {
+ case arondto::ObjectType::AnyObject:
+ bo = ObjectTypeEnum::AnyObject;
+ return;
+ case arondto::ObjectType::KnownObject:
+ bo = ObjectTypeEnum::KnownObject;
+ return;
+ case arondto::ObjectType::UnknownObject:
+ bo = ObjectTypeEnum::UnknownObject;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
+}
+void armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectTypeEnum& bo)
+{
+ switch (bo)
+ {
+ case ObjectTypeEnum::AnyObject:
+ dto.value = arondto::ObjectType::AnyObject;
+ return;
+ case ObjectTypeEnum::KnownObject:
+ dto.value = arondto::ObjectType::KnownObject;
+ return;
+ case ObjectTypeEnum::UnknownObject:
+ dto.value = arondto::ObjectType::UnknownObject;
+ return;
+ }
+ ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
+}
+
+
+void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
+{
+ bo.providerName = dto.providerName;
+
+ fromAron(dto.objectType, bo.objectType);
+ fromAron(dto.objectID, bo.objectID);
+
+ bo.objectPoseRobot = dto.objectPoseRobot;
+ bo.objectPoseGlobal = dto.objectPoseGlobal;
+ bo.objectPoseOriginal = dto.objectPoseOriginal;
+ bo.objectPoseOriginalFrame = dto.objectPoseOriginalFrame;
+
+ bo.robotConfig = dto.robotConfig;
+ bo.robotPose = dto.robotPose;
+
+ if (dto.attachmentValid)
+ {
+ bo.attachment = ObjectAttachmentInfo();
+ fromAron(dto.attachment, *bo.attachment);
+ }
+ else
+ {
+ bo.attachment = std::nullopt;
+ }
+
+ bo.confidence = dto.confidence;
+
+ bo.timestamp = IceUtil::Time::microSeconds(dto.timestamp);
+
+ if (dto.localOOBBValid)
+ {
+ bo.localOOBB = simox::OrientedBoxf();
+ fromAron(dto.localOOBB, *bo.localOOBB);
+ }
+ else
+ {
+ bo.localOOBB = std::nullopt;
+ }
+}
+
+
+void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
+{
+ dto.providerName = bo.providerName;
+
+ toAron(dto.objectType, bo.objectType);
+ toAron(dto.objectID, bo.objectID);
+
+ dto.objectPoseRobot = bo.objectPoseRobot;
+ dto.objectPoseGlobal = bo.objectPoseGlobal;
+ dto.objectPoseOriginal = bo.objectPoseOriginal;
+ dto.objectPoseOriginalFrame = bo.objectPoseOriginalFrame;
+
+ dto.robotConfig = bo.robotConfig;
+ dto.robotPose = bo.robotPose;
+
+ if (bo.attachment)
+ {
+ dto.attachmentValid = true;
+ toAron(dto.attachment, *bo.attachment);
+ }
+ else
+ {
+ dto.attachmentValid = false;
+ toAron(dto.attachment, ObjectAttachmentInfo());
+ }
+
+ dto.confidence = bo.confidence;
+
+ dto.timestamp = bo.timestamp.toMicroSeconds();
+
+ if (bo.localOOBB)
+ {
+ dto.localOOBBValid = true;
+ toAron(dto.localOOBB, *bo.localOOBB);
+ }
+ else
+ {
+ dto.localOOBBValid = false;
+ toAron(dto.localOOBB, simox::OrientedBoxf());
+ }
+}
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
new file mode 100644
index 0000000000000000000000000000000000000000..afe31e500b7ba9a39b76dc33b0eeda0916cc849b
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+
+#include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
+
+
+namespace armarx::objpose
+{
+ void fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo);
+ void toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo);
+
+ void fromAron(const arondto::ObjectType& dto, ObjectTypeEnum& bo);
+ void toAron(arondto::ObjectType& dto, const ObjectTypeEnum& bo);
+
+ void fromAron(const arondto::ObjectPose& dto, ObjectPose& bo);
+ void toAron(arondto::ObjectPose& dto, const ObjectPose& bo);
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
similarity index 73%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
index 95b046d87274b5562b43d1237cf2ffff9144f0aa..b12572855775c7c9f3f11f4f2d571da7ff36a14f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
@@ -8,8 +8,8 @@ namespace armarx::plugins
{
properties->defineOptionalProperty<std::string>(
makePropertyName(PROPERTY_NAME),
- "ObjectPoseObserver",
- "Name of the object pose observer.");
+ "ObjectMemory",
+ "Name of the object memory.");
}
}
@@ -20,12 +20,12 @@ namespace armarx::plugins
void ObjectPoseClientPlugin::preOnConnectComponent()
{
- parent<ObjectPoseClientPluginUser>().objectPoseObserver = createObjectPoseObserver();
+ parent<ObjectPoseClientPluginUser>().objectPoseStorage = createObjectPoseStorage();
}
- objpose::ObjectPoseObserverInterfacePrx ObjectPoseClientPlugin::createObjectPoseObserver()
+ objpose::ObjectPoseStorageInterfacePrx ObjectPoseClientPlugin::createObjectPoseStorage()
{
- return parent<Component>().getProxyFromProperty<objpose::ObjectPoseObserverInterfacePrx>(makePropertyName(PROPERTY_NAME));
+ return parent<Component>().getProxyFromProperty<objpose::ObjectPoseStorageInterfacePrx>(makePropertyName(PROPERTY_NAME));
}
const ObjectFinder& ObjectPoseClientPlugin::setObjectFinderPath(const std::string& path)
@@ -47,19 +47,19 @@ namespace armarx
addPlugin(plugin);
}
- objpose::ObjectPoseObserverInterfacePrx ObjectPoseClientPluginUser::createObjectPoseObserver()
+ objpose::ObjectPoseStorageInterfacePrx ObjectPoseClientPluginUser::createObjectPoseStorage()
{
- return plugin->createObjectPoseObserver();
+ return plugin->createObjectPoseStorage();
}
objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPoses()
{
- if (!objectPoseObserver)
+ if (!objectPoseStorage)
{
ARMARX_WARNING << "No object pose observer.";
return {};
}
- return objpose::fromIce(objectPoseObserver->getObjectPoses());
+ return objpose::fromIce(objectPoseStorage->getObjectPoses());
}
plugins::ObjectPoseClientPlugin& ObjectPoseClientPluginUser::getObjectPoseClientPlugin()
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..996f699acd35dd50afa095f3bdaa66d9d2a7e061
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
@@ -0,0 +1,85 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+namespace armarx::plugins
+{
+ class ObjectPoseClientPlugin : public ComponentPlugin
+ {
+ public:
+
+ using ComponentPlugin::ComponentPlugin;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+ objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
+
+ template<class...Ts>
+ std::optional<ObjectInfo> findObject(Ts&& ...ts) const
+ {
+ return _finder.findObject(std::forward<Ts>(ts)...);
+ }
+
+ template<class...Ts>
+ VirtualRobot::ManipulationObjectPtr
+ findAndLoadObject(Ts&& ...ts) const
+ {
+ return findAndLoadObject(findObject(std::forward<Ts>(ts)...));
+ }
+ VirtualRobot::ManipulationObjectPtr
+ findAndLoadObject(const std::optional<ObjectInfo>& ts) const
+ {
+ return _finder.loadManipulationObject(ts);
+ }
+
+ const ObjectFinder& setObjectFinderPath(const std::string& path);
+ const ObjectFinder& getObjectFinder() const;
+
+
+ private:
+
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+
+ static constexpr const char* PROPERTY_NAME = "ObjectMemoryName";
+
+ ObjectFinder _finder;
+
+ };
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
+ */
+ class ObjectPoseClientPluginUser :
+ virtual public ManagedIceObject
+ {
+ public:
+
+ /// Allow usage like: ObjectPoseClient::getObjects()
+ using ObjectPoseClient = ObjectPoseClientPluginUser;
+
+ ObjectPoseClientPluginUser();
+
+ objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
+ objpose::ObjectPoseStorageInterfacePrx objectPoseStorage;
+
+ objpose::ObjectPoseSeq getObjectPoses();
+
+ plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin();
+ const plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin() const;
+
+ const ObjectFinder& getObjectFinder() const;
+
+
+ private:
+
+ armarx::plugins::ObjectPoseClientPlugin* plugin = nullptr;
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.cpp
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.cpp
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f7c87ef560cad2e4a0b20257387f52b8bd6835d
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
+
+
+namespace armarx::plugins
+{
+
+ class ObjectPoseProviderPlugin : public ComponentPlugin
+ {
+ public:
+
+ using ComponentPlugin::ComponentPlugin;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
+ void postOnConnectComponent() override;
+
+ objpose::ObjectPoseTopicPrx createObjectPoseTopic();
+
+
+ private:
+
+ static constexpr const char* PROPERTY_NAME = "ObjectPoseTopicName";
+
+ };
+
+}
+
+
+namespace armarx
+{
+
+ /**
+ * @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
+ */
+ class ObjectPoseProviderPluginUser :
+ virtual public ManagedIceObject
+ , virtual public objpose::ObjectPoseProvider
+ {
+ public:
+
+ ObjectPoseProviderPluginUser();
+
+ /// Implement to process object requests (empty default implementation).
+ objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
+
+ objpose::ObjectPoseTopicPrx createObjectPoseTopic();
+
+ objpose::ObjectPoseTopicPrx objectPoseTopic;
+
+
+ private:
+
+ armarx::plugins::ObjectPoseProviderPlugin* plugin = nullptr;
+
+ };
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.cpp
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h
similarity index 100%
rename from source/RobotAPI/components/ObjectPoseObserver/plugins/RequestedObjects.h
rename to source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 06c12b18a610b8b05998daea8bc2100cb6839078..1778ba42d2c67c1d6967485ee5fbcaa05241488a 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -19,6 +19,7 @@ add_subdirectory(natik)
add_subdirectory(armem)
add_subdirectory(armem_gui)
+add_subdirectory(armem_objects)
add_subdirectory(armem_robot_localization)
add_subdirectory(armem_robot_mapping)
add_subdirectory(aron)
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index 8e61ccd8332b8ef0c8cc4fc891dd2548a68a5682..a4ca9b08ad6d3b0937294c5a9050adb6680abfcc 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -44,5 +44,7 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+add_library(RobotAPI::ComponentPlugins ALIAS RobotAPIComponentPlugins)
+
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 9e8250ffc08f91c8f515f81c803da0830a6dee6d..0e3d33e1950ebaade350c516dd9e6e66dab1fce4 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -11,17 +11,13 @@ set(LIBS
${LIBBSONCXX_LIBRARIES}
)
-set(ARON_FILES
- core/aron/MemoryID.xml
-)
-
set(LIB_FILES
core/Commit.cpp
core/MemoryID.cpp
core/SuccessHeader.cpp
core/Time.cpp
-
core/ice_conversions.cpp
+ core/aron_conversions.cpp
core/base/detail/MemoryItem.cpp
core/base/detail/MaxHistorySize.cpp
@@ -46,6 +42,7 @@ set(LIB_FILES
core/workingmemory/EntitySnapshot.cpp
core/workingmemory/Memory.cpp
core/workingmemory/ProviderSegment.cpp
+ core/workingmemory/Visitor.cpp
core/workingmemory/ice_conversions.cpp
core/longtermmemory/CoreSegment.cpp
@@ -111,6 +108,9 @@ set(LIB_HEADERS
core/MemoryID.h
core/SuccessHeader.h
core/Time.h
+ core/aron_conversions.h
+ core/ice_conversions.h
+ core/ice_conversions_templates.h
core/error.h
core/error/ArMemError.h
@@ -137,6 +137,7 @@ set(LIB_HEADERS
core/workingmemory/EntitySnapshot.h
core/workingmemory/Memory.h
core/workingmemory/ProviderSegment.h
+ core/workingmemory/Visitor.h
core/workingmemory/ice_conversions.h
core/longtermmemory/CoreSegment.h
@@ -155,8 +156,8 @@ set(LIB_HEADERS
core/diskmemory/Memory.h
core/diskmemory/ProviderSegment.h
- core/ice_conversions.h
core/ice_conversions_templates.h
+ core/ice_conversions.h
client.h
client/ComponentPlugin.h
@@ -213,8 +214,7 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
${LIB_NAME}
ARON_FILES
- ${ARON_FILES}
- #ENABLE_DEBUG_INFO
+ aron/MemoryID.xml
)
diff --git a/source/RobotAPI/libraries/armem/core/aron/MemoryID.xml b/source/RobotAPI/libraries/armem/aron/MemoryID.xml
similarity index 55%
rename from source/RobotAPI/libraries/armem/core/aron/MemoryID.xml
rename to source/RobotAPI/libraries/armem/aron/MemoryID.xml
index 11d0fee395edca2c597dc1ea9de53dfbd7f9eae9..996259c63c7b66028bd8b853714fa3147fd64ed6 100644
--- a/source/RobotAPI/libraries/armem/core/aron/MemoryID.xml
+++ b/source/RobotAPI/libraries/armem/aron/MemoryID.xml
@@ -1,23 +1,23 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<GenerateTypes>
- <Object name='armarx::armem::aron::MemoryID'>
- <ObjectChild key='memoryName'>
+ <Object name="armarx::armem::arondto::MemoryID">
+ <ObjectChild key="memoryName">
<string />
</ObjectChild>
- <ObjectChild key='coreSegmentName'>
+ <ObjectChild key="coreSegmentName">
<string />
</ObjectChild>
- <ObjectChild key='providerSegmentName'>
+ <ObjectChild key="providerSegmentName">
<string />
</ObjectChild>
- <ObjectChild key='entityName'>
+ <ObjectChild key="entityName">
<string />
</ObjectChild>
- <ObjectChild key='timestamp'>
- <long />
+ <ObjectChild key="timestamp">
+ <Time />
</ObjectChild>
- <ObjectChild key='instanceIndex'>
+ <ObjectChild key="instanceIndex">
<int />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5e6d3e0d10cb7b3f1ed51bcc8f665cb48ec3cbf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
@@ -0,0 +1,22 @@
+#include "aron_conversions.h"
+
+
+void armarx::armem::fromAron(const arondto::MemoryID& dto, MemoryID& bo)
+{
+ bo.memoryName = dto.memoryName;
+ bo.coreSegmentName = dto.coreSegmentName;
+ bo.providerSegmentName = dto.providerSegmentName;
+ bo.entityName = dto.entityName;
+ bo.timestamp = IceUtil::Time::microSeconds(dto.timestamp);
+ bo.instanceIndex = dto.instanceIndex;
+}
+
+void armarx::armem::toAron(arondto::MemoryID& dto, const MemoryID& bo)
+{
+ dto.memoryName = bo.memoryName;
+ dto.coreSegmentName = bo.coreSegmentName;
+ dto.providerSegmentName = bo.providerSegmentName;
+ dto.entityName = bo.entityName;
+ dto.timestamp = bo.timestamp.toMicroSeconds();
+ dto.instanceIndex = bo.instanceIndex;
+}
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.h b/source/RobotAPI/libraries/armem/core/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..5d105fd9761f3ff1d8eaa58fabf4aa257719478b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+
+
+namespace armarx::armem
+{
+ void fromAron(const arondto::MemoryID& dto, MemoryID& bo);
+ void toAron(arondto::MemoryID& dto, const MemoryID& bo);
+}
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index 609920d1e6e80054f4526fffe4e76eca2c4a43a5..326a251a6a3a0a62034f01a9c11553bf8f180ec1 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -83,16 +83,16 @@ namespace armarx::armem::base
}
}
- using Base::_checkContainerName;
+ using Base::getEntity;
const EntityT& getEntity(const MemoryID& id) const override
{
- _checkContainerName(id.coreSegmentName, this->getKeyString());
+ this->_checkContainerName(id.coreSegmentName, this->getKeyString());
return getProviderSegment(id.providerSegmentName).getEntity(id);
}
virtual MemoryID update(const EntityUpdate& update) override
{
- _checkContainerName(update.entityID.coreSegmentName, this->name());
+ this->_checkContainerName(update.entityID.coreSegmentName, this->name());
auto it = this->_container.find(update.entityID.providerSegmentName);
if (it != _container.end())
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 0d4c8020910ac140e61df0540136f2bcb0094e43..60f7ee86098c26463ffe14007362eadc114f3e9e 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -85,10 +85,10 @@ namespace armarx::armem::base
}
}
- using Base::_checkContainerName;
+ using Base::getEntity;
const EntityT& getEntity(const MemoryID& id) const override
{
- _checkContainerName(id.memoryName, this->name());
+ this->_checkContainerName(id.memoryName, this->name());
return getCoreSegment(id.coreSegmentName).getEntity(id);
}
@@ -144,7 +144,7 @@ namespace armarx::armem::base
virtual MemoryID update(const EntityUpdate& update) override
{
- _checkContainerName(update.entityID.memoryName, this->name());
+ this->_checkContainerName(update.entityID.memoryName, this->name());
auto it = _container.find(update.entityID.coreSegmentName);
if (it != _container.end())
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index f6f760bc6976c165399cfeeb57f7ebd19cf0a91c..9cbfd4facde379fc5d3cc1f8f0d26631899e5811 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -65,10 +65,10 @@ namespace armarx::armem::base
return _container.count(name) > 0;
}
- using Base::_checkContainerName;
+ using Base::getEntity;
const EntityT& getEntity(const MemoryID& id) const override
{
- _checkContainerName(id.providerSegmentName, this->getKeyString());
+ this->_checkContainerName(id.providerSegmentName, this->getKeyString());
return getEntity(id.entityName);
}
@@ -97,7 +97,7 @@ namespace armarx::armem::base
*/
virtual MemoryID update(const EntityUpdate& update) override
{
- _checkContainerName(update.entityID.providerSegmentName, this->name());
+ this->_checkContainerName(update.entityID.providerSegmentName, this->name());
EntityT* entity;
auto it = this->_container.find(update.entityID.providerSegmentName);
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
index f75b15d117276b14c970110672c3bd327fb0c0f1..233985ebab9bee9353fd23f1b5e92ed570b0ccd2 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
@@ -101,12 +101,15 @@ namespace armarx::armem::base::detail
protected:
- /// @throw `armem::error::ContainerNameMismatch` Of `expectedName != actualName`.
- void _checkContainerName(const std::string& expectedName, const std::string& actualName) const
+ /**
+ * @throw `armem::error::ContainerNameMismatch` if `gottenName` does not match `actualName`.
+ */
+ void _checkContainerName(const std::string& gottenName, const std::string& actualName,
+ bool emptyOk = true) const
{
- if (expectedName != actualName)
+ if (!((emptyOk && gottenName.empty()) || gottenName == actualName))
{
- throw armem::error::ContainerNameMismatch(expectedName, this->getLevelName(), actualName);
+ throw armem::error::ContainerNameMismatch(gottenName, this->getLevelName(), actualName);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
index 2b61bfbac513a1854859abecf81697e5baff3aae..c44a40758a73ba64d8827c6f7a751b9df8fc00c3 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.cpp
@@ -35,16 +35,15 @@ namespace armarx::armem::error
}
- ContainerNameMismatch::ContainerNameMismatch(const std::string& expectedName,
+ ContainerNameMismatch::ContainerNameMismatch(const std::string& gottenName,
const std::string& ownTerm, const std::string& containerName) :
- ArMemError(makeMsg(expectedName, ownTerm, containerName))
+ ArMemError(makeMsg(gottenName, ownTerm, containerName))
{}
- std::string ContainerNameMismatch::makeMsg(
- const std::string& expectedName, const std::string& containerTerm, const std::string& containerName)
+ std::string ContainerNameMismatch::makeMsg(const std::string& gottenName, const std::string& containerTerm, const std::string& containerName)
{
std::stringstream ss;
- ss << "Name '" << expectedName << "' does not match name of " << containerTerm << " '" << containerName << "'.";
+ ss << "Name '" << gottenName << "' does not match name of " << containerTerm << " '" << containerName << "'.";
return ss.str();
}
diff --git a/source/RobotAPI/libraries/armem/core/error/ArMemError.h b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
index 068283a519d74cfe86d7ffb31345d4404e4e0d7f..3456c7a350835e6c67ca1d439d2fc0c44a8be507 100644
--- a/source/RobotAPI/libraries/armem/core/error/ArMemError.h
+++ b/source/RobotAPI/libraries/armem/core/error/ArMemError.h
@@ -45,10 +45,10 @@ namespace armarx::armem::error
{
public:
- ContainerNameMismatch(const std::string& expectedName,
+ ContainerNameMismatch(const std::string& gottenName,
const std::string& containerTerm, const std::string& containerName);
- static std::string makeMsg(const std::string& expectedName,
+ static std::string makeMsg(const std::string& gottenName,
const std::string& containerTerm, const std::string& containerName);
};
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..986612d1e788bee74e572f20b89dbc78fb99ea58
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.cpp
@@ -0,0 +1,151 @@
+#include "Visitor.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+
+
+namespace armarx::armem::wm
+{
+
+ Visitor::Visitor()
+ {
+ }
+
+ Visitor::~Visitor()
+ {
+ }
+
+ bool Visitor::applyTo(Memory& memory)
+ {
+ for (auto& [_, coreSeg] : memory)
+ {
+ if (!applyTo(coreSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(CoreSegment& coreSegment)
+ {
+ for (auto& [_, provSeg] : coreSegment)
+ {
+ if (!applyTo(provSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(ProviderSegment& providerSegment)
+ {
+ for (auto& [_, entity] : providerSegment)
+ {
+ if (!applyTo(entity))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(Entity& entity)
+ {
+ for (auto& [_, snapshot] : entity)
+ {
+ if (!applyTo(snapshot))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(EntitySnapshot& snapshot)
+ {
+ for (auto& instance : snapshot)
+ {
+ if (!applyTo(instance))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(EntityInstance& instance)
+ {
+ return visit(instance);
+ }
+
+
+ bool Visitor::applyTo(const Memory& memory)
+ {
+ for (const auto& [_, coreSeg] : memory)
+ {
+ if (!applyTo(coreSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const CoreSegment& coreSegment)
+ {
+ for (const auto& [_, provSeg] : coreSegment)
+ {
+ if (!applyTo(provSeg))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const ProviderSegment& providerSegment)
+ {
+ for (const auto& [_, entity] : providerSegment)
+ {
+ if (!applyTo(entity))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const Entity& entity)
+ {
+ for (const auto& [_, snapshot] : entity)
+ {
+ if (!applyTo(snapshot))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const EntitySnapshot& snapshot)
+ {
+ for (const auto& instance : snapshot)
+ {
+ if (!applyTo(instance))
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool Visitor::applyTo(const EntityInstance& instance)
+ {
+ return visit(instance);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..c0a3854a242c030113eeb46409c172563660294a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Visitor.h
@@ -0,0 +1,152 @@
+#pragma once
+
+
+namespace armarx::armem::wm
+{
+ class Memory;
+ class CoreSegment;
+ class ProviderSegment;
+ class Entity;
+ class EntitySnapshot;
+ class EntityInstance;
+
+
+ /**
+ * @brief A visitor for the hierarchical Memory data structure.
+ */
+ class Visitor
+ {
+
+ public:
+
+ Visitor();
+ virtual ~Visitor();
+
+ bool applyTo(Memory& memory);
+ bool applyTo(CoreSegment& coreSegment);
+ bool applyTo(ProviderSegment& providerSegment);
+ bool applyTo(Entity& entity);
+ bool applyTo(EntitySnapshot& snapshot);
+ bool applyTo(EntityInstance& instance);
+
+
+ virtual bool visitEnter(Memory& memory)
+ {
+ return visitEnter(const_cast<const Memory&>(memory));
+ }
+ virtual bool visitEnter(CoreSegment& coreSegment)
+ {
+ return visitEnter(const_cast<const CoreSegment&>(coreSegment));
+ }
+ virtual bool visitEnter(ProviderSegment& providerSegment)
+ {
+ return visitEnter(const_cast<const ProviderSegment&>(providerSegment));
+ }
+ virtual bool visitEnter(Entity& entity)
+ {
+ return visitEnter(const_cast<const Entity&>(entity));
+ }
+ virtual bool visitEnter(EntitySnapshot& snapshot)
+ {
+ return visitEnter(const_cast<const EntitySnapshot&>(snapshot));
+ }
+
+ virtual bool visitExit(Memory& memory)
+ {
+ return visitExit(const_cast<const Memory&>(memory));
+ }
+ virtual bool visitExit(CoreSegment& coreSegment)
+ {
+ return visitExit(const_cast<const CoreSegment&>(coreSegment));
+ }
+ virtual bool visitExit(ProviderSegment& providerSegment)
+ {
+ return visitExit(const_cast<const ProviderSegment&>(providerSegment));
+ }
+ virtual bool visitExit(Entity& entity)
+ {
+ return visitExit(const_cast<const Entity&>(entity));
+ }
+ virtual bool visitExit(EntitySnapshot& snapshot)
+ {
+ return visitExit(const_cast<const EntitySnapshot&>(snapshot));
+ }
+
+ virtual bool visit(EntityInstance& instance)
+ {
+ return visit(const_cast<const EntityInstance&>(instance));
+ }
+
+
+
+ // Const versions
+
+ bool applyTo(const Memory& memory);
+ bool applyTo(const CoreSegment& coreSegment);
+ bool applyTo(const ProviderSegment& providerSegment);
+ bool applyTo(const Entity& entity);
+ bool applyTo(const EntitySnapshot& snapshot);
+ bool applyTo(const EntityInstance& instance);
+
+
+ virtual bool visitEnter(const Memory& memory)
+ {
+ (void) memory;
+ return true;
+ }
+ virtual bool visitEnter(const CoreSegment& coreSegment)
+ {
+ (void) coreSegment;
+ return true;
+ }
+ virtual bool visitEnter(const ProviderSegment& providerSegment)
+ {
+ (void) providerSegment;
+ return true;
+ }
+ virtual bool visitEnter(const Entity& entity)
+ {
+ (void) entity;
+ return true;
+ }
+ virtual bool visitEnter(const EntitySnapshot& snapshot)
+ {
+ (void) snapshot;
+ return true;
+ }
+
+ virtual bool visitExit(const Memory& memory)
+ {
+ (void) memory;
+ return true;
+ }
+ virtual bool visitExit(const CoreSegment& coreSegment)
+ {
+ (void) coreSegment;
+ return true;
+ }
+ virtual bool visitExit(const ProviderSegment& providerSegment)
+ {
+ (void) providerSegment;
+ return true;
+ }
+ virtual bool visitExit(const Entity& entity)
+ {
+ (void) entity;
+ return true;
+ }
+ virtual bool visitExit(const EntitySnapshot& snapshot)
+ {
+ (void) snapshot;
+ return true;
+ }
+
+ virtual bool visit(const EntityInstance& instance)
+ {
+ (void) instance;
+ return true;
+ }
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem/mns/ClientPlugin.h b/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
index a818ca771ba76e29aa5feddd4b9ac8ac2e5377e8..4ebc9aad33392ffdf48c019882b14217638174cf 100644
--- a/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
+++ b/source/RobotAPI/libraries/armem/mns/ClientPlugin.h
@@ -76,7 +76,7 @@ namespace armarx::armem::mns::plugins
mns::MemoryNameSystemInterfacePrx memoryNameSystem = nullptr;
bool memoryNameSystemEnabled = true;
- std::string memoryNameSystemName = "ArMemMemoryNameSystem";
+ std::string memoryNameSystemName = "MemoryNameSystem";
};
}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
index c5aff2156b36fc76b06920ba82a2db44539b1f19..536c72f49d6ccb4c656f37c345dc6a4ec70b4f17 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.cpp
@@ -41,7 +41,7 @@ namespace armarx::armem::server::plugins
settings.user = longTermMemoryDatabaseUser;
settings.password = longTermMemoryDatabasePassword;
- parent.longtermmemory.reload(settings);
+ parent.longtermMemory.reload(settings);
}
@@ -58,7 +58,7 @@ namespace armarx::armem::server::plugins
data::RegisterMemoryResult ComponentPlugin::registerMemory(ComponentPluginUser& parent)
{
data::RegisterMemoryInput input;
- input.name = parent.workingmemory.name();
+ input.name = parent.workingMemory.name();
input.proxy = MemoryInterfacePrx::checkedCast(parent.getProxy());
ARMARX_CHECK_NOT_NULL(input.proxy);
data::RegisterMemoryResult result = parent.memoryNameSystem->registerMemory(input);
@@ -81,7 +81,7 @@ namespace armarx::armem::server::plugins
try
{
data::RemoveMemoryInput input;
- input.name = parent.workingmemory.name();
+ input.name = parent.workingMemory.name();
result = parent.memoryNameSystem->removeMemory(input);
if (result.success)
{
@@ -122,7 +122,7 @@ namespace armarx::armem::server
data::AddSegmentsResult ComponentPluginUser::addSegments(const data::AddSegmentsInput& input, bool addCoreSegments)
{
- std::scoped_lock lock(workingmemoryMutex);
+ std::scoped_lock lock(workingMemoryMutex);
data::AddSegmentsResult result = iceMemory.addSegments(input, addCoreSegments);
return result;
}
@@ -130,7 +130,7 @@ namespace armarx::armem::server
data::CommitResult ComponentPluginUser::commit(const data::Commit& commitIce, const Ice::Current&)
{
- std::scoped_lock lock(workingmemoryMutex);
+ std::scoped_lock lock(workingMemoryMutex);
return iceMemory.commit(commitIce);
}
@@ -138,15 +138,15 @@ namespace armarx::armem::server
// READING
armem::query::data::Result ComponentPluginUser::query(const armem::query::data::Input& input, const Ice::Current&)
{
- std::scoped_lock lock(workingmemoryMutex);
+ std::scoped_lock lock(workingMemoryMutex);
return iceMemory.query(input);
}
// LTM LOADING
data::StoreResult ComponentPluginUser::store(const data::StoreInput& input, const Ice::Current&)
{
- std::scoped_lock lock(workingmemoryMutex);
- std::scoped_lock lock2(longtermmemoryMutex);
+ std::scoped_lock lock(workingMemoryMutex);
+ std::scoped_lock lock2(longtermMemoryMutex);
return iceMemory.store(input);
}
@@ -154,7 +154,7 @@ namespace armarx::armem::server
// LTM STORING
armem::query::data::Result ComponentPluginUser::load(const armem::query::data::Input& input, const Ice::Current&)
{
- std::scoped_lock lock(longtermmemoryMutex);
+ std::scoped_lock lock(longtermMemoryMutex);
return iceMemory.load(input);
}
diff --git a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
index c051995c4c6790731850bc4294e9dba59155a795..fc2154f59a0d640a2b6d6e07f5df51465caa2812 100644
--- a/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem/server/ComponentPlugin.h
@@ -96,11 +96,11 @@ namespace armarx::armem::server
public:
/// The actual memory.
- wm::Memory workingmemory;
- std::mutex workingmemoryMutex;
+ wm::Memory workingMemory;
+ std::mutex workingMemoryMutex;
- ltm::Memory longtermmemory;
- std::mutex longtermmemoryMutex;
+ ltm::Memory longtermMemory;
+ std::mutex longtermMemoryMutex;
/// property defaults
std::string memoryListenerDefaultName = "MemoryUpdates";
@@ -109,7 +109,7 @@ namespace armarx::armem::server
std::string longTermMemoryDatabasePasswordDefault = "";
/// Helps connecting `memory` to ice. Used to handle Ice callbacks.
- MemoryToIceAdapter iceMemory { &workingmemory, &longtermmemory};
+ MemoryToIceAdapter iceMemory { &workingMemory, &longtermMemory};
private:
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index bd982e10f95a5703ea4bf99eb7b90cf1735fc20b..6895cf4e5b27ecec5872a38890d8111b41405fa7 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -12,7 +12,8 @@
namespace armarx::armem::server
{
- MemoryToIceAdapter::MemoryToIceAdapter(wm::Memory* workingmemory, ltm::Memory* longtermmemory) : workingMemory(workingmemory), longtermMemory(longtermmemory)
+ MemoryToIceAdapter::MemoryToIceAdapter(wm::Memory* workingMemory, ltm::Memory* longtermMemory) :
+ workingMemory(workingMemory), longtermMemory(longtermMemory)
{
}
@@ -202,4 +203,9 @@ namespace armarx::armem::server
return output;
}
+ client::QueryResult MemoryToIceAdapter::query(const client::QueryInput& input)
+ {
+ return client::QueryResult::fromIce(query(input.toIce()));
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index 36a2081aec732b9b6d348d56a53f280446a02502..919c5fc8dc74ec3f742ac1c33365bc2c680ac768 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -3,8 +3,9 @@
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-#include "../core/workingmemory/Memory.h"
-#include "../core/longtermmemory/Memory.h"
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/longtermmemory/Memory.h>
+#include <RobotAPI/libraries/armem/client/Query.h>
namespace armarx::armem::server
@@ -41,6 +42,7 @@ namespace armarx::armem::server
// READING
query::data::Result query(const armem::query::data::Input& input);
+ client::QueryResult query(const client::QueryInput& input);
// LTM LOADING
query::data::Result load(const armem::query::data::Input& input);
diff --git a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index c0a27825ee0ee7aae1770e3a426054194bdfd8ef..d90a90aa04a3fcc2c3d891bfab8727a991522f44 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -10,7 +10,7 @@ set(LIBRARIES
# ArmarXGui
SimpleConfigDialog
# RobotAPI
- armem
+ aroneigenconverter armem
)
set(SOURCES
@@ -24,9 +24,19 @@ set(SOURCES
instance/GroupBox.cpp
instance/ImageView.cpp
instance/InstanceView.cpp
- instance/TreeDataVisitorBase.cpp
- instance/TreeDataVisitor.cpp
- instance/TreeTypedDataVisitor.cpp
+ instance/sanitize_typename.cpp
+ instance/serialize_path.cpp
+
+ instance/display_visitors/DataDisplayVisitor.cpp
+ instance/display_visitors/TypedDataDisplayVisitor.cpp
+
+ instance/tree_builders/DataTreeBuilder.cpp
+ instance/tree_builders/DataTreeBuilderBase.cpp
+ instance/tree_builders/TypedDataTreeBuilder.cpp
+
+ instance/tree_visitors/TreeDataVisitorBase.cpp
+ instance/tree_visitors/TreeDataVisitor.cpp
+ instance/tree_visitors/TreeTypedDataVisitor.cpp
memory/GroupBox.cpp
memory/TreeWidget.cpp
@@ -47,9 +57,19 @@ set(HEADERS
instance/GroupBox.h
instance/ImageView.h
instance/InstanceView.h
- instance/TreeDataVisitorBase.h
- instance/TreeDataVisitor.h
- instance/TreeTypedDataVisitor.h
+ instance/sanitize_typename.h
+ instance/serialize_path.h
+
+ instance/display_visitors/DataDisplayVisitor.h
+ instance/display_visitors/TypedDataDisplayVisitor.h
+
+ instance/tree_builders/DataTreeBuilder.h
+ instance/tree_builders/DataTreeBuilderBase.h
+ instance/tree_builders/TypedDataTreeBuilder.h
+
+ instance/tree_visitors/TreeDataVisitorBase.h
+ instance/tree_visitors/TreeDataVisitor.h
+ instance/tree_visitors/TreeTypedDataVisitor.h
memory/GroupBox.h
memory/TreeWidget.h
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 694e98f08d3c9262670683392cca3981248ba314..3f40956a61149b1aca6af0b0fa2a9c3f129a18da 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -313,7 +313,7 @@ namespace armarx::armem::gui
void MemoryViewer::loadSettings(QSettings* settings)
{
- mnsName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "ArMemMemoryNameSystem").toString().toStdString();
+ mnsName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "MemoryNameSystem").toString().toStdString();
debugObserverName = settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), "DebugObserver").toString().toStdString();
}
void MemoryViewer::saveSettings(QSettings* settings)
@@ -324,13 +324,17 @@ namespace armarx::armem::gui
void MemoryViewer::writeConfigDialog(SimpleConfigDialog* dialog)
{
- dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>({CONFIG_KEY_MEMORY, "ArMemMemoryNameSystem", "*"});
+ dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>({CONFIG_KEY_MEMORY, "MemoryNameSystem", "*"});
dialog->addProxyFinder<armarx::DebugObserverInterfacePrx>({CONFIG_KEY_DEBUG_OBSERVER, "Debug Observer", "DebugObserver"});
}
void MemoryViewer::readConfigDialog(SimpleConfigDialog* dialog)
{
mnsName = dialog->getProxyName(CONFIG_KEY_MEMORY);
+ if (mnsName.empty())
+ {
+ mnsName = "MemoryNameSystem";
+ }
debugObserverName = dialog->getProxyName(CONFIG_KEY_DEBUG_OBSERVER);
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 97c7befe6092f8e0525327e6c1b9599ee4ddcd1d..429d2f3603967f64728e59113841b85c862b8d46 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -9,6 +9,7 @@
#include <QLayout>
#include <QMenu>
#include <QSplitter>
+#include <QTreeWidget>
#include <QVBoxLayout>
#include <SimoxUtility/algorithm/string.h>
@@ -19,9 +20,10 @@
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
-#include <RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h>
-#include <RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h>
#include <RobotAPI/libraries/armem_gui/instance/ImageView.h>
+#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
+#include <RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h>
+#include <RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h>
namespace armarx::armem::gui::instance
@@ -147,23 +149,23 @@ namespace armarx::armem::gui::instance
void InstanceView::updateData(const aron::datanavigator::DictNavigatorPtr& data, aron::typenavigator::ObjectNavigatorPtr aronType)
{
- armarx::gui::clearItem(treeItemData);
if (!data)
{
+ armarx::gui::clearItem(treeItemData);
QTreeWidgetItem* item = new QTreeWidgetItem({"(No data.)"});
treeItemData->addChild(item);
}
else if (useTypeInfo && aronType)
{
- TreeTypedDataVisitor visitor(treeItemData);
- visitor.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
- visitor.applyTo(*aronType, *data);
+ TypedDataTreeBuilder builder;
+ builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
+ builder.updateTree(treeItemData, *aronType, *data);
}
else
{
- TreeDataVisitor visitor(treeItemData);
- visitor.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
- visitor.applyTo(*data);
+ DataTreeBuilder builder;
+ builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
+ builder.updateTree(treeItemData, *data);
}
treeItemData->setExpanded(true);
}
@@ -211,7 +213,7 @@ namespace armarx::armem::gui::instance
case aron::type::Descriptor::eIVTCByteImage:
{
QStringList qpath = item->data(int(Columns::KEY), Qt::UserRole).toStringList();
- aron::Path path = TreeTypedDataVisitor::deserializePath(qpath);
+ aron::Path path = deserializePath(qpath);
QAction* viewAction = new QAction("Show image");
menu.addAction(viewAction);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
index 640712dffb91bbd019bc8cdb58469b5309487575..5f937f419cd3111daf1146a6640e0ea9348f6d42 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
@@ -6,7 +6,7 @@
#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h>
-#include <RobotAPI/libraries/armem/core/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
class QGroupBox;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp
deleted file mode 100644
index fa51ab6fe0018927ebb03da63828a7aea598035a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-#include "TreeTypedDataVisitor.h"
-
-#include <SimoxUtility/algorithm/string.h>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-
-namespace armarx::armem::gui
-{
-
- std::string TreeTypedDataVisitor::sanitizeTypeName(const std::string& typeName) const
- {
- std::string n = typeName;
- n = simox::alg::replace_all(n, "Aron", "");
- n = simox::alg::replace_all(n, "Type", "");
- return n;
- }
-
- QStringList TreeTypedDataVisitor::serializePath(const aron::Path& path)
- {
- QStringList qpath;
- qpath.append(QString::fromStdString(path.getRootIdentifier()));
- qpath.append(QString::fromStdString(path.getDelimeter()));
- for (const std::string& item : path.getPath())
- {
- qpath.append(QString::fromStdString(item));
- }
- return qpath;
- }
-
- aron::Path TreeTypedDataVisitor::deserializePath(const QStringList& qpath)
- {
- ARMARX_CHECK_GREATER_EQUAL(qpath.size(), 2);
- std::vector<std::string> pathItems;
- for (int i = 2; i < qpath.size(); ++i)
- {
- pathItems.push_back(qpath.at(i).toStdString());
- }
- aron::Path path(qpath.at(0).toStdString(), qpath.at(1).toStdString(), pathItems);
- return path;
- }
-
-
-
-
-}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f41f5d4cb447e9571d5145f26af5cbff6c9db2cf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.cpp
@@ -0,0 +1,76 @@
+#include "DataDisplayVisitor.h"
+
+
+namespace armarx::aron
+{
+
+ std::string DataDisplayVisitor::getValue(DataNavigator& n)
+ {
+ DataDisplayVisitor v;
+ v.applyTo(n);
+ return v.value.str();
+ }
+
+ bool DataDisplayVisitor::visitEnter(DictDataNavigator& n)
+ {
+ value << n.childrenSize() << " items";
+ return false;
+ }
+
+ bool DataDisplayVisitor::visitEnter(ListDataNavigator& n)
+ {
+ value << n.childrenSize() << " items";
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(BoolDataNavigator& b)
+ {
+ if (b.getValue())
+ {
+ value << "true";
+ }
+ else
+ {
+ value << "false";
+ }
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(DoubleDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(FloatDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(IntDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(LongDataNavigator& n)
+ {
+ value << n.getValue();
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(StringDataNavigator& n)
+ {
+ value << "'" << n.getValue() << "'";
+ return false;
+ }
+
+ bool DataDisplayVisitor::visit(NDArrayDataNavigator& n)
+ {
+ value << "shape " << aron::datanavigator::NDArrayNavigator::DimensionsToString(n.getDimensions());
+ return false;
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..8f48a57f0cd42a7e69dec44dd37ec489b50b440d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <sstream>
+
+#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
+
+
+namespace armarx::aron
+{
+
+ class DataDisplayVisitor : public aron::visitor::DataVisitor
+ {
+ public:
+
+ static std::string getValue(DataNavigator& n);
+
+
+ public:
+
+ std::stringstream value;
+
+
+ bool visitEnter(DictDataNavigator& n) override;
+ bool visitEnter(ListDataNavigator& n) override;
+
+ bool visit(BoolDataNavigator& b) override;
+ bool visit(DoubleDataNavigator& n) override;
+ bool visit(FloatDataNavigator& n) override;
+ bool visit(IntDataNavigator& n) override;
+ bool visit(LongDataNavigator& n) override;
+ bool visit(StringDataNavigator& n) override;
+
+ bool visit(NDArrayDataNavigator& n) override;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..827d14a599af43e6067fd1b3fa18bb136dfcbed8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.cpp
@@ -0,0 +1,147 @@
+#include "TypedDataDisplayVisitor.h"
+
+#include <iomanip> // std::setprecision
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include "DataDisplayVisitor.h"
+
+
+namespace armarx::aron
+{
+
+ std::string TypedDataDisplayVisitor::getValue(TypeNavigator& type, DataNavigator& data)
+ {
+ TypedDataDisplayVisitor v;
+ bool r = v.applyTo(type, data);
+ ARMARX_CHECK(!r);
+ return v.value.str();
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(DictTypeNavigator&, DictDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(ObjectTypeNavigator&, DictDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(ListTypeNavigator&, ListDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visitEnter(TupleTypeNavigator&, ListDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(BoolTypeNavigator&, BoolDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(DoubleTypeNavigator&, DoubleDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(FloatTypeNavigator&, FloatDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(IntTypeNavigator&, IntDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(LongTypeNavigator&, LongDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(StringTypeNavigator&, StringDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(TimeTypeNavigator&, LongDataNavigator& data)
+ {
+ armem::Time time = armem::Time::microSeconds(data.getValue());
+ armem::toDateTimeMilliSeconds(time);
+ value << armem::toDateTimeMilliSeconds(time);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(IVTCByteImageTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(OpenCVMatTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(PCLPointCloudTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ value << DataDisplayVisitor::getValue(data);
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(PoseTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
+ value << std::setprecision(2) << std::fixed;
+ value << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", ""));
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(PositionTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
+ value << std::setprecision(2) << std::fixed;
+ value << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, "", coeffSep, "", "", "", ""));
+ return false;
+ }
+
+ bool TypedDataDisplayVisitor::visit(OrientationTypeNavigator&, NDArrayDataNavigator& data)
+ {
+ const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
+ value << std::setprecision(2) << std::fixed;
+ value << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y() << coeffSep << quat.z();
+ return false;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..3923b7d0d74a1ee9c4b1ea7a5664ad3f83396051
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/display_visitors/TypedDataDisplayVisitor.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#include <sstream>
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h>
+
+
+namespace armarx::aron
+{
+
+ class TypedDataDisplayVisitor : public aron::visitor::TypedDataVisitor
+ {
+ public:
+
+ static std::string getValue(TypeNavigator& type, DataNavigator& data);
+
+
+ public:
+
+ std::stringstream value;
+
+
+ bool visitEnter(DictTypeNavigator&, DictDataNavigator& data) override;
+ bool visitEnter(ObjectTypeNavigator&, DictDataNavigator& data) override;
+
+ bool visitEnter(ListTypeNavigator&, ListDataNavigator& data) override;
+ bool visitEnter(TupleTypeNavigator&, ListDataNavigator& data) override;
+
+
+ bool visit(BoolTypeNavigator&, BoolDataNavigator& data) override;
+ bool visit(DoubleTypeNavigator&, DoubleDataNavigator& data) override;
+ bool visit(FloatTypeNavigator&, FloatDataNavigator& data) override;
+ bool visit(IntTypeNavigator&, IntDataNavigator& data) override;
+ bool visit(LongTypeNavigator&, LongDataNavigator& data) override;
+ bool visit(StringTypeNavigator&, StringDataNavigator& data) override;
+ bool visit(TimeTypeNavigator&, LongDataNavigator& data) override;
+
+
+ bool visit(EigenMatrixTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(EigenQuaternionTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(IVTCByteImageTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(OpenCVMatTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(PCLPointCloudTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(PoseTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(PositionTypeNavigator&, NDArrayDataNavigator& data) override;
+ bool visit(OrientationTypeNavigator&, NDArrayDataNavigator& data) override;
+
+
+ protected:
+
+ std::string coeffSep = " ";
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9db04814561de68c4e7390093a7a7b3659f0219c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
@@ -0,0 +1,46 @@
+#include "sanitize_typename.h"
+
+#include <sstream>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+
+
+static const std::string MemoryIDTypeName = armarx::armem::arondto::MemoryID::toInitialAronType()->getName();
+
+std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& typeName)
+{
+ if (typeName == MemoryIDTypeName)
+ {
+ return "MemoryID";
+ }
+
+ namespace s = simox::alg;
+ std::string n = typeName;
+ n = s::replace_all(n, "Aron", "");
+ n = s::replace_all(n, "Type", "");
+ n = s::replace_all(n, "type::", "");
+ if (s::starts_with(n, "Object<") && s::ends_with(n, ">"))
+ {
+ std::string begin = "Object<";
+ std::string end = ">";
+ n = n.substr(begin.size(), n.size() - begin.size() - end.size());
+ }
+
+ if (true)
+ {
+ const std::string del = "::";
+ size_t find = n.rfind(del);
+ if (find != n.npos)
+ {
+ find += del.size(); // include del
+ std::stringstream ss;
+ ss << n.substr(find) << " (" << n.substr(0, find - del.size()) << ")";
+ n = ss.str();
+ }
+ }
+
+ return n;
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
new file mode 100644
index 0000000000000000000000000000000000000000..80c77acf229ecb6d70b55b6ac1c5b951a0cb89bd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <string>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ std::string sanitizeTypeName(const std::string& typeName);
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a01b0b4620748b581db928c4e7ddcbd7af1a3f46
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp
@@ -0,0 +1,35 @@
+#include "serialize_path.h"
+
+#include <RobotAPI/libraries/aron/core/Path.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <QString>
+#include <QStringList>
+
+
+QStringList armarx::armem::gui::instance::serializePath(const aron::Path& path)
+{
+ QStringList qpath;
+ qpath.append(QString::fromStdString(path.getRootIdentifier()));
+ qpath.append(QString::fromStdString(path.getDelimeter()));
+ for (const std::string& item : path.getPath())
+ {
+ qpath.append(QString::fromStdString(item));
+ }
+ return qpath;
+}
+
+armarx::aron::Path armarx::armem::gui::instance::deserializePath(const QStringList& qpath)
+{
+ ARMARX_CHECK_GREATER_EQUAL(qpath.size(), 2);
+ std::vector<std::string> pathItems;
+ for (int i = 2; i < qpath.size(); ++i)
+ {
+ pathItems.push_back(qpath.at(i).toStdString());
+ }
+ aron::Path path(qpath.at(0).toStdString(), qpath.at(1).toStdString(), pathItems);
+ return path;
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h
new file mode 100644
index 0000000000000000000000000000000000000000..26517c6d11e3ee25071bb6f5e01da3996c6553cf
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h
@@ -0,0 +1,18 @@
+#pragma once
+
+
+namespace armarx::aron
+{
+ class Path;
+}
+class QStringList;
+
+
+namespace armarx::armem::gui::instance
+{
+
+ QStringList serializePath(const aron::Path& path);
+ aron::Path deserializePath(const QStringList& qpath);
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..07ccac8866d870b153fba28915006d3dfd46cdae
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
@@ -0,0 +1,58 @@
+#include "DataTreeBuilder.h"
+
+#include <QTreeWidgetItem>
+
+#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ DataTreeBuilder::DataTreeBuilder()
+ {
+ }
+
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, aron::datanavigator::DictNavigator& data)
+ {
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
+ {
+ auto child = data.getElement(key);
+ this->update(item, key, *child);
+ return true;
+ });
+
+ builder.updateTree(parent, data.getAllKeys());
+ }
+
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, aron::datanavigator::ListNavigator& data)
+ {
+ auto children = data.getChildren();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &children](size_t key, QTreeWidgetItem * item)
+ {
+ this->update(item, std::to_string(key), *children.at(key));
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(children.size()));
+ }
+
+
+ void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data)
+ {
+ this->setRowTexts(item, key, data);
+
+ if (auto cast = dynamic_cast<aron::datanavigator::DictNavigator*>(&data))
+ {
+ updateTree(item, *cast);
+ }
+ else if (auto cast = dynamic_cast<aron::datanavigator::ListNavigator*>(&data))
+ {
+ updateTree(item, *cast);
+ }
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9aa8471e5d4720c8e747844deb45459d68b1b90
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
+
+#include "DataTreeBuilderBase.h"
+
+
+namespace armarx::armem::gui::instance
+{
+
+ class DataTreeBuilder : public DataTreeBuilderBase
+ {
+ public:
+
+ DataTreeBuilder();
+
+ void updateTree(QTreeWidgetItem* parent, aron::datanavigator::DictNavigator& data);
+ void updateTree(QTreeWidgetItem* parent, aron::datanavigator::ListNavigator& data);
+
+
+ protected:
+
+ void update(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data);
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f837fe2fa6451cf8edd82793f2c6ec773560ce2c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
@@ -0,0 +1,97 @@
+#include "DataTreeBuilderBase.h"
+
+#include <QTreeWidgetItem>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
+#include <RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ DataTreeBuilderBase::DataTreeBuilderBase()
+ {
+ }
+
+ DataTreeBuilderBase::~DataTreeBuilderBase()
+ {}
+
+ void DataTreeBuilderBase::setColumns(int key, int value, int type)
+ {
+ this->columnKey = key;
+ this->columnType = type;
+ this->columnValue = value;
+ }
+
+
+ QTreeWidgetItem* DataTreeBuilderBase::makeItem(const std::string& key) const
+ {
+ return new QTreeWidgetItem(QStringList{QString::fromStdString(key)});
+ }
+
+ QTreeWidgetItem* DataTreeBuilderBase::makeItem(size_t key) const
+ {
+ QTreeWidgetItem* item = new QTreeWidgetItem();
+ item->setData(0, Qt::UserRole, static_cast<int>(key));
+ return item;
+ }
+
+ void DataTreeBuilderBase::setRowTexts(QTreeWidgetItem* item, const std::string& key, const std::string& value, const std::string& typeName) const
+ {
+ item->setText(columnKey, QString::fromStdString(key));
+ item->setText(columnValue, QString::fromStdString(value));
+ item->setText(columnType, QString::fromStdString(typeName));
+ }
+
+
+ void DataTreeBuilderBase::setRowTexts(
+ QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data)
+ {
+ const std::string value = armarx::aron::DataDisplayVisitor::getValue(data);
+ setRowTexts(item, key, value, data.getName());
+ }
+
+ DataTreeBuilderBase::DictBuilder DataTreeBuilderBase::getDictBuilder() const
+ {
+ DictBuilder builder;
+ builder.setCompareFn([](const std::string & key, QTreeWidgetItem * item)
+ {
+ return armarx::detail::compare(key, item->text(0).toStdString());
+ });
+ builder.setMakeItemFn([this](const std::string & key)
+ {
+ return this->makeItem(key);
+ });
+ return builder;
+ }
+
+
+ DataTreeBuilderBase::ListBuilder DataTreeBuilderBase::getListBuilder() const
+ {
+ ListBuilder builder;
+ builder.setCompareFn([](size_t key, QTreeWidgetItem * item)
+ {
+ return armarx::detail::compare(static_cast<int>(key), item->data(0, Qt::UserRole).toInt());
+ });
+ builder.setMakeItemFn([this](size_t key)
+ {
+ return this->makeItem(key);
+ });
+ return builder;
+ }
+
+
+ std::vector<size_t> DataTreeBuilderBase::getIndex(size_t size) const
+ {
+ std::vector<size_t> index;
+ index.reserve(size);
+ for (size_t i = 0; i < size; ++i)
+ {
+ index.push_back(i);
+ }
+ return index;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..b26a422ea71c90c2b769217a274eac7887eb8f9d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
@@ -0,0 +1,53 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
+
+
+namespace armarx
+{
+ template <class ContainerT> struct TreeWidgetBuilder;
+}
+
+class QTreeWidgetItem;
+
+
+namespace armarx::armem::gui::instance
+{
+
+ class DataTreeBuilderBase
+ {
+ public:
+
+ DataTreeBuilderBase();
+ virtual ~DataTreeBuilderBase();
+
+ void setColumns(int key, int value, int type);
+
+
+ protected:
+
+ using DictBuilder = armarx::TreeWidgetBuilder<std::vector<std::string>>;
+ using ListBuilder = armarx::TreeWidgetBuilder<std::vector<size_t>>;
+
+ DictBuilder getDictBuilder() const;
+ ListBuilder getListBuilder() const;
+ std::vector<size_t> getIndex(size_t size) const;
+
+
+ QTreeWidgetItem* makeItem(const std::string& key) const;
+ QTreeWidgetItem* makeItem(size_t key) const;
+
+ void setRowTexts(QTreeWidgetItem* item, const std::string& key, const std::string& value, const std::string& typeName = "") const;
+ void setRowTexts(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data);
+
+ public:
+
+ int columnKey = 0;
+ int columnValue = 1;
+ int columnType = 2;
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5be64a3f5657893e2c6f36e4695eff4a08c9dc9e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.cpp
@@ -0,0 +1,180 @@
+#include "TypedDataTreeBuilder.h"
+
+#include <QTreeWidgetItem>
+
+#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>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ TypedDataTreeBuilder::TypedDataTreeBuilder()
+ {
+ }
+
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::DictNavigator& type,
+ aron::datanavigator::DictNavigator& data)
+ {
+ auto childType = type.getAcceptedType();
+ if (childType)
+ {
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &childType, &data](const std::string & key, QTreeWidgetItem * item)
+ {
+ auto childData = data.getElement(key);
+ if (childData)
+ {
+ this->update(item, key, *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, data.getAllKeys());
+ }
+ }
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::ObjectNavigator& type,
+ aron::datanavigator::DictNavigator& data)
+ {
+ DictBuilder builder = getDictBuilder();
+ builder.setUpdateItemFn([this, &type, &data](const std::string & key, QTreeWidgetItem * item)
+ {
+ auto childType = type.getMemberType(key);
+ auto childData = data.getElement(key);
+
+ if (childType && childData)
+ {
+ this->update(item, key, *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, type.getAllKeys());
+ }
+
+
+ void TypedDataTreeBuilder::updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::ListNavigator& type,
+ aron::datanavigator::ListNavigator& data)
+ {
+ auto childType = type.getAcceptedType();
+ if (childType)
+ {
+ auto children = data.getChildren();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &children, &childType](size_t key, QTreeWidgetItem * item)
+ {
+ auto childData = children.at(key);
+ if (childData)
+ {
+ this->update(item, std::to_string(key), *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(children.size()));
+ }
+ }
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::PairNavigator& type,
+ aron::datanavigator::ListNavigator& 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));
+
+ if (childType && childData)
+ {
+ this->update(item, std::to_string(i), *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(data.childrenSize()));
+ }
+
+ void TypedDataTreeBuilder::updateTree(
+ QTreeWidgetItem* parent,
+ aron::typenavigator::TupleNavigator& type,
+ aron::datanavigator::ListNavigator& data)
+ {
+ auto childTypes = type.getAcceptedTypes();
+
+ ListBuilder builder = getListBuilder();
+ builder.setUpdateItemFn([this, &data, &childTypes](size_t i, QTreeWidgetItem * item)
+ {
+ auto childType = childTypes.at(i);
+ auto childData = data.getElement(static_cast<unsigned int>(i));
+
+ if (childType && childData)
+ {
+ this->update(item, std::to_string(i), *childType, *childData);
+ }
+ return true;
+ });
+
+ builder.updateTree(parent, getIndex(type.getAcceptedTypes().size()));
+ }
+
+
+ void TypedDataTreeBuilder::update(
+ QTreeWidgetItem* item,
+ const std::string& key,
+ aron::typenavigator::Navigator& type,
+ aron::datanavigator::Navigator& data)
+ {
+ using namespace aron::typenavigator;
+
+ const std::string value = armarx::aron::TypedDataDisplayVisitor::getValue(type, data);
+ const std::string typeName = sanitizeTypeName(type.getName());
+ setRowTexts(item, key, value, typeName);
+
+ item->setData(columnKey, Qt::UserRole, serializePath(data.getPath()));
+ item->setData(columnType, Qt::UserRole, static_cast<int>(type.getDescriptor()));
+
+ if (auto t = dynamic_cast<ObjectNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::DictNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<DictNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::DictNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<ListNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<PairNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ }
+ else if (auto t = dynamic_cast<TupleNavigator*>(&type))
+ {
+ _updateTree<aron::datanavigator::ListNavigator>(item, *t, data);
+ }
+ }
+
+ template <class DataT, class TypeT>
+ void TypedDataTreeBuilder::_updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data)
+ {
+ DataT& dataCast = dynamic_cast<DataT&>(data);
+ updateTree(item, type, dataCast);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
new file mode 100644
index 0000000000000000000000000000000000000000..57580a1f6d3a84ca639131e0da375a6129650e0a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/Navigator.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/List.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/type/Navigator.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/List.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Pair.h>
+#include <RobotAPI/libraries/aron/core/navigator/type/container/Tuple.h>
+
+#include "DataTreeBuilderBase.h"
+
+
+class QStringList;
+
+
+namespace armarx::armem::gui::instance
+{
+
+ class TypedDataTreeBuilder : public DataTreeBuilderBase
+ {
+ public:
+
+ TypedDataTreeBuilder();
+
+
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::DictNavigator& type,
+ aron::datanavigator::DictNavigator& data);
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::ObjectNavigator& type,
+ aron::datanavigator::DictNavigator& data);
+
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::ListNavigator& type,
+ aron::datanavigator::ListNavigator& data);
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::PairNavigator& type,
+ aron::datanavigator::ListNavigator& data);
+ void updateTree(QTreeWidgetItem* parent,
+ aron::typenavigator::TupleNavigator& type,
+ aron::datanavigator::ListNavigator& data);
+
+
+ protected:
+
+ void update(QTreeWidgetItem* item,
+ const std::string& key,
+ aron::typenavigator::Navigator& type,
+ aron::datanavigator::Navigator& data);
+
+ template <class DataT, class TypeT>
+ void _updateTree(QTreeWidgetItem* item, TypeT& type, aron::datanavigator::Navigator& data);
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
similarity index 72%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.cpp
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
index 0d1721ef244c8e4a44eba5003a19302c190518d7..8bc5889d29298e41dbb27cfa6114f9841c7d3f98 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
@@ -3,7 +3,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
similarity index 94%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
index b751084bf8af57f2d7f6598e2ae2e835c0948b6c..61d76bcfc58a81d138a442d07e1f64bc0eb7b922 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
@@ -8,10 +8,10 @@
#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
-#include "TreeDataVisitorBase.h"
+#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
class TreeDataVisitor :
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
similarity index 90%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
index 39ba3d5e35de1247790dfb7c0e8837096b3b1b77..87cd2ffb9bdee5de102b3b026fb508784f0ff0c0 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
@@ -3,7 +3,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
TreeDataVisitorBase::TreeDataVisitorBase()
@@ -62,7 +62,7 @@ namespace armarx::armem::gui
return true;
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss)
+ void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss) const
{
if (n.getValue())
{
@@ -74,12 +74,12 @@ namespace armarx::armem::gui
}
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss)
+ void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss) const
{
ss << "'" << n.getValue() << "'";
}
- void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss)
+ void TreeDataVisitorBase::streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss) const
{
ss << "shape " << aron::datanavigator::NDArrayNavigator::DimensionsToString(n.getDimensions());
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
similarity index 80%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
index 35dcca57a868e050c561045af0a948276f43af60..c9d4ab0cdaf462761b10e422ef41f415ec54d7a1 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeDataVisitorBase.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
@@ -9,7 +9,7 @@
#include <RobotAPI/libraries/aron/core/navigator/visitors/DataVisitor.h>
-namespace armarx::armem::gui
+namespace armarx::armem::gui::instance
{
class TreeDataVisitorBase
@@ -40,26 +40,31 @@ namespace armarx::armem::gui
return true;
}
- template <class Navigator>
- QStringList makeValueRowStrings(const std::string& key, Navigator& n, const std::string& typeName)
+ QStringList makeValueRowStrings(const std::string& key, const std::string& value, const std::string& typeName) const
{
- std::stringstream value;
- streamValueText(n, value);
QStringList cols;
cols.insert(columnKey, QString::fromStdString(key));
- cols.insert(columnValue, QString::fromStdString(value.str()));
+ cols.insert(columnValue, QString::fromStdString(value));
cols.insert(columnType, QString::fromStdString(typeName));
return cols;
}
template <class Navigator>
- void streamValueText(Navigator& n, std::stringstream& ss)
+ QStringList makeValueRowStrings(const std::string& key, Navigator& n, const std::string& typeName) const
+ {
+ std::stringstream value;
+ streamValueText(n, value);
+ return makeValueRowStrings(key, value.str(), typeName);
+ }
+
+ template <class Navigator>
+ void streamValueText(Navigator& n, std::stringstream& ss) const
{
ss << n.getValue();
}
- void streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss);
- void streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss);
- void streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss);
+ void streamValueText(aron::visitor::DataVisitor::BoolDataNavigator& n, std::stringstream& ss) const;
+ void streamValueText(aron::visitor::DataVisitor::StringDataNavigator& n, std::stringstream& ss) const;
+ void streamValueText(aron::visitor::DataVisitor::NDArrayDataNavigator& n, std::stringstream& ss) const;
public:
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4ed919f9b46be2c62966fccbd89fbefa0370449
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.cpp
@@ -0,0 +1,56 @@
+#include "TreeTypedDataVisitor.h"
+
+#include <iomanip> // std::setprecision
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/armem/core.h>
+
+
+namespace armarx::armem::gui::instance
+{
+
+ QTreeWidgetItem* TreeTypedDataVisitor::makeItem(const std::string& key, NDArrayDataNavigator& data, const PoseTypeNavigator& type) const
+ {
+ QTreeWidgetItem* item = makeItem<NDArrayDataNavigator, PoseTypeNavigator>(key, data, type);
+ // ToDo: Do something special.
+ return item;
+ }
+
+ void TreeTypedDataVisitor::streamValueText(LongDataNavigator& data, const TimeTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ armem::Time time = armem::Time::microSeconds(data.getValue());
+ armem::toDateTimeMilliSeconds(time);
+ ss << armem::toDateTimeMilliSeconds(time);
+ }
+
+ void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const PoseTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ const Eigen::Matrix4f pose = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
+ ss << std::setprecision(2) << std::fixed;
+ ss << pose.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, coeffSep, "\n", "", "", "", ""));
+ }
+
+ void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const PositionTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ const Eigen::Vector3f pos = aron::converter::AronEigenConverter::ConvertToVector3f(data);
+ ss << std::setprecision(2) << std::fixed;
+ ss << pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, "", coeffSep, "", "", "", ""));
+ }
+
+ void TreeTypedDataVisitor::streamValueText(NDArrayDataNavigator& data, const OrientationTypeNavigator& type, std::stringstream& ss) const
+ {
+ (void) type;
+ const Eigen::Quaternionf quat = aron::converter::AronEigenConverter::ConvertToQuaternionf(data);
+ ss << std::setprecision(2) << std::fixed;
+ ss << quat.w() << coeffSep << "|" << coeffSep << quat.x() << coeffSep << quat.y() << coeffSep << quat.z();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
similarity index 53%
rename from source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h
rename to source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
index b5493584b2c91eeed01fb6f9758d23a956b5225c..fb1c1baa557167432ffaa19f46e71d223a5c080d 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
@@ -7,10 +7,14 @@
#include <QLabel>
#include <RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h>
-#include "TreeDataVisitorBase.h"
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
-namespace armarx::armem::gui
+#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
+
+
+namespace armarx::armem::gui::instance
{
class TreeTypedDataVisitor :
@@ -83,12 +87,20 @@ namespace armarx::armem::gui
{
return this->addValueRow(key, data, type);
}
+ bool visit(TimeTypeNavigator& type, const std::string& key, LongDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type);
+ }
bool visit(EigenMatrixTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
{
return this->addValueRow(key, data, type);
}
+ bool visit(EigenQuaternionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type);
+ }
bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
{
return this->addValueRow(key, data, type);
@@ -101,33 +113,84 @@ namespace armarx::armem::gui
{
return this->addValueRow(key, data, type);
}
-
-
- std::string sanitizeTypeName(const std::string& typeName) const;
+ bool visit(PoseTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type);
+ }
+ bool visit(PositionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type);
+ }
+ bool visit(OrientationTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data) override
+ {
+ return this->addValueRow(key, data, type);
+ }
protected:
- template <class DataNavigatorT>
- bool addValueRow(const std::string& key, DataNavigatorT& dataNavigator, const TypeNavigator& type)
+ template <class DataNavigatorT, class TypeNavigatorT>
+ bool addValueRow(const std::string& key, DataNavigatorT& data, const TypeNavigatorT& type)
{
if (items.size() > 0)
{
- QTreeWidgetItem* item = new QTreeWidgetItem(this->makeValueRowStrings(key, dataNavigator, sanitizeTypeName(type.getName())));
+ QTreeWidgetItem* item = makeItem(key, data, type);
items.top()->addChild(item);
- item->setData(columnKey, Qt::UserRole, serializePath(dataNavigator.getPath()));
+ item->setData(columnKey, Qt::UserRole, serializePath(data.getPath()));
item->setData(columnType, Qt::UserRole, int(type.getDescriptor()));
+
+ if (false)
+ {
+ QFont font;
+ font.setFamily("Consolas");
+ font.setStyleHint(QFont::Monospace);
+ font.setFixedPitch(true);
+ font.setPointSize(10);
+ item->setFont(columnValue, font);
+ }
}
return true;
}
- public:
+ template <class DataNavigatorT, class TypeNavigatorT>
+ QTreeWidgetItem* makeItem(const std::string& key, DataNavigatorT& data, const TypeNavigatorT& type) const
+ {
+ std::stringstream ss;
+ try
+ {
+ this->streamValueText(data, type, ss);
+ }
+ catch (const aron::error::AronException& e)
+ {
+ ss << "x ";
+ TreeDataVisitorBase::streamValueText(data, ss);
+ std::stringstream es;
+ es << e.what();
+ ss << simox::alg::replace_all(es.str(), "\n", " | ");
+ }
+ return new QTreeWidgetItem(this->makeValueRowStrings(key, ss.str(), sanitizeTypeName(type.getName())));
+ }
+
+ QTreeWidgetItem* makeItem(const std::string& key, NDArrayDataNavigator& data, const PoseTypeNavigator& type) const;
+
+
+ template <class DataNavigatorT, class TypeNavigatorT>
+ void streamValueText(DataNavigatorT& data, const TypeNavigatorT& type, std::stringstream& ss) const
+ {
+ // Fallback to type-agnostic (but data-aware).
+ (void) type;
+ TreeDataVisitorBase::streamValueText(data, ss);
+ }
- static QStringList serializePath(const aron::Path& path);
- static aron::Path deserializePath(const QStringList& qpath);
+ using TreeDataVisitorBase::streamValueText;
+ void streamValueText(LongDataNavigator& data, const TimeTypeNavigator& type, std::stringstream& ss) const;
+ void streamValueText(NDArrayDataNavigator& data, const PoseTypeNavigator& type, std::stringstream& ss) const;
+ void streamValueText(NDArrayDataNavigator& data, const PositionTypeNavigator& type, std::stringstream& ss) const;
+ void streamValueText(NDArrayDataNavigator& data, const OrientationTypeNavigator& type, std::stringstream& ss) const;
+ std::string coeffSep = " ";
};
}
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index 192ec825e85de0782b1839db975dc088ee62c399..3d4c50f66c6263ec474acd552233b86f895a7862 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -1,5 +1,7 @@
#include "TreeWidget.h"
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+
#include <RobotAPI/libraries/aron/core/navigator/type/container/Object.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -292,16 +294,7 @@ namespace armarx::armem::gui::memory
if (container.aronType())
{
typeName = container.aronType()->getName();
-
- std::string del = "::";
- size_t find = typeName.rfind(del);
- if (find != typeName.npos)
- {
- find += del.size(); // include del
- std::stringstream ss;
- ss << typeName.substr(find) << " (" << typeName.substr(0, find) << ")";
- typeName = ss.str();
- }
+ typeName = instance::sanitizeTypeName(typeName);
}
else
{
diff --git a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
index 5ecf962dbef91f033435c532149c49285c408bc2..d1e0e4a011e05edc22c2ab8fb0835b95150c24da 100644
--- a/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
+++ b/source/RobotAPI/libraries/armem_gui/test/ArMemGuiTest.cpp
@@ -29,13 +29,13 @@
#include <iostream>
-#include <RobotAPI/libraries/armem_gui/instance/TreeTypedDataVisitor.h>
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+
+using namespace armarx::armem::gui::instance;
BOOST_AUTO_TEST_CASE(test_sanitizeTypeName)
{
- armarx::armem::gui::TreeTypedDataVisitor v;
-
- BOOST_CHECK_EQUAL(v.sanitizeTypeName("AronDictType<AronFloatType>"), "Dict<Float>");
- BOOST_CHECK_EQUAL(v.sanitizeTypeName("AronListType<AronFloatType>"), "List<Float>");
+ BOOST_CHECK_EQUAL(sanitizeTypeName("AronDictType<AronFloatType>"), "Dict<Float>");
+ BOOST_CHECK_EQUAL(sanitizeTypeName("AronListType<AronFloatType>"), "List<Float>");
}
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..114f2b79413e4ef5c79fcb650d29299598456ad4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -0,0 +1,51 @@
+set(LIB_NAME armem_objects)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # ArmarXGui
+ ArmarXGuiComponentPlugins
+ # RobotAPI
+ RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+ RobotAPI::Core
+ RobotAPI::libraries::armem
+ HEADERS
+ aron_conversions.h
+
+ server/class/Segment.h
+
+ server/instance/Segment.h
+ server/instance/SegmentAdapter.h
+ server/instance/Decay.h
+ server/instance/RobotHeadMovement.h
+ server/instance/Visu.h
+ SOURCES
+ aron_conversions.cpp
+
+ server/class/Segment.cpp
+
+ server/instance/Segment.cpp
+ server/instance/SegmentAdapter.cpp
+ server/instance/Decay.cpp
+ server/instance/RobotHeadMovement.cpp
+ server/instance/Visu.cpp
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/ObjectClass.xml
+ aron/ObjectInstance.xml
+)
+
+add_library(${PROJECT_NAME}::armem_objects ALIAS armem_objects)
+
+# add unit tests
+# add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
new file mode 100644
index 0000000000000000000000000000000000000000..650082eb4ef8b924c8368fdd6cdf8da4578e105a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -0,0 +1,58 @@
+<!--
+Core segment type of Object/Class.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.xml>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.xml>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::ObjectClass">
+
+ <ObjectChild key="id">
+ <armarx::arondto::ObjectID />
+ </ObjectChild>
+
+ <ObjectChild key="simoxXmlPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="meshWrlPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="meshObjPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="aabb">
+ <simox::arondto::AxisAlignedBoundingBox />
+ </ObjectChild>
+ <ObjectChild key="oobb">
+ <simox::arondto::OrientedBox />
+ </ObjectChild>
+
+ <ObjectChild key="names">
+ <armarx::arondto::ObjectNames />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml
new file mode 100644
index 0000000000000000000000000000000000000000..774c6e0d5d32734a16d7fdc918f491d19cfbfa39
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml
@@ -0,0 +1,33 @@
+<!--
+Core segment type of Object/Instance.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.xml>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::ObjectInstance">
+
+ <ObjectChild key="pose">
+ <armarx::objpose::arondto::ObjectPose />
+ </ObjectChild>
+
+ <ObjectChild key="classID">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+ <ObjectChild key="sourceID">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..859f3d601024ba97dd0e59e938afaba2718a95a1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -0,0 +1,27 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+
+
+void armarx::armem::fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
+{
+ bo = dto.pose;
+}
+
+void armarx::armem::toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
+{
+ dto.pose = bo;
+}
+
+
+void armarx::armem::fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
+{
+ objpose::fromAron(dto.pose, bo);
+}
+
+void armarx::armem::toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
+{
+ objpose::toAron(dto.pose, bo);
+}
+
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..7ea733e99c75895ff7aba463f36a0f53d0c4bd38
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+
+
+namespace armarx::armem
+{
+ void fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo);
+ void toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo);
+
+ void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo);
+ void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo);
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fcb0d8156e19658f98faf0da88f1da2223c86e43
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -0,0 +1,301 @@
+#include "Segment.h"
+
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <filesystem>
+
+
+namespace armarx::armem::server::obj::clazz
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("ClassSegment");
+ }
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object clazz core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+
+ defs->optional(p.objectsPackage, prefix + "ObjectsPackgage", "Name of the objects package to load from.");
+ defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
+ "If true, load the objects from the objects package on startup.");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectClass::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+
+ if (p.loadFromObjectsPackage)
+ {
+ loadByObjectFinder(p.objectsPackage);
+ }
+ }
+
+ void Segment::connect(viz::Client arviz)
+ {
+ this->arviz = arviz;
+ }
+
+
+ void Segment::loadByObjectFinder(const std::string& objectsPackage)
+ {
+ loadByObjectFinder(ObjectFinder(objectsPackage));
+ }
+
+ void Segment::loadByObjectFinder(const ObjectFinder& finder)
+ {
+ this->objectFinder = finder;
+ loadByObjectFinder();
+ }
+
+ void Segment::loadByObjectFinder()
+ {
+ const Time now = TimeUtil::GetTime();
+
+ const bool checkPaths = false;
+ std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
+
+ const MemoryID providerID = coreSegment->id().withProviderSegmentName(objectFinder.getPackageName());
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+
+ ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
+ << objectFinder.getPackageName() << "' ...";
+ Commit commit;
+ for (ObjectInfo& info : infos)
+ {
+ info.setLogError(false);
+
+ EntityUpdate& update = commit.updates.emplace_back();
+ update.entityID = providerID.withEntityName(info.id().str());
+ update.timeArrived = update.timeCreated = update.timeSent = now;
+
+ arondto::ObjectClass objectClass = objectClassFromInfo(info);
+ update.instancesData =
+ {
+ objectClass.toAron()
+ };
+ }
+ ARMARX_INFO << "Loaded " << commit.updates.size() << " object classes from '"
+ << objectFinder.getPackageName() << "'.";
+ iceMemory.commit(commit);
+ }
+
+ void Segment::visualizeClass(const MemoryID& entityID, bool showAABB, bool showOOBB)
+ {
+ const Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+
+ viz::Layer layerOrigin = arviz.layer("Origin");
+ layerOrigin.add(viz::Pose("Origin"));
+
+ viz::Layer layerObject = arviz.layer("Class Model");
+ viz::Layer layerAABB = arviz.layer("Class AABB");
+ viz::Layer layerOOBB = arviz.layer("Class OOBB");
+
+ if (coreSegment)
+ {
+ try
+ {
+ const armem::wm::Entity& entity = coreSegment->getEntity(entityID);
+ const armem::wm::EntityInstance& instance = entity.getLatestSnapshot().getInstance(0);
+
+ arondto::ObjectClass aron;
+ aron.fromAron(instance.data());
+
+ if (!aron.simoxXmlPath.package.empty())
+ {
+ layerObject.add(viz::Object(entityID.str())
+ .file(aron.simoxXmlPath.package, aron.simoxXmlPath.path)
+ .pose(pose));
+ }
+
+ if (showAABB)
+ {
+ layerAABB.add(viz::Box("AABB")
+ .pose(pose * simox::math::pose(aron.aabb.center))
+ .size(aron.aabb.extents)
+ .color(simox::Color::cyan(255, 64)));
+ }
+ if (showOOBB)
+ {
+ layerOOBB.add(viz::Box("OOBB")
+ .pose(pose * simox::math::pose(aron.oobb.center, aron.oobb.orientation))
+ .size(aron.oobb.extents)
+ .color(simox::Color::lime(255, 64)));
+ }
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_INFO << "Failed to visualize object class " << entityID << "."
+ << "\nReason: " << e.what();
+ }
+ catch (const aron::error::AronException& e)
+ {
+ ARMARX_INFO << "Failed to visualize object class " << entityID << "."
+ << "\nReason: " << e.what();
+ }
+ }
+
+ arviz.commit(layerObject, layerOrigin, layerAABB, layerOOBB);
+ }
+
+ arondto::ObjectClass Segment::objectClassFromInfo(const ObjectInfo& info)
+ {
+ namespace fs = std::filesystem;
+
+ arondto::ObjectClass data;
+
+ toAron(data.id, info.id());
+
+ if (fs::is_regular_file(info.simoxXML().absolutePath))
+ {
+ toAron(data.simoxXmlPath, info.simoxXML());
+ }
+ if (fs::is_regular_file(info.wavefrontObj().absolutePath))
+ {
+ toAron(data.meshObjPath, info.wavefrontObj());
+ }
+ if (fs::is_regular_file(info.file(".wrl").absolutePath))
+ {
+ toAron(data.meshWrlPath, info.file(".wrl"));
+ }
+
+ auto aabb = info.loadAABB();
+ toAron(data.aabb, aabb ? aabb.value() : simox::AxisAlignedBoundingBox());
+ auto oobb = info.loadOOBB();
+ toAron(data.oobb, oobb ? oobb.value() : simox::OrientedBoxf());
+
+ if (auto recogNames = info.loadRecognizedNames())
+ {
+ data.names.recognizedNames = recogNames.value();
+ }
+ if (auto spokenNames = info.loadSpokenNames())
+ {
+ data.names.spokenNames = spokenNames.value();
+ }
+
+ return data;
+ }
+
+
+ void Segment::RemoteGui::setup(const Segment& segment)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ data.setup(segment);
+ visu.setup(segment);
+
+ VBoxLayout layout;
+ layout.addChildren({data.group, visu.group});
+
+ group = {};
+ group.setLabel("Class");
+ group.addChildren({layout, VSpacer()});
+ }
+
+ void Segment::RemoteGui::update(Segment& segment)
+ {
+ data.update(segment);
+ visu.update(segment);
+ }
+
+ void Segment::RemoteGui::Data::setup(const Segment& segment)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ maxHistorySize.setValue(std::max(1, int(segment.p.maxHistorySize)));
+ maxHistorySize.setRange(1, 1e6);
+ infiniteHistory.setValue(segment.p.maxHistorySize == -1);
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ row++;
+ grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+ row++;
+
+ group = {};
+ group.setLabel("Data");
+ group.addChild(grid);
+ }
+
+ void Segment::RemoteGui::Data::update(Segment& segment)
+ {
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ {
+ std::scoped_lock lock(segment.memoryMutex);
+ segment.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.coreSegment)
+ {
+ segment.coreSegment->setMaxHistorySize(long(segment.p.maxHistorySize));
+ }
+ }
+ }
+
+
+
+ void Segment::RemoteGui::Visu::setup(const Segment& segment)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ showComboBox = {};
+ showOptionsIndex.clear();
+ for (const auto& [_, prov] : *segment.coreSegment)
+ {
+ for (const auto& [_, entity] : prov)
+ {
+ std::stringstream option;
+ option << entity.id().entityName << " (" << entity.id().providerSegmentName << ")";
+ showComboBox.addOption(option.str());
+ showOptionsIndex.push_back(entity.id());
+ }
+ }
+ if (showOptionsIndex.empty())
+ {
+ showComboBox.addOption("<none>");
+ }
+ showButton.setLabel("Visualize Object Class");
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(showComboBox, {row, 0}, {1, 2});
+ row++;
+ grid.add(showButton, {row, 0}, {1, 2});
+ row++;
+
+ group = {};
+ group.setLabel("Visualization");
+ group.addChild(grid);
+ }
+
+ void Segment::RemoteGui::Visu::update(Segment& segment)
+ {
+ if (showButton.wasClicked())
+ {
+ const size_t index = static_cast<size_t>(showComboBox.getIndex());
+ if (/*index >= 0 &&*/ index < showOptionsIndex.size())
+ {
+ std::scoped_lock lock(segment.memoryMutex);
+ segment.visualizeClass(showOptionsIndex.at(index));
+ }
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..92fa7c182db309aa743ebc6b3e863d9a76a03440
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -0,0 +1,105 @@
+#pragma once
+
+#include <mutex>
+#include <string>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+
+
+namespace armarx::armem::server::obj::clazz
+{
+
+ class Segment : public armarx::Logging
+ {
+ public:
+
+ Segment(armem::server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ void init();
+ void connect(viz::Client arviz);
+
+ void loadByObjectFinder(const std::string& objectsPackage);
+ void loadByObjectFinder(const ObjectFinder& finder);
+ void loadByObjectFinder();
+
+ void visualizeClass(const MemoryID& entityID, bool showAABB = true, bool showOOBB = true);
+
+
+ static arondto::ObjectClass objectClassFromInfo(const ObjectInfo& info);
+
+
+ private:
+
+ armem::server::MemoryToIceAdapter& iceMemory;
+ armem::wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+ ObjectFinder objectFinder;
+
+ viz::Client arviz;
+
+
+ struct Properties
+ {
+ std::string coreSegmentName = "Class";
+ long maxHistorySize = -1;
+
+ std::string objectsPackage = ObjectFinder::DefaultObjectsPackageName;
+ bool loadFromObjectsPackage = true;
+ };
+ Properties p;
+
+
+ public:
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ struct Data
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ armarx::RemoteGui::Client::CheckBox infiniteHistory;
+
+ void setup(const Segment& segment);
+ void update(Segment& segment);
+ };
+ Data data;
+
+ struct Visu
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ std::vector<MemoryID> showOptionsIndex;
+ armarx::RemoteGui::Client::ComboBox showComboBox;
+ armarx::RemoteGui::Client::Button showButton;
+
+ void setup(const Segment& segment);
+ void update(Segment& segment);
+ };
+ Visu visu;
+
+ void setup(const Segment& segment);
+ void update(Segment& segment);
+
+ };
+
+ };
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
similarity index 93%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
index 522a5cc13d83acf95a4efe1c3289c3aec2912487..f07f526d60dc409bc2bbf7e2dc37389237d9ec96 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
@@ -5,7 +5,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
void Decay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -24,15 +24,15 @@ namespace armarx::objpose::observer
"Remove objects whose confidence is lower than this value.");
}
- void Decay::updateConfidence(ObjectPose& pose, IceUtil::Time now) const
+ void Decay::updateConfidence(objpose::ObjectPose& pose, IceUtil::Time now) const
{
float confidence = calculateConfidence(pose.timestamp, now);
pose.confidence = confidence;
}
- void Decay::updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now) const
+ void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, IceUtil::Time now) const
{
- for (ObjectPose& pose : objectPoses)
+ for (objpose::ObjectPose& pose : objectPoses)
{
if (pose.attachment)
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
similarity index 87%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
index ec64221f3e202395ff4f0f8c83d3bb737b6571be..a44652d5528bceefed921dec184fe8b87fd19bfe 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
@@ -10,7 +10,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
/**
@@ -23,8 +23,8 @@ namespace armarx::objpose::observer
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "decay.");
- void updateConfidence(ObjectPose& pose, IceUtil::Time now) const;
- void updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now) const;
+ void updateConfidence(objpose::ObjectPose& pose, IceUtil::Time now) const;
+ void updateConfidences(objpose::ObjectPoseSeq& objectPoses, IceUtil::Time now) const;
private:
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
similarity index 87%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
index a5e78b5942704a7e53a3aae34d10c32b07baaa73..973cf5a551228705b918560ad48648db5795ab1f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
@@ -5,7 +5,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
void RobotHeadMovement::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -120,6 +120,30 @@ namespace armarx::objpose::observer
}
+ RobotHeadMovement::Discard RobotHeadMovement::getDiscard()
+ {
+ Discard discard;
+ if (checkHeadVelocity)
+ {
+ if (isMoving())
+ {
+ movementStarts(discardIntervalAfterMoveMS);
+ // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moving! until " << discardUpdatesUntil;
+ discard.all = true;
+ }
+ else if (TimeUtil::GetTime() < discardUpdatesUntil)
+ {
+ discard.all = true;
+ // ARMARX_IMPORTANT << "Ignoring pose update because robot head has moved until: " << discardUpdatesUntil;
+ }
+ else
+ {
+ discard.updatesUntil = discardUpdatesUntil;
+ }
+ }
+ return discard;
+ }
+
void RobotHeadMovement::RemoteGui::setup(const RobotHeadMovement& rhm)
{
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
similarity index 88%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
index fdd6078d9fcefeb619339c15f3e93029e90c3049..5a731720c49fdb33749edab4ca4c9792eedc0599 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
@@ -12,7 +12,7 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
-#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
namespace armarx
@@ -21,7 +21,7 @@ namespace armarx
using PropertyDefinitionsPtr = IceUtil::Handle<PropertyDefinitionContainer>;
}
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
class RobotHeadMovement : public armarx::Logging
{
@@ -40,6 +40,14 @@ namespace armarx::objpose::observer
objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input);
+ struct Discard
+ {
+ std::optional<IceUtil::Time> updatesUntil;
+ bool all = false;
+ };
+ Discard getDiscard();
+
+
public:
bool checkHeadVelocity = true;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..94838ec6ba70b9c857be1e660cfec4105ad2d59d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -0,0 +1,713 @@
+#include "Segment.h"
+
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.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/ArmarXObjects/ObjectFinder.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/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <sstream>
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
+ iceMemory(memoryToIceAdapter),
+ memoryMutex(memoryMutex)
+ {
+ Logging::setTag("InstanceSegment");
+
+ oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
+ {
+ // Try to get OOBB from repository.
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ {
+ try
+ {
+ 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;
+ }
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ });
+
+ classNameToDatasetCache.setFetchFn([this](const std::string & className)
+ {
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
+ return objectInfo ? objectInfo->dataset() : "";
+ });
+ }
+
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ 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.");
+
+ decay.defineProperties(defs, prefix + "decay.");
+ }
+
+ void Segment::init()
+ {
+ ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectInstance::toInitialAronType());
+ coreSegment->setMaxHistorySize(p.maxHistorySize);
+ }
+
+
+ Segment::CommitStats Segment::commitObjectPoses(
+ const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ std::optional<armem::Time> discardUpdatesUntil)
+ {
+ CommitStats stats;
+
+ // Build new poses.
+ objpose::ObjectPoseSeq newObjectPoses;
+ stats.numUpdated = 0;
+ for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
+ {
+ const IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
+
+ // Check whether we have an old snapshot for this object.
+ std::optional<objpose::ObjectPose> previousPose;
+ const armem::wm::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
+ if (entity)
+ {
+ const arondto::ObjectInstance data = getLatestInstanceData(*entity);
+
+ previousPose = objpose::ObjectPose();
+ fromAron(data, *previousPose);
+ }
+
+ bool discard = false;
+ if (discardUpdatesUntil && timestamp < discardUpdatesUntil.value())
+ {
+ // Dicard updates temporarily (e.g. due to head movement).
+ discard = true;
+ }
+ else if (previousPose)
+ {
+ if (p.discardSnapshotsWhileAttached && previousPose->attachment)
+ {
+ // Discard update due to active attachemnt.
+ discard = true;
+ }
+ else if (timestamp == previousPose->timestamp)
+ {
+ // Discard update as it is not new.
+ discard = true;
+ }
+ }
+
+ if (!discard)
+ {
+ // Update the entity.
+ stats.numUpdated++;
+
+ objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
+ newPose.fromProvidedPose(provided, robot);
+
+ if (previousPose && previousPose->attachment)
+ {
+ // Keep current attachment.
+ ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
+ newPose.attachment = previousPose->attachment;
+ }
+
+ if (newPose.objectID.dataset().empty())
+ {
+ // Try to find the data set.
+ const std::string dataset = classNameToDatasetCache.get(newPose.objectID.className());
+ if (!dataset.empty())
+ {
+ newPose.objectID = { dataset, newPose.objectID.className(), newPose.objectID.instanceName() };
+ }
+ }
+ if (!provided.localOOBB)
+ {
+ // Try to load oobb from disk.
+ newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ }
+ }
+ }
+
+ commitObjectPoses(providerName, newObjectPoses);
+
+ return stats;
+ }
+
+ void Segment::commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+
+ // Update memory.
+ const MemoryID providerSegmentID = coreSegment->id().withProviderSegmentName(providerName);
+ if (!coreSegment->hasProviderSegment(providerSegmentID.providerSegmentName))
+ {
+ coreSegment->addProviderSegment(providerSegmentID.providerSegmentName);
+ }
+
+ Commit commit;
+ for (const objpose::ObjectPose& pose : objectPoses)
+ {
+ EntityUpdate& update = commit.updates.emplace_back();
+ {
+ update.entityID = providerSegmentID.withEntityName(pose.objectID.str());
+ update.timeArrived = TimeUtil::GetTime();
+ update.timeCreated = pose.timestamp;
+ update.confidence = pose.confidence;
+
+ arondto::ObjectInstance dto;
+ toAron(dto, pose);
+ // Search for object class.
+ if (auto instance = findClassInstance(pose.objectID))
+ {
+ toAron(dto.classID, instance->id());
+ }
+ update.instancesData.push_back(dto.toAron());
+ }
+
+ iceMemory.commit(commit);
+ }
+ }
+
+
+ wm::CoreSegment& Segment::getCoreSegment()
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return *coreSegment;
+ }
+
+ const wm::CoreSegment& Segment::getCoreSegment() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return *coreSegment;
+ }
+
+ objpose::ObjectPoseSeq Segment::getObjectPoses(IceUtil::Time now)
+ {
+ ObjectPoseSeq objectPoses = getLatestObjectPoses();
+ updateObjectPoses(objectPoses, now);
+ return filterObjectPoses(objectPoses);
+ }
+
+
+ objpose::ObjectPoseSeq Segment::getObjectPosesByProvider(
+ const std::string& providerName,
+ IceUtil::Time now)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ ObjectPoseSeq objectPoses = getLatestObjectPoses(coreSegment->getProviderSegment(providerName));
+ updateObjectPoses(objectPoses, now);
+ return filterObjectPoses(objectPoses);
+ }
+
+ armem::wm::Entity* Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ armem::MemoryID entityID = armem::MemoryID().withEntityName(objectID.str());
+ if (providerName.empty())
+ {
+ for (auto& [_, prov] : *coreSegment)
+ {
+ if (prov.hasEntity(entityID.entityName))
+ {
+ return &prov.getEntity(entityID);
+ }
+ }
+ return nullptr;
+ }
+ else
+ {
+ entityID.providerSegmentName = providerName;
+ if (coreSegment->hasProviderSegment(providerName))
+ {
+ armem::wm::ProviderSegment& prov = coreSegment->getProviderSegment(providerName);
+ return prov.hasEntity(entityID.entityName) ? &prov.getEntity(entityID) : nullptr;
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+ }
+
+
+ void Segment::updateObjectPoses(ObjectPoseSeq& objectPoses, IceUtil::Time now)
+ {
+ bool agentSynchronized = false;
+
+ for (ObjectPose& objectPose : objectPoses)
+ {
+ updateObjectPose(objectPose, now, robot, agentSynchronized);
+ }
+ }
+
+
+ void Segment::updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
+ {
+ for (ObjectPose& pose : objectPoses)
+ {
+ updateObjectPose(pose, now, agent, agentSynchronized);
+ }
+ }
+
+
+ void Segment::updateObjectPose(
+ ObjectPose& objectPose,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
+ {
+ updateAttachement(objectPose, agent, agentSynchronized);
+
+ if (decay.enabled)
+ {
+ decay.updateConfidence(objectPose, now);
+ }
+ }
+
+
+ objpose::ObjectPoseSeq Segment::filterObjectPoses(const ObjectPoseSeq& objectPoses) const
+ {
+ ObjectPoseSeq result;
+ for (const ObjectPose& objectPose : objectPoses)
+ {
+ if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
+ {
+ result.push_back(objectPose);
+ }
+ }
+ return result;
+ }
+
+
+ void Segment::updateAttachement(
+ ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
+ {
+ if (!objectPose.attachment)
+ {
+ // No attachment, nothing to do.
+ return;
+ }
+ ARMARX_CHECK(objectPose.attachment);
+
+ if (!synchronized) // Synchronize only once.
+ {
+ RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
+ synchronized = true;
+ }
+ objectPose.updateAttached(agent);
+ }
+
+ objpose::ObjectPoseSeq Segment::getLatestObjectPoses() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return getLatestObjectPoses(*coreSegment);
+ }
+
+ objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg)
+ {
+ ObjectPoseSeq result;
+ getLatestObjectPoses(coreSeg, result);
+ return result;
+ }
+
+ objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSeg)
+ {
+ ObjectPoseSeq result;
+ getLatestObjectPoses(provSeg, result);
+ return result;
+ }
+
+ objpose::ObjectPose Segment::getLatestObjectPose(const armem::wm::Entity& entity)
+ {
+ ObjectPose result;
+ getLatestObjectPose(entity, result);
+ return result;
+ }
+
+ void Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg, ObjectPoseSeq& out)
+ {
+ for (const auto& [_, provSegment] : coreSeg)
+ {
+ getLatestObjectPoses(provSegment, out);
+ }
+ }
+
+ void Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSegment, ObjectPoseSeq& out)
+ {
+ for (const auto& [_, entity] : provSegment)
+ {
+ if (!entity.empty())
+ {
+ ObjectPose& pose = out.emplace_back();
+ getLatestObjectPose(entity, pose);
+ }
+ }
+ }
+
+ void Segment::getLatestObjectPose(const armem::wm::Entity& entity, ObjectPose& out)
+ {
+ for (const armem::wm::EntityInstance& instance : entity.getLatestSnapshot())
+ {
+ arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
+
+ fromAron(dto, out);
+ }
+ }
+
+
+ arondto::ObjectInstance Segment::getLatestInstanceData(const armem::wm::Entity& entity)
+ {
+ ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
+ const armem::wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
+
+ ARMARX_CHECK_EQUAL(snapshot.size(), 1);
+ const armem::wm::EntityInstance& instance = snapshot.getInstance(0);
+
+ arondto::ObjectInstance data;
+ data.fromAron(instance.data());
+
+ return data;
+ }
+
+ std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
+ {
+ return oobbCache.get(id);
+ }
+
+ objpose::ProviderInfo Segment::getProviderInfo(const std::string& providerName)
+ {
+ try
+ {
+ return providers.at(providerName);
+ }
+ catch (const std::out_of_range&)
+ {
+ std::stringstream ss;
+ ss << "No provider with name '" << providerName << "' available.\n";
+ ss << "Available are:\n";
+ for (const auto& [name, _] : providers)
+ {
+ ss << "- '" << name << "'\n";
+ }
+ throw std::out_of_range(ss.str());
+ }
+ }
+
+
+
+ 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.
+
+ ObjectID objectID = armarx::fromIce(input.objectID);
+
+ if (input.agentName != "" && input.agentName != this->robot->getName())
+ {
+ ARMARX_WARNING << "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: " << std::vector<std::string> {this->robot->getName()};
+ return output;
+ }
+ VirtualRobot::RobotPtr agent = this->robot;
+
+ if (!agent->hasRobotNode(input.frameName))
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
+ << "' of agent '" << agent->getName() << "'.";
+ return output;
+ }
+ std::string frameName = input.frameName;
+
+
+ // Find object pose (provider name can be empty).
+ armem::wm::Entity* objectEntity = this->findObjectEntity(objectID, input.providerName);
+ if (!objectEntity || objectEntity->empty())
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
+ << "' of agent '" << agent->getName() << "', but object is currently not provided.";
+ return output;
+ }
+ arondto::ObjectInstance data = getLatestInstanceData(*objectEntity);
+
+ objpose::ObjectAttachmentInfo info;
+ info.agentName = agent->getName();
+ info.frameName = frameName;
+
+ if (input.poseInFrame)
+ {
+ info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
+ }
+ else
+ {
+ RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
+
+ armarx::FramedPose framed(data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
+ if (frameName == armarx::GlobalFrame)
+ {
+ info.poseInFrame = framed.toGlobalEigen(this->robot);
+ }
+ else
+ {
+ framed.changeFrame(this->robot, info.frameName);
+ info.poseInFrame = framed.toEigen();
+ }
+ }
+
+ // Store attachment in new entity snapshot.
+ {
+ armem::EntityUpdate update;
+ update.entityID = objectEntity->id();
+ update.timeCreated = now;
+ {
+ arondto::ObjectInstance updated = data;
+ toAron(updated.pose.attachment, info);
+ updated.pose.attachmentValid = true;
+ update.instancesData = { updated.toAron() };
+ }
+ objectEntity->update(update);
+ }
+
+ 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;
+
+ output.success = true;
+ output.attachment = new objpose::data::ObjectAttachmentInfo();
+ output.attachment->frameName = info.frameName;
+ output.attachment->agentName = info.agentName;
+ output.attachment->poseInFrame = new Pose(info.poseInFrame);
+
+ return output;
+ }
+
+ objpose::DetachObjectFromRobotNodeOutput Segment::detachObjectFromRobotNode(
+ const objpose::DetachObjectFromRobotNodeInput& input)
+ {
+ const armem::Time now = armem::Time::now();
+
+ ObjectID objectID = armarx::fromIce(input.objectID);
+ std::string providerName = input.providerName;
+
+ std::optional<objpose::arondto::ObjectAttachmentInfo> attachment;
+ {
+ // Remove from latest pose (if it was cached).
+ // Find object pose (provider name can be empty).
+ armem::wm::Entity* entity = this->findObjectEntity(objectID, input.providerName);
+ if (entity)
+ {
+ const arondto::ObjectInstance data = getLatestInstanceData(*entity);
+ if (data.pose.attachmentValid)
+ {
+ attachment = data.pose.attachment;
+
+ // Store non-attached pose in new snapshot.
+ storeDetachedSnapshot(*entity, data, now, input.commitAttachedPose);
+ }
+
+ if (providerName.empty())
+ {
+ providerName = data.pose.providerName;
+ }
+ }
+ }
+
+ objpose::DetachObjectFromRobotNodeOutput output;
+ output.wasAttached = bool(attachment);
+ if (attachment)
+ {
+ 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 << "' "
+ << "from robot node, but it was not attached.";
+ }
+
+ return output;
+ }
+
+
+ struct DetachVisitor : public armem::wm::Visitor
+ {
+ Segment& owner;
+ armem::Time now;
+ bool commitAttachedPose;
+
+ int numDetached = 0;
+
+ DetachVisitor(Segment& owner, armem::Time now, bool commitAttachedPose) :
+ owner(owner), now(now), commitAttachedPose(commitAttachedPose)
+ {
+ }
+
+ virtual bool visitEnter(armem::wm::Entity& entity) override;
+ };
+
+ bool DetachVisitor::visitEnter(armem::wm::Entity& entity)
+ {
+ const arondto::ObjectInstance data = owner.getLatestInstanceData(entity);
+ if (data.pose.attachmentValid)
+ {
+ numDetached++;
+ // Store non-attached pose in new snapshot.
+ owner.storeDetachedSnapshot(entity, data, now, commitAttachedPose);
+ }
+
+ return false; // Stop descending.
+ }
+
+
+ objpose::DetachAllObjectsFromRobotNodesOutput Segment::detachAllObjectsFromRobotNodes(
+ const objpose::DetachAllObjectsFromRobotNodesInput& input)
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+
+ const armem::Time now = armem::Time::now();
+
+ DetachVisitor visitor(*this, now, input.commitAttachedPose);
+ visitor.applyTo(*coreSegment);
+
+ objpose::DetachAllObjectsFromRobotNodesOutput output;
+ output.numDetached = visitor.numDetached;
+
+ ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
+
+ return output;
+ }
+
+ void Segment::storeDetachedSnapshot(
+ armem::wm::Entity& entity,
+ const arondto::ObjectInstance& data,
+ armem::Time now,
+ bool commitAttachedPose)
+ {
+ armem::EntityUpdate update;
+ update.entityID = entity.id();
+ update.timeCreated = now;
+ {
+ arondto::ObjectInstance updated;
+ if (commitAttachedPose && data.pose.attachmentValid)
+ {
+ ObjectPose objectPose;
+ fromAron(data, objectPose);
+
+ bool agentSynchronized = false;
+ updateAttachement(objectPose, robot, agentSynchronized);
+
+ objectPose.attachment = std::nullopt;
+ toAron(updated, objectPose);
+ }
+ else
+ {
+ updated = data;
+ updated.pose.attachmentValid = false;
+ toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
+ }
+
+ update.instancesData = { updated.toAron() };
+ }
+ entity.update(update);
+ }
+
+
+ std::optional<wm::EntityInstance> Segment::findClassInstance(const ObjectID& objectID)
+ {
+ const ObjectID classID = { objectID.dataset(), objectID.className() };
+ try
+ {
+ for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment("Class"))
+ {
+ return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ }
+ return std::nullopt;
+ }
+ catch (const armem::error::ArMemError&)
+ {
+ // Some segment or entity did not exist.
+ return std::nullopt;
+ }
+ }
+
+
+
+ void Segment::RemoteGui::setup(const Segment& data)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+ maxHistorySize.setRange(1, 1e6);
+ infiniteHistory.setValue(data.p.maxHistorySize == -1);
+ discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+ 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});
+ row++;
+
+ group.setLabel("Data");
+ group.addChild(grid);
+ }
+
+ void Segment::RemoteGui::update(Segment& data)
+ {
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ || discardSnapshotsWhileAttached.hasValueChanged())
+ {
+ std::scoped_lock lock(data.memoryMutex);
+
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ {
+ data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (data.coreSegment)
+ {
+ data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+ }
+ }
+
+ data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..54314054a38f5f104552c045fe37eb8b401651ae
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -0,0 +1,191 @@
+#pragma once
+
+#include <map>
+#include <string>
+#include <optional>
+
+#include <SimoxUtility/caching/CacheMap.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+
+#include "Decay.h"
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ class Segment : public armarx::Logging
+ {
+ public:
+
+ struct CommitStats
+ {
+ int numUpdated = 0;
+ };
+ using ObjectPose = objpose::ObjectPose;
+ using ObjectPoseSeq = objpose::ObjectPoseSeq;
+
+
+ public:
+
+ Segment(server::MemoryToIceAdapter& iceMemory,
+ std::mutex& memoryMutex);
+
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+
+
+ CommitStats commitObjectPoses(
+ const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ std::optional<Time> discardUpdatesUntil = std::nullopt);
+ void commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses);
+
+
+ wm::CoreSegment& getCoreSegment();
+ const wm::CoreSegment& getCoreSegment() const;
+
+
+ objpose::ObjectPoseSeq getObjectPoses(IceUtil::Time now);
+ objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
+
+ 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);
+
+
+ /**
+ * @brief If the object is attached to a robot node, update it according to the current robot state.
+ *
+ * If there is no attachement info in `objectPose` itself, the internal data
+ * structure `attachments` is queried. If an attachment is found there,
+ * it is written into the given `objectPose` (thus, it is "cached" in the
+ * info `objectPose`).
+ *
+ * @param synchronized Indicates whether the agent is already synchronized to the current time.
+ */
+ void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
+ bool& synchronized) const;
+
+
+ static ObjectPoseSeq getLatestObjectPoses(const wm::CoreSegment& coreSeg);
+ static ObjectPoseSeq getLatestObjectPoses(const wm::ProviderSegment& provSeg);
+ static ObjectPose getLatestObjectPose(const wm::Entity& entity);
+
+ static void getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseSeq& out);
+ static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseSeq& out);
+ static void getLatestObjectPose(const wm::Entity& entity, ObjectPose& out);
+
+ static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
+
+
+ private:
+
+ ObjectPoseSeq getLatestObjectPoses() const;
+
+ void updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now);
+ void updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized
+ ) const;
+ void updateObjectPose(
+ ObjectPose& objectPose,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized
+ ) const;
+
+
+ ObjectPoseSeq filterObjectPoses(const ObjectPoseSeq& objectPoses) const;
+
+
+ void storeDetachedSnapshot(
+ wm::Entity& entity,
+ const arondto::ObjectInstance& data,
+ Time now,
+ bool commitAttachedPose);
+
+
+ std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID);
+
+
+ friend struct DetachVisitor;
+
+
+ public:
+
+ RobotStateComponentInterfacePrx robotStateComponent;
+ VirtualRobot::RobotPtr robot;
+
+ objpose::ProviderInfoMap providers;
+
+
+ ObjectFinder objectFinder;
+
+ /// Decay model.
+ Decay decay;
+
+
+ private:
+
+ server::MemoryToIceAdapter& iceMemory;
+ wm::CoreSegment* coreSegment = nullptr;
+ std::mutex& memoryMutex;
+
+
+ struct Properties
+ {
+ std::string coreSegmentName = "Instance";
+ long maxHistorySize = -1;
+ bool discardSnapshotsWhileAttached = true;
+ };
+ Properties p;
+
+
+ /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
+ simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
+
+ /// Class name -> dataset name.
+ simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
+
+
+ public:
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
+ armarx::RemoteGui::Client::CheckBox infiniteHistory;
+ armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
+
+ void setup(const Segment& data);
+ void update(Segment& data);
+ };
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f570ca9b40c4e57f559cc55cdfdd535eba6e3d03
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -0,0 +1,498 @@
+/*
+ * 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::armem_objects::SegmentAdapter
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "SegmentAdapter.h"
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <VirtualRobot/Robot.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex) :
+ segment(iceMemory, memoryMutex),
+ memoryMutex(memoryMutex)
+ {
+ }
+
+ std::string SegmentAdapter::getName() const
+ {
+ return Logging::tag.tagName;
+ }
+
+ void SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ calibration.defineProperties(defs, prefix + "calibration.");
+ segment.defineProperties(defs, prefix);
+ robotHead.defineProperties(defs, prefix + "head.");
+ visu.defineProperties(defs, prefix + "visu.");
+ }
+
+ void SegmentAdapter::init()
+ {
+ segment.setTag(getName());
+ segment.decay.setTag(getName());
+ robotHead.setTag(getName());
+ visu.setTag(getName());
+
+ segment.init();
+ }
+
+ void SegmentAdapter::connect(
+ RobotStateComponentInterfacePrx robotStateComponent,
+ VirtualRobot::RobotPtr robot,
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver
+ )
+ {
+ this->debugObserver = debugObserver;
+ this->arviz = arviz;
+
+ segment.robot = robot;
+ segment.robotStateComponent = robotStateComponent;
+
+ robotHead.kinematicUnitObserver = kinematicUnitObserver;
+ robotHead.debugObserver = debugObserver;
+ robotHead.fetchDatafields();
+
+ visu.arviz = arviz;
+ if (!visu.updateTask)
+ {
+ visu.updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ visu.updateTask->start();
+ }
+ }
+
+ void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
+ {
+ updateProviderInfo(providerName, info);
+ }
+
+
+ void SegmentAdapter::reportObjectPoses(
+ const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
+ {
+ ARMARX_VERBOSE << "Received object " << providedPoses.size() << " poses from provider '" << providerName << "'.";
+ updateObjectPoses(providerName, providedPoses);
+ }
+
+
+ void SegmentAdapter::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
+ {
+ if (!info.proxy)
+ {
+ ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
+ << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
+ return;
+ }
+ {
+ std::scoped_lock lock(memoryMutex);
+ std::stringstream ss;
+ for (const auto& id : info.supportedObjects)
+ {
+ ss << "- " << id << "\n";
+ }
+ ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
+ << "Supported objects: \n" << ss.str();
+ segment.providers[providerName] = info;
+ }
+ }
+
+
+ void SegmentAdapter::updateObjectPoses(
+ const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses)
+ {
+ TIMING_START(tReportObjectPoses);
+
+ RobotHeadMovement::Discard discard;
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ discard = robotHead.getDiscard();
+ }
+ if (debugObserver)
+ {
+ StringVariantBaseMap map;
+ map["Discarding All Updates"] = new Variant(discard.all ? 1.f : 0.f);
+ if (discard.all)
+ {
+ map["Proportion Updated Poses"] = new Variant(0.f);
+ }
+ debugObserver->setDebugChannel(getName(), map);
+ }
+
+ if (discard.all)
+ {
+ return;
+ }
+
+ {
+ std::scoped_lock lock(memoryMutex);
+ RemoteRobot::synchronizeLocalClone(segment.robot, segment.robotStateComponent);
+
+ if (segment.robot->hasRobotNode(calibration.robotNode))
+ {
+ VirtualRobot::RobotNodePtr robotNode = segment.robot->getRobotNode(calibration.robotNode);
+ float value = robotNode->getJointValue();
+ robotNode->setJointValue(value + calibration.offset);
+ }
+
+ TIMING_START(tCommitObjectPoses);
+ Segment::CommitStats stats =
+ segment.commitObjectPoses(providerName, providedPoses, discard.updatesUntil);
+ TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "Discarding All Updates", new Variant(discard.all ? 1 : 0) },
+ { "Proportion Updated Poses", new Variant(static_cast<float>(stats.numUpdated) / providedPoses.size()) }
+ });
+ }
+
+ handleProviderUpdate(providerName);
+
+ TIMING_END_STREAM(tReportObjectPoses, ARMARX_VERBOSE);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t ReportObjectPoses [ms]", new Variant(tReportObjectPoses.toMilliSecondsDouble()) },
+ { "t MemorySetObjectPoses [ms]", new Variant(tCommitObjectPoses.toMilliSecondsDouble()) },
+ });
+ }
+ }
+ }
+
+
+ void SegmentAdapter::handleProviderUpdate(const std::string& providerName)
+ {
+ // Initialized to 0 on first access.
+ if (segment.providers.count(providerName) == 0)
+ {
+ segment.providers[providerName] = objpose::ProviderInfo();
+ }
+ }
+
+
+ objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPoses(const Ice::Current&)
+ {
+ TIMING_START(tGetObjectPoses);
+
+ TIMING_START(tGetObjectPosesLock);
+ std::scoped_lock lock(memoryMutex);
+ TIMING_END_STREAM(tGetObjectPosesLock, ARMARX_VERBOSE);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPoses(now));
+
+ TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t GetObjectPoses() [ms]", new Variant(tGetObjectPoses.toMilliSecondsDouble()) },
+ { "t GetObjectPoses() lock [ms]", new Variant(tGetObjectPosesLock.toMilliSecondsDouble()) }
+ });
+ }
+
+ return result;
+ }
+
+ objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
+ {
+ TIMING_START(GetObjectPoses);
+
+ TIMING_START(GetObjectPosesLock);
+ std::scoped_lock lock(memoryMutex);
+ TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(segment.getObjectPosesByProvider(providerName, now));
+
+ TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t GetObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
+ { "t GetObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
+ });
+ }
+
+ return result;
+ }
+
+
+ objpose::observer::RequestObjectsOutput SegmentAdapter::requestObjects(
+ const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
+ {
+ std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
+ std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
+
+ objpose::observer::RequestObjectsOutput output;
+
+ auto updateProxy = [&](const std::string & providerName)
+ {
+ if (proxies.count(providerName) == 0)
+ {
+ if (auto it = segment.providers.find(providerName); it != segment.providers.end())
+ {
+ proxies[providerName] = it->second.proxy;
+ }
+ else
+ {
+ ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
+ proxies[providerName] = nullptr;
+ }
+ }
+ };
+
+ if (input.provider.size() > 0)
+ {
+ providerRequests[input.provider] = input.request;
+ updateProxy(input.provider);
+ }
+ else
+ {
+ std::scoped_lock lock(memoryMutex);
+ for (const auto& objectID : input.request.objectIDs)
+ {
+ bool found = true;
+ for (const auto& [providerName, info] : segment.providers)
+ {
+ // ToDo: optimize look up.
+ if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
+ {
+ providerRequests[providerName].objectIDs.push_back(objectID);
+ updateProxy(providerName);
+ break;
+ }
+ }
+ if (!found)
+ {
+ ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
+ output.results[objectID].providerName = "";
+ }
+ }
+ }
+
+ for (const auto& [providerName, request] : providerRequests)
+ {
+ if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
+ {
+ ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
+ << providerName << "' for " << request.relativeTimeoutMS << " ms.";
+ objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
+
+ int successful = 0;
+ for (const auto& [objectID, result] : providerOutput.results)
+ {
+ objpose::observer::ObjectRequestResult& res = output.results[objectID];
+ res.providerName = providerName;
+ res.result = result;
+ successful += int(result.success);
+ }
+ ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
+ }
+ }
+ return output;
+ }
+
+ objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.providers;
+ }
+
+ Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return simox::alg::get_keys(segment.providers);
+ }
+
+ objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.getProviderInfo(providerName);
+ }
+
+ bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.providers.count(providerName) > 0;
+ }
+
+
+ objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
+ const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.attachObjectToRobotNode(input);
+ }
+
+ objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
+ const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.detachObjectFromRobotNode(input);
+ }
+
+ objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
+ const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+ return segment.detachAllObjectsFromRobotNodes(input);
+ }
+
+
+ objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
+ {
+ std::scoped_lock lock(memoryMutex);
+
+ objpose::AgentFramesSeq output;
+ std::vector<VirtualRobot::RobotPtr> agents = { segment.robot };
+ for (VirtualRobot::RobotPtr agent : agents)
+ {
+ objpose::AgentFrames& frames = output.emplace_back();
+ frames.agent = agent->getName();
+ frames.frames = agent->getRobotNodeNames();
+ }
+ return output;
+ }
+
+ objpose::SignalHeadMovementOutput
+ SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ return robotHead.signalHeadMovement(input);
+ }
+
+
+ void SegmentAdapter::visualizeRun()
+ {
+ CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
+ while (visu.updateTask && !visu.updateTask->isStopped())
+ {
+ {
+ std::scoped_lock lock(visuMutex);
+
+ if (visu.enabled)
+ {
+ TIMING_START(Visu);
+
+ objpose::ObjectPoseSeq objectPoses;
+ ObjectFinder objectFinder;
+ visu.minConfidence = -1;
+ {
+ std::scoped_lock lock(memoryMutex);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ objectPoses = segment.getObjectPoses(now);
+ objectFinder = segment.objectFinder;
+ if (segment.decay.enabled)
+ {
+ visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
+ }
+ }
+ const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
+ arviz.commit(layers);
+
+ TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ });
+ }
+ }
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+
+ void SegmentAdapter::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
+ defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
+ }
+
+
+
+ void SegmentAdapter::RemoteGui::setup(const SegmentAdapter& adapter)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ this->visu.setup(adapter.visu);
+ this->segment.setup(adapter.segment);
+ this->decay.setup(adapter.segment.decay);
+ this->robotHead.setup(adapter.robotHead);
+
+ layout = VBoxLayout
+ {
+ this->visu.group, this->segment.group, this->decay.group, this->robotHead.group,
+ VSpacer()
+ };
+
+ group.setLabel("Instance");
+ group.addChild(layout);
+ }
+
+ void SegmentAdapter::RemoteGui::update(SegmentAdapter& adapter)
+ {
+ // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
+ {
+ std::scoped_lock lock(adapter.visuMutex);
+ this->visu.update(adapter.visu);
+ }
+ {
+ std::scoped_lock lock(adapter.memoryMutex);
+ this->segment.update(adapter.segment);
+ this->decay.update(adapter.segment.decay);
+ }
+ {
+ std::scoped_lock lock(adapter.robotHeadMutex);
+ this->robotHead.update(adapter.robotHead);
+ }
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
new file mode 100644
index 0000000000000000000000000000000000000000..a1e356e53c77608937b0df952798d0427a3cc86b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
@@ -0,0 +1,169 @@
+/*
+ * 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::armem_objects::Adapter
+ * @author Rainer Kartmann ( rainer dot kartmann 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 <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Decay.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Visu.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h>
+
+
+#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
+
+
+namespace armarx::armem::server::obj::instance
+{
+
+ /**
+ * @brief Helps implementing the `armarx::armem::server::ObjectInstanceSegmentInterface`.
+ */
+ class SegmentAdapter :
+ virtual public armarx::Logging
+ , virtual public armarx::armem::server::ObjectInstanceSegmentInterface
+ {
+ public:
+
+ SegmentAdapter(MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+
+ std::string getName() const;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ void init();
+ void connect(
+ RobotStateComponentInterfacePrx robotStateComponent,
+ VirtualRobot::RobotPtr robot,
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver
+ );
+
+
+ // ObjectPoseTopic interface
+ public:
+ virtual void reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
+ virtual void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
+
+ // ObjectInstanceSegmentInterface interface
+ public:
+
+ // OBJECT POSES
+
+ virtual objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
+ virtual objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+
+ // PROVIDER INFORMATION
+
+ virtual bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+ virtual objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
+ virtual Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
+ virtual objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
+
+
+ // REQUESTING
+
+ virtual objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
+
+ // ATTACHING
+
+ virtual objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
+ virtual objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
+ virtual objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input, ICE_CURRENT_ARG) override;
+
+ virtual objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
+
+ // HEAD MOVEMENT SIGNALS
+
+ virtual objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
+
+
+ private:
+
+ void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
+
+ void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
+ void handleProviderUpdate(const std::string& providerName);
+
+
+ // Visualization
+
+ void visualizeRun();
+
+
+ private:
+
+ viz::Client arviz;
+ DebugObserverInterfacePrx debugObserver;
+
+ instance::Segment segment;
+ std::mutex& memoryMutex;
+
+ instance::RobotHeadMovement robotHead;
+ std::mutex robotHeadMutex;
+
+ instance::Visu visu;
+ std::mutex visuMutex;
+
+
+ struct Calibration
+ {
+ std::string robotNode = "Neck_2_Pitch";
+ float offset = 0.0f;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
+ };
+ Calibration calibration;
+
+
+ public:
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+ armarx::RemoteGui::Client::VBoxLayout layout;
+
+ instance::Visu::RemoteGui visu;
+ instance::Segment::RemoteGui segment;
+ instance::Decay::RemoteGui decay;
+ instance::RobotHeadMovement::RemoteGui robotHead;
+
+ void setup(const SegmentAdapter& adapter);
+ void update(SegmentAdapter& adapter);
+ };
+
+ };
+
+}
+
+#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
similarity index 71%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 467dba24f947b38c02b47044986cf3086775b6a1..5cc251ca984e22a0129411b6b05a59db79d8a543 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -1,11 +1,13 @@
#include "Visu.h"
+#include <SimoxUtility/math/pose.h>
+
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -30,47 +32,71 @@ namespace armarx::objpose::observer
std::vector<viz::Layer> Visu::visualizeCommit(
- const std::map<std::string, ObjectPoseSeq>& objectPoses,
- float minConfidence,
+ const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
const ObjectFinder& objectFinder) const
{
std::vector<viz::Layer> layers;
for (const auto& [name, poses] : objectPoses)
{
- layers.push_back(visualizeProvider(name, poses, minConfidence, objectFinder));
+ layers.push_back(visualizeProvider(name, poses, objectFinder));
}
return layers;
}
+ std::vector<viz::Layer> Visu::visualizeCommit(
+ const objpose::ObjectPoseSeq& objectPoses,
+ const ObjectFinder& objectFinder) const
+ {
+ std::map<std::string, viz::Layer> layers;
+
+ auto getLayer = [this, &layers](const std::string & providerName) -> viz::Layer &
+ {
+ auto it = layers.find(providerName);
+ if (it == layers.end())
+ {
+ it = layers.emplace(providerName, arviz.layer(providerName)).first;
+ }
+ return it->second;
+ };
+
+ for (const objpose::ObjectPose& objectPose : objectPoses)
+ {
+ visualizeObjectPose(getLayer(objectPose.providerName), objectPose, objectFinder);
+ }
+
+ return simox::alg::get_values(layers);
+ }
+
viz::Layer Visu::visualizeProvider(
const std::string& providerName,
- const ObjectPoseSeq& objectPoses,
- float minConfidence,
+ const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder) const
{
viz::Layer layer = arviz.layer(providerName);
- for (const ObjectPose& objectPose : objectPoses)
+ for (const objpose::ObjectPose& objectPose : objectPoses)
{
- const bool show = not(objectPose.confidence < minConfidence);
- if (show)
- {
- visualizeObjectPose(layer, objectPose, objectFinder);
- }
+ visualizeObjectPose(layer, objectPose, objectFinder);
}
return layer;
}
void Visu::visualizeObjectPose(
viz::Layer& layer,
- const ObjectPose& objectPose,
+ const objpose::ObjectPose& objectPose,
const ObjectFinder& objectFinder) const
{
+ const bool show = not(objectPose.confidence < minConfidence);
+ if (!show)
+ {
+ return;
+ }
const armarx::ObjectID id = objectPose.objectID;
const std::string key = id.str();
- Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
{
- viz::Object object = viz::Object(key).pose(pose);
+ viz::Object object(key);
+ object.pose(pose);
if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
{
object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
@@ -92,9 +118,10 @@ namespace armarx::objpose::observer
if (oobbs && objectPose.localOOBB)
{
- const simox::OrientedBoxf& oobb = *objectPose.localOOBB;
- layer.add(viz::Box(key + " OOBB").set(oobb.transformed(pose))
- .color(simox::Color::lime(255, 64)));
+ const simox::OrientedBoxf oobb = inGlobalFrame
+ ? objectPose.oobbGlobal().value()
+ : objectPose.oobbRobot().value();
+ layer.add(viz::Box(key + " OOBB").set(oobb).color(simox::Color::lime(255, 64)));
}
if (objectFrames)
{
@@ -130,7 +157,7 @@ namespace armarx::objpose::observer
row++;
grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
row++;
- grid.add(Label("Alpha By Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
+ grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
row++;
grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
row++;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
similarity index 78%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index 9677f6198adcd9f52918061925876860640ba999..1cd1d41c835e56e820ce3db8b3397b1e117d8202 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -14,7 +14,7 @@ namespace armarx
{
class ObjectFinder;
}
-namespace armarx::objpose::observer
+namespace armarx::armem::server::obj::instance
{
/**
@@ -28,21 +28,26 @@ namespace armarx::objpose::observer
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
std::vector<viz::Layer> visualizeCommit(
- const std::map<std::string, ObjectPoseSeq>& objectPoses,
- float minConfidence,
+ const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
+ const ObjectFinder& objectFinder
+ ) const;
+
+ /// Visualize the given object poses, with one layer per provider.
+ std::vector<viz::Layer> visualizeCommit(
+ const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder
) const;
viz::Layer visualizeProvider(
const std::string& providerName,
- const ObjectPoseSeq& objectPoses,
- float minConfidence,
+ const objpose::ObjectPoseSeq& objectPoses,
const ObjectFinder& objectFinder
) const;
+
void visualizeObjectPose(
viz::Layer& layer,
- const ObjectPose& objectPose,
+ const objpose::ObjectPose& objectPose,
const ObjectFinder& objectFinder
) const;
@@ -51,10 +56,11 @@ namespace armarx::objpose::observer
viz::Client arviz;
- bool enabled = false;
+ bool enabled = true;
float frequencyHz = 25;
bool inGlobalFrame = true;
+ float minConfidence = -1;
float alpha = 1.0;
bool alphaByConfidence = false;
bool oobbs = false;
diff --git a/source/RobotAPI/libraries/aron/CMakeLists.txt b/source/RobotAPI/libraries/aron/CMakeLists.txt
index 124be827dda9a63c01b5af5da088806dbf99d47b..c45ff0856ef2185644f899a23be41b9943eb9dfa 100644
--- a/source/RobotAPI/libraries/aron/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/CMakeLists.txt
@@ -1,3 +1,4 @@
add_subdirectory(core)
add_subdirectory(converter)
add_subdirectory(codegenerationhelper)
+add_subdirectory(common)
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7a4f383eb5118009de6cfb43299490e5fd8b83ab
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -0,0 +1,40 @@
+set(LIB_NAME aroncommon)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # RobotAPI
+ aron
+
+ HEADERS
+ aron_conversions.h
+ aron_conversions/core.h
+ aron_conversions/armarx.h
+ aron_conversions/simox.h
+ aron_conversions/stl.h
+
+ SOURCES
+ aron_conversions/core.cpp
+ aron_conversions/armarx.cpp
+ aron_conversions/simox.cpp
+ aron_conversions/stl.cpp
+)
+
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/AxisAlignedBoundingBox.xml
+ aron/OrientedBox.xml
+ aron/PackagePath.xml
+)
+
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.xml b/source/RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4001f42e7c5f6530ea24ad563446f5318535ec0f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.xml
@@ -0,0 +1,22 @@
+<!--
+The ARON DTO of simox::AxisAlignedBoundingBox.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ </CodeIncludes>
+ <GenerateTypes>
+
+ <Object name="simox::arondto::AxisAlignedBoundingBox">
+ <ObjectChild key="center">
+ <Position />
+ </ObjectChild>
+ <ObjectChild key="extents">
+ <Position />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml b/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
new file mode 100644
index 0000000000000000000000000000000000000000..c4a4e407fd7ce3aca8cdca5aacc991f05645023f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/OrientedBox.xml
@@ -0,0 +1,25 @@
+<!--
+The ARON DTO of simox::OrientedBoxf.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ </CodeIncludes>
+ <GenerateTypes>
+
+ <Object name="simox::arondto::OrientedBox">
+ <ObjectChild key='center'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='orientation'>
+ <Orientation />
+ </ObjectChild>
+ <ObjectChild key='extents'>
+ <Position />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron/PackagePath.xml b/source/RobotAPI/libraries/aron/common/aron/PackagePath.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1c2faf0902ac04c0c2b3051f279683e65e37a9bb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron/PackagePath.xml
@@ -0,0 +1,19 @@
+<!--
+The ARON DTO of armarx::PackagePath.
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name="armarx::arondto::PackagePath">
+ <ObjectChild key='package'>
+ <String />
+ </ObjectChild>
+ <ObjectChild key='path'>
+ <String />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions.h b/source/RobotAPI/libraries/aron/common/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..2f96d0285b9fb4dbd96abdb92c12fe9518ee88b6
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "aron_conversions/core.h"
+
+#include "aron_conversions/armarx.h"
+#include "aron_conversions/simox.h"
+#include "aron_conversions/stl.h"
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da205a5238351e06ee78224198c66e6d37151df9
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
@@ -0,0 +1,14 @@
+#include "armarx.h"
+
+
+void armarx::fromAron(const armarx::arondto::PackagePath& dto, armarx::PackagePath& bo)
+{
+ bo = { dto.package, dto.path };
+}
+void armarx::toAron(armarx::arondto::PackagePath& dto, const armarx::PackagePath& bo)
+{
+ 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/armarx.h b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
new file mode 100644
index 0000000000000000000000000000000000000000..241067b7b70ef2f0016c3bfbd33e7a5beff2624b
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>
+
+
+namespace armarx
+{
+ void fromAron(const arondto::PackagePath& dto, PackagePath& bo);
+ void toAron(arondto::PackagePath& dto, const PackagePath& bo);
+}
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/core.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/core.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..790714d9c04f963f91a7492b396f95b32f684754
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/core.cpp
@@ -0,0 +1,2 @@
+#include "core.h"
+
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/core.h b/source/RobotAPI/libraries/aron/common/aron_conversions/core.h
new file mode 100644
index 0000000000000000000000000000000000000000..bcc537a288613e28f4441477654a8466bac56d7d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/core.h
@@ -0,0 +1,81 @@
+#pragma once
+
+
+
+namespace armarx::aron
+{
+
+ /**
+ * Framework for converting ARON DTOs (Data Transfer Objects) to C++ BOs
+ * (Business Objects) and back.
+ *
+ * To allow conversion between custom ARON and C++ types, declare two
+ * functions in the namespace of the BO:
+ *
+ * @code
+ * // aron_conversions.h
+ *
+ * namespace bo_namespace
+ * {
+ * void toAron(arondto::MyObject& dto, const MyObject& bo);
+ * void fromAron(const arondto::MyObject& dto, MyObject& bo);
+ * }
+ * @endcode
+ *
+ * Note that the DTO always comes first, and the target object is
+ * non-const.
+ *
+ * In the implementation,
+ *
+ *
+ * @code
+ * // aron_conversions.cpp
+ *
+ * #include "aron_conversions.h"
+ * #include <Path/to/MyValue/aron_conversions.h>
+ *
+ * void bo_namespace::toAron(arondto::MyObject& dto, const MyObject& bo)
+ * {
+ * dto.name = bo.name;
+ * toAron(dto.myValue, bo.myValue);
+ * }
+ *
+ * void bo_namespace::fromAron(const arondto::MyObject& dto, MyObject& bo)
+ * {
+ * bo.name = dto.name;
+ * fromAron(dto.myValue, bo.myValue);
+ * }
+ * @endcode
+ */
+
+ // Same type
+ template <class T>
+ void toAron(T& dto, const T& bo)
+ {
+ dto = bo;
+ }
+ template <class T>
+ void fromAron(const T& dto, T& bo)
+ {
+ bo = dto;
+ }
+
+
+ // Generic return version
+
+ template <class DtoT, class BoT>
+ DtoT toAron(const BoT& bo)
+ {
+ DtoT dto;
+ toAron(dto, bo);
+ return dto;
+ }
+ template <class BoT, class DtoT>
+ BoT fromAron(const DtoT& dto)
+ {
+ BoT bo;
+ fromAron(dto, bo);
+ return bo;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c87d13cdbd7e30aa5b7984cda69cb80261c378a7
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.cpp
@@ -0,0 +1,30 @@
+#include "simox.h"
+
+
+
+void simox::fromAron(const arondto::AxisAlignedBoundingBox& dto, AxisAlignedBoundingBox& bo)
+{
+ bo.center() = dto.center;
+ bo.extents() = dto.extents;
+}
+
+void simox::toAron(arondto::AxisAlignedBoundingBox& dto, const AxisAlignedBoundingBox& bo)
+{
+ dto.center = bo.center();
+ dto.extents = bo.extents();
+}
+
+
+
+void simox::fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo)
+{
+ bo = OrientedBoxf(dto.center, dto.orientation, dto.extents);
+}
+
+void simox::toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo)
+{
+ dto.center = bo.center();
+ dto.orientation = bo.rotation();
+ dto.extents = bo.dimensions();
+}
+
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h
new file mode 100644
index 0000000000000000000000000000000000000000..64e847afbb06c5b58d92db3817c7adccf7e3b563
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/simox.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.aron.generated.h>
+
+#include <SimoxUtility/shapes/OrientedBox.h>
+#include <RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>
+
+
+namespace simox
+{
+ void fromAron(const arondto::AxisAlignedBoundingBox& dto, AxisAlignedBoundingBox& bo);
+ void toAron(arondto::AxisAlignedBoundingBox& dto, const AxisAlignedBoundingBox& bo);
+
+ void fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo);
+ void toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo);
+}
+
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/stl.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/stl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..857d161ec035df81f9b7c1aca873c30f5f4a21aa
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/stl.cpp
@@ -0,0 +1 @@
+#include "stl.h"
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/stl.h b/source/RobotAPI/libraries/aron/common/aron_conversions/stl.h
new file mode 100644
index 0000000000000000000000000000000000000000..12c353e15d4e40975f6fae10cd927cd1393284d8
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/stl.h
@@ -0,0 +1,187 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <optional>
+#include <vector>
+
+#include "core.h"
+
+
+namespace armarx::aron
+{
+
+ // std::unique_ptr
+
+ template <class DtoT, class BoT>
+ void toAron(DtoT& dto, const std::unique_ptr<BoT>& bo)
+ {
+ if (bo)
+ {
+ toAron(dto, *bo);
+ }
+ }
+ template <class DtoT, class BoT>
+ void fromAron(const DtoT& dto, std::unique_ptr<BoT>& bo)
+ {
+ bo = std::make_unique<BoT>();
+ fromAron(dto, *bo);
+ }
+
+
+ // std::optional
+
+ template <class DtoT, class BoT>
+ void toAron(std::optional<DtoT>& dto, const std::optional<BoT>& bo)
+ {
+ if (bo.has_value())
+ {
+ dto = DtoT{};
+ toAron(*dto, *bo);
+ }
+ else
+ {
+ dto = std::nullopt;
+ }
+ }
+ template <class DtoT, class BoT>
+ void fromAron(const std::optional<DtoT>& dto, std::optional<BoT>& bo)
+ {
+ if (dto.has_value())
+ {
+ bo = BoT{};
+ fromAron(*dto, *bo);
+ }
+ else
+ {
+ bo = std::nullopt;
+ }
+ }
+
+ // Flag-controlled optional
+ template <class DtoT, class BoT>
+ void toAron(DtoT& dto, bool& dtoValid, const BoT& bo, bool boValid)
+ {
+ dtoValid = boValid;
+ if (boValid)
+ {
+ toAron(dto, bo);
+ }
+ else
+ {
+ dto = {};
+ }
+ }
+ template <class DtoT, class BoT>
+ void fromAron(const DtoT& dto, bool dtoValid, BoT& bo, bool& boValid)
+ {
+ boValid = dtoValid;
+ if (dtoValid)
+ {
+ fromAron(dto, bo);
+ }
+ else
+ {
+ bo = {};
+ }
+ }
+
+ template <class DtoT, class BoT>
+ void toAron(DtoT& dto, bool& dtoValid, const std::optional<BoT>& bo)
+ {
+ dtoValid = bo.has_value();
+ if (dtoValid)
+ {
+ toAron(dto, *bo);
+ }
+ else
+ {
+ dto = {};
+ }
+ }
+ template <class DtoT, class BoT>
+ void fromAron(const DtoT& dto, bool dtoValid, std::optional<BoT>& bo)
+ {
+ if (dtoValid)
+ {
+ bo = BoT{};
+ fromAron(dto, *bo);
+ }
+ else
+ {
+ bo = std::nullopt;
+ }
+ }
+
+
+ // std::vector
+
+ template <class DtoT, class BoT>
+ void toAron(std::vector<DtoT>& dtos, const std::vector<BoT>& bos)
+ {
+ dtos.clear();
+ dtos.reserve(bos.size());
+ for (const auto& bo : bos)
+ {
+ toAron(dtos.emplace_back(), bo);
+ }
+ }
+ template <class DtoT, class BoT>
+ void fromAron(const std::vector<DtoT>& dtos, std::vector<BoT>& bos)
+ {
+ bos.clear();
+ bos.reserve(dtos.size());
+ for (const auto& dto : dtos)
+ {
+ fromAron(dto, bos.emplace_back());
+ }
+ }
+
+ template <class DtoT, class BoT>
+ std::vector<DtoT> toAron(const std::vector<BoT>& bos)
+ {
+ std::vector<DtoT> dtos;
+ toAron(dtos, bos);
+ return dtos;
+ }
+
+
+ // std::map
+
+ template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
+ void toAron(std::map<DtoKeyT, DtoValueT>& dtoMap,
+ const std::map<BoKeyT, BoValueT>& boMap)
+ {
+ dtoMap.clear();
+ for (const auto& [boKey, boValue] : boMap)
+ {
+ DtoKeyT dtoKey;
+ toAron(dtoKey, boKey);
+ auto [it, _] = dtoMap.emplace(std::move(dtoKey), DtoValueT{});
+ toAron(it->second, boValue);
+ }
+ }
+ template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
+ void fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap,
+ std::map<BoKeyT, BoValueT>& boMap)
+ {
+ boMap.clear();
+ for (const auto& [dtoKey, dtoValue] : dtoMap)
+ {
+ BoKeyT boKey;
+ fromAron(dtoKey, boKey);
+ auto [it, _] = boMap.emplace(boKey, BoValueT{});
+ fromAron(dtoValue, it->second);
+ }
+ }
+
+
+ template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
+ std::map<DtoKeyT, DtoValueT> toAron(const std::map<BoKeyT, BoValueT>& boMap)
+ {
+ std::map<DtoKeyT, DtoValueT> dtoMap;
+ toAron(dtoMap, boMap);
+ return dtoMap;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/common/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8562347796faaae591d21c755fa2730776fa74e3
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/test/CMakeLists.txt
@@ -0,0 +1,14 @@
+
+add_library(aroncommon_test_files SHARED
+ MyCustomType.h
+ MyCustomType.cpp
+)
+target_link_libraries(aroncommon_test_files PRIVATE aroncommon)
+
+
+# Libs required for the tests
+SET(LIBS
+ aroncommon
+ aroncommon_test_files
+)
+armarx_add_test(aron_common_test aron_common_test.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/aron/common/test/MyCustomType.cpp b/source/RobotAPI/libraries/aron/common/test/MyCustomType.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6431732ee0e295d826a8af01995e20584789c7eb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/test/MyCustomType.cpp
@@ -0,0 +1,67 @@
+/*
+ * 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::aron_common::aron_common
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "MyCustomType.h"
+
+#include <ostream>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+
+
+template <class CustomTypeT>
+std::ostream& ostreamop(std::ostream& os, const CustomTypeT& rhs)
+{
+ return os << "(name='" << rhs.name << "'"
+ << " index=" << rhs.index
+ << " value=" << rhs.value
+ << ")";
+}
+
+std::ostream& my::operator<<(std::ostream& os, const CustomType& rhs)
+{
+ return ostreamop(os, rhs);
+}
+
+std::ostream& my::arondto::operator<<(std::ostream& os, const CustomType& rhs)
+{
+ return ostreamop(os, rhs);
+}
+
+bool my::operator==(const CustomType& lhs, const arondto::CustomType& rhs)
+{
+ return lhs.name == rhs.name
+ && lhs.index == rhs.index
+ && lhs.value == rhs.value;
+}
+
+
+void my::toAron(arondto::CustomType& dto, const CustomType& bo)
+{
+ dto.name = bo.name;
+ armarx::aron::toAron(dto.index, bo.index);
+ armarx::aron::toAron(dto.value, bo.value);
+}
+void my::fromAron(const arondto::CustomType& dto, CustomType& bo)
+{
+ bo.name = dto.name;
+ armarx::aron::fromAron(dto.index, bo.index);
+ armarx::aron::fromAron(dto.value, bo.value);
+}
diff --git a/source/RobotAPI/libraries/aron/common/test/MyCustomType.h b/source/RobotAPI/libraries/aron/common/test/MyCustomType.h
new file mode 100644
index 0000000000000000000000000000000000000000..31a4507dbfc02a4100bae5426b4c2b2a9d87c447
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/test/MyCustomType.h
@@ -0,0 +1,117 @@
+/*
+ * 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::aron_common::aron_common
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <ostream>
+#include <string>
+#include <vector>
+
+
+namespace my
+{
+ struct CustomType
+ {
+ std::string name;
+ int index = 0;
+ float value = 0.0;
+ };
+
+ std::ostream& operator<<(std::ostream& os, const CustomType& rhs);
+
+
+ namespace arondto
+ {
+ struct CustomType
+ {
+ std::string name;
+ int index;
+ float value;
+ };
+
+ std::ostream& operator<<(std::ostream& os, const arondto::CustomType& rhs);
+ }
+
+ bool operator==(const CustomType& lhs, const arondto::CustomType& rhs);
+ bool operator!=(const CustomType& lhs, const arondto::CustomType& rhs)
+ {
+ return !(lhs == rhs);
+ }
+ bool operator==(const arondto::CustomType& lhs, const CustomType& rhs)
+ {
+ return rhs == lhs;
+ }
+ bool operator!=(const arondto::CustomType& lhs, const CustomType& rhs)
+ {
+ return !(rhs == lhs);
+ }
+
+ void toAron(arondto::CustomType& dto, const CustomType& bo);
+ void fromAron(const arondto::CustomType& dto, CustomType& bo);
+}
+
+
+namespace std
+{
+
+ template <class L1, class L2, class R1, class R2>
+ bool operator==(const std::pair<L1, L2>& lhs,
+ const std::pair<R1, R2>& rhs)
+ {
+ return lhs.first == rhs.first && lhs.second == rhs.second;
+ }
+ template <class L1, class L2, class R1, class R2>
+ bool operator!=(const std::pair<L1, L2>& lhs,
+ const std::pair<R1, R2>& rhs)
+ {
+ return !(lhs == rhs);
+ }
+
+ template <class L1, class L2>
+ std::ostream& operator<<(std::ostream& os, const std::pair<L1, L2>& pair)
+ {
+ return os << "(" << pair.first << " | " << pair.second << ")";
+ }
+
+
+ template <class L, class R>
+ bool operator==(const std::vector<L>& lhs, const std::vector<R>& rhs)
+ {
+ if (lhs.size() != rhs.size())
+ {
+ return false;
+ };
+ for (size_t i = 0; i < lhs.size(); ++i)
+ {
+ if (lhs[i] != rhs[i])
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+ template <class L, class R>
+ bool operator!=(const std::vector<L>& lhs, const std::vector<R>& rhs)
+ {
+ return !(lhs == rhs);
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/aron/common/test/aron_common_test.cpp b/source/RobotAPI/libraries/aron/common/test/aron_common_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0503f01327f4f8c6653bbf09d379f79549ccc5fb
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/test/aron_common_test.cpp
@@ -0,0 +1,261 @@
+/*
+ * 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::ArmarXObjects
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::aron_common
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+
+#include <iostream>
+
+#include "MyCustomType.h"
+
+
+BOOST_AUTO_TEST_CASE(test_direct)
+{
+ const my::CustomType bo { "name", -10, 42.42f };
+
+ my::arondto::CustomType dto;
+ toAron(dto, bo);
+ BOOST_CHECK_EQUAL(dto, bo);
+
+ my::CustomType boOut;
+ fromAron(dto, boOut);
+ BOOST_CHECK_EQUAL(boOut, dto);
+}
+
+
+template <class BOs, class DTOs>
+void test_complex(const BOs bos, DTOs& dtos)
+{
+ armarx::aron::toAron(dtos, bos);
+ BOOST_CHECK_EQUAL_COLLECTIONS(dtos.begin(), dtos.end(),
+ bos.begin(), bos.end());
+
+ BOs bosOut;
+ armarx::aron::fromAron(dtos, bosOut);
+ BOOST_CHECK_EQUAL_COLLECTIONS(bosOut.begin(), bosOut.end(),
+ dtos.begin(), dtos.end());
+}
+
+
+BOOST_AUTO_TEST_CASE(test_stl_vector)
+{
+ const std::vector<my::CustomType> bos
+ {
+ { "name", -10, 42.42f },
+ { "name2", 20, -128.128f },
+ };
+ std::vector<my::arondto::CustomType> dtos;
+
+ test_complex(bos, dtos);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_stl_map)
+{
+ const std::map<std::string, my::CustomType> bos
+ {
+ { "key1", { "name", -10, 42.42f } },
+ { "key2", { "name2", 20, -128.128f } },
+ };
+ std::map<std::string, my::arondto::CustomType> dtos;
+
+ test_complex(bos, dtos);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_stl_vector_of_vector)
+{
+ const std::vector<std::vector<my::CustomType>> bos
+ {
+ { { "name", -10, 42.42f } },
+ { },
+ { { "name2", 20, -128.128f }, { "name2.1", 40, -64.64f } },
+ };
+ std::vector<std::vector<my::arondto::CustomType>> dtos;
+
+ test_complex(bos, dtos);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_stl_map_of_vector)
+{
+ const std::map<std::string, std::vector<my::CustomType>> bos
+ {
+ { "key1", { { "name", -10, 42.42f } } },
+ { "key2", { { "name2", 20, -128.128f }, { "name2.1", 40, -64.64f } } },
+ { "key3", { } },
+ };
+ std::map<std::string, std::vector<my::arondto::CustomType>> dtos;
+
+ test_complex(bos, dtos);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_optional)
+{
+ using std::nullopt;
+
+ std::optional<my::CustomType> bo;
+ std::optional<my::arondto::CustomType> dto;
+
+ auto reset = [&](
+ std::optional<my::CustomType> theBo,
+ std::optional<my::arondto::CustomType> theDto)
+ {
+ bo = theBo;
+ dto = theDto;
+ };
+
+ reset(nullopt, nullopt);
+ {
+ armarx::aron::toAron(dto, bo);
+ BOOST_CHECK(!dto.has_value());
+ }
+ reset(nullopt, nullopt);
+ {
+ armarx::aron::fromAron(dto, bo);
+ BOOST_CHECK(!bo.has_value());
+ }
+
+ reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ {
+ armarx::aron::toAron(dto, bo);
+ BOOST_CHECK(dto.has_value());
+ BOOST_CHECK_EQUAL(dto.value(), bo.value());
+ }
+ reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ {
+ armarx::aron::fromAron(dto, bo);
+ BOOST_CHECK(!bo.has_value());
+ }
+
+ reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::toAron(dto, bo);
+ BOOST_CHECK(!dto.has_value());
+ }
+ reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::fromAron(dto, bo);
+ BOOST_CHECK(bo.has_value());
+ BOOST_CHECK_EQUAL(bo.value(), dto.value());
+ }
+
+ reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::toAron(dto, bo);
+ BOOST_CHECK(dto.has_value());
+ BOOST_CHECK_EQUAL(dto.value(), bo.value());
+ BOOST_CHECK_EQUAL(dto->name, "bo");
+ }
+ reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::fromAron(dto, bo);
+ BOOST_CHECK(bo.has_value());
+ BOOST_CHECK_EQUAL(bo.value(), dto.value());
+ BOOST_CHECK_EQUAL(bo->name, "dto");
+ }
+}
+
+
+
+BOOST_AUTO_TEST_CASE(test_optional_value_flagged)
+{
+ using std::nullopt;
+
+ std::optional<my::CustomType> bo;
+ my::arondto::CustomType dto;
+ bool dtoValid;
+
+ auto reset = [&](
+ std::optional<my::CustomType> theBo,
+ std::optional<my::arondto::CustomType> theDto)
+ {
+ bo = theBo;
+ if (theDto)
+ {
+ dto = *theDto;
+ dtoValid = true;
+ }
+ else
+ {
+ dto = {};
+ dtoValid = false;
+ }
+ };
+
+ reset(nullopt, nullopt);
+ {
+ armarx::aron::toAron(dto, dtoValid, bo);
+ BOOST_CHECK(!dtoValid);
+ }
+ reset(nullopt, nullopt);
+ {
+ armarx::aron::fromAron(dto, dtoValid, bo);
+ BOOST_CHECK(!bo.has_value());
+ }
+
+ reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ {
+ armarx::aron::toAron(dto, dtoValid, bo);
+ BOOST_CHECK(dtoValid);
+ BOOST_CHECK_EQUAL(dto, bo.value());
+ }
+ reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ {
+ armarx::aron::fromAron(dto, dtoValid, bo);
+ BOOST_CHECK(!bo.has_value());
+ }
+
+ reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::toAron(dto, dtoValid, bo);
+ BOOST_CHECK(!dtoValid);
+ }
+ reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::fromAron(dto, dtoValid, bo);
+ BOOST_CHECK(bo.has_value());
+ BOOST_CHECK_EQUAL(bo.value(), dto);
+ }
+
+ reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::toAron(dto, dtoValid, bo);
+ BOOST_CHECK(dtoValid);
+ BOOST_CHECK_EQUAL(dto, bo.value());
+ BOOST_CHECK_EQUAL(dto.name, "bo");
+ }
+ reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ {
+ armarx::aron::fromAron(dto, dtoValid, bo);
+ BOOST_CHECK(bo.has_value());
+ BOOST_CHECK_EQUAL(bo.value(), dto);
+ BOOST_CHECK_EQUAL(bo->name, "dto");
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp
index e266d962d5b1b2d6bc691354f5a80b1b4c38761d..faa6fadc1fbf5f3776ca73ee2363ff980713b41e 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.cpp
@@ -53,4 +53,44 @@ namespace armarx::aron::converter
{
return ConvertToMatrix<double, 4, 4>(n);
}
+
+ Eigen::Quaternion<float> AronEigenConverter::ConvertToQuaternionf(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToQuaternion<float>(n);
+ }
+ Eigen::Quaternion<double> AronEigenConverter::ConvertToQuaterniond(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToQuaternion<double>(n);
+ }
+ Eigen::Vector3f AronEigenConverter::ConvertToVector3f(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToVector<float, 3>(n);
+ }
+ Eigen::Vector3d AronEigenConverter::ConvertToVector3d(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToVector<double, 3>(n);
+ }
+ Eigen::Matrix4f AronEigenConverter::ConvertToMatrix4f(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToMatrix<float, 4, 4>(n);
+ }
+ Eigen::Matrix4d AronEigenConverter::ConvertToMatrix4d(const datanavigator::NDArrayNavigator& n)
+ {
+ return ConvertToMatrix<double, 4, 4>(n);
+ }
+
+ void AronEigenConverter::checkDimensions(
+ const datanavigator::NDArrayNavigator& nav, const std::vector<int>& expected,
+ const std::string& method, const std::string& caller)
+ {
+ if (nav.getDimensions() != expected)
+ {
+ std::stringstream ss;
+ ss << "The size of an NDArray does not match.";
+ ss << "\n Expected: \t" << datanavigator::NDArrayNavigator::DimensionsToString(expected);
+ ss << "\n Got: \t" << datanavigator::NDArrayNavigator::DimensionsToString(nav.getDimensions());
+ ss << "\n";
+ throw error::AronException(caller, method, ss.str(), nav.getPath());
+ }
+ }
}
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 06a973559c24b42dd306b62f474e12a40f0badfa..787ae30a23f097821989fcd9658a3fa31d6b4761 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -43,62 +43,85 @@ namespace armarx::aron::converter
public:
// TODO: Add inplace methods
- // TODO: Remove Ptrs for references
static Eigen::Quaternion<float> ConvertToQuaternionf(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Quaternion<float> ConvertToQuaternionf(const datanavigator::NDArrayNavigator&);
static Eigen::Quaternion<double> ConvertToQuaterniond(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Quaternion<double> ConvertToQuaterniond(const datanavigator::NDArrayNavigator&);
static Eigen::Vector3f ConvertToVector3f(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Vector3f ConvertToVector3f(const datanavigator::NDArrayNavigator&);
static Eigen::Vector3d ConvertToVector3d(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Vector3d ConvertToVector3d(const datanavigator::NDArrayNavigator&);
static Eigen::Matrix4f ConvertToMatrix4f(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Matrix4f ConvertToMatrix4f(const datanavigator::NDArrayNavigator&);
static Eigen::Matrix4d ConvertToMatrix4d(const datanavigator::NDArrayNavigatorPtr&);
+ static Eigen::Matrix4d ConvertToMatrix4d(const datanavigator::NDArrayNavigator&);
+
template<typename T>
static Eigen::Quaternion<T> ConvertToQuaternion(const datanavigator::NDArrayNavigatorPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
+ return ConvertToQuaternion<T>(*nav);
+ }
- if (nav->getDimensions() != std::vector<int>({1, 4, sizeof(T)}))
- {
- throw error::AronException("AronEigenConverter", "ConvertToQuaternion", "The size of an NDArray does not match.", nav->getPath());
- }
- auto dims = nav->getDimensions();
+ template<typename T>
+ static Eigen::Quaternion<T> ConvertToQuaternion(const datanavigator::NDArrayNavigator& nav)
+ {
+ checkDimensions(nav, {1, 4, sizeof(T)}, "ConvertToQuaternion");
+ auto dims = nav.getDimensions();
Eigen::Quaternion<T> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.coeffs().data()), nav->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ memcpy(reinterpret_cast<unsigned char*>(ret.coeffs().data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
return ret;
}
+
template<typename T, int Size>
static Eigen::Matrix<T, Size, 1> ConvertToVector(const datanavigator::NDArrayNavigatorPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
+ return ConvertToVector<T, Size>(*nav);
+ }
- if (nav->getDimensions() != std::vector<int>({Size, 1, sizeof(T)}))
- {
- throw error::AronException("AronEigenConverter", "ConvertToVector", "The size of an NDArray does not match.", nav->getPath());
- }
- auto dims = nav->getDimensions();
+ template<typename T, int Size>
+ static Eigen::Matrix<T, Size, 1> ConvertToVector(const datanavigator::NDArrayNavigator& nav)
+ {
+ checkDimensions(nav, {Size, 1, sizeof(T)}, "ConvertToVector");
+ auto dims = nav.getDimensions();
Eigen::Matrix<T, Size, 1> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
return ret;
}
+
template<typename T, int Rows, int Cols>
static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigatorPtr& nav)
{
ARMARX_CHECK_NOT_NULL(nav);
+ return ConvertToMatrix<T, Rows, Cols>(*nav);
+ }
- if (nav->getDimensions() != std::vector<int>({Rows, Cols, sizeof(T)}))
- {
- throw error::AronException("AronEigenConverter", "ConvertToMatrix", "The size of an NDArray does not match.", nav->getPath());
- }
- auto dims = nav->getDimensions();
+ template<typename T, int Rows, int Cols>
+ static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigator& nav)
+ {
+ checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
+ auto dims = nav.getDimensions();
Eigen::Matrix<T, Rows, Cols> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
return ret;
}
+
+ private:
+
+ /**
+ * @throw `error::AronException` If `nav`'s dimensions do not match `expected`.
+ */
+ static void checkDimensions(const datanavigator::NDArrayNavigator& nav, const std::vector<int>& expected,
+ const std::string& method, const std::string& caller = "AronEigenConverter");
+
};
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
index a431295f0583429740b3c43f0b64dfccf24b5499..42fe5d66dc70e3c5e769aaf65abc58e7ac7728c9 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
@@ -81,7 +81,7 @@ namespace armarx::aron::cppcodegenerator::serializer
CppBlockPtr OrientationSerializer::getWriteBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".coeffs().data()));");
+ b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + ", 4}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".coeffs().data()));");
return b;
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
index b7039894e985935fb8fb38e0d97fc10af7f7bd85..98a1dbae29a52a5392bfbbd537be7deea856a94d 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
@@ -81,7 +81,7 @@ namespace armarx::aron::cppcodegenerator::serializer
CppBlockPtr PoseSerializer::getWriteBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
+ b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + ", 4}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
return b;
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
index 4bb664d2ece7171047ca837e1642a45250de19cf..a0f7a0d6d553d203e71dfec31d43c996c226b634 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
@@ -81,7 +81,7 @@ namespace armarx::aron::cppcodegenerator::serializer
CppBlockPtr PositionSerializer::getWriteBlock(const std::string& accessor) const
{
CppBlockPtr b = CppBlockPtr(new CppBlock());
- b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
+ b->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->getDimensions(), ", ") + ", 4}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + ".data()));");
return b;
}
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
index d9d22766cf2f51e163758c27c962661a3cb6758f..b038334fbb5dcc6ac39206c33174320f6b8e44d0 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
@@ -182,7 +182,7 @@ namespace armarx::aron::cppcodegenerator::serializer
doc << "@brief operator=() - Assignment operator for copy \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("void operator=(" + getFullCppTypename() + "& c)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(getFullCppTypename() + "& operator=(const " + getFullCppTypename() + "& c)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("value = c.value;");
m->setBlock(b);
@@ -195,7 +195,7 @@ namespace armarx::aron::cppcodegenerator::serializer
doc << "@brief operator=() - Assignment operator for the internally defined enum \n";
doc << "@return - nothing";
- CppMethodPtr m = CppMethodPtr(new CppMethod("void operator=(" + enumName + " v)", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod(getFullCppTypename() + "& operator=(" + enumName + " v)", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("value = v;");
m->setBlock(b);
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
index 263253c8830ccd065648fd210be9721a1ea28030..55a30cff91afbd74bbf4cbae769df34992843096 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.cpp
@@ -87,8 +87,20 @@ namespace armarx::aron::visitor
{
return visit(*t, key, dynamic_cast<StringDataNavigator&>(data));
}
+ else if (auto t = dynamic_cast<TimeTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<LongDataNavigator&>(data));
+ }
- if (auto t = dynamic_cast<EigenMatrixTypeNavigator*>(&type))
+ if (auto t = dynamic_cast<PCLPointCloudTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<EigenMatrixTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<EigenQuaternionTypeNavigator*>(&type))
{
return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
}
@@ -100,7 +112,15 @@ namespace armarx::aron::visitor
{
return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
}
- else if (auto t = dynamic_cast<PCLPointCloudTypeNavigator*>(&type))
+ else if (auto t = dynamic_cast<PoseTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<PositionTypeNavigator*>(&type))
+ {
+ return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
+ }
+ else if (auto t = dynamic_cast<OrientationTypeNavigator*>(&type))
{
return visit(*t, key, dynamic_cast<NDArrayDataNavigator&>(data));
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
index 13800fa67e320bfa45d144b004baf3ec584f7225..f577903a4524da4ce220d6d402e9719f1afdac7a 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/visitors/TypedDataVisitor.h
@@ -29,8 +29,8 @@ namespace armarx::aron::visitor
* To the the location of the passed navigator in the original `AronType`,
* use `navigator.getPath()`.
*
- * @see `aron::Type::AronTypePtr`
- * @see `aron::Typenavigator::Navigator`
+ * @see `aron::Type::AronTypePtr`
+ * @see `aron::Typenavigator::Navigator`
* @see `std::stack` To manage Type with stack semantics.
*/
class TypedDataVisitor
@@ -73,12 +73,18 @@ namespace armarx::aron::visitor
using IntTypeNavigator = typenavigator::IntNavigator;
using LongTypeNavigator = typenavigator::LongNavigator;
using StringTypeNavigator = typenavigator::StringNavigator;
+ using TimeTypeNavigator = typenavigator::TimeNavigator;
+
// Array-valued
using EigenMatrixTypeNavigator = typenavigator::EigenMatrixNavigator;
+ using EigenQuaternionTypeNavigator = typenavigator::EigenQuaternionNavigator;
using IVTCByteImageTypeNavigator = typenavigator::IVTCByteImageNavigator;
using OpenCVMatTypeNavigator = typenavigator::OpenCVMatNavigator;
using PCLPointCloudTypeNavigator = typenavigator::PCLPointCloudNavigator;
+ using PoseTypeNavigator = typenavigator::PoseNavigator;
+ using PositionTypeNavigator = typenavigator::PositionNavigator;
+ using OrientationTypeNavigator = typenavigator::OrientationNavigator;
public:
@@ -166,26 +172,55 @@ namespace armarx::aron::visitor
(void) type, (void) data;
return true;
}
+ virtual bool visit(TimeTypeNavigator& type, LongDataNavigator& data)
+ {
+ return visit(type, IceUtil::Time::microSeconds(data.getValue()));
+ }
+ virtual bool visit(TimeTypeNavigator& type, const IceUtil::Time& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
virtual bool visit(EigenMatrixTypeNavigator& type, NDArrayDataNavigator& data)
{
- (void) type, (void) type, (void) data;
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(EigenQuaternionTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) data;
return true;
}
virtual bool visit(IVTCByteImageTypeNavigator& type, NDArrayDataNavigator& data)
{
- (void) type, (void) type, (void) data;
+ (void) type, (void) data;
return true;
}
virtual bool visit(OpenCVMatTypeNavigator& type, NDArrayDataNavigator& data)
{
- (void) type, (void) type, (void) data;
+ (void) type, (void) data;
return true;
}
virtual bool visit(PCLPointCloudTypeNavigator& type, NDArrayDataNavigator& data)
{
- (void) type, (void) type, (void) data;
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(PoseTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(PositionTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) data;
+ return true;
+ }
+ virtual bool visit(OrientationTypeNavigator& type, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) data;
return true;
}
@@ -194,119 +229,130 @@ namespace armarx::aron::visitor
virtual bool visitEnter(DictTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(DictTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visitEnter(ObjectTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(ObjectTypeNavigator& type, const std::string& key, DictDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visitEnter(ListTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(ListTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visitEnter(TupleTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitEnter(type, data);
- return true;
+ return visitEnter(type, data);
}
virtual bool visitExit(TupleTypeNavigator& type, const std::string& key, ListDataNavigator& data)
{
(void) key;
- visitExit(type, data);
- return true;
+ return visitExit(type, data);
}
virtual bool visit(BoolTypeNavigator& type, const std::string& key, BoolDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(DoubleTypeNavigator& type, const std::string& key, DoubleDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(FloatTypeNavigator& type, const std::string& key, FloatDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(IntTypeNavigator& type, const std::string& key, IntDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(LongTypeNavigator& type, const std::string& key, LongDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(StringTypeNavigator& type, const std::string& key, StringDataNavigator& data)
{
(void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
+ }
+ virtual bool visit(TimeTypeNavigator& type, const std::string& key, LongDataNavigator& data)
+ {
+ (void) key;
+ return visit(type, data);
+ }
+ virtual bool visit(TimeTypeNavigator& type, const std::string& key, const IceUtil::Time& data)
+ {
+ (void) key;
+ return visit(type, data);
}
virtual bool visit(EigenMatrixTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
+ }
+ virtual bool visit(EigenQuaternionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ return visit(type, data);
}
virtual bool visit(IVTCByteImageTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(OpenCVMatTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
}
virtual bool visit(PCLPointCloudTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
{
(void) type, (void) key;
- visit(type, data);
- return true;
+ return visit(type, data);
+ }
+ virtual bool visit(PoseTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ return visit(type, data);
+ }
+ virtual bool visit(PositionTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ return visit(type, data);
+ }
+ virtual bool visit(OrientationTypeNavigator& type, const std::string& key, NDArrayDataNavigator& data)
+ {
+ (void) type, (void) key;
+ return visit(type, data);
}
-
private:
@@ -322,76 +368,3 @@ namespace armarx::aron::visitor
};
}
-
-
-/* Copy-and-paste example below.
- * Remove functions you don't need.
- * Add `const std::string& key, ` to parameter list if you need an items's key
- * in the parent container.
- */
-
-#if 0
-
-struct MyDerivedAronTypeVisitor : public armarx::aron::visitor::AronTypeVisitor
-{
-
- bool visitEnter(DictTypeNavigator& dict) override
- {
- (void) dict;
- return true;
- }
- bool visitExit(DictTypeNavigator& dict) override
- {
- (void) dict;
- return true;
- }
- bool visitEnter(ListTypeNavigator& list) override
- {
- (void) list;
- return true;
- }
- bool visitExit(ListTypeNavigator& list) override
- {
- (void) list;
- return true;
- }
-
- bool visit(BoolTypeNavigator& b) override
- {
- (void) b;
- return true;
- }
- bool visit(DoubleTypeNavigator& d) override
- {
- (void) d;
- return true;
- }
- bool visit(FloatTypeNavigator& f) override
- {
- (void) f;
- return true;
- }
- bool visit(IntTypeNavigator& i) override
- {
- (void) i;
- return true;
- }
- bool visit(LongTypeNavigator& l) override
- {
- (void) l;
- return true;
- }
- bool visit(NDArrayTypeNavigator& array) override
- {
- (void) array;
- return true;
- }
- bool visit(StringTypeNavigator& string) override
- {
- (void) string;
- return true;
- }
-
-};
-
-#endif
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 5743f098b27b9de73ed56047ee020089a046af22..9babf0b23f1eeae6bc2bc01e9238d93809c6c40c 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -49,7 +49,7 @@ set(LIB_FILES
CartesianVelocityControllerWithRamp.cpp
CartesianNaturalPositionController.cpp
#CartesianNaturalVelocityController.cpp
-
+
visualization/DebugDrawerTopic.cpp
visualization/GlasbeyLUT.cpp
@@ -102,7 +102,7 @@ set(LIB_HEADERS
CartesianNaturalPositionController.h
#CartesianNaturalVelocityController.h
EigenHelpers.h
-
+
visualization/DebugDrawerTopic.h
visualization/GlasbeyLUT.h
@@ -114,3 +114,5 @@ set(LIB_HEADERS
add_subdirectory(test)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+add_library(RobotAPI::Core ALIAS RobotAPICore)