diff --git a/scenarios/dasd/config/DynamicObstacleManager.cfg b/scenarios/dasd/config/DynamicObstacleManager.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..c2e4f0a514d269ca6fdc5cb0daa164c2be4e6cd3
--- /dev/null
+++ b/scenarios/dasd/config/DynamicObstacleManager.cfg
@@ -0,0 +1,327 @@
+# ==================================================================
+# DynamicObstacleManager 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.DynamicObstacleManager.AllowInRobotSpawning: Allow obstacles to spawn inside the robot
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DynamicObstacleManager.AllowInRobotSpawning = false
+
+
+# ArmarX.DynamicObstacleManager.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.DynamicObstacleManager.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.DynamicObstacleManager.EnableProfiling = false
+
+
+# ArmarX.DynamicObstacleManager.EnableRemove: Delete Obstacles when value < 0
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+ArmarX.DynamicObstacleManager.EnableRemove = true
+
+
+# ArmarX.DynamicObstacleManager.LineSecurityMargin: Security margin of line obstacles
+# Attributes:
+# - Default: 400
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.LineSecurityMargin = 250
+
+
+# ArmarX.DynamicObstacleManager.LineThickness: The thickness of line obstacles
+# Attributes:
+# - Default: 200
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.LineThickness = 200
+
+
+# ArmarX.DynamicObstacleManager.MaxLengthOfLines: Maximum length of lines in mm
+# Attributes:
+# - Default: 10000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.MaxLengthOfLines = 10000
+
+
+# ArmarX.DynamicObstacleManager.MaxObstacleSize: The maximal obstacle size in mm.
+# Attributes:
+# - Default: 600
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MaxObstacleSize = 400
+
+
+# ArmarX.DynamicObstacleManager.MaxObstacleValue: Maximum value for the obstacles
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.MaxObstacleValue = 5000
+
+
+# ArmarX.DynamicObstacleManager.MinLengthOfLines: Minimum length of lines in mm
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinLengthOfLines = 50
+
+
+# ArmarX.DynamicObstacleManager.MinObstacleSize: The minimal obstacle size in mm.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinObstacleSize = 5
+
+
+# ArmarX.DynamicObstacleManager.MinObstacleValueForAccepting: Minimum value for the obstacles to get accepted
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinObstacleValueForAccepting = 5
+
+
+# ArmarX.DynamicObstacleManager.MinSampleRatioPerEllipsis: Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle
+# Attributes:
+# - Default: 0.699999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.MinSampleRatioPerEllipsis = 0.699999988
+
+
+# ArmarX.DynamicObstacleManager.MinSampleRatioPerLineSegment: Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle
+# Attributes:
+# - Default: 0.899999976
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinSampleRatioPerLineSegment = 0.5
+
+
+# ArmarX.DynamicObstacleManager.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.DynamicObstacleManager.MinimumLoggingLevel = Debug
+
+
+# ArmarX.DynamicObstacleManager.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.ObjectName = ""
+
+
+# ArmarX.DynamicObstacleManager.ObstacleAvoidanceName: The name of the used obstacle avoidance interface
+# Attributes:
+# - Default: PlatformObstacleAvoidance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.ObstacleAvoidanceName = PlatformObstacleAvoidance
+
+
+# ArmarX.DynamicObstacleManager.ObstacleSecurityMargin: Security margin of ellipsis obstacles
+# Attributes:
+# - Default: 500
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.ObstacleSecurityMargin = 300
+
+
+# ArmarX.DynamicObstacleManager.OnlyVisualizeObstacles: Connection to obstacle avoidance
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DynamicObstacleManager.OnlyVisualizeObstacles = false
+
+
+# ArmarX.DynamicObstacleManager.UpdateInterval: The interval to check the obstacles
+# Attributes:
+# - Default: 500
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.UpdateInterval = 350
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/dasd/config/ObstacleAwarePlatformUnit.cfg b/scenarios/dasd/config/ObstacleAwarePlatformUnit.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..c397d978b79078ebfd9908eb5bfd7c9a3e103c59
--- /dev/null
+++ b/scenarios/dasd/config/ObstacleAwarePlatformUnit.cfg
@@ -0,0 +1,332 @@
+# ==================================================================
+# ObstacleAwarePlatformUnit 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.ObstacleAwarePlatformUnit.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ObstacleAwarePlatformUnit.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.ObstacleAwarePlatformUnit.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.ObstacleAwarePlatformUnit.EnableProfiling = false
+
+
+# ArmarX.ObstacleAwarePlatformUnit.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.ObstacleAwarePlatformUnit.MinimumLoggingLevel = Debug
+
+
+# ArmarX.ObstacleAwarePlatformUnit.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.ObjectName = ""
+
+
+# ArmarX.ObstacleAwarePlatformUnit.PlatformName: Name of the platform (will publish values on PlatformName + 'State')
+# Attributes:
+# - Default: Platform
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObstacleAwarePlatformUnit.PlatformName = Armar6PlatformUnit
+
+
+# ArmarX.ObstacleAwarePlatformUnit.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.ObstacleAwarePlatformUnit.cmp.DynamicObstacleManager: Ice object name of the `DynamicObstacleManager` component.
+# Attributes:
+# - Default: ObstacleAvoidingPlatformUnit
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObstacleAwarePlatformUnit.cmp.DynamicObstacleManager = DynamicObstacleManager
+
+
+# ArmarX.ObstacleAwarePlatformUnit.cmp.PlatformUnit: Ice object name of the `PlatformUnit` component.
+# Attributes:
+# - Default: Platform
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObstacleAwarePlatformUnit.cmp.PlatformUnit = Armar6PlatformUnit
+
+
+# ArmarX.ObstacleAwarePlatformUnit.cmp.RobotStateComponent: Ice object name of the `RobotStateComponent` component.
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.cmp.RobotStateComponent = RobotStateComponent
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.pos.kp:
+# Attributes:
+# - Default: 3.5
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.pos.kp = 3.5
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.pose.cycle_time: Control loop cycle time.
+# Attributes:
+# - Default: 10 ms
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.pose.cycle_time = 10 ms
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kd:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kd = 0
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.ki:
+# Attributes:
+# - Default: 9.00000014e-05
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.ki = 9.00000014e-05
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kp:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kp = 1
+
+
+# ArmarX.ObstacleAwarePlatformUnit.doa.agent_safety_margin:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.doa.agent_safety_margin = 0
+
+
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_general: Velocity in [mm/s] the robot should at least set on general
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_general = 100
+
+
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_near_target: Velocity in [mm/s] the robot should at least set when near the target
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_near_target = 50
+
+
+# ArmarX.ObstacleAwarePlatformUnit.pos_near_threshold: Distance in [mm] after which the robot is considered to be near the target for min velocity, smoothing, etc.
+# Attributes:
+# - Default: 250
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.pos_near_threshold = 250
+
+
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.GlobalRobotPoseLocalization: Name of the `GlobalRobotPoseLocalization` topic to publish data to.
+# Attributes:
+# - Default: GlobalRobotPoseLocalization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.GlobalRobotPoseLocalization = GlobalRobotPoseLocalization
+
+
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.Odometry: Name of the `Odometry` topic to publish data to.
+# Attributes:
+# - Default: Odometry
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.Odometry = Odometry
+
+
+# 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/dasd/config/global.cfg b/scenarios/dasd/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..897f1c82a0578538e7b463f4246587ba134e24de
--- /dev/null
+++ b/scenarios/dasd/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario dasd
+# ==================================================================
+
diff --git a/scenarios/dasd/dasd.scx b/scenarios/dasd/dasd.scx
new file mode 100644
index 0000000000000000000000000000000000000000..b12d5a0c9d6feb2eb4f19076d28212d9294eff02
--- /dev/null
+++ b/scenarios/dasd/dasd.scx
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="dasd" creation="2021-06-07.18:00:09" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ObstacleAwarePlatformUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DynamicObstacleManager" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index 41fc3e85f7a6a7250c47fe58c0730973a16480be..f15496da384cd8c25169bdcc5decf93b730cff6e 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -259,8 +259,12 @@ namespace armarx
{
std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
+ const Eigen::Vector2f diff = goal - agentPosition;
+ const Eigen::Vector2f orthogonal_normalized = Eigen::Vector2f(Eigen::Rotation2Df(M_PI_2f32) * diff).normalized();
+
const float sample_step = 5; // in [mm], sample step size towards goal.
- const float distance_to_goal = (goal - agentPosition).norm() + safetyRadius;
+ const float distance_to_goal = diff.norm() + safetyRadius;
+
float current_distance = safetyRadius;
while (current_distance < distance_to_goal)
@@ -268,6 +272,9 @@ namespace armarx
for (const auto man_obstacle : m_managed_obstacles)
{
Eigen::Vector2f sample = agentPosition + ((goal - agentPosition).normalized() * current_distance);
+ Eigen::Vector2f sample_left = sample + (orthogonal_normalized * 250);
+ Eigen::Vector2f sample_right = sample - (orthogonal_normalized * 250);
+
obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
@@ -275,6 +282,16 @@ namespace armarx
{
return current_distance - safetyRadius;
}
+
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample_left))
+ {
+ return current_distance - safetyRadius;
+ }
+
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample_right))
+ {
+ return current_distance - safetyRadius;
+ }
}
current_distance += sample_step;
diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index bfe0764272dbee12270de46cfe7cf0b90f851bb0..2a0eb6e217a769fe4bab33dd0ab896e2faeda8e5 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/source/RobotAPI/interface/armem/query.ice
@@ -11,9 +11,17 @@ module armarx
{
module data
{
+ enum QueryTarget
+ {
+ WMAndLTM,
+ WM,
+ LTM // only for debug. remove later
+ };
+
/// Which entity snapshots to get from an entity?
class EntityQuery
{
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<EntityQuery> EntityQuerySeq;
@@ -107,6 +115,7 @@ module armarx
class ProviderSegmentQuery
{
EntityQuerySeq entityQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<ProviderSegmentQuery> ProviderSegmentQuerySeq;
module provider
@@ -129,6 +138,7 @@ module armarx
class CoreSegmentQuery
{
ProviderSegmentQuerySeq providerSegmentQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<CoreSegmentQuery> CoreSegmentQuerySeq;
@@ -152,6 +162,7 @@ module armarx
class MemoryQuery
{
CoreSegmentQuerySeq coreSegmentQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
sequence<MemoryQuery> MemoryQuerySeq;
dictionary<string, MemoryQuerySeq> MemoryQueriesDict;
@@ -177,6 +188,7 @@ module armarx
{
/// Dict of memory name to
MemoryQueriesDict memoryQueries;
+ QueryTarget target = QueryTarget::WMAndLTM;
};
struct Input
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 7e9acde0bdc18f3bd937f5972ce0081727aabc98..c375e084dea6b48720d50e59cc9e828e070826ba 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -40,9 +40,10 @@ namespace armarx
positionSensors.push_back(positionSensor);
};
- tcp = rns->getTCP();
- ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(),
- VirtualRobot::JacobiProvider::eSVDDamped));
+ tcp = rns->getTCP();
+ ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ ik->setDampedSvdLambda(0.0001);
+
numOfJoints = targets.size();
// set DMP
TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.cpp b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
index e7ce598a68d393d6da88a9e4ec52a801d10999c5..5979ea365a863dc0ce1c0c0b91a0d77fc3f331e9 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
@@ -8,6 +8,12 @@ namespace armarx::armem::client::query
{
}
+ Builder& Builder::queryTarget(const armem::query::data::QueryTarget target)
+ {
+ this->_target = target;
+ return *this;
+ }
+
QueryInput Builder::buildQueryInput() const
{
QueryInput input;
@@ -27,6 +33,7 @@ namespace armarx::armem::client::query
{
for (const armem::query::data::MemoryQueryPtr& query : child.buildQueries())
{
+ query->target = _target;
memoryQueries.push_back(query);
}
}
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.h b/source/RobotAPI/libraries/armem/client/query/Builder.h
index 7f4c2b7c63440f8182d2e489e74ea33ab98e8317..eae09294320f216aa3e996821c429376ff7675a0 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.h
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.h
@@ -28,7 +28,7 @@ namespace armarx::armem::client::query
public:
Builder(DataMode dataMode = DataMode::WithData);
-
+ Builder& queryTarget(const armem::query::data::QueryTarget target);
/// Start specifying core segments.
CoreSegmentSelector& coreSegments();
@@ -66,7 +66,7 @@ namespace armarx::armem::client::query
public:
-
+ armem::query::data::QueryTarget _target = armem::query::data::QueryTarget::WMAndLTM;
DataMode dataMode;
};
diff --git a/source/RobotAPI/libraries/armem/core/Commit.cpp b/source/RobotAPI/libraries/armem/core/Commit.cpp
index 5c1946ed0f55591457c7ce7fcf7a8a5b9a325ee4..fed7ed660b450bb0865a018f9c8d7119dd96a7b4 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.cpp
+++ b/source/RobotAPI/libraries/armem/core/Commit.cpp
@@ -86,4 +86,17 @@ namespace armarx::armem
return updates.emplace_back(update);
}
+ void Commit::add(const std::vector<EntityUpdate>& updates)
+ {
+ for (const auto& update : updates)
+ {
+ add(update);
+ }
+ }
+
+ void Commit::append(const Commit& c)
+ {
+ add(c.updates);
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index 48707600ab242ad3a624c6e26965acdef18839fa..fc69a4b53dcaecc52d703aa42c00405f4162825f 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -10,6 +10,16 @@
namespace armarx::armem
{
+
+ /**
+ * @brief The type of an update
+ */
+ enum class UpdateType
+ {
+ UpdatedExisting,
+ InsertedNew
+ };
+
/**
* @brief An update of an entity for a specific point in time.
*/
@@ -87,6 +97,8 @@ namespace armarx::armem
EntityUpdate& add();
EntityUpdate& add(const EntityUpdate& update);
+ void add(const std::vector<EntityUpdate>& updates);
+ void append(const Commit& c);
friend std::ostream& operator<<(std::ostream& os, const Commit& rhs);
};
diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
index a6e47cafd667d2e0de624acb3fb4c15979727912..eefb1b5ce73fdc3f83b5f15101ad3e10e9f0f989 100644
--- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h
@@ -33,6 +33,22 @@ namespace armarx::armem::base
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType coreSegmentUpdateType;
+ armarx::armem::UpdateType providerSegmentUpdateType;
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ UpdateResult(const typename ProviderSegmentT::UpdateResult& c) :
+ providerSegmentUpdateType(c.providerSegmentUpdateType),
+ entityUpdateType(c.entityUpdateType),
+ id(c.id),
+ removedSnapshots(c.removedSnapshots)
+ {}
+ };
public:
@@ -129,26 +145,60 @@ namespace armarx::armem::base
}
}
- virtual MemoryID update(const EntityUpdate& update) override
+ /**
+ * @brief Updates an entity's history.
+ *
+ * Missing entity entries are added before updating.
+ */
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.coreSegmentName, this->name());
+ ProviderSegmentT* provSeg;
+ UpdateType updateType = UpdateType::InsertedNew;
+
auto it = this->_container.find(update.entityID.providerSegmentName);
- if (it != this->_container.end())
+ if (it == this->_container.end())
{
- return it->second.update(update);
+ if (_addMissingProviderSegmentDuringUpdate)
+ {
+ // Insert into map.
+ provSeg = &addProviderSegment(update.entityID.providerSegmentName);
+ provSeg->setMaxHistorySize(_maxHistorySize);
+ updateType = UpdateType::InsertedNew;
+ }
+ else
+ {
+ throw error::MissingEntry::create<EntitySnapshotT>(update.entityID.providerSegmentName, *this);
+ }
}
else
{
- if (_addMissingProviderSegmentDuringUpdate)
+ provSeg = &it->second;
+ updateType = UpdateType::UpdatedExisting;
+ }
+
+ // Update entry.
+ UpdateResult ret(provSeg->update(update));
+ ret.coreSegmentUpdateType = updateType;
+ return ret;
+ }
+
+ void append(const _Derived& m)
+ {
+ ARMARX_INFO << "CoreSegment: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
{
- // Add the missing provider segment (with this core segment's type).
- ProviderSegmentT& provSeg = addProviderSegment(update.entityID.providerSegmentName);
- return provSeg.update(update);
+ // segment already exists
+ it->second.append(s);
}
else
{
- throw armem::error::MissingEntry::create<ProviderSegmentT>(update.entityID.providerSegmentName, *this);
+ auto wms = this->_container.emplace(std::make_pair(k, this->id().withProviderSegmentName(k)));
+ wms.first->second.append(s);
}
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.h b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
index c6fefc5b18133bc7aa36797156687d2a75384f3e..ab227c33ee321475f129d8f864b5cbf31e3f4845 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.h
@@ -52,6 +52,14 @@ namespace armarx::armem::base
using EntitySnapshotT = _EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ };
public:
@@ -119,7 +127,12 @@ namespace armarx::armem::base
/**
* @brief Indicates whether a history entry for the given time exists.
*/
- virtual bool hasSnapshot(Time time) const
+ virtual bool hasSnapshot(const Time& time)
+ {
+ return const_cast<const EntityBase*>(this)->hasSnapshot(time);
+ }
+
+ virtual bool hasSnapshot(const Time& time) const
{
return this->_container.count(time) > 0;
}
@@ -128,6 +141,11 @@ namespace armarx::armem::base
* @brief Get the latest timestamp.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
*/
+ Time getLatestTimestamp()
+ {
+ return const_cast<const EntityBase*>(this)->getLatestTimestamp();
+ }
+
Time getLatestTimestamp() const
{
return getLatestItem().first;
@@ -136,6 +154,11 @@ namespace armarx::armem::base
/**
* @brief Get all timestamps in the history.
*/
+ virtual std::vector<Time> getTimestamps()
+ {
+ return const_cast<const EntityBase*>(this)->getTimestamps();
+ }
+
virtual std::vector<Time> getTimestamps() const
{
return simox::alg::get_keys(this->_container);
@@ -148,14 +171,13 @@ namespace armarx::armem::base
* @return The snapshot, if it exists.
*
* @throws `armem::error::MissingEntry` If there is no such entry.
- * @throws `armem::error::MissingData` If the entry has no data.
*/
- EntitySnapshotT& getSnapshot(Time time)
+ virtual EntitySnapshotT& getSnapshot(const Time& time)
{
return const_cast<EntitySnapshotT&>(const_cast<const EntityBase*>(this)->getSnapshot(time));
}
- virtual const EntitySnapshotT& getSnapshot(Time time) const
+ virtual const EntitySnapshotT& getSnapshot(const Time& time) const
{
auto it = this->_container.find(time);
if (it != this->_container.end())
@@ -175,7 +197,7 @@ namespace armarx::armem::base
const EntitySnapshotT& getSnapshot(const MemoryID& id) const
{
- checkEntityName(id.entityName);
+ this->_checkContainerName(id.entityName, this->name());
return getSnapshot(id.timestamp);
}
@@ -183,7 +205,6 @@ namespace armarx::armem::base
* @brief Return the snapshot with the most recent timestamp.
* @return The latest snapshot.
* @throw `armem::error::EntityHistoryEmpty` If the history is empty.
- * @throw `armem::error::MissingData` If the latest snapshot has no data.
*/
EntitySnapshotT& getLatestSnapshot()
{
@@ -195,16 +216,153 @@ namespace armarx::armem::base
return getLatestItem().second;
}
+ /**
+ * @brief Return the lastest snapshot before or at time.
+ * @param time The time.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getLatestSnapshotBeforeOrAt(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.begin())
+ {
+ if (refIt->first == time)
+ {
+ return refIt->second;
+ }
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+
+ // ... or we return the element before if possible
+ if (refItPrev != this->_container.begin())
+ {
+ return refItPrev->second;
+ }
+
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ /**
+ * @brief Return the lastest snapshot before time.
+ * @param time The time.
+ * @return The latest snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getLatestSnapshotBefore(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.begin())
+ {
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ // last element less than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItPrev = std::prev(refIt);
+
+ // we return the element before if possible
+ if (refItPrev != this->_container.begin())
+ {
+ return refItPrev->second;
+ }
+
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ /**
+ * @brief Return all snapshots before or at time.
+ * @param time The time.
+ * @return The latest snapshots.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshots
+ */
+ virtual const std::vector<std::reference_wrapper<const EntitySnapshotT>> getSnapshotsBefore(const Time& time) const
+ {
+ std::vector<std::reference_wrapper<const EntitySnapshotT>> ret;
+ const EntitySnapshotT& latest = getLatestSnapshotBefore(time);
+
+ for (const auto& [timestamp, snapshot] : this->_container)
+ {
+ ret.emplace_back(snapshot);
+ if (timestamp == latest.id().timestamp)
+ {
+ break;
+ }
+ }
+ return ret;
+ }
+
+ /**
+ * @brief Return first snapshot after or at time.
+ * @param time The time.
+ * @return The first snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getFirstSnapshotAfterOrAt(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.end())
+ {
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+ return refIt->second;
+ }
+
+ /**
+ * @brief Return first snapshot after time.
+ * @param time The time.
+ * @return The first snapshot.
+ * @throw `armem::error::EntityHistoryEmpty` If the history is empty.
+ * @throw `armem::error::MissingEntry` If no such snapshot
+ */
+ virtual const EntitySnapshotT& getFirstSnapshotAfter(const Time& time) const
+ {
+ // first element equal or greater
+ typename std::map<Time, EntitySnapshotT>::const_iterator refIt = this->_container.upper_bound(time);
+
+ if (refIt == this->_container.end())
+ {
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ if (refIt->first > time)
+ {
+ return refIt->second;
+ }
+
+ // first element greater than
+ typename std::map<Time, EntitySnapshotT>::const_iterator refItNext = std::next(refIt);
+
+ if (refItNext != this->_container.end())
+ {
+ return refItNext->second;
+ }
+
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
/**
* @brief Add the given update to this entity's history.
* @param update The update.
* @return The snapshot ID of the update.
*/
- virtual MemoryID update(const EntityUpdate& update)
+ UpdateResult update(const EntityUpdate& update)
{
- checkEntityName(update.entityID.entityName);
- this->id() = update.entityID;
+ this->_checkContainerName(update.entityID.entityName, this->name());
+ UpdateResult ret;
EntitySnapshotT* snapshot;
@@ -213,16 +371,39 @@ namespace armarx::armem::base
{
// Insert into history.
snapshot = &addSnapshot(update.timeCreated);
- truncateHistoryToSize();
+ ret.removedSnapshots = truncate();
+ ret.entityUpdateType = UpdateType::InsertedNew;
}
else
{
snapshot = &it->second;
+ ret.entityUpdateType = UpdateType::UpdatedExisting;
}
// Update entry.
- snapshot->update(update);
+ snapshot->update(update, this->id());
+ ret.id = snapshot->id();
- return snapshot->id();
+ return ret;
+ }
+
+ void append(const DerivedT& m)
+ {
+ ARMARX_INFO << "Entity: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
+ {
+ // segment already exists
+ // We assume that a snapshot does not change, so ignore
+ }
+ else
+ {
+ auto snapshot = s.copy();
+ snapshot.id() = this->id().withTimestamp(k); // update id (e.g. memory name) if necessary
+ this->_container.insert(std::make_pair(k, std::move(snapshot)));
+ }
+ }
}
/**
@@ -256,7 +437,7 @@ namespace armarx::armem::base
void setMaxHistorySize(long maxSize) override
{
MaxHistorySize::setMaxHistorySize(maxSize);
- truncateHistoryToSize();
+ truncate();
}
std::string getKeyString() const override
@@ -272,24 +453,19 @@ namespace armarx::armem::base
protected:
/// If maximum size is set, ensure `history`'s is not higher.
- void truncateHistoryToSize()
+ std::vector<EntitySnapshotT> truncate()
{
+ std::vector<EntitySnapshotT> removedElements;
if (_maxHistorySize >= 0)
{
while (this->_container.size() > size_t(_maxHistorySize))
{
+ removedElements.push_back(std::move(this->_container.begin()->second));
this->_container.erase(this->_container.begin());
}
ARMARX_CHECK_LESS_EQUAL(this->_container.size(), _maxHistorySize);
}
- }
-
- void checkEntityName(const std::string& name) const
- {
- if (name != this->name())
- {
- throw armem::error::ContainerNameMismatch(name, getLevelName(), getKeyString());
- }
+ return removedElements;
}
/**
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
index aab70193435c7eec54b9e2127e7c57528ebba842..8dd474c667687e6235a96a191e577163ca194cba 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
@@ -53,7 +53,6 @@ namespace armarx::armem::base
*/
virtual void update(const EntityUpdate& update, int index) = 0;
-
virtual bool equalsDeep(const DerivedT& other) const = 0;
virtual DerivedT copy() const
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index 95a5be4f13c9baf4c32ce302158b78acd8cfa8c1..a74f41d3c0a9efd34d00b2f6fa6849bafd9dca1e 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -49,7 +49,7 @@ namespace armarx::armem::base
EntitySnapshotBase& operator=(EntitySnapshotBase&& other) = default;
- virtual bool equalsDeep(const EntitySnapshotBase& other) const
+ bool equalsDeep(const EntitySnapshotBase& other) const
{
//std::cout << "EntitySnapshot::equalsDeep" << std::endl;
if (this->size() != other.size())
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 4a01a472015e86fd824e0867e48ff62bda19fda1..69cd01fc128338e3d251a2ca99e54849c962dc9e 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -33,6 +33,25 @@ namespace armarx::armem::base
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType memoryUpdateType;
+ armarx::armem::UpdateType coreSegmentUpdateType;
+ armarx::armem::UpdateType providerSegmentUpdateType;
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ UpdateResult(const typename CoreSegmentT::UpdateResult& c) :
+ coreSegmentUpdateType(c.coreSegmentUpdateType),
+ providerSegmentUpdateType(c.providerSegmentUpdateType),
+ entityUpdateType(c.entityUpdateType),
+ id(c.id),
+ removedSnapshots(c.removedSnapshots)
+ {}
+ };
+
public:
MemoryBase()
@@ -198,14 +217,37 @@ namespace armarx::armem::base
return segments;
}
- virtual MemoryID update(const EntityUpdate& update) override
+
+ /**
+ * @brief Store all updates in `commit`.
+ * @param commit The commit.
+ * @return The resulting memory IDs.
+ */
+ std::vector<UpdateResult> update(const Commit& commit)
+ {
+ std::vector<UpdateResult> results;
+ for (const auto& update : commit.updates)
+ {
+ results.push_back(this->update(update));
+ }
+ return results;
+ }
+
+ /**
+ * @brief Store the given update.
+ * @param update The update.
+ * @return The resulting entity snapshot's ID.
+ */
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.memoryName, this->name());
auto it = this->_container.find(update.entityID.coreSegmentName);
if (it != this->_container.end())
{
- return it->second.update(update);
+ UpdateResult ret(it->second.update(update));
+ ret.memoryUpdateType = UpdateType::UpdatedExisting;
+ return ret;
}
else
{
@@ -213,6 +255,29 @@ namespace armarx::armem::base
}
}
+ /**
+ * @brief Merge another memory into this one. Append all data
+ * @param m The other memory
+ */
+ void append(const _Derived& m)
+ {
+ ARMARX_INFO << "Memory: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
+ {
+ // segment already exists
+ it->second.append(s);
+ }
+ else
+ {
+ auto wms = this->_container.emplace(std::make_pair(k, this->id().withCoreSegmentName(k)));
+ wms.first->second.append(s);
+ }
+ }
+ }
+
virtual bool equalsDeep(const MemoryBase& other) const
{
//std::cout << "Memory::equalsDeep" << std::endl;
diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
index ef8ed9fd94f3ee465dfa2d9e4df511598889b548..a53595c8784ea64cc4994f3f77b4ac22a1510cb1 100644
--- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h
@@ -32,6 +32,20 @@ namespace armarx::armem::base
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
+ struct UpdateResult
+ {
+ armarx::armem::UpdateType providerSegmentUpdateType;
+ armarx::armem::UpdateType entityUpdateType;
+ MemoryID id;
+ std::vector<EntitySnapshotT> removedSnapshots;
+
+ UpdateResult() = default;
+ UpdateResult(const typename EntityT::UpdateResult& c) :
+ entityUpdateType(c.entityUpdateType),
+ id(c.id),
+ removedSnapshots(c.removedSnapshots)
+ {}
+ };
public:
@@ -128,24 +142,49 @@ namespace armarx::armem::base
*
* Missing entity entries are added before updating.
*/
- virtual MemoryID update(const EntityUpdate& update) override
+ UpdateResult update(const EntityUpdate& update)
{
this->_checkContainerName(update.entityID.providerSegmentName, this->name());
EntityT* entity;
- auto it = this->_container.find(update.entityID.providerSegmentName);
+ UpdateType updateType = UpdateType::InsertedNew;
+
+ auto it = this->_container.find(update.entityID.entityName);
if (it == this->_container.end())
{
// Add entity entry.
entity = &addEntity(update.entityID.entityName);
entity->setMaxHistorySize(_maxHistorySize);
+ updateType = UpdateType::InsertedNew;
}
else
{
entity = &it->second;
+ updateType = UpdateType::UpdatedExisting;
}
// Update entity.
- return entity->update(update);
+ UpdateResult ret(entity->update(update));
+ ret.providerSegmentUpdateType = updateType;
+ return ret;
+ }
+
+ void append(const _Derived& m)
+ {
+ ARMARX_INFO << "ProviderSegment: Merge name '" << m.name() << "' into '" << name() << "'";
+
+ for (const auto& [k, s] : m.container())
+ {
+ if (const auto& it = this->_container.find(k); it != this->_container.end())
+ {
+ // segment already exists
+ it->second.append(s);
+ }
+ else
+ {
+ auto wms = this->_container.emplace(std::make_pair(k, this->id().withEntityName(k)));
+ wms.first->second.append(s);
+ }
+ }
}
/// Add an empty entity with the given name.
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
index 09733b648d096c7e33179c2903f3dbe241dca8d2..95993d560c6435fd147ddfc7a2a7aa9267d94855 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h
@@ -32,42 +32,18 @@ namespace armarx::armem::base::detail
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
using EntityInstanceT = typename EntitySnapshotT::EntityInstanceT;
-
public:
using Base::MemoryContainerBase;
using Base::operator=;
-
- /**
- * @brief Store all updates in `commit`.
- * @param commit The commit.
- * @return The resulting memory IDs.
- */
- std::vector<MemoryID> update(const Commit& commit)
- {
- std::vector<MemoryID> ids;
- for (const auto& update : commit.updates)
- {
- ids.push_back(this->update(update));
- }
- return ids;
- }
- /**
- * @brief Store the given update.
- * @param update The update.
- * @return The resulting entity snapshot's ID.
- */
- virtual MemoryID update(const EntityUpdate& update) = 0;
-
-
/**
* @brief Retrieve an entity.
* @param id The entity ID.
* @return The entity.
* @throw An exception deriving from `armem::error::ArMemError` if the entity is missing.
*/
- virtual EntityT& getEntity(const MemoryID& id)
+ EntityT& getEntity(const MemoryID& id)
{
return const_cast<EntityT&>(const_cast<const EntityContainerBase*>(this)->getEntity(id));
}
@@ -97,12 +73,12 @@ namespace armarx::armem::base::detail
* @return The entity snapshot.
* @throw An exception deriving from `armem::error::ArMemError` if the snapshot is missing.
*/
- virtual EntitySnapshotT& getEntitySnapshot(const MemoryID& id)
+ EntitySnapshotT& getEntitySnapshot(const MemoryID& id)
{
return const_cast<EntitySnapshotT&>(const_cast<const EntityContainerBase*>(this)->getEntitySnapshot(id));
}
- virtual const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const
+ const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const
{
const EntityT& entity = getEntity(id);
@@ -116,12 +92,12 @@ namespace armarx::armem::base::detail
}
}
- virtual EntityInstanceT& getEntityInstance(const MemoryID& id)
+ EntityInstanceT& getEntityInstance(const MemoryID& id)
{
return getEntitySnapshot(id).getInstance(id);
}
- virtual const EntityInstanceT& getEntityInstance(const MemoryID& id) const
+ const EntityInstanceT& getEntityInstance(const MemoryID& id) const
{
return getEntitySnapshot(id).getInstance(id);
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
index 69539df5c4063b8d27cd751b79ee82eb90ff5b7e..1fc07ed85e54716d24d2447fa17cb95b291f2766 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MaxHistorySize.h
@@ -4,6 +4,7 @@
namespace armarx::armem::base::detail
{
+ // TODO: Replace by ConstrainedHistorySize (not only max entries, e.g. delete oldest / delete least accessed / ...)
class MaxHistorySize
{
public:
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
index b2155c70dbf645ebe0e2783fd3189bf9b58800af..1a6a216653d243da9613ec967363dc5d44b5c503 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
@@ -7,7 +7,6 @@
namespace armarx::armem::base::detail
{
-
/**
* @class Provides default implmentations of `MemoryContainer`, as well as
* iterators (which requires a template).
@@ -19,7 +18,6 @@ namespace armarx::armem::base::detail
using Base = MemoryItem;
public:
-
using DerivedT = _DerivedT;
using ContainerT = _ContainerT;
@@ -42,15 +40,19 @@ namespace armarx::armem::base::detail
// Container methods
- virtual bool empty() const
+ bool empty() const
{
return _container.empty();
}
- virtual std::size_t size() const
+ std::size_t size() const
{
return _container.size();
}
- virtual void clear()
+ bool hasData() const
+ {
+ return size() > 0;
+ }
+ void clear()
{
return _container.clear();
}
@@ -81,10 +83,8 @@ namespace armarx::armem::base::detail
return _container;
}
-
// Copying
-
- virtual DerivedT copy() const
+ DerivedT copy() const
{
DerivedT d;
this->_copySelf(d);
@@ -92,7 +92,7 @@ namespace armarx::armem::base::detail
}
/// Make a copy not containing any elements.
- virtual DerivedT copyEmpty() const
+ DerivedT copyEmpty() const
{
DerivedT d;
this->_copySelfEmpty(d);
@@ -128,7 +128,7 @@ namespace armarx::armem::base::detail
protected:
- ContainerT _container;
+ mutable ContainerT _container;
};
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
index a99518db24ed00819be69f8085670272281daf58..7c4307da5395a58a6bdfce57c18705cefe535f7f 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
@@ -8,23 +8,27 @@
namespace armarx::armem::ltm
{
- wm::CoreSegment CoreSegment::convert(const MongoDBConnectionManager::MongoDBSettings& dbsettings) const
+ wm::CoreSegment CoreSegment::convert() const
{
+ ARMARX_INFO << "CoreSegment: Converting with connection to: " << dbsettings.toString();
+
wm::CoreSegment m(id());
for (const auto& [_, s] : _container)
{
- m.addProviderSegment(s.convert(dbsettings));
+ m.addProviderSegment(s.convert());
}
return m;
}
- void CoreSegment::reload(const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void CoreSegment::reload()
{
_container.clear();
+ ARMARX_INFO << "CoreSegment: (Re)Establishing connection to: " << dbsettings.toString();
+
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().coreSegmentName];
+ mongocxx::collection coll = db[id().str()];
mongocxx::cursor cursor = coll.find({});
@@ -48,22 +52,23 @@ namespace armarx::armem::ltm
else
{
auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
- wms.first->second.reload(dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
}
}
}
- void CoreSegment::append(const wm::CoreSegment& m, const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void CoreSegment::append(const wm::CoreSegment& m)
{
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().coreSegmentName];
+ mongocxx::collection coll = db[id().str()];
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
{
- it->second.append(s, dbsettings);
+ it->second.append(s);
}
else
{
@@ -74,7 +79,8 @@ namespace armarx::armem::ltm
coll.insert_one(foreign_key.view());
auto wms = _container.emplace(std::make_pair(k, id().withProviderSegmentName(k)));
- wms.first->second.append(s, dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.append(s);
}
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
index 1bbcd26b7a056907498c8b78607f1d77ebc0f5a4..3a7225058a265949de50e98a63d1d3c6ce5bf371 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.h
@@ -25,14 +25,27 @@ namespace armarx::armem::ltm
// Conversion
- wm::CoreSegment convert(const MongoDBConnectionManager::MongoDBSettings&) const;
+ wm::CoreSegment convert() const;
// MongoDB connection
- void reload(const MongoDBConnectionManager::MongoDBSettings&);
- void append(const wm::CoreSegment&, const MongoDBConnectionManager::MongoDBSettings&);
+ void reload();
+ void append(const wm::CoreSegment&);
+
+ protected:
+ virtual void _copySelf(CoreSegment& other) const override
+ {
+ Base::_copySelf(other);
+ other.dbsettings = dbsettings;
+ }
+
+ virtual void _copySelfEmpty(CoreSegment& other) const override
+ {
+ Base::_copySelfEmpty(other);
+ other.dbsettings = dbsettings;
+ }
- private:
- static const constexpr char* MONGO_DB_COLLECTION_PREFIX = "CoreSegment__";
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
index 00dbd2ae85c2d8fac7ab2ff73131c17da41eb7fe..3865e6df0dfb2112d14d5ee23c40e3a740626307 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
@@ -3,33 +3,41 @@
namespace armarx::armem::ltm
{
- wm::Entity Entity::convert(const MongoDBConnectionManager::MongoDBSettings& dbsettings) const
+ wm::Entity Entity::convert() const
{
+ ARMARX_INFO << "Entity: Converting with connection to: " << dbsettings.toString();
+
wm::Entity m(id());
for (const auto& [_, s] : _container)
{
- m.addSnapshot(s.convert(dbsettings));
+ m.addSnapshot(s.convert());
}
return m;
}
- void Entity::reload(const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void Entity::reload()
{
_container.clear();
- ARMARX_INFO << "Reloading entity: " << id().entityName;
+ ARMARX_INFO << "Entity: (Re)Establishing connection to: " << dbsettings.toString();
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().entityName];
+ mongocxx::collection coll = db[id().str()];
mongocxx::cursor cursor = coll.find({});
+ int i = 0;
for (auto doc : cursor)
{
+ if (i > MAX_HISTORY_SIZE)
+ {
+ // TODO: Add logic for most queried data?
+ break;
+ }
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
auto k = armem::Time::microSeconds(json.at("timestamp"));
- ARMARX_INFO << "Entity: Found timestamp: " << std::to_string(k.toMicroSeconds());
+ //ARMARX_INFO << "Entity: Found timestamp: " << std::to_string(k.toMicroSeconds());
if (const auto& it = _container.find(k); it != _container.end())
{
@@ -38,29 +46,106 @@ namespace armarx::armem::ltm
else
{
auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
- wms.first->second.reload(dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
}
+ ++i;
}
}
- void Entity::append(const wm::Entity& m, const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void Entity::append(const wm::Entity& m)
{
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().entityName];
- coll.drop();
+ mongocxx::collection coll = db[id().str()];
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
{
- it->second.setTo(s, k, dbsettings);
+ // segment already exists
+ // We assume that a snapshot does not change, so ignore
}
else
{
auto wms = _container.emplace(std::make_pair(k, id().withTimestamp(k)));
- wms.first->second.setTo(s, k, dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.setTo(s, k);
+ //truncateHistoryToSize();
}
}
}
+
+ bool Entity::hasSnapshot(const Time& time) const
+ {
+ // check cache
+ if (Base::hasSnapshot(time))
+ {
+ return true;
+ }
+ // check mongodb
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
+ bsoncxx::stdx::optional<bsoncxx::document::value> maybe_result = coll.find_one(document{} << "timestamp" << time.toMicroSeconds() << finalize);
+ if (!maybe_result)
+ {
+ return false;
+ }
+
+ auto json = nlohmann::json::parse(bsoncxx::to_json(*maybe_result));
+ auto id = MemoryID::fromString(json["id"].get<std::string>());
+ auto instances = json["instances"];
+ EntitySnapshot snapshot(id);
+ snapshot.dbsettings = dbsettings;
+
+ for (unsigned int i = 0; i < instances.size(); ++i)
+ {
+ EntityInstance instance(id.withInstanceIndex(i));
+ snapshot.addInstance(instance);
+ }
+
+ _container.insert({time, snapshot});
+ //truncateHistoryToSize();
+ return true;
+ }
+
+ std::vector<Time> Entity::getTimestamps() const
+ {
+ // get from cache
+ std::vector<Time> ret; // = Base::getTimestamps();
+
+ // get missing from mongodb
+ mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
+ mongocxx::database db = client[dbsettings.database];
+ mongocxx::collection coll = db[id().str()];
+
+ auto cursor = coll.find({});
+ for (auto doc : cursor)
+ {
+ auto json = nlohmann::json::parse(bsoncxx::to_json(doc));
+ auto ts = json["timestamp"].get<long>();
+ ret.push_back(Time::microSeconds(ts));
+ }
+ return ret;
+ }
+
+ const EntitySnapshot& Entity::getSnapshot(const Time& time) const
+ {
+ if (!hasSnapshot(time))
+ {
+ throw error::MissingEntry::create<EntitySnapshotT>(toDateTimeMilliSeconds(time), *this);
+ }
+
+ // the above command already puts the reference to the cache
+ return Base::getSnapshot(time);
+ }
+
+ const std::map<Time, EntitySnapshot>::value_type& Entity::getLatestItem() const
+ {
+ // Directly query mongodb (cache cant know whether it is the latest or not)
+ // TODO
+ return Base::getLatestItem();
+ }
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
index 54e004f4875242a30b538864695d13fe1f553582..a5bca08c7ee0b7aff2a79d2c45aa37fc4baec1c4 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.h
@@ -41,62 +41,43 @@ namespace armarx::armem::ltm
Entity()
{
// the history of snapshots is just a cache of frequently used elements
- setMaxHistorySize(20);
+ setMaxHistorySize(MAX_HISTORY_SIZE);
}
// Conversion
- wm::Entity convert(const MongoDBConnectionManager::MongoDBSettings&) const;
+ wm::Entity convert() const;
// MongoDB connection
- void reload(const MongoDBConnectionManager::MongoDBSettings&);
- void append(const wm::Entity&, const MongoDBConnectionManager::MongoDBSettings&);
+ void reload();
+ void append(const wm::Entity&);
- // virtual overrides for LTM storage
- /*virtual bool hasSnapshot(Time time) const override
- {
- // check cache
- if (Base::hasSnapshot(time))
- {
- return true;
- }
- // check mongodb
- return false;
- // TODO
- }
- virtual std::vector<Time> getTimestamps() const override
+ // virtual overrides for LTM lookups
+ virtual bool hasSnapshot(const Time& time) const override;
+ virtual std::vector<Time> getTimestamps() const override;
+ virtual const EntitySnapshot& getSnapshot(const Time& time) const override;
+
+ protected:
+ virtual void _copySelf(Entity& other) const override
{
- // get from cache
- std::vector<Time> ret = Base::getTimestamps();
- // get missing from mongodb
- // TODO
- return ret;
+ Base::_copySelf(other);
+ other.dbsettings = dbsettings;
}
- virtual const EntitySnapshotT& getSnapshot(Time time) const override
+
+ virtual void _copySelfEmpty(Entity& other) const override
{
- try
- {
- return Base::getSnapshot(time);
- }
- catch (const error::MissingEntry& _)
- {
- // if not in cache then get from mongo db
- // TODO
- return Base::getSnapshot(time);
- }
+ Base::_copySelfEmpty(other);
+ other.dbsettings = dbsettings;
}
- protected:
+ protected:
// virtual overrides for LTM storage
- virtual const typename std::map<Time, EntitySnapshotT>::value_type& getLatestItem() const override
- {
- // Directly query mongodb (cache cant know whether it is the latest or not)
- // TODO
- return Base::getLatestItem();
- }*/
+ virtual const std::map<Time, EntitySnapshot>::value_type& getLatestItem() const override;
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
private:
- static const constexpr char* MONGO_DB_COLLECTION_PREFIX = "Entity__";
+ const static constexpr int MAX_HISTORY_SIZE = 200;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
index 412dbe1c41603f80960f9087c2a6277274f67c89..47a680731f9794c99433a114dd94a9ad22f5cbea 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
@@ -11,20 +11,22 @@
namespace armarx::armem::ltm
{
- wm::EntitySnapshot EntitySnapshot::convert(const MongoDBConnectionManager::MongoDBSettings& dbsettings, const aron::typenavigator::NavigatorPtr& expectedStructure) const
+ wm::EntitySnapshot EntitySnapshot::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
{
+ ARMARX_INFO << "EntitySnapshot: Converting with connection to: " << dbsettings.toString();
+
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().coreSegmentName];
+ mongocxx::collection coll = db[id().getEntityID().str()];
auto res = coll.find_one(document{} << "id" << id().getEntitySnapshotID().str() << finalize);
if (!res)
{
- throw error::ArMemError("Could not load an instance from the memory. Tried to access: " + id().getEntitySnapshotID().str());
+ throw error::ArMemError("Could not load an instance from the memory '" + id().getEntityID().str() + "'. Tried to access: " + id().getEntitySnapshotID().str());
}
- nlohmann::json json = bsoncxx::to_json(*res);
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
nlohmann::json instances = json["instances"];
if (instances.size() != _container.size())
@@ -46,41 +48,46 @@ namespace armarx::armem::ltm
return m;
}
- void EntitySnapshot::reload(const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void EntitySnapshot::reload()
{
_container.clear();
+ ARMARX_INFO << "EntitySnapshot: (Re)Establishing connection to: " << dbsettings.toString();
+
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().coreSegmentName];
+ mongocxx::collection coll = db[id().getEntityID().str()];
+
+ auto res = coll.find_one(document{} << "id" << id().getEntitySnapshotID().str() << finalize);
- mongocxx::cursor cursor = coll.find({});
- for (auto doc : cursor)
+ if (!res)
{
- nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
- for (unsigned int i = 0; i < json.at("instances").size(); ++i)
- {
- auto wms = _container.emplace_back(id().withInstanceIndex(i));
- }
+ throw error::ArMemError("Could not load an instance from the memory '" + id().getEntityID().str() + "'. Tried to access: " + id().getEntitySnapshotID().str());
+ }
- ARMARX_INFO << "Entity: Found instances: " << json.at("instances").size();
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
+ for (unsigned int i = 0; i < json.at("instances").size(); ++i)
+ {
+ auto wms = _container.emplace_back(id().withInstanceIndex(i));
}
+
+ ARMARX_INFO << "Entity '" + id().str() + "': Found instances in LTM: " << json.at("instances").size();
}
- void EntitySnapshot::setTo(const wm::EntitySnapshot& m, const armem::Time& t, const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void EntitySnapshot::setTo(const wm::EntitySnapshot& m, const armem::Time& t)
{
// We remove the contente here and reset it with new values
_container.clear();
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().entityName];
+ mongocxx::collection coll = db[id().getEntityID().str()];
bsoncxx::builder::stream::document builder{};
auto in_array = builder
<< "id" << id().getEntitySnapshotID().str()
<< "timestamp" << t.toMicroSeconds()
- << "instances" << bsoncxx::builder::stream::open_array;
+ << "instances";
auto array_builder = bsoncxx::builder::basic::array{};
int i = 0;
@@ -97,7 +104,7 @@ namespace armarx::armem::ltm
array_builder.append(doc_value);
}
- auto after_array = in_array << array_builder << bsoncxx::builder::stream::close_array;
+ auto after_array = in_array << array_builder;
bsoncxx::document::value doc = after_array << bsoncxx::builder::stream::finalize;
coll.insert_one(doc.view());
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
index b81f0f8e1140eaaa6d0e916ac38d86655ab86140..02361d2e708365303fbce81f56eaa9b49fb07a61 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.h
@@ -25,13 +25,26 @@ namespace armarx::armem::ltm
// Conversion
- wm::EntitySnapshot convert(const MongoDBConnectionManager::MongoDBSettings&, const aron::typenavigator::NavigatorPtr& = nullptr) const;
+ wm::EntitySnapshot convert(const aron::typenavigator::NavigatorPtr& = nullptr) const;
// MongoDB connection
- void reload(const MongoDBConnectionManager::MongoDBSettings&);
- void setTo(const wm::EntitySnapshot&, const armem::Time& t, const MongoDBConnectionManager::MongoDBSettings&);
+ void reload();
+ void setTo(const wm::EntitySnapshot&, const armem::Time& t);
+
+ protected:
+ virtual void _copySelf(EntitySnapshot& other) const override
+ {
+ Base::_copySelf(other);
+ other.dbsettings = dbsettings;
+ }
+
+ virtual void _copySelfEmpty(EntitySnapshot& other) const override
+ {
+ Base::_copySelfEmpty(other);
+ other.dbsettings = dbsettings;
+ }
- private:
- static const constexpr char* MONGO_DB_COLLECTION_PREFIX = "Entity__";
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
index 4079ebde68babd7d77fc01c1bea827c99e2d11a5..4a96e410dc50a1c7cbe8520bb1d35479336a3ad0 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
@@ -12,10 +12,12 @@ namespace armarx::armem::ltm
wm::Memory Memory::convert() const
{
+ ARMARX_INFO << "Converting with connection to: " << dbsettings.toString();
+
wm::Memory m(id());
for (const auto& [_, s] : _container)
{
- m.addCoreSegment(s.convert(dbsettings));
+ m.addCoreSegment(s.convert());
}
return m;
}
@@ -27,7 +29,7 @@ namespace armarx::armem::ltm
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().memoryName];
+ mongocxx::collection coll = db[id().str()];
mongocxx::cursor cursor = coll.find({});
for (auto doc : cursor)
@@ -50,27 +52,28 @@ namespace armarx::armem::ltm
else
{
auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
- wms.first->second.reload(dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
}
}
}
void Memory::append(const wm::Memory& m)
{
- ARMARX_INFO << "Appending Memory with name: " << m.name();
+ ARMARX_INFO << "Merge memory with name '" << m.name() << "' into the LTM with name '" << name() << "'";
TIMING_START(LTM_Append);
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().memoryName];
+ mongocxx::collection coll = db[id().str()];
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
{
// TODO check if foreign key exists
- it->second.append(s, dbsettings);
+ it->second.append(s);
}
else
{
@@ -81,7 +84,8 @@ namespace armarx::armem::ltm
coll.insert_one(foreign_key.view());
auto wms = _container.emplace(std::make_pair(k, id().withCoreSegmentName(k)));
- wms.first->second.append(s, dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.append(s);
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
index 9caa6b795732dc9b0ab82254027519c5e014ae71..0c14d481093d60af2eab0003bb2bb21ff8c4016b 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
@@ -52,14 +52,24 @@ namespace armarx::armem::ltm
void reload();
void append(const wm::Memory&);
+ protected:
+ virtual void _copySelf(Memory& other) const override
+ {
+ Base::_copySelf(other);
+ other.dbsettings = dbsettings;
+ }
+
+ virtual void _copySelfEmpty(Memory& other) const override
+ {
+ Base::_copySelfEmpty(other);
+ other.dbsettings = dbsettings;
+ }
+
public:
MongoDBConnectionManager::MongoDBSettings dbsettings;
PeriodicTransfer periodic_transfer;
FilledTransferMode filled_transfer;
-
- private:
- static const constexpr char* MONGO_DB_COLLECTION_PREFIX = "Memory__";
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
index d8ca92c97755c4f26f1553d5b98479cb87b19d00..7bf439bfec2282d6919d69af1f94d9d7384cc4f8 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
@@ -8,23 +8,27 @@
namespace armarx::armem::ltm
{
- wm::ProviderSegment ProviderSegment::convert(const MongoDBConnectionManager::MongoDBSettings& dbsettings) const
+ wm::ProviderSegment ProviderSegment::convert() const
{
+ ARMARX_INFO << "ProviderSegment: Converting with connection to: " << dbsettings.toString();
+
wm::ProviderSegment m(id());
for (const auto& [_, s] : _container)
{
- m.addEntity(s.convert(dbsettings));
+ m.addEntity(s.convert());
}
return m;
}
- void ProviderSegment::reload(const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void ProviderSegment::reload()
{
_container.clear();
+ ARMARX_INFO << "ProviderSegment: (Re)Establishing connection to: " << dbsettings.toString();
+
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().providerSegmentName];
+ mongocxx::collection coll = db[id().str()];
mongocxx::cursor cursor = coll.find({});
for (auto doc : cursor)
@@ -47,22 +51,23 @@ namespace armarx::armem::ltm
else
{
auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
- wms.first->second.reload(dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.reload();
}
}
}
- void ProviderSegment::append(const wm::ProviderSegment& m, const MongoDBConnectionManager::MongoDBSettings& dbsettings)
+ void ProviderSegment::append(const wm::ProviderSegment& m)
{
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
- mongocxx::collection coll = db[MONGO_DB_COLLECTION_PREFIX + id().providerSegmentName];
+ mongocxx::collection coll = db[id().str()];
for (const auto& [k, s] : m.container())
{
if (const auto& it = _container.find(k); it != _container.end())
{
- it->second.append(s, dbsettings);
+ it->second.append(s);
}
else
{
@@ -73,7 +78,8 @@ namespace armarx::armem::ltm
coll.insert_one(foreign_key.view());
auto wms = _container.emplace(std::make_pair(k, id().withEntityName(k)));
- wms.first->second.append(s, dbsettings);
+ wms.first->second.dbsettings = dbsettings;
+ wms.first->second.append(s);
}
}
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
index 3c19ec5816ceb2c629a252e45d0b25d14cb8219e..bb49f232e9fe231e1cd500e3f9739ddaa0d7d83f 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.h
@@ -25,14 +25,27 @@ namespace armarx::armem::ltm
// Conversion
- wm::ProviderSegment convert(const MongoDBConnectionManager::MongoDBSettings&) const;
+ wm::ProviderSegment convert() const;
// MongoDB connection
- void reload(const MongoDBConnectionManager::MongoDBSettings&);
- void append(const wm::ProviderSegment&, const MongoDBConnectionManager::MongoDBSettings&);
+ void reload();
+ void append(const wm::ProviderSegment&);
+
+ protected:
+ virtual void _copySelf(ProviderSegment& other) const override
+ {
+ Base::_copySelf(other);
+ other.dbsettings = dbsettings;
+ }
+
+ virtual void _copySelfEmpty(ProviderSegment& other) const override
+ {
+ Base::_copySelfEmpty(other);
+ other.dbsettings = dbsettings;
+ }
- private:
- static const constexpr char* MONGO_DB_COLLECTION_PREFIX = "ProviderSegment__";
+ public:
+ MongoDBConnectionManager::MongoDBSettings dbsettings;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
index b4341c9da6b9bf6c1c9be6e1141ff712d9e00132..285235b76e397952301c23d8218abb11f1f7aeaf 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.cpp
@@ -7,6 +7,17 @@
namespace armarx::armem::wm
{
+
+ Commit CoreSegment::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ c.append(s.toCommit());
+ }
+ return c;
+ }
+
void CoreSegment::_copySelfWithoutData(CoreSegment& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
index 1eaf327338929b1290e476951aa69bdde0d3723d..7847bbb71cc6a1a269556aeec07738c396e42a68 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h
@@ -27,8 +27,11 @@ namespace armarx::armem::wm
CoreSegment& operator=(const CoreSegment& other) = default;
CoreSegment& operator=(CoreSegment&& other) = default;
- virtual ~CoreSegment() override = default;
-
+ /**
+ * @brief Convert the content of this segmnet into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
index 022864c19d9ea98583d9d3998bec46643921cdc8..e704e96d5454d27bfb095120c1901e000611122a 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.cpp
@@ -2,6 +2,20 @@
namespace armarx::armem::wm
{
+
+ Commit Entity::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ EntityUpdate& up = c.add();
+ up.entityID = id();
+ up.timeCreated = k;
+ up.instancesData = s.getAronData();
+ }
+ return c;
+ }
+
void Entity::_copySelfWithoutData(Entity& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
index 6a31286722e0e78d53423f6aa6b517583f83ca09..a6a7621e21272db6447e9e09b695900eefd263c1 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Entity.h
@@ -42,8 +42,11 @@ namespace armarx::armem::wm
Entity& operator=(const Entity& other) = default;
Entity& operator=(Entity&& other) = default;
- virtual ~Entity() override = default;
-
+ /**
+ * @brief Convert the content of this segmnet into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
index fb68d050a0ad19e1e2627e4cb5732a23a2ce4493..76d5400131a7ec6a57478f13afb0dfc38a4da174 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.cpp
@@ -7,6 +7,16 @@
namespace armarx::armem::wm
{
+ std::vector<aron::datanavigator::DictNavigatorPtr> EntitySnapshot::getAronData() const
+ {
+ std::vector<aron::datanavigator::DictNavigatorPtr> ret;
+ for (const auto& aron : _container)
+ {
+ ret.push_back(aron.data());
+ }
+ return ret;
+ }
+
void EntitySnapshot::_copySelfWithoutData(EntitySnapshot& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
index ee0c78c5d201ed2e9fb1fe5c333efc728a9dccfc..3eedb48d0b4aaa5191541d8ca1586ffc59ab5998 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/EntitySnapshot.h
@@ -23,6 +23,7 @@ namespace armarx::armem::wm
using Base::EntitySnapshotBase;
using Base::operator=;
+ std::vector<aron::datanavigator::DictNavigatorPtr> getAronData() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
index b7af1a6456687b1741710d46fa157b9091295db7..3d2efd2249f969308b126608d01533500f3bfa73 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
@@ -3,6 +3,16 @@
namespace armarx::armem::wm
{
+ Commit Memory::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ c.append(s.toCommit());
+ }
+ return c;
+ }
+
void Memory::_copySelfWithoutData(Memory& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
index e8b7cb35349837820c3dd47f2d584ef6b9760461..3c64c0841b46a5c95f7f7365af26a1b20226f4dc 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
@@ -27,6 +27,11 @@ namespace armarx::armem::wm
Memory& operator=(const Memory& other) = default;
Memory& operator=(Memory&& other) = default;
+ /**
+ * @brief Convert the content of this memory into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
index 517b35bfad666cdac02041694e787523c4c1e894..bd03cf21cd8b616a3afcc4d531377288e55c5f2f 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.cpp
@@ -8,6 +8,16 @@
namespace armarx::armem::wm
{
+ Commit ProviderSegment::toCommit() const
+ {
+ Commit c;
+ for (const auto& [k, s] : _container)
+ {
+ c.append(s.toCommit());
+ }
+ return c;
+ }
+
void ProviderSegment::_copySelfWithoutData(ProviderSegment& other) const
{
other.id() = _id;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
index 0b703617d9b1b642153da505882e71aab8aa3723..7d1e9db335ee39433351a8e15113059836bf825d 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h
@@ -28,7 +28,11 @@ namespace armarx::armem::wm
ProviderSegment& operator=(const ProviderSegment& other) = default;
ProviderSegment& operator=(ProviderSegment&& other) = default;
- virtual ~ProviderSegment() override = default;
+ /**
+ * @brief Convert the content of this segmnet into a commit
+ * @return The resulting commit
+ */
+ Commit toCommit() const;
protected:
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp
index 5e01c69eff1b98b7123a0c15ad97912dfa85521a..e7499d8b42edf895bbbdb2f0d06a1fe3ed2ee823 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/json_conversions.cpp
@@ -1,5 +1,7 @@
#include "json_conversions.h"
+#include <RobotAPI/libraries/aron/core/Debug.h>
+
namespace armarx::armem
{
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 75e38944378144ee47fe13cbd733445555fbe48f..db546ddc586a83356d445202fdfaf94f5b1b916b 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -5,9 +5,8 @@
#include "../error.h"
#include "../core/workingmemory/ice_conversions.h"
#include "query_proc/workingmemory/MemoryQueryProcessor.h"
+#include "query_proc/longtermmemory/MemoryQueryProcessor.h"
-//#include "../core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h"
-//#include "../core/io/diskReader/NlohmannJSON/NlohmannJSONDiskReader.h"
namespace armarx::armem::server
{
@@ -151,16 +150,22 @@ namespace armarx::armem::server
EntityUpdateResult& result = commitResult.results.emplace_back();
try
{
- MemoryID snapshotID = workingMemory->update(update);
+ auto updateResult = workingMemory->update(update);
result.success = true;
- result.snapshotID = snapshotID;
+ result.snapshotID = updateResult.id;
result.timeArrived = update.timeArrived;
+ // TODO: Add if and param here (fabian.peller)
+ for (const auto& snapshot : updateResult.removedSnapshots)
+ {
+ ARMARX_IMPORTANT << "The id " << snapshot.id() << " was removed from wm";
+ }
+
if (publishUpdates)
{
data::MemoryID& id = updatedIDs.emplace_back();
- armem::toIce(id, snapshotID);
+ armem::toIce(id, result.snapshotID);
}
}
catch (const error::ArMemError& e)
@@ -195,13 +200,47 @@ namespace armarx::armem::server
{
ARMARX_CHECK_NOT_NULL(workingMemory);
- armem::wm::query_proc::MemoryQueryProcessor processor(
+ armem::wm::query_proc::MemoryQueryProcessor wm_processor(
input.withData ? armem::DataMode::WithData
: armem::DataMode::NoData);
- return processor.process(input, *workingMemory);
+
+ armem::query::data::Result result;
+ wm::Memory wm_result = wm_processor.process(input, *workingMemory, /* ignore if: */ { query::data::QueryTarget::LTM });
+
+ armem::ltm::query_proc::MemoryQueryProcessor ltm_processor;
+ ltm::Memory ltm_result = ltm_processor.process(input, *longtermMemory, /* ignore if: */ { query::data::QueryTarget::WM });
+
+
+ if (ltm_result.hasData())
+ {
+ // ATTENTION: This code block moves data from LTM back into WM.
+ // However, since some segments are constrained, the WM might send data back to LTM.
+ // This may also affect the data returned by the current query.
+ // However, this is expected behavior, since we copy the data in the processor (copyEmpty) we can safely return the copy and
+ // remove the original memory reference from WM here.
+ wm::Memory ltm_converted = ltm_result.convert();
+ wm_result.append(ltm_converted);
+
+ // query again to limit output size (TODO: Skip if querytype is all)
+ wm::Memory merged_result = wm_processor.process(input, wm_result);
+ result.memory = toIce<data::MemoryPtr>(merged_result);
+
+ // also move results of ltm to wm
+ //this->commit(ltm_converted.toCommit());
+
+ // mark removed entries of wm in viewer
+ // TODO
+ }
+ else
+ {
+ result.memory = toIce<data::MemoryPtr>(wm_result);
+ }
+
+ result.success = true;
+ return result;
}
- // LTM LOADING
+ // LTM LOADING FROM LTM
query::data::Result MemoryToIceAdapter::load(const armem::query::data::Input& query)
{
ARMARX_CHECK_NOT_NULL(longtermMemory);
@@ -218,6 +257,13 @@ namespace armarx::armem::server
ARMARX_CHECK_NOT_NULL(longtermMemory);
data::StoreResult output;
+ for (const auto& query : input.query.memoryQueries)
+ {
+ // force query to only query WM
+ // TODO: Think about other query class (fabian.peller)
+ query->target = query::data::QueryTarget::WM;
+ }
+
armem::query::data::Result queryResult = this->query(input.query);
output.success = queryResult.success;
@@ -228,7 +274,6 @@ namespace armarx::armem::server
longtermMemory->append(m);
}
- ARMARX_IMPORTANT << "Finsihed";
return output;
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
index 3ea2b0bdd45230dab810bd628cf338d2b685662b..69a305e45befca5a9189c6fb264985126d16fbf3 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
@@ -2,8 +2,6 @@
#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/DataMode.h>
-
namespace armarx::armem::base::query_proc
{
@@ -20,34 +18,42 @@ namespace armarx::armem::base::query_proc
public:
- DataT process(const QueryT& query, const DataT& data) const
+ DataT process(const QueryT& query, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
DataT result = data.copyEmpty();
+
+ if (std::find(ignoreTargets.begin(), ignoreTargets.end(), query.target) != ignoreTargets.end())
+ {
+ return result;
+ }
this->process(result, query, data);
return result;
}
- DataT process(const QueryPtrT& query, const DataT& data) const
+ DataT process(const QueryPtrT& query, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
- return this->process(*query, *data);
+ return this->process(*query, *data, ignoreTargets);
}
- DataT process(const QuerySeqT& queries, const DataT& data) const
+ DataT process(const QuerySeqT& queries, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
DataT result = data.copyEmpty();
- this->process(result, queries, data);
+ this->process(result, queries, data, ignoreTargets);
return result;
}
- void process(DataT& result, const QuerySeqT& queries, const DataT& data) const
+ void process(DataT& result, const QuerySeqT& queries, const DataT& data, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
{
for (const auto& query : queries)
{
+ if (std::find(ignoreTargets.begin(), ignoreTargets.end(), query->target) != ignoreTargets.end())
+ {
+ continue;
+ }
this->process(result, *query, data);
}
}
virtual void process(DataT& result, const QueryT& query, const DataT& data) const = 0;
};
-
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index b96b22e8921c8a9a51190e7ec8828a9416526c86..40113207f194c76301fbe8768a6d42c22f8b01db 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -108,7 +108,8 @@ namespace armarx::armem::base::query_proc
const Time time = fromIce<Time>(query.timestamp);
try
{
- addResultSnapshot(result, entity.getSnapshot(time));
+ auto snapshot = entity.getSnapshot(time);
+ addResultSnapshot(result, snapshot);
}
catch (const armem::error::MissingEntry&)
{
@@ -187,61 +188,6 @@ namespace armarx::armem::base::query_proc
}
- inline auto lastElementBeforeOrAt(const auto& history, const auto timestamp) const
- {
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
-
- // last element less than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
-
- // last element not greater than => if this is invalid, we have no suitable elements
- typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
- if (refIt == history.rend())
- {
- return history.end();
- }
-
- // now either refItFwd is a perfect match ...
- if (refItFwd->first == timestamp)
- {
- return refItFwd;
- }
-
- // ... or we return the element before if possible
- if (refItFwdLt != history.begin())
- {
- return refItFwdLt;
- }
-
- return history.end();
- }
-
- inline auto lastElementBefore(const auto& history, const auto& timestamp) const
- {
- // first element equal or greater
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwd = history.upper_bound(timestamp);
-
- // last element less than
- typename std::map<Time, EntitySnapshotT>::const_iterator refItFwdLt = std::prev(refItFwd);
-
- // last element not greater than => if this is invalid, we have no suitable elements
- typename std::map<Time, EntitySnapshotT>::const_reverse_iterator refIt(refItFwd);
- if (refIt == history.rend())
- {
- return history.end();
- }
-
- // we return the element before if possible
- if (refItFwdLt != history.begin())
- {
- return refItFwdLt;
- }
-
- return history.end();
- }
-
-
void process(_EntityT& result,
const armem::query::data::entity::BeforeOrAtTime& query,
const _EntityT& entity) const
@@ -249,12 +195,15 @@ namespace armarx::armem::base::query_proc
const auto referenceTimestamp = fromIce<Time>(query.timestamp);
ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
- const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
-
- if (beforeOrAt != entity.history().end())
+ try
{
+ auto beforeOrAt = entity.getLatestSnapshotBeforeOrAt(referenceTimestamp);
addResultSnapshot(result, beforeOrAt);
}
+ catch (const armem::error::MissingEntry&)
+ {
+ // Leave empty.
+ }
}
@@ -267,31 +216,24 @@ namespace armarx::armem::base::query_proc
const auto maxEntries = fromIce<std::int64_t>(query.maxEntries);
- const auto itBefore = lastElementBefore(entity.history(), referenceTimestamp);
- if (itBefore == entity.history().end())
+ try
{
- ARMARX_WARNING << "No suitable element found";
- return;
- }
-
- const auto nEntriesBefore = std::distance(entity.history().begin(), itBefore);
+ const auto before = entity.getSnapshotsBefore(referenceTimestamp);
- const int nEntries = [&]()
- {
- // see query.ice
- if (maxEntries > 0)
+ int i = 0;
+ for (const auto& snapshot : before)
{
- return std::min(nEntriesBefore, maxEntries);
+ if (maxEntries >= 0 && i >= maxEntries)
+ {
+ break;
+ }
+ addResultSnapshot(result, snapshot);
+ ++i;
}
-
- return nEntriesBefore; // all elements before timestamp
}
- ();
-
- auto it = itBefore;
- for (std::int64_t i = 0; i < nEntries; i++, --it)
+ catch (const error::MissingEntry&)
{
- addResultSnapshot(result, it);
+ // Leave empty.
}
}
@@ -299,7 +241,6 @@ namespace armarx::armem::base::query_proc
const armem::query::data::entity::TimeApprox& query,
const _EntityT& entity) const
{
-
const auto referenceTimestamp = fromIce<Time>(query.timestamp);
ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
@@ -317,67 +258,37 @@ namespace armarx::armem::base::query_proc
return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
};
- // first element before or at timestamp
- const auto beforeOrAt = lastElementBeforeOrAt(entity.history(), referenceTimestamp);
- const bool isBeforeOrAtValid = beforeOrAt != entity.history().end();
- const auto isPerfectMatch = isBeforeOrAtValid && beforeOrAt->first == referenceTimestamp;
-
- // first element greater
- const auto after = entity.history().upper_bound(referenceTimestamp);
- const bool isAfterValid = after != entity.history().end();
-
- // if both are invalid, there is nothing to be gained here.
- if ((not isBeforeOrAtValid) and (not isAfterValid))
- {
- const armem::Time dt = referenceTimestamp - entity.getLatestTimestamp();
-
- ARMARX_WARNING << "Lookup " << dt.toMilliSeconds() << " ms into the future.";
- return;
- }
- // -> now one or both are valid ...
-
- // is 'before' a perfect match?
- if (isPerfectMatch)
- {
- addResultSnapshot(result, beforeOrAt);
- return;
- }
-
- // only 'before' valid?
- if (not isAfterValid)
+ try
{
- if (isInRange(beforeOrAt->first))
+ // last element before or at timestamp
+ auto beforeOrAt = entity.getLatestSnapshotBeforeOrAt(referenceTimestamp);
+ const auto timestampOfMatchBefore = beforeOrAt.id().timestamp;
+ const auto isPerfectMatch = timestampOfMatchBefore == referenceTimestamp;
+ if (isInRange(timestampOfMatchBefore))
{
addResultSnapshot(result, beforeOrAt);
}
- return;
- }
- // only 'after' valid?
- if (not isBeforeOrAtValid)
- {
- if (isInRange(after->first))
+ // earsly stop, not necessary to also get the next since the match is perfect
+ if (isPerfectMatch)
+ {
+ return;
+ }
+
+ // first element after or at timestamp (or at because of fewer checks, we can assure that there is not element at)
+ const auto after = entity.getFirstSnapshotAfterOrAt(referenceTimestamp);
+ const auto timestampOfMatchAfter = after.id().timestamp;
+ if (isInRange(timestampOfMatchAfter))
{
addResultSnapshot(result, after);
}
- return;
}
- // -> now both are valid
-
- // return both (if in range) => user can interpolate
- if (isInRange(beforeOrAt->first))
+ catch (const armem::error::MissingEntry&)
{
- addResultSnapshot(result, beforeOrAt);
- }
- if (isInRange(after->first))
- {
- addResultSnapshot(result, after);
}
-
}
-
static size_t negativeIndexSemantics(long index, size_t size)
{
const size_t max = size > 0 ? size - 1 : 0;
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
index c50086b553164b70f10ed1559a18420649eb3892..5de194d7ba2debada4ef9363005b1a7f2e927e94 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
@@ -32,6 +32,13 @@ namespace armarx::armem::base::query_proc
using EntityT = typename ProviderSegmentT::EntityT;
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
+
+ using Base::process;
+ _MemoryT process(const armem::query::data::Input& input, const _MemoryT& memory, const std::vector<query::data::QueryTarget>& ignoreTargets = {}) const
+ {
+ return this->process(input.memoryQueries, memory, ignoreTargets);
+ }
+
void process(_MemoryT& result,
const armem::query::data::MemoryQuery& query,
const _MemoryT& memory) const override
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h
index 5ec4c5dde481e16f0af30589d57ff2a447d81221..952d759c8552c7939f6e08266e7feb880a85438e 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/EntityQueryProcessor.h
@@ -31,6 +31,7 @@ namespace armarx::armem::ltm::query_proc
void addResultSnapshot(ltm::Entity& result, const ltm::EntitySnapshot& snapshot) const override
{
+ ARMARX_INFO << "addResultSnapshot" << __LINE__;
result.addSnapshot(snapshot.copy());
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
index b017c3b561569c1da5d3d7f661ee604bbb1cae12..b0616c658a7915568fd910a8f35c9d4701aa1d0b 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.h
@@ -21,28 +21,6 @@ namespace armarx::armem::wm::query_proc
Base(dataMode), coreSegmentProcessor(dataMode)
{}
- armem::query::data::Result process(const armem::query::data::Input& input,
- const wm::Memory& memory) const
- {
- armem::query::data::Result result;
- result.memory = processToIce(input.memoryQueries, memory);
- result.success = true;
- return result;
- }
-
- using Base::process;
- data::MemoryPtr processToIce(const armem::query::data::MemoryQuery& query,
- const wm::Memory& memory) const
- {
- return toIce<data::MemoryPtr>(process(query, memory));
- }
-
- data::MemoryPtr processToIce(const armem::query::data::MemoryQuerySeq& queries,
- const wm::Memory& memory) const
- {
- return toIce<data::MemoryPtr>(process(queries, memory));
- }
-
protected:
virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
{
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index 789600c2fe82857b865c845d6892fc2da44d3d6a..b2d07571d619531f45381d1238ca8201b8e0175e 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -134,7 +134,7 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_latest)
BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
{
- addResults(query::entity::Single { 3000 });
+ addResults(query::entity::Single { query::QueryTarget::WMAndLTM, 3000 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -148,7 +148,7 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
BOOST_AUTO_TEST_CASE(test_entity_Single_non_existing)
{
- addResults(query::entity::Single { 3500 });
+ addResults(query::entity::Single { query::QueryTarget::WMAndLTM, 3500 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -182,7 +182,7 @@ BOOST_AUTO_TEST_CASE(test_entity_All)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
{
- addResults(query::entity::TimeRange{ 1500, 3500 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 1500, 3500 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -203,7 +203,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
{
- addResults(query::entity::TimeRange{ 2000, 4000 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 2000, 4000 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -223,8 +223,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
{
- addResults(query::entity::TimeRange{ 0, 10000 });
- addResults(query::entity::TimeRange{ -1, -1 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 0, 10000 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, -1, -1 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -242,8 +242,8 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
{
- addResults(query::entity::TimeRange{ 2400, 2600 });
- addResults(query::entity::TimeRange{ 6000, 1000 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 2400, 2600 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 6000, 1000 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -257,7 +257,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
{
- addResults(query::entity::TimeRange{ -1, 2500 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, -1, 2500 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -278,7 +278,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
{
- addResults(query::entity::TimeRange{ 2500, -1 });
+ addResults(query::entity::TimeRange{ query::QueryTarget::WMAndLTM, 2500, -1 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -302,7 +302,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeTime{ 3500, 1 });
+ addResults(query::entity::BeforeTime{ query::QueryTarget::WMAndLTM, 3500, 1 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -318,7 +318,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeTime{ 3500, 2});
+ addResults(query::entity::BeforeTime{ query::QueryTarget::WMAndLTM, 3500, 2});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -343,7 +343,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeOrAtTime{ 3500 });
+ addResults(query::entity::BeforeOrAtTime{ query::QueryTarget::WMAndLTM, 3500 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -358,7 +358,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeOrAtTime{ 3000 });
+ addResults(query::entity::BeforeOrAtTime{ query::QueryTarget::WMAndLTM, 3000 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -373,7 +373,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::BeforeOrAtTime{ 1 });
+ addResults(query::entity::BeforeOrAtTime{ query::QueryTarget::WMAndLTM, 1 });
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -393,7 +393,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_lookup_past)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3500, -1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3500, -1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -419,7 +419,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3500, 600});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3500, 600});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -445,7 +445,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3500, 100});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3500, 100});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -464,7 +464,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3700, 400});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3700, 400});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -489,7 +489,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3300, 400});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3300, 400});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -514,7 +514,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 3000, -1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 3000, -1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -539,7 +539,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 1, 1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 1, 1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -557,7 +557,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_past)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 10'000, 1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 10'000, 1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -575,7 +575,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::TimeApprox{ 10'000, -1});
+ addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, 10'000, -1});
BOOST_REQUIRE_EQUAL(results.size(), 2);
for (const auto& result : results)
@@ -594,7 +594,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_invalid_timestamp)
{
- BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox{ -1, 1}), ::armarx::LocalException);
+ BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox{ query::QueryTarget::WMAndLTM, -1, 1}), ::armarx::LocalException);
}
@@ -634,7 +634,7 @@ BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
{
addResults(query::entity::IndexRange());
- addResults(query::entity::IndexRange(0, -1));
+ addResults(query::entity::IndexRange(query::QueryTarget::WMAndLTM, 0, -1));
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -654,10 +654,10 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::IndexRange{ 1, 3 }); // => [1, 2, 3]
- addResults(query::entity::IndexRange{ 1, -2 }); // 5 - 2 = 3
- addResults(query::entity::IndexRange{ -4, 3 }); // 5 - 4 = 1
- addResults(query::entity::IndexRange{ -4, -2 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, 3 }); // => [1, 2, 3]
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, -2 }); // 5 - 2 = 3
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, -4, 3 }); // 5 - 4 = 1
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, -4, -2 });
BOOST_REQUIRE_GT(results.size(), 0);
for (const armem::wm::Entity& result : results)
@@ -679,12 +679,12 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_range)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::IndexRange{ 1, 0 });
- addResults(query::entity::IndexRange{ 2, 1 });
- addResults(query::entity::IndexRange{ 5, 3 });
- addResults(query::entity::IndexRange{ 4, -3 }); // 5-3 = 2
- addResults(query::entity::IndexRange{ 3, -3 });
- addResults(query::entity::IndexRange{ 1, -5 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, 0 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 2, 1 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 5, 3 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 4, -3 }); // 5-3 = 2
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 3, -3 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 1, -5 });
BOOST_REQUIRE_GT(results.size(), 0);
@@ -701,12 +701,12 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_entity)
{
entity.clear();
BOOST_REQUIRE_EQUAL(entity.size(), 0);
- addResults(query::entity::IndexRange{ 0, 0 });
- addResults(query::entity::IndexRange{ 0, 10 });
- addResults(query::entity::IndexRange{-10, -1 });
- addResults(query::entity::IndexRange{ 2, 5 });
- addResults(query::entity::IndexRange{ 3, -3 });
- addResults(query::entity::IndexRange{ -1, 10 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 0, 0 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 0, 10 });
+ addResults(query::entity::IndexRange{query::QueryTarget::WMAndLTM, -10, -1 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 2, 5 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, 3, -3 });
+ addResults(query::entity::IndexRange{ query::QueryTarget::WMAndLTM, -1, 10 });
BOOST_REQUIRE_GT(results.size(), 0);
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index cc65c5a304154d6aa4c7b4ac86a29ca1634be0da..2b4ab121dabc7d52188f1d10b8adb501f2c72d32 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -191,7 +191,7 @@ namespace armarx::armem::gui::instance
{
DataTreeBuilder builder;
builder.setColumns(int(Columns::KEY), int(Columns::VALUE), int(Columns::TYPE));
- builder.updateTree(treeItemData, *data);
+ builder.updateTree(treeItemData, data);
}
treeItemData->setExpanded(true);
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
index 07ccac8866d870b153fba28915006d3dfd46cdae..a2811c0bc5a01ef29fbb9cc41d7b5f47118f5a9a 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
@@ -12,27 +12,27 @@ namespace armarx::armem::gui::instance
{
}
- void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, aron::datanavigator::DictNavigator& data)
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::datanavigator::DictNavigatorPtr& data)
{
DictBuilder builder = getDictBuilder();
builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
{
- auto child = data.getElement(key);
- this->update(item, key, *child);
+ auto child = data->getElement(key);
+ this->update(item, key, child);
return true;
});
- builder.updateTree(parent, data.getAllKeys());
+ builder.updateTree(parent, data->getAllKeys());
}
- void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, aron::datanavigator::ListNavigator& data)
+ void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::datanavigator::ListNavigatorPtr& data)
{
- auto children = data.getChildren();
+ 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));
+ this->update(item, std::to_string(key), children.at(key));
return true;
});
@@ -40,17 +40,23 @@ namespace armarx::armem::gui::instance
}
- void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data)
+ void DataTreeBuilder::update(QTreeWidgetItem* item, const std::string& key, const aron::datanavigator::NavigatorPtr& data)
{
- this->setRowTexts(item, key, data);
+ if (!data)
+ {
+ this->setRowTexts(item, key, "null");
+ return;
+ }
+
+ this->setRowTexts(item, key, *data);
- if (auto cast = dynamic_cast<aron::datanavigator::DictNavigator*>(&data))
+ if (auto cast = aron::datanavigator::DictNavigator::DynamicCast(data))
{
- updateTree(item, *cast);
+ updateTree(item, cast);
}
- else if (auto cast = dynamic_cast<aron::datanavigator::ListNavigator*>(&data))
+ else if (auto cast = aron::datanavigator::ListNavigator::DynamicCast(data))
{
- updateTree(item, *cast);
+ 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
index a9aa8471e5d4720c8e747844deb45459d68b1b90..cd408a34c3d3ebe2d36e5efe4cc7435ebefb0d94 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
@@ -17,13 +17,13 @@ namespace armarx::armem::gui::instance
DataTreeBuilder();
- void updateTree(QTreeWidgetItem* parent, aron::datanavigator::DictNavigator& data);
- void updateTree(QTreeWidgetItem* parent, aron::datanavigator::ListNavigator& data);
+ void updateTree(QTreeWidgetItem* parent, const aron::datanavigator::DictNavigatorPtr& data);
+ void updateTree(QTreeWidgetItem* parent, const aron::datanavigator::ListNavigatorPtr& data);
protected:
- void update(QTreeWidgetItem* item, const std::string& key, aron::datanavigator::Navigator& data);
+ void update(QTreeWidgetItem* item, const std::string& key, const aron::datanavigator::NavigatorPtr& data);
};
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
index 6ac42aee3f0870279f8f8fe823fdb9ca5340c4c7..ae9af3c213fbbea4d4a44852e8e02d0af15b8de9 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
@@ -45,6 +45,7 @@ namespace armarx::armem::gui
{
armem::client::query::Builder qb(dataMode());
qb
+ .queryTarget(_snapshotSelectorWidget->queryTarget())
.coreSegments().all()
.providerSegments().all()
.entities().all()
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
index 762421d132997ffaec145d170cc71b708e2cb414..87d94376e673492d07a7fe2f0206359259ea327a 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
@@ -15,6 +15,11 @@ namespace armarx::armem::gui
return _selector;
}
+ query::data::QueryTarget SnapshotSelectorWidget::queryTarget() const
+ {
+ return _queryTarget;
+ }
+
SnapshotSelectorWidget::SnapshotSelectorWidget()
{
@@ -22,19 +27,30 @@ namespace armarx::armem::gui
setLayout(_pageLayout);
connect(this, &This::queryOutdated, this, &This::updateSelector);
+ connect(this, &This::queryTargetOutdated, this, &This::updateQueryTarget);
{
QHBoxLayout* typeLayout = new QHBoxLayout();
+ // snapshot select box
_queryComboBox = new QComboBox();
_queryComboBox->addItems({"All", "Single", "Time Range", "Index Range"});
_queryComboBox->setCurrentIndex(3);
typeLayout->addWidget(_queryComboBox);
- connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::showSelectedForm);
+ connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::showSelectedFormForQuery);
connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::queryOutdated);
+ // query type select box
+ _queryTargetComboBox = new QComboBox();
+ _queryTargetComboBox->addItems({"WM", "WM & LTM", "LTM (debug only)"});
+ _queryTargetComboBox->setCurrentIndex(0);
+
+ typeLayout->addWidget(_queryTargetComboBox);
+
+ connect(_queryTargetComboBox, &QComboBox::currentTextChanged, this, &This::queryTargetOutdated);
+
_pageLayout->addLayout(typeLayout);
}
@@ -45,8 +61,14 @@ namespace armarx::armem::gui
addForm("Time Range", new SnapshotFormTimeRange());
addForm("Index Range", new SnapshotFormIndexRange());
- showSelectedForm(_queryComboBox->currentText());
+ // Add query targets
+ _queryTargets.insert({"WM", query::data::QueryTarget::WM});
+ _queryTargets.insert({"WM & LTM", query::data::QueryTarget::WMAndLTM});
+ _queryTargets.insert({"LTM (debug only)", query::data::QueryTarget::LTM});
+
+ showSelectedFormForQuery(_queryComboBox->currentText());
updateSelector();
+ updateQueryTarget();
}
void SnapshotSelectorWidget::updateSelector()
@@ -55,7 +77,13 @@ namespace armarx::armem::gui
emit queryChanged();
}
- void SnapshotSelectorWidget::showSelectedForm(QString selected)
+ void SnapshotSelectorWidget::updateQueryTarget()
+ {
+ this->_queryTarget = _queryTargets.at(_queryTargetComboBox->currentText());
+ emit queryTargetChanged();
+ }
+
+ void SnapshotSelectorWidget::showSelectedFormForQuery(QString selected)
{
for (auto& [name, form] : _queryForms)
{
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
index 2241b67e810292b685fe0837f43ea621cfb2fb93..92db5fbb13c74b011ca0f36a64fbf82a58536717 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
@@ -36,6 +36,7 @@ namespace armarx::armem::gui
armem::DataMode dataMode() const;
client::query::SnapshotSelector& selector();
+ query::data::QueryTarget queryTarget() const;
public slots:
@@ -43,19 +44,22 @@ namespace armarx::armem::gui
signals:
void queryChanged();
+ void queryTargetChanged();
private slots:
void updateSelector();
+ void updateQueryTarget();
void hideAllForms();
- void showSelectedForm(QString selected);
+ void showSelectedFormForQuery(QString selected);
signals:
void queryOutdated();
+ void queryTargetOutdated();
private:
@@ -66,11 +70,14 @@ namespace armarx::armem::gui
public:
client::query::SnapshotSelector _selector;
+ query::data::QueryTarget _queryTarget;
QVBoxLayout* _pageLayout;
QComboBox* _queryComboBox;
+ QComboBox* _queryTargetComboBox;
/// The forms for the different query types. Hidden when not selected.
std::map<QString, SnapshotForm*> _queryForms;
+ std::map<QString, query::data::QueryTarget> _queryTargets;
};
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
index 8191457b3895e5dfaaae4b2e4603fae3b518261e..e2769dd1c0f19152ec635c08ee7d580f223fab9f 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
@@ -78,6 +78,10 @@ namespace armarx::aron::dataIO
virtual std::string readKey() = 0;
virtual void loadMember(const std::string&) = 0;
+ // This method is for converters or visitors. The code generation knows what to call (e.g. readStartNDArray) since it knows the type.
+ // But if the type is unknown (and null), we need a method to increase the cnt of a token to the next element
+ virtual void readNull() = 0;
+
// Helper functions
virtual data::Descriptor getTypeOfNext() const = 0;
};
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
index 1e124a8080ec1ec5aa53b8563068b1c0b176e2d1..b8dbb60419279e56ca1a8c9cdb2d6db286a64550 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
@@ -154,9 +154,15 @@ namespace armarx::aron::dataIO
break;
}
- default:
+ case data::Descriptor::eUnknown:
{
- throw error::DescriptorNotValidException("Converter", "ReadAndConvert", "Data-Type could not be resolved", desc);
+ // it seems like we found an optional value
+ if (t_desc != type::Descriptor::eUnknown && expectedStructure->getMaybe() == type::Maybe::eNone)
+ {
+ throw error::DescriptorNotValidException("Converter", "ReadAndConvert", "The Descriptor is unknown but the expected type assumes a non-maybe type!", desc);
+ }
+ reader.readNull();
+ writer.writeNull();
}
}
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
index fc3868d16583dd7de9bd2712029b56ce67a7d884..d9f2a7bde5ec5bb45278b36e6fb9f73165de6732 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
@@ -24,6 +24,9 @@
#include <memory>
#include <string>
+// Simox
+#include <SimoxUtility/algorithm/string.h>
+
// ArmarX
#include <RobotAPI/libraries/aron/core/Config.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
@@ -126,6 +129,12 @@ namespace armarx::aron::dataIO
return currentIndex;
}
+ std::string allMemberNamesToString() const
+ {
+ return simox::alg::to_string(allMemberNames, ", ");
+ }
+
+
protected:
// members
data::Descriptor descriptor = data::Descriptor::eUnknown;
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
index 58a778a865bd1beda0db793228b4acf24a3b257f..c431b5b49ea2547f262da4fd657be6aa6fed1fac 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
@@ -252,6 +252,17 @@ namespace armarx::aron::dataIO::reader
token->setCurrentKey(k);
}
+ void NavigatorReader::readNull()
+ {
+ auto token = stack.top();
+ auto current_nav = token->getNextElement();
+ if (!current_nav)
+ {
+ throw error::AronException("NavigatorReader", "readNull", "The value of a navigator is not null!");
+ }
+ token->increaseCounter();
+ }
+
// Helper functions
data::Descriptor NavigatorReader::getTypeOfNext() const
{
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
index 0b1c5739ecb1a0112e29ffc7cb2822f892b303bd..26f7628de7747d030f9f7f38a4ca9d2c3fa5ac39 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
@@ -68,6 +68,8 @@ namespace armarx::aron::dataIO::reader
virtual std::string readKey() override;
virtual void loadMember(const std::string&) override;
+ virtual void readNull() override;
+
virtual data::Descriptor getTypeOfNext() const override;
private:
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
index 67560f75256df7661eff8fa9b6152dcb1a8e7b54..5d614b7025a52239be1ecc9933202c5a5aa3d26f 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -57,22 +57,20 @@ namespace armarx::aron::dataIO::reader
ReaderInterface::ReadStartDictReturnType NlohmannJSONReader::readStartDict()
{
+ auto lastToken = stack.top();
if (!readFirstStartDict)
{
// we already added in constructor
readFirstStartDict = true;
- auto lastToken = stack.top();
auto current_json = lastToken->getElement();
- int c = current_json.size();
+ int c = lastToken->getElementChildrenSize();
return {c, true};
}
- auto lastToken = stack.top();
-
auto current_json = lastToken->getNextElement();
if (current_json.is_null())
{
- lastToken->increaseCounter();
+ lastToken->increaseCounter(); // no need to call readEndDict anymore
return {0, false};
}
@@ -86,8 +84,8 @@ namespace armarx::aron::dataIO::reader
throw error::StringNotValidException("NlohmannJSONReader", "readStartList", "The json is not of dict type!", current_json.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_DICT_TYPENAME_SLUG);
}
- int c = current_json.size();
auto newToken = std::make_shared<NlohmannJSONReaderToken>(current_json);
+ int c = newToken->getElementChildrenSize();
stack.push(newToken);
return {c, true};
}
@@ -119,7 +117,7 @@ namespace armarx::aron::dataIO::reader
auto current_json = lastToken->getNextElement();
if (current_json.is_null())
{
- lastToken->increaseCounter();
+ lastToken->increaseCounter(); // no need to call readEndList anymore
return {0, false};
}
@@ -133,8 +131,8 @@ namespace armarx::aron::dataIO::reader
throw error::StringNotValidException("NlohmannJSONReader", "readStartList", "The json is not of list type!", current_json.at(io::Data::READER_WRITER_TYPE_SLUG), io::Data::READER_WRITER_LIST_TYPENAME_SLUG);
}
- int c = current_json.size();
auto newToken = std::make_shared<NlohmannJSONReaderToken>(current_json);
+ int c = newToken->getElementChildrenSize();
stack.push(newToken);
return {c, true};
}
@@ -191,6 +189,7 @@ namespace armarx::aron::dataIO::reader
std::vector<int> dims = current_json.at(io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG);
std::vector<unsigned char> d = current_json.at(io::Data::READER_WRITER_NDARRAY_DATA_SLUG);
memcpy(data, d.data(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ lastToken->increaseCounter();
}
// Read primitives
@@ -304,6 +303,17 @@ namespace armarx::aron::dataIO::reader
token->setCurrentKey(k);
}
+ void NlohmannJSONReader::readNull()
+ {
+ auto token = stack.top();
+ auto current_json = token->getNextElement();
+ if (!current_json.is_null())
+ {
+ throw error::AronException("NlohmannJSONReader", "readNull", "The value of a json is not null!");
+ }
+ token->increaseCounter();
+ }
+
// Helper functions
data::Descriptor NlohmannJSONReader::getTypeOfNext() const
{
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
index 84ac1f42cb1d6743a1ec1e5f2099dccbb157ca2c..c2cf0b3ef9ea8fdb2fc6efeecedfd27bd5ea852b 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
@@ -66,6 +66,8 @@ namespace armarx::aron::dataIO::reader
virtual std::string readKey() override;
virtual void loadMember(const std::string&) override;
+ virtual void readNull() override;
+
virtual data::Descriptor getTypeOfNext() const override;
private:
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
index 58f38d9d14677173ec054076dcf790efc3228cef..0e273108d6944bbabad1e3a34d57e22177e5a138 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
@@ -93,7 +93,7 @@ namespace armarx::aron::dataIO::reader
return data::Descriptor::eUnknown;
}
- std::string type_as_str = next.at(io::Data::READER_WRITER_NDARRAY_DATA_SLUG);
+ std::string type_as_str = next.at(io::Data::READER_WRITER_TYPE_SLUG);
data::Descriptor desc = data::STRING_TO_DESCRIPTOR(type_as_str);
return desc;
}
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
index 269c14e4cf1663cdee884e77c06e6ffa8b88ac76..b12b59774aa483a71dfbff040ad493abda841e79 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/writer/nlohmannJSON/NlohmannJSONWriter.cpp
@@ -53,7 +53,7 @@ namespace armarx::aron::dataIO::writer
{
nlohmann::json data;
data[io::Data::READER_WRITER_TYPE_SLUG] = io::Data::READER_WRITER_LIST_TYPENAME_SLUG;
- data[io::Data::READER_WRITER_VALUE_SLUG] = {};
+ data[io::Data::READER_WRITER_VALUE_SLUG] = nlohmann::json::array();
auto new_token = std::make_shared<NlohmannJSONWriterToken>(data);
stack.push(new_token);
}
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
index a4f0f8be1532830e63bb49fe585160dbed493776..ef42b1cca0b28496e2b7e7d06f80822608b87e52 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.cpp
@@ -118,6 +118,11 @@ namespace armarx::aron::datanavigator
return ret;
}
+ std::string DictNavigator::getAllKeysAsString() const
+ {
+ return simox::alg::to_string(getAllKeys(), ", ");
+ }
+
void DictNavigator::addElement(const std::string& key, const NavigatorPtr& data)
{
if (data)
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h
index 88e434f3edd552261ef43559151785d41d194778..2ea343434d5d5c7b4ef77fce7b33525ceb2cd397 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/container/Dict.h
@@ -58,6 +58,7 @@ namespace armarx::aron::datanavigator
// public member functions
data::AronDictPtr toAronDictPtr() const;
std::vector<std::string> getAllKeys() const;
+ std::string getAllKeysAsString() const;
void addElement(const std::string& key, const NavigatorPtr&);
bool hasElement(const std::string&) const;
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h b/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h
index d18112d5fb4d9e77465d714484f5f0759cf0ff91..4e91d3235c4f4234d7a9e3db943571a3dfb438fb 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/detail/PrimitiveNavigatorBase.h
@@ -62,6 +62,12 @@ namespace armarx::aron::datanavigator::detail
return *this;
}
+ // Already implemented through thge constructor of a primitive navigator
+ /*bool operator==(const ValueT& x) const
+ {
+ return this->aron->value == x;
+ }*/
+
// virtual implementations
virtual std::vector<NavigatorPtr> getChildren() const override
{
diff --git a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
index 59892d744e814a6c0ece1b32cf208b6ba9e064a5..239c92ecff42e75ea7be37b5e43c8d442983f678 100644
--- a/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/test/CMakeLists.txt
@@ -1,9 +1,21 @@
-######################
-# ARON OPERATOR TEST #
-######################
find_package(Simox QUIET)
armarx_build_if(Simox_FOUND "Simox not available")
+find_package(Eigen3 QUIET)
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+
+find_package(IVT COMPONENTS ivt ivtopencv QUIET)
+armarx_build_if(IVT_FOUND "IVT not available")
+
+find_package(OpenCV QUIET)
+armarx_build_if(OpenCV_FOUND "OpenCV not available")
+
+find_package(PCL QUIET)
+armarx_build_if(PCL_FOUND "PCL not available")
+
+######################
+# ARON OPERATOR TEST #
+######################
armarx_add_test(
TEST_NAME
aronOperatorTest
@@ -18,11 +30,47 @@ armarx_add_test(
)
######################
-# ARON NAVIGATE TEST #
+# ARON CODE GEN TEST #
######################
-find_package(Simox QUIET)
-armarx_build_if(Simox_FOUND "Simox not available")
+armarx_add_test(
+ TEST_NAME
+ aronCodeGenerationTest
+ TEST_FILE
+ aronCodeGenerationTest.cpp
+ LIBS
+ Simox::SimoxUtility
+ ArmarXCore
+ RobotAPI::aron
+ ARON_FILES
+ # xmls/BaseClass.xml
+ # xmls/DerivedClassTest.xml
+ xmls/DictTest.xml
+ xmls/EigenMatrixTest.xml
+ xmls/EigenQuaternionTest.xml
+ xmls/EnumTest.xml
+ xmls/HumanPoseTest.xml
+ xmls/IVTCByteImageTest.xml
+ xmls/ListTest.xml
+ xmls/NaturalIKTest.xml
+ xmls/ObjectTest.xml
+ xmls/OpenCVMatTest.xml
+ xmls/OrientationTest.xml
+ xmls/PCLPointCloudTest.xml
+ xmls/PoseTest.xml
+ xmls/PositionTest.xml
+ xmls/PrimitiveTest.xml
+ xmls/OptionalTest.xml
+ INCLUDE_DIRECTORIES
+ ${Simox_INCLUDE_DIR}
+ ${Eigen3_INCLUDE_DIR}
+ ${IVT_INCLUDE_DIRS}
+ ${OpenCV_INCLUDE_DIRS}
+ ${PCL_INCLUDE_DIRS}
+)
+######################
+# ARON NAVIGATE TEST #
+######################
armarx_add_test(
TEST_NAME
aronNavigateTest
@@ -42,18 +90,6 @@ armarx_add_test(
########################
# ARON RANDOMIZED TEST #
########################
-find_package(Eigen3 QUIET)
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-
-find_package(IVT COMPONENTS ivt ivtopencv QUIET)
-armarx_build_if(IVT_FOUND "IVT not available")
-
-find_package(OpenCV QUIET)
-armarx_build_if(OpenCV_FOUND "OpenCV not available")
-
-find_package(PCL QUIET)
-armarx_build_if(PCL_FOUND "PCL not available")
-
armarx_add_test(
TEST_NAME
aronRandomizedTest
diff --git a/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c1db4b859aa464794c3bba2e1025feb5436a5bba
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/core/test/aronCodeGenerationTest.cpp
@@ -0,0 +1,212 @@
+/*
+ * 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::aron
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::aron
+
+#define ARMARX_BOOST_TEST
+
+// STD/STL
+#include <iostream>
+#include <cstdlib>
+#include <ctime>
+#include <numeric>
+
+// Boost
+#include <boost/algorithm/string.hpp>
+
+// Test
+#include <RobotAPI/Test.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+// Aron
+#include <RobotAPI/libraries/aron/core/Debug.h>
+#include <RobotAPI/libraries/aron/core/Randomizer.h>
+
+// Generated File
+#include <RobotAPI/libraries/aron/core/test/aron/ListTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/DictTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/IVTCByteImageTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EigenMatrixTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EigenQuaternionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OpenCVMatTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PCLPointCloudTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OrientationTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PoseTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EnumTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OptionalTest.aron.generated.h>
+
+using namespace armarx;
+using namespace aron;
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationListTest)
+{
+ std::cout << "Running Code Gen List test" << std::endl;
+ ListTest p_tmp;
+ ListTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(typeid(p.intList) == typeid(std::vector<int>), true);
+ BOOST_CHECK_EQUAL(typeid(p.longList) == typeid(std::vector<long>), true);
+ BOOST_CHECK_EQUAL(typeid(p.floatList) == typeid(std::vector<float>), true);
+ BOOST_CHECK_EQUAL(typeid(p.doubleList) == typeid(std::vector<double>), true);
+ BOOST_CHECK_EQUAL(typeid(p.stringList) == typeid(std::vector<std::string>), true);
+ BOOST_CHECK_EQUAL(typeid(p.boolList) == typeid(std::vector<bool>), true);
+ //BOOST_CHECK_EQUAL(typeid(p.objectList) == typeid(std::vector<ListTest::ListClass>), true);
+
+ BOOST_CHECK_EQUAL(p.intList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.longList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.floatList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.doubleList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.stringList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.boolList.size() == 0, true);
+ BOOST_CHECK_EQUAL(p.objectList.size() == 0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationDictTest)
+{
+ std::cout << "Running Code Gen Dict test" << std::endl;
+ DictTest p_tmp;
+ DictTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.dict.size() == 0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenMatrixTest)
+{
+ std::cout << "Running Code Gen EigenMatrix test" << std::endl;
+ EigenMatrixTest p_tmp;
+ EigenMatrixTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.the_short_eigen_matrix.rows() == 5, true);
+ BOOST_CHECK_EQUAL(p.the_short_eigen_matrix.cols() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_int_eigen_matrix.rows() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_int_eigen_matrix.cols() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_long_eigen_matrix.rows() == 7, true);
+ BOOST_CHECK_EQUAL(p.the_long_eigen_matrix.cols() == 5, true);
+ BOOST_CHECK_EQUAL(p.the_float_eigen_matrix.rows() == 1, true);
+ BOOST_CHECK_EQUAL(p.the_float_eigen_matrix.cols() == 9, true);
+ BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.rows() == 25, true);
+ BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.cols() == 1, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenQuaternionTest)
+{
+ std::cout << "Running Code Gen EigenQuaternion test" << std::endl;
+ EigenQuaternionTest p_tmp;
+ EigenQuaternionTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.w() == 0, true);
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.x() == 0, true);
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.y() == 0, true);
+ //BOOST_CHECK_EQUAL(p.the_double_eigen_matrix.z() == 0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenPositionTest)
+{
+ std::cout << "Running Code Gen EigenPosition test" << std::endl;
+ PositionTest p_tmp;
+ PositionTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.position.rows() == 3, true);
+ BOOST_CHECK_EQUAL(p.position.cols() == 1, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationEigenPoseTest)
+{
+ std::cout << "Running Code Gen EigenPose test" << std::endl;
+ PoseTest p_tmp;
+ PoseTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.pose.rows() == 4, true);
+ BOOST_CHECK_EQUAL(p.pose.cols() == 4, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationIntEnumTest)
+{
+ std::cout << "Running Code Gen IntEnum test" << std::endl;
+ TheObjectThatUsesTheIntEnum p_tmp;
+ TheObjectThatUsesTheIntEnum p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.the_int_enum.value == TheIntEnum::INT_ENUM_VALUE_0, true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationPrimitiveTest)
+{
+ std::cout << "Running Code Gen Primitive test" << std::endl;
+ PrimitiveTest p_tmp;
+ PrimitiveTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(p.the_bool == false, true);
+ BOOST_CHECK_EQUAL(p.the_double == 0.0, true);
+ BOOST_CHECK_EQUAL(p.the_float == 0.0, true);
+ BOOST_CHECK_EQUAL(p.the_int == 0, true);
+ BOOST_CHECK_EQUAL(p.the_long == 0, true);
+ BOOST_CHECK_EQUAL(p.the_string == "", true);
+ BOOST_CHECK_EQUAL(p.the_time == IceUtil::Time(), true);
+}
+
+BOOST_AUTO_TEST_CASE(AronCodeGenerationOptionalTest)
+{
+ std::cout << "Running Code Gen Primitive test" << std::endl;
+ OptionalTest p_tmp;
+ OptionalTest p = p_tmp; // test assignment
+ BOOST_CHECK_EQUAL(p_tmp == p, true);
+
+ BOOST_CHECK_EQUAL(typeid(p.some_dict) == typeid(std::optional<std::map<std::string, float>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_dict_with_optional_type) == typeid(std::map<std::string, std::optional<float>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_eigen_matrix) == typeid(std::optional<Eigen::Matrix<long, 25, 10>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_float) == typeid(std::optional<float>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_list) == typeid(std::optional<std::vector<double>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_list_with_optional_list) == typeid(std::optional<std::vector<std::optional<std::vector<std::optional<float>>>>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_list_with_optional_type) == typeid(std::vector<std::optional<double>>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_obj) == typeid(std::optional<armarx::OptionalTestElement>), true);
+ BOOST_CHECK_EQUAL(typeid(p.some_string) == typeid(std::optional<std::string>), true);
+
+
+ auto aronType = p.toAronType();
+ BOOST_CHECK_EQUAL(aronType->getMemberType("some_float")->getMaybe() == aron::type::Maybe::eOptional, true);
+
+ BOOST_CHECK_EQUAL(p.some_float.has_value() == false, true);
+ auto aron = p.toAron();
+ BOOST_CHECK_EQUAL(aron->getElement("some_float") == nullptr, true);
+
+ p.some_float = 5.0f;
+ BOOST_CHECK_EQUAL(p.some_float.has_value() == true, true);
+ aron = p.toAron();
+ BOOST_CHECK_EQUAL(*aron->getElement("some_float") == std::make_shared<datanavigator::FloatNavigator>(5.0f), true);
+ BOOST_CHECK_EQUAL(*aron->getElement("some_float") == nullptr, false);
+
+}
diff --git a/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml b/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
index 8e27d3c0263af6decff1d945acb40cc1d3081df4..7b402cb38e4b904723c31ce7097ebc7623c296ba 100644
--- a/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
@@ -15,7 +15,7 @@
<Float optional="1"/>
</ObjectChild>
- <!--<ObjectChild key='some_string'>
+ <ObjectChild key='some_string'>
<String optional="true"/>
</ObjectChild>
@@ -57,7 +57,7 @@
<Float optional="ja" />
</List>
</List>
- </ObjectChild>-->
+ </ObjectChild>