diff --git a/README.md b/README.md
index e355791ca5849b535672904a4c313e47d16c14f5..bc80752a58c470472d59fa92acbc4eb3ad12e1f4 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,7 @@
# RobotAPI
-## Documentation
-
-[Wiki](docs)
+[Online Documentation](https://armarx.humanoids.kit.edu/RobotAPI-Overview.html),
+in particular on
+[ARON ("ArmarX Object Notation") / IDF (Interpretable Data Format)](https://armarx.humanoids.kit.edu/aron.html)
+and
+[ArMem ("ArmarX Memory Framework")](https://armarx.humanoids.kit.edu/memory_system.html)
\ No newline at end of file
diff --git a/README.txt b/README.txt
deleted file mode 100644
index f3b3611ca8958eb5cb7e65846a2639fd68855f88..0000000000000000000000000000000000000000
--- a/README.txt
+++ /dev/null
@@ -1 +0,0 @@
-Stub to make the package tool work.
diff --git a/docs/README.md b/docs/README.md
deleted file mode 100644
index 090ef176fc179e5adb7d27857b0fe9933928c0b0..0000000000000000000000000000000000000000
--- a/docs/README.md
+++ /dev/null
@@ -1,3 +0,0 @@
-[Aron](aron)
-
-[ArMem](armem)
diff --git a/docs/armem/README.md b/docs/armem/README.md
deleted file mode 100644
index 1742eec0abd08fe86b0724828722338718348fd5..0000000000000000000000000000000000000000
--- a/docs/armem/README.md
+++ /dev/null
@@ -1,3 +0,0 @@
-* [Introduction](introduction)
-* [Existing Memories (Memory Servers)](existing_memory_servers_and_segments)
-* [How to create a new Core Segment or Memory Server](how_to_create_a_new_core_segment_or_memory_server)
diff --git a/docs/armem/existing_memory_servers_and_segments/README.md b/docs/armem/existing_memory_servers_and_segments/README.md
deleted file mode 100644
index edc04ab4a3eda6652d59f2356687aa70d34a3db1..0000000000000000000000000000000000000000
--- a/docs/armem/existing_memory_servers_and_segments/README.md
+++ /dev/null
@@ -1,96 +0,0 @@
-This is a list of existing memory servers, their core segments and their ARON data types.
-
-# `RobotState`
-
-* Memory Server: [`RobotStateMemory` (RobotAPI)](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/tree/master/source/RobotAPI/components/armem/server/RobotStateMemory)
-* Core Segments:
-
-| Core Segment Name | Type | Description | Example Entities |
-|-------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------|-------------------------|
-| `Description` | [RobotDescription](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml) | Robot description (e.g. link to XML or URDF file) | |
-| `Proprioception` | ToDo | Robot configuration and internal sensor values. | tbd |
-| [`Localization`](RobotState/Localization) | [Transform](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml) | Transformations between frames {world,map,robot} to retrieve global robot pose. | armarx::aron::Transform |
-
-Missing:
-- Robot calibration: should be part of `Description` core segment
-
-# `Object`
-
-* Memory Server: [`ObjectMemory` (RobotAPI)](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/components/armem/server/ObjectMemory)
-* Core Segments:
-
-| Core Segment Name | Type | Description | Example Entities |
-|-----------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|------------------------------------------------------------------------------------------|-----------------------------------------------------|
-| `Class` | [ObjectClass](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml) | Static description of a known object class. Entity names are (ArmarXObjects) object IDs. | `KIT/CoffeeFilters`, `YCB/001_chips_can` |
-| `Instance` | [ObjectInstance](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml) | Localized object instance. Entity names are (ArmarXObjects) object IDs. | `KIT/CoffeeFilters`, `YCB/001_chips_can/instance_1` |
-| `ArticulatedObjectClass` | [RobotDescription](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml) | Static description of a known object class. Entity names are (ArmarXObjects) object IDs. | |
-| `ArticulatedObjectInstance` | [Robot](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_robot/aron/Robot.xml) | Localized object instance. Entity names are (ArmarXObjects) object IDs. | |
-| `Attachments` | [ObjectAttachment and ArticulatedObjectAttachment](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_objects/aron/Attachment.xml) | | |
-
-
-# `Vision`
-
-* Memory Server: [`ArMemVisionMemory` (VisionX)](https://git.h2t.iar.kit.edu/sw/armarx/visionx/-/tree/master/source/VisionX/components/armem/ArMemVisionMemory)
-* Core Segments:
-
-| Core Segment Name | Type | Description | Example Entities |
-|-------------------|--------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------|------------------|
-| `ImageRGB` | [`ImageRGB`](https://git.h2t.iar.kit.edu/sw/armarx/visionx/-/blob/master/source/VisionX/libraries/ArMem/aron/ImageRGB.xml) | RGB images (mono and stereo) of a camera | image |
-| `ImageDepth` | [`ImageDepth`](https://git.h2t.iar.kit.edu/sw/armarx/visionx/-/blob/master/source/VisionX/libraries/ArMem/aron/ImageDepth.xml) | Depth images of an RGB-D or stereo camera | image |
-
-
-
-# `Speech`
-
-* Memory Server: [`SpeechMemory` (SpeechX)](https://git.h2t.iar.kit.edu/sw/armarx/speechx/-/tree/master/source/SpeechX/components/SpeechMemory)
-* Core Segments:
-
-| Core Segment Name | Type | Description | Example Entities |
-|-------------------------|-----------------|---------------------------------------------------------------------------|------------------|
-| `Command` | `SpeechCommand` | Language commands from speech (topic `Speech_Commands`). | `command` |
-| `TextListenerInterface` | `Text` | Data from `TextListenerInterface` topics. (`TextToSpeech`, `RobotAction`) | `text` |
-
-...
-
-
-# `Skills`
-
-* Memory Server: [`SkillsMemory` (RobotAPI)](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/tree/master/source/RobotAPI/components/armem/server/SkillsMemory)
-* Core Segments:
-
-| Core Segment Name | Type | Description | Example Entities |
-|-------------------|--------------|-------------------------------------|------------------|
-| `Statechart` | `Transition` | Transitions in ArmarX state charts. | tbd |
-
-
-# `GeneralPurpose`
-
-This memory is meant to allow for a quick-and-dirty integration of your data type in the memory framework. When your development finishes a stable state, consider creating moving the core segment(s) to an existing memory server or creating a new dedicated memory server.
-
-* Memory Server: [`GeneralPurposeMemory` (RobotAPI)](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/tree/master/source/RobotAPI/components/armem/server/GeneralPurposeMemory)
-* Core Segments:
-
-| Core Segment Name | Type | Description | Example Entities |
-|----------------------------------------|------|-------------|------------------|
-| None. You can add segments on the fly. | | | |
-
-# `Mapping`
-
-* Memory Server: `ToDo`
-* Core Segments:
-
-| Core Segment Name | <br/>Type | Description | Example Entities |
-|---------------------------------|-----------|--------------------------------------------------|------------------|
-| [`Mapping`](RobotState/Mapping) | `ToDo` | Mapping related sensor data (e.g. point clouds). | tbd |
-
-
-# `Example`
-
-An example memory server (alongside a matching client) showing how to implement a standard memory server and test the framework during development.
-
-* Memory Server: [`ExampleMemory` (RobotAPI)](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/tree/master/source/RobotAPI/components/armem/server/ExampleMemory)
-* Core Segments:
-
-| Core Segment Name | Type | Description | Example Entities |
-|-------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------|------------------|
-| `ExampleData` | [ExampleData](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml) | Some example data. | example |
\ No newline at end of file
diff --git a/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/README.md b/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/README.md
deleted file mode 100644
index fe8ac7af138bca553c7a80c3683ff74db0156c90..0000000000000000000000000000000000000000
--- a/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/README.md
+++ /dev/null
@@ -1,36 +0,0 @@
-# The localization segment within the RobotState memory
-
-## Concept
-
-There exist various frames that are needed to obtain the robot's global pose (pose within in the global frame):
-
-* [global] the "world" frame.
-* [map] During mapping, the robot does not have knowledge about the world but creates a local map (with "map" as root frame). All map features (e.g. point clouds, occupancy grids, ...) are linked to this frame.
-* [odom] The odometry as an integration of the robot's velocity.
-* [robot] The robot's root frame
-
-
-
-For each transformation between frames, there exist individual components:
-
-* [global->map] **Map registration:** given a scene (objects with known poses, e.g. walls, ...), this component obtains a static transformation between a the scene and the map.
-* [map->odom] **Localization** The localization component computes the transformation [map->robot] and performs map corrections by updating [map->odom].
-* [odom->robot] **Odometry**
-
-
-### Memory design
-
-Within the core segment, each robot has its own provider segment (here: Armar6). Within the provider segment, entities describe the transformations between the frames ("Global,Map" -> [global->map]).
-
-
-
-### Using the localization memory
-
-See **RobotAPI/libraries/armem_robot_localization**
-
-There exist two classes:
-
-* TransformWriter to send transformations to memory
-* TransformReader to obtain transformations between any of the frames; can also be used to obtain the "robot's global pose"
-
-For an example, see RobotComponents/components/carographer_mapping_and_localization (currently branch feature/cartographer_integration)
diff --git a/docs/armem/existing_memory_servers_and_segments/RobotState/Mapping/README.md b/docs/armem/existing_memory_servers_and_segments/RobotState/Mapping/README.md
deleted file mode 100644
index 167e058d44b78ebec0f59d97c2dc4f7ffc7eef72..0000000000000000000000000000000000000000
--- a/docs/armem/existing_memory_servers_and_segments/RobotState/Mapping/README.md
+++ /dev/null
@@ -1,9 +0,0 @@
-# The mapping segment within the RobotState memory
-
-## Concept
-
-Within the memory segment, sensor data is stored that is relevant for
-* mapping
-* map registration
-
-
diff --git a/docs/armem/existing_memory_servers_and_segments/RobotState/Mapping/res/transformations_frames-Page-2.png b/docs/armem/existing_memory_servers_and_segments/RobotState/Mapping/res/transformations_frames-Page-2.png
deleted file mode 100644
index 65e856849ff9fc62016d27b1631f8d456b5a22b1..0000000000000000000000000000000000000000
Binary files a/docs/armem/existing_memory_servers_and_segments/RobotState/Mapping/res/transformations_frames-Page-2.png and /dev/null differ
diff --git a/docs/armem/introduction/README.md b/docs/armem/introduction/README.md
deleted file mode 100644
index 43f2e3ac5e1b169c900f5793875bc5788a79c37a..0000000000000000000000000000000000000000
--- a/docs/armem/introduction/README.md
+++ /dev/null
@@ -1,42 +0,0 @@
-# Distributed Memory
-
-The ArMem memory system is distributed: It is comprised of several **memory servers** storing data in a hierarchical structure.
-A memory server can be defined in any ArmarX package and defines which kind of data it stores (e.g. robot state, object classes and instances, visual data, speech, ...).
-As a memory server knows which data it stores, it can provide special behaviour, such as custom visualization, aggregation, interpolation or prediction.
-As each server is a separate component describing its own structure, it is simple to add new memory servers containing special data types, which are defined in ArmarX packages further down in the dependency chain.
-
-All memory servers follow the same hierarchical data structure and self-describing data format.
-The data structure consists of several levels, storing histories/timelines of different entities stored in different segments (see "Memory Levels" below).
-The common data format is ARON (ArmarX Object Notation), which is a self-describing, hierarchical data format, allowing extensive introspection as well as general storage.
-Each item in the memory (i.e. entries in all levels) can be identified with a **Memory ID**, which contains the keys of each (specified) level. For example, a core segment ID specifies the memory name and core segment name, while an entity instance ID specifies the whole path to the leaf of the data structure.
-
-
-Technically, each memory server is one ArmarX component implementing the `armem::server::MemoryInterface` (comprising a `ReadingMemoryInterface` and a `WritingMemoryInterface`).
-Memory servers register themselves in the **Memory Name System (MNS)**, where they can be found by their (semantic) memory name (which is usually a short, human-readable name such as "Robot", "Object" or "Vision").
-Memory servers receive data via **commits** (via the `WritingMemoryInterface`), and return data according to **queries** (via the `ReadingMemoryInterface`).
-
-
-# Memory Levels
-
-A memory server stores data in a hierarchical structure consisting of several levels:
-```
-- Grasping (Memory)
- - Grasps (Core Segment)
- - KnownObjectGraspPlanner (Provider Segment)
- - my_object (Entity)
- - t = 2021-03-09 14:24:20.064524 (Entity Snapshot)
- - 00 (Entity Instance)
- - 01 (Entity Instance)
- - 02 (Entity Instance)
-```
-
-| Level | Key | Description | Examples | Implementation |
-| ----- | --- | ----------- | -------- | -------------- |
-| Memory | Name (String) | Semantic grouping of one or more core segments. Corresponds to one memory server. | Robot, Object, Vision | [Memory](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem/core/Memory.h) |
-| Core Segment | Name (String) | Building block of a memory containing homogeneous data of a specific (ARON) type. Specifies a _Core Segment Type_ which all provided data must comply to. | (Robot) Configuration, (Object) Classes, Known (Object) Instances, ImageRGB | [CoreSegment](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem/core/CoreSegment.h) |
-| Provider Segment | Name (String) | Sub-segment of a core segment which contains the data from a single source (e.g. a camera, a method, a component). Can define a _Provider Segment Type_ which extends the _Core Segment Type_. | Primsense, SimTrack, MyGraspPlanner | [ProviderSegment](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem/core/ProviderSegment.h) |
-| Entity | Name (String) | A single thing or concept whose properties change over time. An entity has a history / timeline, i.e. a sequence of snapshots storing the entity's properties at a specific point in time. | `image`, `some_object`, `some_object_grasps` | [Entity](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem/core/Entity.h) |
-| Entity Snapshot | Timestamp (`armem::Time` aka `IceUtil::Time`) | An entry of an entity's history, i.e. the state of an entity at a specific point in time. Can contain multiple (entitiy) instances. | Image, object instance, grasp hypotheses at a time *t* | [EntitySnapshot](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem/core/EntitySnapshot.h) |
-| Entity Instance | Index (int) | One instance of the segment's ARON type. | left/right stereo image (at time t), object (at time t), grasp hypothesis (one of many at time t) | [EntityInstance](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem/core/EntityInstance.h) |
-| Entity Instance Metadata | -- | Metadata stored alongside the instance's data. | Further timesteps, confidence | [EntityInstanceMetadata](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem/core/EntityInstance.h) |
-
diff --git a/docs/aron/README.md b/docs/aron/README.md
deleted file mode 100644
index daff059aabe7c576936c41e30789b87fc63c6b1c..0000000000000000000000000000000000000000
--- a/docs/aron/README.md
+++ /dev/null
@@ -1,4 +0,0 @@
-* [Introduction](introduction)
-* [Code Generation](code_generation)
-* [Visitors](visitors)
-* [Readers, Writers and Conversion](conversion)
diff --git a/docs/aron/conversion/README.md b/docs/aron/conversion/README.md
deleted file mode 100644
index a2233c26e99620a5331c2a1ea2448a7c2189656d..0000000000000000000000000000000000000000
--- a/docs/aron/conversion/README.md
+++ /dev/null
@@ -1,35 +0,0 @@
-# Aron conversion
-
-Aron offers ways to simply convert any aron object into another representation (e.g. from variant to nlohmann::json or vice versa). To do so, it makes use of specific readers and writers. In the following we will only describe the readers, writers and converters for Aron data, not for types (but the principle is the same).
-
-## Readers
-
-An Aron reader is used to get information of an Aron object in a specific representation. Assume you have an nlohmann::json aron object which contains some information (E.g. it is a dict, containing some members, ...). We need this information to create another object in another representation with the same content.
-
-To do so, you only have to implement the armarx::aron::data::ReaderInterface class. It needs one template parameter for your InputType (e.g. here const nlohmann::json).
-
-The interface provides the following pure virtual methods:
-```cpp
- virtual void readList(InputType& input, std::vector<InputTypeNonConst>& elements) = 0;
- virtual void readDict(InputType& input, std::map<std::string, InputTypeNonConst>& elements) = 0;
- virtual void readNDArray(InputType& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data) = 0;
- virtual void readInt(InputType& input, int& i) = 0;
- virtual void readLong(InputType& input, long& i) = 0;
- virtual void readFloat(InputType& input, float& i) = 0;
- virtual void readDouble(InputType& input, double& i) = 0;
- virtual void readString(InputType& input, std::string& s) = 0;
- virtual void readBool(InputType& input, bool& i) = 0;
-```
-You have to implement the function so that the non-const arguments of the method will be filled with the values of the input. E.g. the implementation of the readString method for the nlohmann::json reader would be:
-```cpp
- void NlohmannJSONReader::readString(const nlohmann::json& input, std::string& i)
- {
- if (input[rw::json::constantes::TYPE_SLUG] != expectedType)
- {
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Wrong type in json encountered.", input[rw::json::constantes::TYPE_SLUG], expectedType);
- }
- i = input[rw::json::constantes::VALUE_SLUG];
- }
-```
-
-Of course, the way to get the member depend on the way how to construct (writer) the nlohmann::json.
\ No newline at end of file
diff --git a/docs/aron/introduction/README.md b/docs/aron/introduction/README.md
deleted file mode 100644
index 1a9c9987d50c3784a87807595353b68362419293..0000000000000000000000000000000000000000
--- a/docs/aron/introduction/README.md
+++ /dev/null
@@ -1,213 +0,0 @@
-[[_TOC_]]
-
-# Aron structure
-
-Aron (ArmarX Object Notation) is the ArmarX implementation of variants which can be transferred over the network via ZeroC Ice. Further, Aron is the data representation used in the ArmarX Memory System [ArMem](ArMem/ArMem). We distinguish between:
-
-- type specification vs. data information
-- data transfer object (in the following called Aron DTO) vs. its corresponding c++ wrapper (in the following called Aron object). The DTO specification is done in ice so that every Aron object can be transferred via ice.
-
-## Aron Type specification
-
-An Aron type specification defines the static type for an Aron data DTO or an Aron data object. It does not contain any data (e.g. an aron type list only knows the accepted type, not the list members). Since Aron supports maybe types (raw ptr, smart ptr, optional), every Aron type DTO contains a special member. It can only consist of the following types and information (AronVariant means any Type):
-
-```cpp
- Object { // dto called armarx::aron::type::dto::AronObject
- string name;
- AronVariant extends;
- map<string, AronVariant> memberTypes;
- }
- Dict { // dto called armarx::aron::type::dto::Dict
- AronVariant acceptedType;
- }
- List { // dto called armarx::aron::type::dto::List
- AronVariant acceptedType;
- }
- Pair { // ...
- AronVariant acceptedType1;
- AronVariant acceptedType2;
- }
- Tuple {
- vector<AronVariant> acceptedTypes;
- }
- IntEnum {
- string name;
- map<string, int> acceptedValues;
- }
- NDArray {
- int ndim;
- type type; // in [uint8, int8, uint16, int16, uint32, int32, float32, float64]
- }
- Matrix {
- int rows, cols;
- type type; // in [int16, int32, int64, float32, float64]
- }
- Quaternion {
- type type; // in [float32, float64]
- }
- Position {
- }
- Orientation {
- }
- Pose {
- }
- Image {
- pixelType type; // in [rgb24, depth32]
- }
- PointCloud {
- voxelType type; // in [PointXYZ, PointXYZI, PointXYZL, PointXYZRGB, PointXYZRGBA, PointXYZRGBL, PointXYZHSV]
- }
- Int { // dto called armarx::aron::type::dto::AronInt
- }
- Long { // dto called armarx::aron::type::dto::AronLong
- }
- Float { // dto called armarx::aron::type::dto::AronFloat
- }
- Double { // dto called armarx::aron::type::dto::AronDouble
- }
- String { // dto called armarx::aron::type::dto::AronString
- }
- Bool { // dto called armarx::aron::type::dto::AronBool
- }
- Time { // dto called armarx::aron::type::dto::AronTime
- }
-```
-
-## Aron Data specification
-
-Aron data objects and Aron data DTOs are similar structured to type objects and type DTOs. However, there are less classes for data objects and data DTOs. Aron data is completely decoupled from the static types and contains the data members (e.g. an aron data list only contains the list members (as AronVariants) and not the accepted type):
-
-```cpp
- Dict { // dto called armarx::aron::data::dto::Dict
- map<string, AronVariant> members;
- }
- List { // dto called armarx::aron::data::dto::List
- vector<AronVariant> elements;
- }
- NDArray { // ...
- vector<int> shape;
- string type;
- vector<byte> data;
- }
- Int { // dto called armarx::aron::data::dto::AronInt
- int value;
- }
- Long { // dto called armarx::aron::data::dto::AronLong
- long value;
- }
- Float { // dto called armarx::aron::data::dto::AronFloat
- float value;
- }
- Double { // dto called armarx::aron::data::dto::AronDouble
- double value;
- }
- String { // dto called armarx::aron::data::dto::AronString
- string value;
- }
- Bool { // dto called armarx::aron::data::dto::AronBool
- bool value;
- }
-```
-
-Please note that every Aron data object or DTO can be NULL!. The reason is, that if the type supports maybe types and a member is e.g. optional the data must support maybetype as well. If a generated Aron class (We come to the code generation later) has an optional member, this member will be translated into a NULL Aron object and DTO.
-
-## Connection of Aron data and Aron type
-
-Aron data contains less classes than Aron type. Because Aron data objects and DTOs do not check the type of the members (they can be any aron data (AronVariant)), we can only validate an aron data object or DTO if we have the type information. The following mapping describes, how data and types correspond.
-```cpp
- Aron Type | Aron Data
- -------------------------------
- Object -> Dict
- Dict -> Dict
- List -> List
- Pair -> List
- Tuple -> List
- NDArray -> NDArray
- Matrix -> NDArray
- Quaternion -> NDArray
- Position -> NDArray
- Orientation -> NDArray
- Pose -> NDArray
- Image -> NDArray
- PointCloud -> NDArray
- IntEnum -> Int
- Int -> Int
- Long -> Long
- Float -> Float
- Double -> Double
- String -> String
- Bool -> Bool
- Time -> Long
-```
-
-If no type object or DTO is available, we can at least derive some information from the data object (e.g. "it is a data dict, so the type cant be list").
-
-To differ between data and type makes it easier to work with these Variants. In most cases, the type information is not relevant. Then you only have to check for the data type.
-
-# Aron data and type objects
-
-As already mentioned, we implemented wrapper classes around the DTO. These classes offer convenience methods and conversions and make lots things easier when working directly with Aron. These classes can be found at aron/core/[data,type]/variant/Variant.h.
-
-The Aron objects have more or less the same structure as the DTOs (see above). Everything is stored as a shared_ptr (e.g. members, elements, acceptedTypes, ...). If you want to implement a method which takes any Aron data object as input, you can use the base class of every Aron data object:
-```cpp
- void myFancyMethod(const armarx::aron::data::VariantPtr& variant);
-```
-
-If you want to check, what a variant really is, you can use the descriptor of the object:
-```cpp
- armarx::aron::data::VariantPtr variant; // <-- the variant
- auto desc = variant->getDescriptor();
- switch(desc)
- {
- case armarx::aron::data::Descriptor::eDict: ...
- case armarx::aron::data::Descriptor::eList: ...
- ...
- }
-```
-
-If you have a DTO, you dont know the exact type but you want to have a Aron object you can make use of the static construction methods:
-```cpp
- armarx::aron::data::dto::GenericData dto; // <-- the DTO variant
- auto aron = armarx::aron::data::Variant::FromAronDTO(dto);
-```
-
-If you know the type, you can use the constructor the specific Aron object.
-
-## Example to create an Aron data dict from scratch
-
-Goal: Have an Aron dict with a list as member "the_list" and some values in it. Then echo the members and their descriptor as a string.
-
-```cpp
- using namespace armarx;
-
- // setup aron object
- auto dict = std::make_shared<aron::data::Dict>();
- {
- auto list = std::make_shared<aron::data::List>();
- for (unsigned int i = 0; i < 10; ++i)
- {
- list->addElement(std::make_shared<aron::data::Int>(i));
- }
- dict->addElement("the_list", list);
- }
-
- // echo
- auto listVariant = dict->getElement("the_list"); // will return a aron::data::VariantPtr
- auto list = aron::data::List::DynamicCastAndCheck(listVariant); // cast and check whether the cast was successful
- for (const auto& intVar : list->getElements())
- {
- auto i = aron::data::Int::DynamicCastAndCheck(intVar);
- std::cout << "The value is: " << i->getValue() << std::endl;
- std::cout << "The descriptor is: " << aron::data::defaultconversion::string::Descriptor2String.at(i->getDescriptor()) << std::endl;
- }
-```
-
-Please note that we might add more methods or make the current ones more flexible (e.g. such as nlohmann::json).
-
-# Lessons learned
-
-- There is a difference between Aron data and Aron type
-- There is a difference between Aron objects and Aron DTOs
-- How are Aron DTOs structured
-- How do Aron data and Aron types correspond
-- How to use the Aron objects
\ No newline at end of file
diff --git a/docs/aron/visitors/README.md b/docs/aron/visitors/README.md
deleted file mode 100644
index c72e3a92e31b44b4bccbe2cfcd73be2aecb0c1b4..0000000000000000000000000000000000000000
--- a/docs/aron/visitors/README.md
+++ /dev/null
@@ -1,101 +0,0 @@
-[[_TOC_]]
-
-# Visitors
-
-Visitors are a useful tool to define specific methods for specific types without specifying the `switch(): case: ...` again and again. Further, recursive visitors offer a convenient way to iterate through the tree-like structure of any Aron object. (Please note that right now the implementation does not check for cycles!). Again, we will only look at data visitors since the implementation of type visitors is more or less similar.
-
-## Non-recursive Visitors
-
-First, there are "normal" visitors. They are used to get rid of the big switch-case statement when getting an Aron object with unknown type. Further, these visitors can be used to convert an Aron object from one representation into another (see [Aron/Conversion](Aron/Conversion)).
-The Visitor Base-class offers the following pure virtual methods which you have to implement:
-```cpp
- virtual data::Descriptor getDescriptor(Input&) = 0;
- virtual void visitDict(Input&) {};
- virtual void visitList(Input&) {};
- virtual void visitNDArray(Input&) {};
- virtual void visitInt(Input&) {};
- virtual void visitLong(Input&) {};
- virtual void visitFloat(Input&) {};
- virtual void visitDouble(Input&) {};
- virtual void visitBool(Input&) {};
- virtual void visitString(Input&) {};
- virtual void visitUnknown(Input&) { throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor."); }
-```
-`Input` is a template parameter, defining the Input-type (e.g. `const armarx::aron::data::VariantPtr` for a `VariantVisitor` Implementation).
-
-The method `getDescriptor(Input&)` is used for the switch case interally.
-
-As you can see the `VariantVisitor` always gets a generic variant as input in each function. This means that, although the Visitor guarantees that the input is correct, you have to cast the class to the correct type. For widely used Visitors, the Aron library offers convenience implementations (e.g. for Variants and for nlohmann::json inputs). The convenience class for Variants additionally adds overloads where the input is already casted correctly (you can decide which method you want to override).
-
-To use a Visitor, you have to use the function
-```cpp
- void visit(VisitorImplementation& v, typename VisitorImplementation::Input& o);
-```
-which takes the visitor and the input as arguments and does the switch-case.
-
-## Recursive Vistors
-
-Recursive visitors are similar to normal vistors but they continue "visiting" until the object is fully visited. For containers (e.g. dicts, lists) they offer two methods each:
-```cpp
- virtual void visitDictOnEnter(Input& element) {};
- virtual void visitDictOnExit(Input& element) {};
- virtual void visitListOnEnter(Input& element) {};
- virtual void visitListOnExit(Input& element) {};
-```
-The `*OnEnter` method is called before iterating over all children of the container. The `*OnExit` is called after the iteration.
-
-Further, in order to get the elements of a container, you must implement the methods:
-```cpp
- virtual MapElements getDictElements(Input&) = 0;
- virtual ListElements getListElements(Input&) = 0;
-```
-Again, Input can be any aron object (e.g. Variant or nlohmann::json).
-
-As for normal visitors, the Aron library offers convenience implementations for Variants and nlohmann::json inputs.
-
-To use the RecursiveVisitor, you have to use the function
-```cpp
- void visitRecursive(RecursiveVisitorImplementation& v, typename RecursiveVisitorImplementation::Input& o);
-```
-
-### Example
-
-Assume you want to count how many objects of each kind you have in a Variant.
-```cpp
- #include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
- class CountingRecursiveVisitor : public RecursiveConstVariantVisitor
- {
- public:
- int dicts = 0;
- int lists = 0;
- int ndarrays = 0;
- int ints = 0;
- int floats = 0;
- int longs = 0;
- int doubles = 0;
- int strings = 0;
- int bools = 0;
- public:
- void visitAronVariantOnEnter(const data::DictPtr&) override { dicts++; }
- void visitAronVariantOnEnter(const data::ListPtr&) override { lists++; }
- void visitAronVariant(const data::NDArrayPtr&) override { ndarrays++; }
- void visitAronVariant(const data::IntPtr&) override { ints++; }
- void visitAronVariant(const data::LongPtr&) override { longs++; }
- void visitAronVariant(const data::FloatPtr&) override { floats++; }
- void visitAronVariant(const data::DoublePtr&) override { doubles++; }
- void visitAronVariant(const data::BoolPtr&) override { strings++; }
- void visitAronVariant(const data::StringPtr&) override { bools++; }
- };
-
- void countHowMany(const aron::data::VariantPtr& aron)
- {
- CountingRecursiveVisitor visitor;
- aron::data::visitRecursive(visitor, aron);
-
- std::cout << "The aron has " << visitor.dicts << " dicts." << std::endl;
- }
-```
-
-## Typed Visitors
-
-# Lessons Learned
\ No newline at end of file
diff --git a/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/res/frames.png b/etc/doxygen/images/armem/RobotState/localization/frames.png
similarity index 100%
rename from docs/armem/existing_memory_servers_and_segments/RobotState/Localization/res/frames.png
rename to etc/doxygen/images/armem/RobotState/localization/frames.png
diff --git a/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/res/localization-armem.png b/etc/doxygen/images/armem/RobotState/localization/localization-armem.png
similarity index 100%
rename from docs/armem/existing_memory_servers_and_segments/RobotState/Localization/res/localization-armem.png
rename to etc/doxygen/images/armem/RobotState/localization/localization-armem.png
diff --git a/etc/doxygen/pages/HowTos.dox b/etc/doxygen/pages/HowTos.dox
index 73d1f6aa03ce74996e4fb74e9476b829dffb5f12..2fe48c81ed35c95d80515081fe3adf118d0fd89b 100644
--- a/etc/doxygen/pages/HowTos.dox
+++ b/etc/doxygen/pages/HowTos.dox
@@ -137,4 +137,17 @@ If a complete robot model (including 3d models) is needed, you can pass a filena
\endcode
This model can be synchronized in the same way as the first model.
+
+
+\section RobotAPI-HowTos-Memory-Plot-Values How to Plot Values from the Memory System
+
+You can use the component \ref Component-MemoryToDebugObserver to query data from memory servers
+and send them to the \ref Component-DebugObserver.
+These values can then be viewed using
+the \ref ArmarXGui-GuiPlugins-ObserverWidget
+or the \ref ArmarXGui-GuiPlugins-PlotterPlugin.
+
+To use this functionality in your own code, use the class
+armarx::armem::client::util::MemoryToDebugObserver.
+
*/
diff --git a/etc/doxygen/pages/Overview.dox b/etc/doxygen/pages/Overview.md
similarity index 93%
rename from etc/doxygen/pages/Overview.dox
rename to etc/doxygen/pages/Overview.md
index 614040b1cae1d61bbf94f2c7ab2e62916596adb1..427e270c221a737fb5aa455a8e27033a0368fb10 100644
--- a/etc/doxygen/pages/Overview.dox
+++ b/etc/doxygen/pages/Overview.md
@@ -1,11 +1,12 @@
-/**
-\page RobotAPI-Overview RobotAPI Overview
+# RobotAPI Overview {#RobotAPI-Overview}
+
The RobotAPI package provides a robot-intependent API that other ArmarX packages can use for accessing central functionality of a robot.
The central elements of RobotAPI are Sensor-Actor Units, i.e. components responsible for the propagation of sensor values and actor targets.
Sensor-Actor Units work as a link between higher level ArmarX components and the actual robot hardware or a robot simulation environment, respectively.
The RobotAPI package contains base classes for Sensor-Actor Units for different purposes, e.g. KinematicUnit for sensors and actors related to robotic joints.
Concrete Sensor-Actor Units inherit from these base classes in order to provide a common interface.
-Each type of hardware and each type of simulation environment requires a distinct set of Sensor-Actor Units that handle the hardware/simulator access, e.g. KinematicUnitArmar3 and KinematicUnitArmar4 for accessing the kinematic elements of ARMAR-III and ARMAR-4, respectively:
+Each type of hardware and each type of simulation environment requires a distinct set of Sensor-Actor Units that handle the hardware/simulator access,
+e.g. KinematicUnitArmar3 and KinematicUnitArmar4 for accessing the kinematic elements of ARMAR-III and ARMAR-4, respectively:
\image html sensoractorunits2.svg "Inheritance diagram for Sensor-Actor Units"
@@ -62,10 +63,16 @@ RobotAPI also provides a GUI-plugin for robot visualization.
\par
\link RobotAPI-GuiPlugins Read more \endlink
+\par RobotUnit
+\subpage RobotUnit
+
+
+\par GamepadUnit
+\subpage GamepadUnit
+
\defgroup RobotAPI RobotAPI
\copydoc RobotAPI-Overview
\defgroup RobotAPI-Statecharts Statecharts
\ingroup RobotAPI
-*/
diff --git a/etc/doxygen/pages/Tutorials.dox b/etc/doxygen/pages/Tutorials.dox
deleted file mode 100644
index ff5d3d89165100062c7016e19466717f3decd6be..0000000000000000000000000000000000000000
--- a/etc/doxygen/pages/Tutorials.dox
+++ /dev/null
@@ -1,13 +0,0 @@
-/**
-
-\page RobotAPI-Tutorials RobotAPI Tutorials
-
-Tutorials related to RobotAPI:
-
-\li \subpage RobotAPI-Tutorial-MoveRobotArmAlongRectangle
-
-\li \ref Component-ArVizExample (Example component for how to use ArViz)
-
-\li <a href="https://git.h2t.iar.kit.edu/sw/armarx/meta/academy/-/tree/master/tutorials/100_beginner/301_memory_server_and_client_tutorial_cpp">Memory Server and Client Tutorial</a>
-
-*/
diff --git a/etc/doxygen/pages/Tutorials.md b/etc/doxygen/pages/Tutorials.md
new file mode 100644
index 0000000000000000000000000000000000000000..d57a5324602e818f24b3b1163293c5293b14a9e1
--- /dev/null
+++ b/etc/doxygen/pages/Tutorials.md
@@ -0,0 +1,7 @@
+# RobotAPI Tutorials {#RobotAPI-Tutorials}
+
+Tutorials related to RobotAPI:
+
+- \subpage RobotAPI-Tutorial-MoveRobotArmAlongRectangle
+- \ref Component-ArVizExample (Example component for how to use ArViz)
+- \ref tutorials_120_memory_server_and_client_cpp
diff --git a/etc/doxygen/pages/armem/RobotState/localization.md b/etc/doxygen/pages/armem/RobotState/localization.md
new file mode 100644
index 0000000000000000000000000000000000000000..6a9b89dc843ca1271ec64e9d0d16ce8c068e69ff
--- /dev/null
+++ b/etc/doxygen/pages/armem/RobotState/localization.md
@@ -0,0 +1,42 @@
+# Localization Segment (RobotState Memory) {#memory_system-localization}
+
+## Concept
+
+There exist various frames that are needed to obtain the robot's global pose (pose within in the global frame):
+
+| Name | Description |
+|----------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| `global` | The "world" frame |
+| `map` | During mapping, the robot does not have knowledge about the world but creates a local map (with `map` as root frame). All map features (e.g. point clouds, occupancy grids, ...) are linked to this frame. |
+| `odom` | The odometry as an integration of the robot's velocity. |
+| `robot` | The robot's root frame |
+
+
+
+For each transformation between frames, there exist individual components:
+
+| Involved Frames | Description |
+|-----------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| `global->map` | **Map registration:** given a scene (objects with known poses, e.g. walls, ...), this component obtains a static transformation between a the scene and the map. |
+| `map->odom` | **Localization** The localization component computes the transformation `map->robot` and performs map corrections by updating `map->odom`. |
+| `odom->robot` | **Odometry** |
+
+
+### Memory Design
+
+Within the core segment, each robot has its own provider segment (here: `Armar6`).
+Within the provider segment, entities describe the transformations between the frames (`Global,Map` -> `global->map`).
+
+
+
+### Using the Localization Memory
+
+See `armarx::armem::client::robot_state::localization` (include `RobotAPI/libraries/armem_robot_localization/`).
+
+There exist two classes:
+
+* `armarx::armem::client::robot_state::localization::TransformWriter` to send transformations to memory
+* `armarx::armem::client::robot_state::localization::TransformReader` to obtain transformations between any of the
+ frames; can also be used to obtain the "robot's global pose"
+
+For an example, see `RobotComponents/components/carographer_mapping_and_localization` (currently branch `feature/cartographer_integration`)
diff --git a/docs/armem/how_to_create_a_new_core_segment_or_memory_server/README.md b/etc/doxygen/pages/armem/how_to_create_a_new_core_segment_or_memory_server.md
similarity index 98%
rename from docs/armem/how_to_create_a_new_core_segment_or_memory_server/README.md
rename to etc/doxygen/pages/armem/how_to_create_a_new_core_segment_or_memory_server.md
index 9aaf1d9795a9ea38ae6414dc39ad03566f9f13da..6c1fa08fc6e0bc83c124389e74eecff72179018d 100644
--- a/docs/armem/how_to_create_a_new_core_segment_or_memory_server/README.md
+++ b/etc/doxygen/pages/armem/how_to_create_a_new_core_segment_or_memory_server.md
@@ -1,6 +1,13 @@
+# How to Create a New Core segment or Memory Server {#memory_system-how_to_create_a_new_core_segment_or_memory_server}
+
+@see
+\ref tutorials_120_memory_server_and_client_cpp
+
+\todo
+Merge the information from here into \ref tutorials_120_memory_server_and_client_cpp
+
In order to make a new type of data available in the memory system, you first need to create an ARON XML file. Please refer to [Aron/CodeGeneration](Aron/CodeGeneration) if you want to know how to do so.
-[[_TOC_]]
<!--
# Define your data type in ARON
@@ -390,4 +397,4 @@ When you now start your new memory server and the MemoryNameSystem (MNS) (e.g. u
That's it! You now have a memory server providing a place where your nice and interesting data can feel welcome, and where interested listeners would look to find it.
-[Next, we learn how to commit data to the memory, and query data from the memory.](ArMem/How-to-read-from-and-write-to-a-Memory-Server)
\ No newline at end of file
+[Next, we learn how to commit data to the memory, and query data from the memory.](ArMem/How-to-read-from-and-write-to-a-Memory-Server)
diff --git a/etc/doxygen/pages/armem/memory_system.md b/etc/doxygen/pages/armem/memory_system.md
new file mode 100644
index 0000000000000000000000000000000000000000..e516fa69c80d034daa4c4077fb7818068eaffd44
--- /dev/null
+++ b/etc/doxygen/pages/armem/memory_system.md
@@ -0,0 +1,151 @@
+# ArmarX Memory System {#memory_system}
+
+\tableofcontents
+
+@see
+- \ref tutorials_120_memory_server_and_client_cpp
+- \subpage memory_system-how_to_create_a_new_core_segment_or_memory_server
+
+## Introduction to the Memory System {#memory_system-introduction}
+
+### Distributed Memory
+
+The ArMem memory system is distributed: It consists of several **memory servers** storing data in a hierarchical
+structure.
+A memory server can be defined in any ArmarX package and defines which kind of data it stores
+(e.g. robot state, object classes and instances, visual data, speech, ...).
+As a memory server knows which data it stores, it can provide special behaviour, such as custom visualization,
+aggregation, interpolation or prediction.
+As each server is a separate component describing its own structure, it is simple to add new memory servers containing
+special data types, which are defined in ArmarX packages further down in the dependency chain.
+
+All memory servers follow the same hierarchical data structure and self-describing data format.
+The data structure consists of several levels, storing histories/timelines of different entities stored in different
+segments (see "Memory Levels" below).
+The common data format is \ref aron, which is a self-describing, hierarchical data format,
+allowing extensive introspection as well as general storage.
+Each item in the memory (i.e. entries in all levels) can be identified with a **Memory ID**, which contains the keys
+of each (specified) level. For example, a core segment ID specifies the memory name and core segment name, while an
+entity instance ID specifies the whole path to the leaf of the data structure.
+
+Technically, each memory server is one ArmarX component implementing the `armarx::armem::server::MemoryInterface`
+(comprising a `armarx::armem::server::ReadingMemoryInterface` and a `armarx::armem::server::WritingMemoryInterface`).
+Memory servers register themselves in the **Memory Name System (MNS)**, where they can be found by their (semantic)
+memory name (which is usually a short, human-readable name such as "Robot", "Object" or "Vision").
+Memory servers receive data via **commits** (via the `armarx::armem::server::WritingMemoryInterface`),
+and return data according to **queries** (via the `armarx::armem::server::ReadingMemoryInterface`).
+
+
+### Memory Levels
+
+A memory server stores data in a hierarchical structure consisting of several levels:
+```
+- Grasping (Memory)
+ - Grasps (Core Segment)
+ - KnownObjectGraspPlanner (Provider Segment)
+ - my_object (Entity)
+ - t = 2021-03-09 14:24:20.064524 (Entity Snapshot)
+ - 00 (Entity Instance)
+ - 01 (Entity Instance)
+ - 02 (Entity Instance)
+```
+
+| Level | Key | Description | Examples | Implementation |
+|--------------------------|----------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------|---------------------------------------------|
+| Memory | Name (String) | Semantic grouping of one or more core segments. Corresponds to one memory server. | Robot, Object, Vision | `armarx::armem::wm::Memory` |
+| Core Segment | Name (String) | Building block of a memory containing homogeneous data of a specific (ARON) type. Specifies a _Core Segment Type_ which all provided data must comply to. | (Robot) Proprioception, (Object) Classes, Known (Object) Instances, ImageRGB | `armarx::armem::wm::CoreSegment` |
+| Provider Segment | Name (String) | Sub-segment of a core segment which contains the data from a single source (e.g. a camera, a method, a component). Can define a _Provider Segment Type_ which extends the _Core Segment Type_. | Primesense, MyObjectLocalizer, MyGraspPlanner | `armarx::armem::wm::ProviderSegment` |
+| Entity | Name (String) | A single thing or concept whose properties change over time. An entity has a history / timeline, i.e. a sequence of snapshots storing the entity's properties at a specific point in time. | `image`, `some_object`, `some_object_grasps` | `armarx::armem::wm::Entity` |
+| Entity Snapshot | Timestamp (`armarx::armem::Time` aka `armarx::DateTime`) | An entry of an entity's history, i.e. the state of an entity at a specific point in time. Can contain multiple (entitiy) instances. | Image, object instance, grasp hypotheses at a time *t* | `armarx::armem::wm::EntitySnapshot` |
+| Entity Instance | Index (int) | One instance of the segment's ARON type. | left/right stereo image (at time t), object (at time t), grasp hypothesis (one of many at time t) | `armarx::armem::wm::EntityInstance` |
+| Entity Instance Metadata | -- | Metadata stored alongside the instance's data. | Further timesteps, confidence | `armarx::armem::wm::EntityInstanceMetadata` |
+
+
+## Existing Memory Servers and Segments {#memory_system-existing_servers_segments}
+
+This is a list of existing memory servers, their core segments and their \ref ARON "ARON" data types.
+
+
+### RobotState {#memory_system-robot_state}
+
+* Memory Server: [`RobotStateMemory` (RobotAPI)](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/tree/master/source/RobotAPI/components/armem/server/RobotStateMemory)
+* Core Segments:
+
+| Core Segment Name | Type | Description | Example Entities |
+|----------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------------------------------------------------------------|----------------------------------------------|
+| Description | [RobotDescription](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_robot/aron/RobotDescription.xml) | Robot description (e.g. link to XML or URDF file) | Description of ARMAR-6 |
+| Proprioception | [Proprioception](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml) | Robot configuration and internal sensor values. | Proprioception of ARMAR-6 |
+| \subpage memory_system-localization "Localization" | [Transform](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_robot_localization/aron/Transform.xml) | Transformations between frames {world,map,robot} to retrieve global robot pose. | Transfrom from robot root frame to map frame |
+
+Missing:
+- Robot calibration: should be part of `Description` core segment
+
+
+### Object
+
+* Memory Server: [`ObjectMemory` (RobotAPI)](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/components/armem/server/ObjectMemory)
+* Core Segments:
+
+| Core Segment Name | Type | Description | Example Entities |
+|--------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|------------------------------------------------------------------------------------------|-----------------------------------------------------|
+| Class | [ObjectClass](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml) | Static description of a known object class. Entity names are (ArmarXObjects) object IDs. | `KIT/CoffeeFilters`, `YCB/001_chips_can` |
+| Instance | [ObjectInstance](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml) | Localized object instance. Entity names are (ArmarXObjects) object IDs. | `KIT/CoffeeFilters`, `YCB/001_chips_can/instance_1` |
+| Attachments | [ObjectAttachment and ArticulatedObjectAttachment](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/libraries/armem_objects/aron/Attachment.xml) | | |
+
+
+### Vision
+
+* Memory Server: [`ArMemVisionMemory` (VisionX)](https://git.h2t.iar.kit.edu/sw/armarx/visionx/-/tree/master/source/VisionX/components/armem/ArMemVisionMemory)
+* Core Segments:
+
+| Core Segment Name | Type | Description | Example Entities |
+|-------------------|--------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------|------------------|
+| ImageRGB | [`ImageRGB`](https://git.h2t.iar.kit.edu/sw/armarx/visionx/-/blob/master/source/VisionX/libraries/ArMem/aron/ImageRGB.xml) | RGB images (mono and stereo) of a camera | image |
+| ImageDepth | [`ImageDepth`](https://git.h2t.iar.kit.edu/sw/armarx/visionx/-/blob/master/source/VisionX/libraries/ArMem/aron/ImageDepth.xml) | Depth images of an RGB-D or stereo camera | image |
+
+
+### Speech
+
+* Memory Server: [`SpeechMemory` (SpeechX)](https://git.h2t.iar.kit.edu/sw/armarx/speechx/-/tree/master/source/SpeechX/components/SpeechMemory)
+* Core Segments:
+
+| Core Segment Name | Type | Description | Example Entities |
+|-----------------------|-----------------|---------------------------------------------------------------------------|------------------|
+| Command | `SpeechCommand` | Language commands from speech (topic `Speech_Commands`). | `command` |
+| TextListenerInterface | `Text` | Data from `TextListenerInterface` topics. (`TextToSpeech`, `RobotAction`) | `text` |
+
+...
+
+
+### Skills
+
+* Memory Server: [`SkillsMemory` (RobotAPI)](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/tree/master/source/RobotAPI/components/armem/server/SkillsMemory)
+* Core Segments:
+
+| Core Segment Name | Type | Description | Example Entities |
+|-------------------|--------------|-------------------------------------|------------------|
+| Statechart | `Transition` | Transitions in ArmarX state charts. | tbd |
+
+
+### Example
+
+An example memory server (alongside a matching client) showing how to implement a standard memory server and test the framework during development.
+
+* Memory Server: [`ExampleMemory` (RobotAPI)](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/tree/master/source/RobotAPI/components/armem/server/ExampleMemory)
+* Core Segments:
+
+| Core Segment Name | Type | Description | Example Entities |
+|-------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------|------------------|
+| ExampleData | [ExampleData](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/blob/master/source/RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.xml) | Some example data. | example |
+
+
+### GeneralPurpose
+
+This memory is meant to allow for a quick-and-dirty integration of your data type in the memory framework. When your development finishes a stable state, consider creating moving the core segment(s) to an existing memory server or creating a new dedicated memory server.
+
+* Memory Server: [`GeneralPurposeMemory` (RobotAPI)](https://git.h2t.iar.kit.edu/sw/armarx/robot-api/-/tree/master/source/RobotAPI/components/armem/server/GeneralPurposeMemory)
+* Core Segments:
+
+| Core Segment Name | Type | Description | Example Entities |
+|----------------------------------------|------|-------------|------------------|
+| None. You can add segments on the fly. | | | |
diff --git a/etc/doxygen/pages/aron/aron.md b/etc/doxygen/pages/aron/aron.md
new file mode 100644
index 0000000000000000000000000000000000000000..66989a0710057023a502cb30fe80b48653f37fbc
--- /dev/null
+++ b/etc/doxygen/pages/aron/aron.md
@@ -0,0 +1,251 @@
+# ArmarX Object Notation (ARON) / Interpretable Data Format (IDF) {#aron}
+
+\tableofcontents
+
+\see
+- \subpage aron-code_generation
+- \subpage aron-visitors
+- \subpage aron-conversion
+
+
+ARON (ArmarX Object Notation) is the ArmarX implementation of variants which can be transferred over the network via
+ZeroC Ice.
+Further, ARON is the data representation used in the \ref memory_system.
+We distinguish between:
+
+- type specification vs. data information
+- data transfer object (in the following called "ARON DTO") vs. its corresponding C++ wrapper
+ (in the following called "ARON object").
+ The DTO specification is done in ice so that every ARON object can be transferred via ice.
+
+
+## ARON Type specification
+
+An ARON type specification defines the static type for an ARON data DTO or an ARON data object. It does not contain any
+data (e.g. an ARON type list only knows the accepted type, not the list members).
+Since ARON supports maybe types (raw ptr, smart ptr, optional), every ARON type DTO contains a special member.
+It can only consist of the following types and information (ARONVariant means any Type):
+
+```cpp
+Object { // dto called armarx::aron::type::dto::AronObject
+ string name;
+ AronVariant extends;
+ map<string, AronVariant> memberTypes;
+}
+Dict { // dto called armarx::aron::type::dto::Dict
+ AronVariant acceptedType;
+}
+List { // dto called armarx::aron::type::dto::List
+ AronVariant acceptedType;
+}
+Pair { // ...
+ AronVariant acceptedType1;
+ AronVariant acceptedType2;
+}
+Tuple {
+ vector<AronVariant> acceptedTypes;
+}
+IntEnum {
+ string name;
+ map<string, int> acceptedValues;
+}
+NDArray {
+ int ndim;
+ type type; // in [uint8, int8, uint16, int16, uint32, int32, float32, float64]
+}
+Matrix {
+ int rows, cols;
+ type type; // in [int16, int32, int64, float32, float64]
+}
+Quaternion {
+ type type; // in [float32, float64]
+}
+Position {
+}
+Orientation {
+}
+Pose {
+}
+Image {
+ pixelType type; // in [rgb24, depth32]
+}
+PointCloud {
+ voxelType type; // in [PointXYZ, PointXYZI, PointXYZL, PointXYZRGB, PointXYZRGBA, PointXYZRGBL, PointXYZHSV]
+}
+Int { // dto called armarx::aron::type::dto::AronInt
+}
+Long { // dto called armarx::aron::type::dto::AronLong
+}
+Float { // dto called armarx::aron::type::dto::AronFloat
+}
+Double { // dto called armarx::aron::type::dto::AronDouble
+}
+String { // dto called armarx::aron::type::dto::AronString
+}
+Bool { // dto called armarx::aron::type::dto::AronBool
+}
+Time { // dto called armarx::aron::type::dto::AronTime
+}
+```
+
+## ARON Data specification
+
+ARON data objects and ARON data DTOs are similar structured to type objects and type DTOs.
+However, there are fewer classes for data objects and data DTOs.
+ARON data is completely decoupled from the static types and contains the data members
+(e.g. an aron data list only contains the list members (as ARONVariants) and not the accepted type):
+
+```cpp
+Dict { // dto called armarx::aron::data::dto::Dict
+ map<string, AronVariant> members;
+}
+List { // dto called armarx::aron::data::dto::List
+ vector<AronVariant> elements;
+}
+NDArray { // ...
+ vector<int> shape;
+ string type;
+ vector<byte> data;
+}
+Int { // dto called armarx::aron::data::dto::AronInt
+ int value;
+}
+Long { // dto called armarx::aron::data::dto::AronLong
+ long value;
+}
+Float { // dto called armarx::aron::data::dto::AronFloat
+ float value;
+}
+Double { // dto called armarx::aron::data::dto::AronDouble
+ double value;
+}
+String { // dto called armarx::aron::data::dto::AronString
+ string value;
+}
+Bool { // dto called armarx::aron::data::dto::AronBool
+ bool value;
+}
+```
+
+\note
+Note that every ARON data object or DTO can be `nullptr`!
+The reason is that if the type supports maybe types and a member is, for example, optional,
+the data must support maybetype as well.
+If a generated ARON class (We come to the code generation later) has an optional member,
+this member will be translated into a NULL ARON object and DTO.
+
+
+## Connection of ARON data and ARON type
+
+ARON data contains fewer classes than ARON type.
+Because ARON data objects and DTOs do not check the type of the members (they can be any aron data (ARONVariant)),
+we can only validate an aron data object or DTO if we have the type information. The following mapping describes,
+how data and types correspond.
+
+| ARON Type | ARON Data |
+|-------------|-----------|
+| Object | Dict |
+| Dict | Dict |
+| List | List |
+| Pair | List |
+| Tuple | List |
+| NDArray | NDArray |
+| Matrix | NDArray |
+| Quaternion | NDArray |
+| Position | NDArray |
+| Orientation | NDArray |
+| Pose | NDArray |
+| Image | NDArray |
+| PointCloud | NDArray |
+| IntEnum | Int |
+| Int | Int |
+| Long | Long |
+| Float | Float |
+| Double | Double |
+| String | String |
+| Bool | Bool |
+| Time | Long |
+
+
+If no type object or DTO is available, we can at least derive some information from the data object
+(e.g. "it is a data dict, so the type cant be list").
+
+To differ between data and type makes it easier to work with these Variants.
+In most cases, the type information is not relevant.
+Then you only have to check for the data type.
+
+
+## ARON Data and Type Objects
+
+As already mentioned, we implemented wrapper classes around the DTO.
+These classes offer convenience methods and conversions and make lots things easier when working directly with ARON.
+These classes can be found at `aron/core/{data,type}/variant/Variant.h`.
+
+The ARON objects have more or less the same structure as the DTOs (see above).
+Everything is stored as a `std::shared_ptr` (e.g. members, elements, acceptedTypes, ...).
+If you want to implement a method which takes any ARON data object as input,
+you can use the base class of every ARON data object:
+```cpp
+void myFancyMethod(const armarx::aron::data::VariantPtr& variant);
+```
+
+If you want to check, what a variant really is, you can use the descriptor of the object:
+```cpp
+armarx::aron::data::VariantPtr variant; // <-- the variant
+auto desc = variant->getDescriptor();
+switch(desc)
+{
+ case armarx::aron::data::Descriptor::eDict: ...
+ case armarx::aron::data::Descriptor::eList: ...
+ ...
+}
+```
+
+If you have a DTO, you do not know the exact type,
+but you want to have a ARON object you can make use of the static construction methods:
+```cpp
+armarx::aron::data::dto::GenericData dto; // <-- the DTO variant
+auto aron = armarx::aron::data::Variant::FromAronDTO(dto);
+```
+
+If you know the type, you can use the constructor the specific ARON object.
+
+### Example to Create an ARON Data Dict from Scratch
+
+Goal: Have an ARON dict with a list as member "the_list" and some values in it.
+Then echo the members and their descriptor as a string.
+
+```cpp
+using namespace armarx;
+
+// setup aron object
+auto dict = std::make_shared<aron::data::Dict>();
+{
+ auto list = std::make_shared<aron::data::List>();
+ for (unsigned int i = 0; i < 10; ++i)
+ {
+ list->addElement(std::make_shared<aron::data::Int>(i));
+ }
+ dict->addElement("the_list", list);
+}
+
+// echo
+auto listVariant = dict->getElement("the_list"); // will return a aron::data::VariantPtr
+auto list = aron::data::List::DynamicCastAndCheck(listVariant); // cast and check whether the cast was successful
+for (const auto& intVar : list->getElements())
+{
+ auto i = aron::data::Int::DynamicCastAndCheck(intVar);
+ std::cout << "The value is: " << i->getValue() << std::endl;
+ std::cout << "The descriptor is: " << aron::data::defaultconversion::string::Descriptor2String.at(i->getDescriptor()) << std::endl;
+}
+```
+
+Please note that we might add more methods or make the current ones more flexible (e.g. such as nlohmann::json).
+
+## Lessons Learned
+
+- There is a difference between ARON data and ARON type
+- There is a difference between ARON objects and ARON DTOs
+- How are ARON DTOs structured
+- How do ARON data and ARON types correspond
+- How to use the ARON objects
diff --git a/docs/aron/code_generation/README.md b/etc/doxygen/pages/aron/code_generation.md
similarity index 83%
rename from docs/aron/code_generation/README.md
rename to etc/doxygen/pages/aron/code_generation.md
index a48594a45348e392a3d074ccb752c94c7ead9b2d..b00af60dedf28ffc3a793b13c430d0b684132ab1 100644
--- a/docs/aron/code_generation/README.md
+++ b/etc/doxygen/pages/aron/code_generation.md
@@ -1,21 +1,31 @@
-[[_TOC_]]
+# ARON Code Generation {#aron-code_generation}
-# Aron Type Reading
+\tableofcontents
-As already mention in the introduction, aron supports code generation in order to create a c++ (other languages also possible) class with plain c++ members which you can use for convenience. Further, the generated class offers methods to convert itself to a Aron object and to set the members from a Aron object. Code generation only makes sense for Aron data, however we need an Aron type specification in order to generate the class.
+## Aron Type Reading
+
+As already mention in the introduction, aron supports code generation in order to create a C++
+(other languages also possible) class with plain C++ members which you can use for convenience.
+Further, the generated class offers methods to convert itself to an Aron object and to set the members from an Aron
+object.
+Code generation only makes sense for Aron data,
+however we need an Aron type specification in order to generate the class.
In the following we will describe how to specify Aron types in XML and how the generated code looks like.
-## XML type description and creation
+### XML type description and creation
-In order to make Aron generate a c++ class for you, you first have to tell the program how the object should look like. Second you need to add the file to cmake in order to create the code generation target.
+In order to make Aron generate a C++ class for you, you first have to tell the program how the object should look like.
+Second you need to add the file to cmake in order to create the code generation target.
-### XML type description
+#### XML type description
Consider you want to use a data type called `MyData` (in the namespace `armarx::mydata`).
-* Choose a library in an ArmarX package where your data type will be defined (or create a new one). Here, we will call it `MyDataLib`.
-* In the directory `MyDataLib/`, create a directory `aron/` (if necessary). Inside, add a file `MyData.xml`. The directory structure should now look like this:
+* Choose a library in an ArmarX package where your data type will be defined (or create a new one).
+ Here, we will call it `MyDataLib`.
+* In the directory `MyDataLib/`, create a directory `aron/` (if necessary). Inside, add a file `MyData.xml`.
+ The directory structure should now look like this:
```
.../libraries/
@@ -45,7 +55,13 @@ Then start defining your data in ARON. A hello world example could look like thi
</AronTypeDefinition>
```
-Every type specification must have the top-level-tag `AronTypeDefinition` and must at least define one type in `GenerateTypes`. In the `GenerateTypes`-tag you can add as many `Object` or `IntEnum` definitions as you want. `<Object>` defines a new ARON object type (generating a C++ class) containing a number of children. Inside the `Object`-tag you can add as many members as you want through the `ObjectChild`-tag (generating a public member variable of the generated C++ class, with `key` specifying the member's name). If a member should be e.g. optional you have to add the attribute `optional="true"` to the member:
+Every type specification must have the top-level tag `AronTypeDefinition`
+and must at least define one type in `GenerateTypes`.
+In the `GenerateTypes` tag you can add as many `Object` or `IntEnum` definitions as you want.
+`<Object>` defines a new ARON object type (generating a C++ class) containing a number of children.
+Inside the `Object`-tag you can add as many members as you want through the `ObjectChild`-tag
+(generating a public member variable of the generated C++ class, with `key` specifying the member's name).
+If a member should be e.g. optional you have to add the attribute `optional="true"` to the member:
```xml
<ObjectChild key="helloWorld">
<String optional="true" />
@@ -54,34 +70,29 @@ Every type specification must have the top-level-tag `AronTypeDefinition` and mu
The same way you can define `raw_ptr`, `shared_ptr` or `unique_ptr` members.
Conventions:
-- Put your ARON object type into a `arondto::` namespace inside your usual namespace (e.g., `armarx::mydata::arondto::MyData`), where "DTO" stands for "Data Transfer Object". This way, you can define or use a custom or existing C++ type with more methods/intelligence in the usual namespace in your business logic code (e.g. `armarx::mydata::MyData`).
+- Put your ARON object type into a `arondto::` namespace inside your usual namespace
+ (e.g., `armarx::mydata::arondto::MyData`), where "DTO" stands for "Data Transfer Object".
+ This way, you can define or use a custom or existing C++ type with more methods/intelligence in the usual namespace
+ in your business logic code (e.g. `armarx::mydata::MyData`).
-If you want to use a definition of another Aron file, you can include the file using the `AronIncludes`-section. Simply add the files you want to include using a `include`-tag:
+If you want to use a definition of another Aron file, you can include the file using the `AronIncludes` section.
+Simply add the files you want to include using a `include` tag:
```xml
- <AronIncludes>
- <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.xml>" autoinclude="true" />
- ...
- </AronIncludes>
+<AronIncludes>
+ <Include include="RobotAPI/libraries/aron/common/aron/PackagePath.xml" />
+ ...
+</AronIncludes>
```
-The option `autoinclude` automatically adds the generated class of the Aron xml file to the c++ class.
After that, you can use the type definitions in your current xml file (you must specify the full namespace):
```xml
- <ObjectChild key="referenceToMemoryID">
- <armarx::arondto::PackagePath />
- </ObjectChild>
+<ObjectChild key="referenceToMemoryID">
+ <armarx::arondto::PackagePath />
+</ObjectChild>
```
-(At the moment), you need to specify `<CodeIncludes>` to Eigen to use `Pose`, `Position` and `Orientation` (which are translated to `Eigen::Matrix4f`, `Eigen::Vector3f`, and `Eigen::Quaternionf` in C++). In that section, you can add includes in your target language (right now only C++), e.g.:
-```xml
- <CodeIncludes>
- <Include include="<Eigen/Core>" />
- </CodeIncludes>
-```
-
-If you do not need the `CodeIncludes` and `AronIncludes` you can remove these tags.
-### Examples
+#### Examples
In the following we define a class that uses all more complex types once:
```xml
@@ -158,9 +169,10 @@ In the following we define a class that uses all more complex types once:
</AronTypeDefinition>
```
-### CMake specification
+#### CMake Specification
-In the `CMakeLists.txt`, add or extend after the definition of the target (e.g. through add_library or add_component):
+In the `CMakeLists.txt`, add or extend after the definition of the target
+(e.g. through `add_library` or `add_component`):
```cmake
armarx_enable_aron_file_generation_for_target(
@@ -171,9 +183,10 @@ armarx_enable_aron_file_generation_for_target(
)
```
-### Important changes
+#### Important changes
-- I changed the xml type reader so that embedded classes are not allowed anymore! Before that you were able to define a new class inside an existing one, e.g.:
+- I changed the xml type reader so that embedded classes are not allowed anymore!
+ Before that you were able to define a new class inside an existing one, e.g.:
```xml
<!-- MyData containing a helloWorld member -->
<?xml version="1.0" encoding="UTF-8" ?>
@@ -191,7 +204,8 @@ armarx_enable_aron_file_generation_for_target(
```
This is not supported anymore!
-## Full example
+
+### Full example
Say you have the following Aron XML type description:
```xml
@@ -223,7 +237,7 @@ Say you have the following Aron XML type description:
</AronTypeDefinition>
```
-The generated c++ file looks like:
+The generated C++ file looks like:
```cpp
#pragma once
@@ -651,29 +665,45 @@ namespace armarx
```
-As you see, the code generation creates a class for the `NaturalIKControlMode` enum. This special class provides convenience methods compared to normal c++ enums.
+As you see, the code generation creates a class for the `NaturalIKControlMode` enum.
+This special class provides convenience methods compared to normal C++ enums.
Further, it creates a class for the `NaturalIKResult` object.
-Both, the enum and the object provide methods for reading and writing. The `read` method is used to parse a Aron object in an arbitrary representation (e.g. Aron variant or nlohmann::json) and to set the c++ members to the same values as the Aron object. To do so it uses a ReaderInterface implementation (See [Converter](Aron/Converter)). The `write` method is used to create an Aron object in an arbitrary representation with the same values as the c++ object. This method uses a WriterInterface implementation.
-
-For convenience, the `toAron()` and `fromAron()` methods, which internally use the `read` and `write` methods, are created. To get the structure of the c++ class as an Aron type object you can use the static `toAronType()` method which internally uses the `writeType` method.
+Both, the enum and the object provide methods for reading and writing.
+The `read` method is used to parse a Aron object in an arbitrary representation
+(e.g. Aron variant or nlohmann::json) and to set the C++ members to the same values as the Aron object.
+To do so it uses a ReaderInterface implementation (see \ref aron-conversion).
+The `write` method is used to create an Aron object in an arbitrary representation with the same values as the C++
+object.
+This method uses a WriterInterface implementation.
-Futher, the code generation creates methods to compare two classes with each other (right now only operator==).
+For convenience, the `toAron()` and `fromAron()` methods, which internally use the `read` and `write` methods,
+are created.
+To get the structure of the C++ class as an Aron type object you can use the static `toAronType()` method which
+internally uses the `writeType` method.
-This means, when using Aron, you don't need to mess around with the Aron DTOs and Aron objects, you only have to set and use plain c++ members of a generated class!
+Further, the code generation creates methods to compare two classes with each other (right now only operator==).
+This means, when using Aron, you don't need to mess around with the Aron DTOs and Aron objects, you only have to set
+and use plain C++ members of a generated class!
+## (Optional) Add conversions to existing (potentially more intelligent) C++ types
-# (Optional) Add conversions to existing (potentially more intelligent) C++ types
+Consider the case of `simox::OrientedBoxf` and `simox::arondto::OrientedBox`. The former is the business object (BO),
+used in daily business logic code (as it offers useful methods to construct and manipulate it).
+The latter is a mere data storage used to transfer the data, which is called a data transfer object (DTO).
-Consider the case of `simox::OrientedBoxf` and `simox::arondto::OrientedBox`. The former is the business object (BO), used in daily business logic code (as it offers useful methods to construct and manipulate it). The latter is a mere data storage used to transfer the data, which is called a data transfer object (DTO).
-
-Therefore, you usually want to convert a `simox::arondto::OrientedBox` to a `simox::OrientedBoxf` as soon as you want to do things other than passing it around over network (e.g., at the beginning of a component method or after reading it from the working memory). Likewise, you want to convert a `simox::OrientedBoxf` to a `simox::arondto::OrientedBox` when you send the information over network or store it in the memory.
+Therefore, you usually want to convert a `simox::arondto::OrientedBox` to a `simox::OrientedBoxf` as soon as you want
+to do things other than passing it around over network (e.g., at the beginning of a component method or after reading
+it from the working memory).
+Likewise, you want to convert a `simox::OrientedBoxf` to a `simox::arondto::OrientedBox` when you send the information
+over network or store it in the memory.
To allow this conversion, follow these steps:
-- In your library, add a file pair `aron_conversions.{h, cpp}` next to your `aron/` directory and add them to the `CMakeLists.txt`
+- In your library, add a file pair `aron_conversions.{h, cpp}` next to your `aron/` directory and add them to
+ the `CMakeLists.txt`
- In the header, declare functions following this form:
```cpp
@@ -723,11 +753,14 @@ void simox::toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo)
}
```
-Note:
+\note
+The implementation depends on your data.
+It might be as simple as copying all members, or require more complex conversions.
+Especially, you might use `fromAron()` for `toAron()` for other ARON object types.
+
-- The implementation depends on your data. It might be as simple as copying all members, or require more complex conversions. Especially, you might use `fromAron()` for `toAron()` for other ARON object types.
+## Lessons learned
-# Lessons learned
- How to create a XML file with a valid Aron specification
- How to add this file to CMake
-- How to use the generated class and how to convert it to an Aron object (e.g. for sending it to the memory)
\ No newline at end of file
+- How to use the generated class and how to convert it to an Aron object (e.g. for sending it to the memory)
diff --git a/etc/doxygen/pages/aron/conversion.md b/etc/doxygen/pages/aron/conversion.md
new file mode 100644
index 0000000000000000000000000000000000000000..ba51d4c7aaf223f687a26af117c6c6ebe21f0eb1
--- /dev/null
+++ b/etc/doxygen/pages/aron/conversion.md
@@ -0,0 +1,47 @@
+# ARON Readers, Writers and Conversion {#aron-conversion}
+
+\ref aron "ARON" offers ways to simply convert any aron object into another representation
+(e.g. from variant to `nlohmann::json` or vice versa).
+To do so, it makes use of specific readers and writers.
+In the following we will only describe the readers, writers and converters for ARON data,
+not for types (but the principle is the same).
+
+
+## Readers
+
+An ARON reader is used to get information of an ARON object in a specific representation.
+Assume you have an `nlohmann::json` aron object which contains some information
+(for example, it is a dict, containing some members, ...).
+We need this information to create another object in another representation with the same content.
+
+To do so, you only have to implement the `armarx::aron::data::ReaderInterface` class.
+It needs one template parameter for your InputType (e.g. here const `nlohmann::json`).
+
+The interface provides the following pure virtual methods:
+```cpp
+virtual void readList(InputType& input, std::vector<InputTypeNonConst>& elements) = 0;
+virtual void readDict(InputType& input, std::map<std::string, InputTypeNonConst>& elements) = 0;
+virtual void readNDArray(InputType& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data) = 0;
+virtual void readInt(InputType& input, int& i) = 0;
+virtual void readLong(InputType& input, long& i) = 0;
+virtual void readFloat(InputType& input, float& i) = 0;
+virtual void readDouble(InputType& input, double& i) = 0;
+virtual void readString(InputType& input, std::string& s) = 0;
+virtual void readBool(InputType& input, bool& i) = 0;
+```
+
+You have to implement the function so that the non-const arguments of the method will be filled with the values of the
+input.
+For example, the implementation of the readString method for the `nlohmann::json` reader would be:
+```cpp
+void NlohmannJSONReader::readString(const nlohmann::json& input, std::string& i)
+{
+ if (input[rw::json::constantes::TYPE_SLUG] != expectedType)
+ {
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Wrong type in json encountered.", input[rw::json::constantes::TYPE_SLUG], expectedType);
+ }
+ i = input[rw::json::constantes::VALUE_SLUG];
+}
+```
+
+Of course, the way to get the member depend on the way how to construct (writer) the `nlohmann::json`.
diff --git a/etc/doxygen/pages/aron/visitors.md b/etc/doxygen/pages/aron/visitors.md
new file mode 100644
index 0000000000000000000000000000000000000000..1cafd35c8ee4b87152c180f144e9c3b6b5944445
--- /dev/null
+++ b/etc/doxygen/pages/aron/visitors.md
@@ -0,0 +1,120 @@
+# ARON Visitors {#aron-visitors}
+
+\tableofcontents
+
+Visitors are a useful tool to define specific methods for specific types
+without specifying the `switch(): case: ...` again and again.
+Further, recursive visitors offer a convenient way to iterate through the tree-like structure of any Aron object.
+(Please note that right now the implementation does not check for cycles!).
+Again, we will only look at data visitors since the implementation of type visitors is more or less similar.
+
+
+## Non-recursive Visitors
+
+First, there are "normal" visitors.
+They are used to get rid of the big switch-case statement when getting an Aron object with unknown type.
+Further, these visitors can be used to convert an Aron object from one representation into another (see \ref aron-conversion).
+The `armarx::aron::data::Visitor` base class offers the following pure virtual methods which you have to implement:
+
+```cpp
+virtual data::Descriptor getDescriptor(Input&) = 0;
+virtual void visitDict(Input&) {};
+virtual void visitList(Input&) {};
+virtual void visitNDArray(Input&) {};
+virtual void visitInt(Input&) {};
+virtual void visitLong(Input&) {};
+virtual void visitFloat(Input&) {};
+virtual void visitDouble(Input&) {};
+virtual void visitBool(Input&) {};
+virtual void visitString(Input&) {};
+virtual void visitUnknown(Input&) { throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor."); }
+```
+
+`Input` is a template parameter, defining the Input-type
+(e.g. `const armarx::aron::data::VariantPtr` for a `VariantVisitor` Implementation).
+
+The method `getDescriptor(Input&)` is used for the switch case internally.
+
+As you can see the `VariantVisitor` always gets a generic variant as input in each function.
+This means that, although the Visitor guarantees that the input is correct, you have to cast the class to the correct
+type.
+For widely used Visitors, the Aron library offers convenience implementations
+(e.g. for Variants and for nlohmann::json inputs).
+The convenience class for Variants additionally adds overloads where the input is already cast correctly
+(you can decide which method you want to override).
+
+To use a Visitor, you have to use the function
+```cpp
+void visit(VisitorImplementation& v, typename VisitorImplementation::Input& o);
+```
+which takes the visitor and the input as arguments and does the switch-case.
+
+
+## Recursive Visitors
+
+Recursive visitors are similar to normal visitors, but they continue "visiting" until the object is fully visited.
+For containers (e.g. dicts, lists) they offer two methods each:
+```cpp
+virtual void visitDictOnEnter(Input& element) {};
+virtual void visitDictOnExit(Input& element) {};
+virtual void visitListOnEnter(Input& element) {};
+virtual void visitListOnExit(Input& element) {};
+```
+The `*OnEnter` method is called before iterating over all children of the container.
+The `*OnExit` is called after the iteration.
+
+Further, in order to get the elements of a container, you must implement the methods:
+```cpp
+virtual MapElements getDictElements(Input&) = 0;
+virtual ListElements getListElements(Input&) = 0;
+```
+Again, Input can be any aron object (e.g. Variant or nlohmann::json).
+
+As for normal visitors, the Aron library offers convenience implementations for Variants and nlohmann::json inputs.
+
+To use the RecursiveVisitor, you have to use the function
+```cpp
+void visitRecursive(RecursiveVisitorImplementation& v, typename RecursiveVisitorImplementation::Input& o);
+```
+
+### Example
+
+Assume you want to count how many objects of each kind you have in a Variant.
+```cpp
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+class CountingRecursiveVisitor : public RecursiveConstVariantVisitor
+{
+public:
+ int dicts = 0;
+ int lists = 0;
+ int ndarrays = 0;
+ int ints = 0;
+ int floats = 0;
+ int longs = 0;
+ int doubles = 0;
+ int strings = 0;
+ int bools = 0;
+public:
+ void visitAronVariantOnEnter(const data::DictPtr&) override { dicts++; }
+ void visitAronVariantOnEnter(const data::ListPtr&) override { lists++; }
+ void visitAronVariant(const data::NDArrayPtr&) override { ndarrays++; }
+ void visitAronVariant(const data::IntPtr&) override { ints++; }
+ void visitAronVariant(const data::LongPtr&) override { longs++; }
+ void visitAronVariant(const data::FloatPtr&) override { floats++; }
+ void visitAronVariant(const data::DoublePtr&) override { doubles++; }
+ void visitAronVariant(const data::BoolPtr&) override { strings++; }
+ void visitAronVariant(const data::StringPtr&) override { bools++; }
+};
+
+void countHowMany(const aron::data::VariantPtr& aron)
+{
+ CountingRecursiveVisitor visitor;
+ aron::data::visitRecursive(visitor, aron);
+
+ std::cout << "The aron has " << visitor.dicts << " dicts." << std::endl;
+}
+```
+
+## Typed Visitors
+
+# Lessons Learned
diff --git a/scenarios/RobotHealthTest/config/RobotHealthApp.cfg b/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
index ebffea693fd85455a4f16d02ab490a8cad798bc3..6f9edad535a2e6b28b385b5b9db1c096931be675 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
@@ -18,6 +18,15 @@
# ArmarX.ApplicationName = ""
+# ArmarX.AutodiscoverPackages: If enabled, will discover all ArmarX packages based on the environment variables. Otherwise, the `DefaultPackages` and `AdditionalPackages` properties are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AutodiscoverPackages = true
+
+
# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
diff --git a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
index 0f14e83d287e44accc961037ba069f3ea77dce46..8b0ec175e86890f817e9e651b22133cdfbd37453 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
@@ -18,6 +18,15 @@
# ArmarX.ApplicationName = ""
+# ArmarX.AutodiscoverPackages: If enabled, will discover all ArmarX packages based on the environment variables. Otherwise, the `DefaultPackages` and `AdditionalPackages` properties are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AutodiscoverPackages = true
+
+
# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
diff --git a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
index e157c2704c9191a48e3daccf14a545f125e72c51..a19fd84b5a9f5c2eeaf16b8e933a98d327496607 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
@@ -18,6 +18,15 @@
# ArmarX.ApplicationName = ""
+# ArmarX.AutodiscoverPackages: If enabled, will discover all ArmarX packages based on the environment variables. Otherwise, the `DefaultPackages` and `AdditionalPackages` properties are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AutodiscoverPackages = true
+
+
# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
diff --git a/scenarios/memory_to_debug_observer/config/MemoryToDebugObserver.cfg b/scenarios/memory_to_debug_observer/config/MemoryToDebugObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b55058e92b04b70190bedc2ecfcd4a675a843557
--- /dev/null
+++ b/scenarios/memory_to_debug_observer/config/MemoryToDebugObserver.cfg
@@ -0,0 +1,247 @@
+# ==================================================================
+# MemoryToDebugObserver 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.AutodiscoverPackages: If enabled, will discover all ArmarX packages based on the environment variables. Otherwise, the `DefaultPackages` and `AdditionalPackages` properties are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.AutodiscoverPackages = true
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - 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.MemoryToDebugObserver.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.MemoryToDebugObserver.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.MemoryToDebugObserver.EnableProfiling = false
+
+
+# ArmarX.MemoryToDebugObserver.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.MemoryToDebugObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryToDebugObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.ObjectName = ""
+
+
+# ArmarX.MemoryToDebugObserver.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.MemoryToDebugObserver.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MemoryToDebugObserver.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.MemoryToDebugObserver.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.MemoryToDebugObserver.p.memoryToDebugObserverJson: Configuration of MemoryToDebugObserver in JSON format.
+# Attributes:
+# - Default: {"plottedValues":[{"aronPath":["joints","position","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","positionTarget","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","relativePosition","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocityTarget","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","torque","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","motorCurrent","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","currentTarget","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","position","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","positionTarget","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","relativePosition","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocityTarget","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","torque","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","motorCurrent","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","currentTarget","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","position","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","positionTarget","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","relativePosition","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocityTarget","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","torque","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","motorCurrent","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","currentTarget","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]}]}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.p.memoryToDebugObserverJson = {"plottedValues":[{"aronPath":["joints","position","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","positionTarget","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","relativePosition","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocityTarget","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","torque","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","motorCurrent","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","currentTarget","Neck_1_Yaw"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_1_Yaw.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","position","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","positionTarget","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","relativePosition","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocityTarget","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","torque","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","motorCurrent","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","currentTarget","Neck_2_Hemisphere_A"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_2_Hemisphere_A.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","position","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","positionTarget","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","relativePosition","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocity","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","velocityTarget","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","torque","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","motorCurrent","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["joints","currentTarget","Neck_3_Hemisphere_B"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]},{"aronPath":["extraLongs","Neck_3_Hemisphere_B.absoluteEncoderTicks"],"entityID":["RobotState","Proprioception","Armar7","Armar7"]}]}
+
+
+# ArmarX.MemoryToDebugObserver.p.pollFrequencyHz:
+# Attributes:
+# - Default: 30
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryToDebugObserver.p.pollFrequencyHz = 30
+
+
+# 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/memory_to_debug_observer/config/global.cfg b/scenarios/memory_to_debug_observer/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..973c65ddcf2132916ac3d1a2305dff27851fe5aa
--- /dev/null
+++ b/scenarios/memory_to_debug_observer/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario memory_to_debug_observer
+# ==================================================================
+
diff --git a/scenarios/memory_to_debug_observer/memory_to_debug_observer.scx b/scenarios/memory_to_debug_observer/memory_to_debug_observer.scx
new file mode 100644
index 0000000000000000000000000000000000000000..c3c1418d716246324848bc989aa5dbf06a9ce56c
--- /dev/null
+++ b/scenarios/memory_to_debug_observer/memory_to_debug_observer.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="memory_to_debug_observer" creation="1970-01-01.01:00:00" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="MemoryToDebugObserver" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index e5762482e69b710fa361bf26dd5705849b4b0efb..35f54669285b085c2c14fb7929585f2bba6bc0c0 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -176,7 +176,7 @@ namespace armarx::articulated_object
const float t = static_cast<float>((now - start).toSecondsDouble());
// move joints at certain frequency
- const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
+ const float k = (1 + std::sin(t / (M_2_PI))) / 2; // in [0,1]
auto jointValues = articulatedObject->getJointValues();
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index 61497d62d399adf6f1c89a0a0de1c544a902ec51..5d077bca27a123793f5bd34023d9a6878dab9ae6 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -260,7 +260,7 @@ 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 Eigen::Vector2f orthogonal_normalized = Eigen::Vector2f(Eigen::Rotation2Df(M_PI_2) * diff).normalized();
const float sample_step = 5; // in [mm], sample step size towards goal.
const float distance_to_goal = diff.norm() + safetyRadius;
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/Editor.h b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
index f1a6ebc2a168de7938e552b44f1d2144a915dd35..e94e1a7cc2aa544650681583ca6a1aae11f9bef6 100644
--- a/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
+++ b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
@@ -86,7 +86,7 @@ namespace armarx
struct Placeholder
{
simox::OrientedBoxf box;
- Eigen::Matrix4f transform;
+ Eigen::Matrix4f transform{Eigen::Matrix4f::Identity()};
};
void addPlaceholder(simox::OrientedBoxf box);
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index a17005e6d573ff98accd7b40e9c45376ed8e85c4..d646b7ae3a9d5a6224b25470d10bba6ecff0fadc 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -37,6 +37,8 @@
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
+#include <RobotAPI/components/units/RobotUnit/util/NonRtTiming.h>
using namespace Eigen;
@@ -296,10 +298,10 @@ namespace armarx
{
if (timestamp <= 0)
{
- timestamp = IceUtil::Time::now().toMicroSeconds();
+ timestamp = armarx::rtNow().toMicroSeconds();
}
- IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
+ IceUtil::Time time = mapRtTimestampToNonRtTimestamp(IceUtil::Time::microSeconds(timestamp));
ARMARX_DEBUG << deactivateSpam(1) << "Got new jointangles: " << jointAngles
<< " from timestamp " << time.toDateTime()
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
index c9afa372dc20c08fc6e80ce2666214618d366286..c944eda90774140a09c4b2376bcfa734b000817f 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -86,7 +86,7 @@ namespace armarx::armem
}
// is this corect??
- prop.platform.acceleration = Eigen::Vector3f();
+ prop.platform.acceleration = Eigen::Vector3f::Zero();
prop.platform.relativePosition = Eigen::Vector3f(update.platformPose.x, // this should be globasl AFAIK
update.platformPose.y,
update.platformPose.rotationAroundZ);
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index 9e9e2da67d9f2b3c1ed686fcf71f15c0c7e6ad8c..284ce7a27afd70e99981da558c6c1e0c6be234c3 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1,5 +1,6 @@
add_subdirectory(ExampleMemoryClient)
add_subdirectory(GraspProviderExample)
+add_subdirectory(MemoryToDebugObserver)
add_subdirectory(ObjectInstanceToIndex)
add_subdirectory(RobotStatePredictionClientExample)
add_subdirectory(SimpleVirtualRobot)
diff --git a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/CMakeLists.txt b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..797cb6b4789199a86085ccec3e47c367e9fae462
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/CMakeLists.txt
@@ -0,0 +1,28 @@
+armarx_component_set_name(MemoryToDebugObserver)
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ ArmarXCoreComponentPlugins
+ ArmarXGuiComponentPlugins
+ RobotAPICore RobotAPIInterfaces
+ armem
+)
+
+set(SOURCES
+ Component.cpp
+)
+
+set(HEADERS
+ Component.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+armarx_generate_and_add_component_executable(
+ COMPONENT_INCLUDE
+ "${CMAKE_CURRENT_LIST_DIR}/Component.h"
+ COMPONENT_NAMESPACE
+ armarx::memory_to_debug_observer
+ COMPONENT_CLASS_NAME
+ Component
+)
diff --git a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2217e1d8137ab275a387b917ea623f6d59b36518
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.cpp
@@ -0,0 +1,168 @@
+/*
+ * 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::MemoryToDebugObserver
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Component.h"
+
+#include <SimoxUtility/json.h>
+
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+namespace armarx::memory_to_debug_observer
+{
+
+ armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ {
+ armem::MemoryID robotEntityId{"RobotState", "Proprioception", "Armar7", "Armar7"};
+
+ std::vector<std::string> jointNames{
+ "Neck_1_Yaw",
+ "Neck_2_Hemisphere_A",
+ "Neck_3_Hemisphere_B",
+ };
+
+ for (const std::string& jointName : jointNames)
+ {
+ std::vector<std::string> attributes{
+ "position",
+ "positionTarget",
+ "relativePosition",
+ "velocity",
+ "velocityTarget",
+ "torque",
+ "motorCurrent",
+ "currentTarget",
+ };
+ for (const std::string& attribute : attributes)
+ {
+ properties.memoryToDebugObserver.plottedValues.push_back({
+ .entityID = robotEntityId,
+ .aronPath = {{"joints", attribute, jointName}},
+ });
+ properties.memoryToDebugObserver.plottedValues.push_back({
+ .entityID = robotEntityId,
+ .aronPath = {{"extraLongs", jointName + ".absoluteEncoderTicks"}},
+ });
+ }
+ }
+
+ simox::json::json j = properties.memoryToDebugObserver;
+ properties.memoryToDebugObserverJson = j.dump();
+ }
+ defs->optional(properties.memoryToDebugObserverJson,
+ "p.memoryToDebugObserverJson",
+ "Configuration of MemoryToDebugObserver in JSON format.");
+
+ defs->optional(properties.pollFrequencyHz, "p.pollFrequencyHz");
+
+ return defs;
+ }
+
+ Component::Component()
+ {
+ }
+
+ std::string
+ Component::getDefaultName() const
+ {
+ return "MemoryToDebugObserver";
+ }
+
+ void
+ Component::onInitComponent()
+ {
+ DebugObserverComponentPluginUser::setDebugObserverBatchModeEnabled(true);
+
+ {
+ simox::json::json j = simox::json::json::parse(properties.memoryToDebugObserverJson);
+ j.get_to(properties.memoryToDebugObserver);
+ }
+ }
+
+ void
+ Component::onConnectComponent()
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+
+ task = new RunningTask<Component>(this, &Component::runLoop);
+ task->start();
+ }
+
+ void
+ Component::onDisconnectComponent()
+ {
+ task->stop();
+ task = nullptr;
+ }
+
+ void
+ Component::onExitComponent()
+ {
+ }
+
+ void
+ Component::runLoop()
+ {
+ armem::client::util::MemoryToDebugObserver::Services services{
+ .memoryNameSystem = memoryNameSystem(),
+ .debugObserver = getDebugObserverComponentPlugin(),
+ };
+ armem::client::util::MemoryToDebugObserver memoryToDebugObserver{
+ properties.memoryToDebugObserver, services};
+
+ Frequency frequency = Frequency::Hertz(properties.pollFrequencyHz);
+ Metronome metronome(frequency);
+ while (task and not task->isStopped())
+ {
+ memoryToDebugObserver.pollOnce();
+ metronome.waitForNextTick();
+ }
+ }
+
+ void
+ Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ tab.group = GroupBox();
+ tab.group.setLabel("Todo ...");
+
+ VBoxLayout root = {tab.group, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+ void
+ Component::RemoteGui_update()
+ {
+ if (tab.rebuild.exchange(false))
+ {
+ createRemoteGuiTab();
+ }
+ }
+
+} // namespace armarx::memory_to_debug_observer
diff --git a/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h
new file mode 100644
index 0000000000000000000000000000000000000000..7ce514c96ec2a4554e04f81244a0c6e48a58f603
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/MemoryToDebugObserver/Component.h
@@ -0,0 +1,105 @@
+/*
+ * 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::MemoryToDebugObserver
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <ArmarXCore/util/tasks.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h>
+
+namespace armarx::memory_to_debug_observer
+{
+
+ /**
+ * @defgroup Component-MemoryToDebugObserver MemoryToDebugObserver
+ * @ingroup RobotAPI-Components
+ *
+ * Transfers data from the memory system to the \ref Component-DebugObserver "Debug Observer",
+ * allowing to visualize them in the \ref ArmarXGui-GuiPlugins-PlotterPlugin "Live Plotter".
+ *
+ * @see armarx::armem::client::util::MemoryToDebugObserver for the business logic
+ * implementation.
+ *
+ * @class Component
+ * @ingroup Component-MemoryToDebugObserver
+ * @brief Implementation of \ref Component-MemoryToDebugObserver.
+ *
+ * @see armarx::armem::client::util::MemoryToDebugObserver for the business logic
+ * implementation.
+ */
+ class Component :
+ virtual public armarx::Component,
+ virtual public armarx::armem::ClientPluginUser,
+ virtual public armarx::DebugObserverComponentPluginUser,
+ virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ {
+ public:
+ Component();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ // LightweightRemoteGuiComponentPluginUser interface
+ public:
+ void createRemoteGuiTab();
+ void RemoteGui_update() override;
+
+
+ protected:
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+ void runLoop();
+
+
+ private:
+ struct Properties
+ {
+ armem::client::util::MemoryToDebugObserver::Properties memoryToDebugObserver;
+ std::string memoryToDebugObserverJson;
+
+ float pollFrequencyHz = 30;
+ };
+
+ Properties properties;
+
+ armarx::RunningTask<Component>::pointer_type task;
+
+ struct RemoteGuiTab : RemoteGui::Client::Tab
+ {
+ std::atomic_bool rebuild = false;
+
+ RemoteGui::Client::GroupBox group;
+ };
+
+ RemoteGuiTab tab;
+ };
+} // namespace armarx::memory_to_debug_observer
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt
index a87ba755cb2744ece8508856d8d80e5ac8251744..678a03c3aaaa372f5f0fd9bdd28a14e29d9c9b2d 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt
@@ -15,6 +15,7 @@ armarx_add_component(
VirtualRobotWriterExample.h
)
+
# Generate the application
armarx_generate_and_add_component_executable(
# If your component is not defined in ::armarx, specify its namespace here:
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
index d2a7eff51a3aaf9155ddb8915f61aca9a26c7535..675d2dae9e79c28d5a2fe2d8ce9630963d878dc0 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
@@ -133,7 +133,7 @@ namespace armarx::virtual_robot_writer_example
const float t = float((now - start).toSecondsDouble());
// move joints at certain frequency
- const float m = (1 + std::sin(t / (M_2_PIf32 * 10))) / 2; // in [0,1]
+ const float m = (1 + std::sin(t / (M_2_PI * 10))) / 2; // in [0,1]
auto jointValues = robot->getJointValues();
for (auto& [k, v] : jointValues)
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
index edf918402a4c8ed1bbe510ab553d6dc568540dd5..d3e66e838eeae19ae089f4af76d4d9d4f9cfc33b 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
@@ -29,7 +29,8 @@ namespace armarx::skills::provider
.rootProfileDefaults = root_profile_params.toAron(),
.timeout = armarx::core::time::Duration::MilliSeconds(1000),
.parametersType =
- armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
+ armarx::skills::Example::HelloWorldAcceptedType::ToAronType(),
+ .resultType = armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
}
Skill::MainResult
@@ -42,6 +43,6 @@ namespace armarx::skills::provider
.dump(2)
<< "\n"
<< "(executed at: " << IceUtil::Time::now() << ")";
- return {TerminatedSkillStatus::Succeeded, nullptr};
+ return {TerminatedSkillStatus::Succeeded, in.parameters.toAron()};
}
} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index f6710bc22978c2816aa8056da5795b0a84b5578a..25bdbe36948d1fecc27c6e1e7b963652e412756f 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -74,6 +74,7 @@ set(LIB_FILES
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+target_compile_options("${LIB_NAME}" PRIVATE -Wno-error=array-bounds)
add_subdirectory(ObstacleAvoidingPlatformUnit)
add_subdirectory(ObstacleAwarePlatformUnit)
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
index 14f1348ce043e1fb9158d9d2eca4268b15c17d1c..3166408f4190460632625b1739066112fd1d2965 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
@@ -586,7 +586,7 @@ bool armarx::ObstacleAvoidingPlatformUnit::is_near_target(const Eigen::Vector2f&
const float sim = simox::math::cosine_similarity(target_direction, control_direction);
// if almost pointing into same direction
- if (sim > cos(M_PI_4f32))
+ if (sim > cos(M_PI_4))
{
return true;
}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
index fc0274a7eb932135018cf356ca39105ea70890c6..d18722607d92879c22184fd8afc06a5ed0fa9bb9 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -564,7 +564,7 @@ const noexcept
const float sim = simox::math::cosine_similarity(target_direction, control_direction);
// if almost pointing into same direction
- if (sim > cos(M_PI_4f32))
+ if (sim > cos(M_PI_4))
{
return true;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index afb8f6a0c352085ffc08e72e7f0c74f1f419680f..dbaaf4873bd64b8aede4752f0b8cdc8de3029579 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -78,6 +78,7 @@ set(LIB_HEADERS
util/introspection/ClassMemberInfo.h
util/RtLogging.h
util/RtTiming.h
+ util/NonRtTiming.h
util/CtrlUtil.h
#robot unit modules need to be added to the list below (but not here)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 15baf198eb03202a929341675f01ef1ab831d234..67e940d30ad6899cc9773c14c950f8207092f905 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -269,7 +269,7 @@ namespace armarx
{
ft.force = _rtFTfake.force;
ft.torque = _rtFTfake.torque;
- ARMARX_RT_LOGF("Using fake ft values");
+ ARMARX_RT_LOGF_VERBOSE("Using fake ft values");
}
_rtFTHistory.at(_rtFTHistoryIndex) = ft;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 8ce9b397c9147d041290f5f8f73c203d6b8cb326..2956bbcf81a23ed99c16ac0ee8dc7ab5b697ace0 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -159,7 +159,7 @@ namespace armarx
getWriterControlStruct().target << x, y;
getWriterControlStruct().targetOrientation =
- simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
+ simox::math::periodic_clamp(static_cast<double>(yaw), -M_PI, M_PI);
getWriterControlStruct().translationAccuracy = translationAccuracy;
getWriterControlStruct().rotationAccuracy = rotationAccuracy;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index 4e07bdb3e790f36875e2b080b1a1470b461eaa8b..f59f1b7bef8c52d860490b75f1dc8e2f9eb6462e 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -66,8 +66,8 @@ namespace armarx
struct NJointHolonomicPlatformGlobalPositionControllerTarget
{
- Eigen::Vector2f target; // x,y
- float targetOrientation;
+ Eigen::Vector2f target{0, 0}; // x,y
+ float targetOrientation = 0;
float translationAccuracy = 0.0f;
float rotationAccuracy = 0.0f;
bool newTargetSet = false;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index ad5b035266524a4079f84811ad8677279019f856..62760e241a7b2a4c7b4457fa6669545d040dff63 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -60,8 +60,8 @@ namespace armarx
struct NJointHolonomicPlatformRelativePositionControllerTarget
{
- Eigen::Vector2f target; // x,y
- float targetOrientation;
+ Eigen::Vector2f target{0, 0}; // x,y
+ float targetOrientation = 0;
float translationAccuracy = 0.0f;
float rotationAccuracy = 0.0f;
bool newTargetSet = false;
@@ -105,7 +105,7 @@ namespace armarx
MultiDimPIDController pid;
PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
ControlTargetHolonomicPlatformVelocity* target;
- Eigen::Vector2f startPosition, currentPosition;
+ Eigen::Vector2f startPosition{0, 0}, currentPosition{0, 0};
float startOrientation;
float currentOrientation;
// float rad2MMFactor;
diff --git a/source/RobotAPI/components/units/RobotUnit/README.md b/source/RobotAPI/components/units/RobotUnit/README.md
index 5533e6588b3a89ac7d0d45b9dc4ed6ebaba323c9..f08be8ab3fb312b6e69d09457cc24a96f6475e3d 100644
--- a/source/RobotAPI/components/units/RobotUnit/README.md
+++ b/source/RobotAPI/components/units/RobotUnit/README.md
@@ -1,4 +1,5 @@
-# RobotUnit
+# RobotUnit {#RobotUnit}
+
The RobotUnit can be used for real-time control.
The central principle is that all controllers are executed synchronously.
The controllers are arranged in a 2-layer architecture.
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 725bb380ed3638ac9ad71102375c0c4a4a0c8f5d..9175ba0a587a520980ac4bd6966222016921f1c8 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -30,6 +30,8 @@
#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include "RobotAPI/components/units/RobotUnit/util/NonRtTiming.h"
+
#include "../util/ControlThreadOutputBuffer.h"
#include "RobotUnitModuleControlThreadDataBuffer.h"
#include "RobotUnitModuleDevices.h"
@@ -427,8 +429,8 @@ namespace armarx::RobotUnitModule
continue; //do not add to result and skipp during processing
}
auto& descrEntr = descr.entries[valData.fields.at(i).name];
-//*INDENT-OFF*
-// clang-format off
+ //*INDENT-OFF*
+ // clang-format off
#define make_case(Type, TName) \
(typeid(Type) == *valData.fields.at(i).type) \
{ \
@@ -494,7 +496,7 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard{rtLoggingMutex};
-
+
if (rtDataStreamingEntry.count(receiver) == 0u)
{
ARMARX_INFO << "stopDataStreaming called for a nonexistent log";
@@ -755,441 +757,449 @@ namespace armarx::RobotUnitModule
}
// Process devices.
- {// Sensors.
- {ARMARX_TRACE;
- durations.sens.start();
- //sensors
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
- {
- const SensorValueBase* val = data.sensors.at(idxDev);
- //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
+ { // Sensors.
{
- if (!rtLoggingEntries.empty())
+ ARMARX_TRACE;
+ durations.sens.start();
+ //sensors
+ for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
{
- durations.sens_csv.start();
- const auto str = val->getDataFieldAs<std::string>(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
+ const SensorValueBase* val = data.sensors.at(idxDev);
+ //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
+ ++idxField)
{
- if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
+ if (!rtLoggingEntries.empty())
{
- entry->stream << ";" << str;
+ durations.sens_csv.start();
+ const auto str = val->getDataFieldAs<std::string>(idxField);
+ for (auto& [_, entry] : rtLoggingEntries)
+ {
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
+ {
+ entry->stream << ";" << str;
+ }
+ }
+ durations.sens_csv.stop();
+ }
+ if (!rtDataStreamingEntry.empty())
+ {
+ durations.sens_stream.start();
+ for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
+ {
+ durations.sens_stream_elem.start();
+ rtStreamingEntry.processSens(*val, idxDev, idxField);
+ durations.sens_stream_elem.stop();
+ }
+ durations.sens_stream.stop();
}
}
- durations.sens_csv.stop();
}
- if (!rtDataStreamingEntry.empty())
+ durations.sens.stop();
+ }
+
+ // Controller.
+ {
+ durations.ctrl.start();
+ ARMARX_TRACE;
+ //joint controllers
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
{
- durations.sens_stream.start();
- for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
+ const auto& vals = data.control.at(idxDev);
+ //control value (e.g. v_platform)
+ for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
- durations.sens_stream_elem.start();
- rtStreamingEntry.processSens(*val, idxDev, idxField);
- durations.sens_stream_elem.stop();
+ const ControlTargetBase* val = vals.at(idxVal);
+ //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
+ ++idxField)
+ {
+ if (!rtLoggingEntries.empty())
+ {
+ durations.ctrl_csv.start();
+ std::string str;
+ val->getDataFieldAs(idxField, str); // expensive function
+ for (auto& [_, entry] : rtLoggingEntries)
+ {
+ if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(
+ idxField))
+ {
+ entry->stream << ";" << str;
+ }
+ }
+ durations.ctrl_csv.stop();
+ }
+ if (!rtDataStreamingEntry.empty())
+ {
+ durations.ctrl_stream.start();
+ for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
+ {
+ durations.ctrl_stream_elem.start();
+ rtStreamingEntry.processCtrl(*val, idxDev, idxVal, idxField);
+ durations.ctrl_stream_elem.stop();
+ }
+ durations.ctrl_stream.stop();
+ }
+ }
}
- durations.sens_stream.stop();
}
+
+ durations.ctrl.stop();
}
- }
- durations.sens.stop();
- }
+ } // namespace armarx::RobotUnitModule
- // Controller.
- {
- durations.ctrl.start();
- ARMARX_TRACE;
- //joint controllers
- for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
+ //finish processing
{
- const auto& vals = data.control.at(idxDev);
- //control value (e.g. v_platform)
- for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
+ //store data to backlog
{
- const ControlTargetBase* val = vals.at(idxVal);
- //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
+ if (rtLoggingBacklogEnabled)
{
- if (!rtLoggingEntries.empty())
+ durations.backlog.start();
+ ARMARX_TRACE;
+ if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
- durations.ctrl_csv.start();
- std::string str;
- val->getDataFieldAs(idxField, str); // expensive function
- for (auto& [_, entry] : rtLoggingEntries)
- {
- if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
- {
- entry->stream << ";" << str;
- }
- }
- durations.ctrl_csv.stop();
+ backlog.emplace_back(data, true); //true for minimal copy
}
- if (!rtDataStreamingEntry.empty())
+ durations.backlog.stop();
+ }
+ }
+ //print + reset messages
+ {
+ durations.msg.start();
+ ARMARX_TRACE;
+ for (const ::armarx::detail::RtMessageLogEntryBase* ptr :
+ data.messages.getEntries())
+ {
+ if (!ptr)
{
- durations.ctrl_stream.start();
- for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
- {
- durations.ctrl_stream_elem.start();
- rtStreamingEntry.processCtrl(*val, idxDev, idxVal, idxField);
- durations.ctrl_stream_elem.stop();
- }
- durations.ctrl_stream.stop();
+ break;
}
+ ptr->print(controlThreadId);
}
+ durations.msg.stop();
}
}
-
- durations.ctrl.stop();
}
-} // namespace armarx::RobotUnitModule
-//finish processing
-{
- //store data to backlog
+ bool
+ Logging::MatchName(const std::string& pattern, const std::string& name)
{
- if (rtLoggingBacklogEnabled)
+ ARMARX_TRACE;
+ if (pattern.empty())
{
- durations.backlog.start();
- ARMARX_TRACE;
- if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
- {
- backlog.emplace_back(data, true); //true for minimal copy
- }
- durations.backlog.stop();
+ return false;
}
- }
- //print + reset messages
- {
- durations.msg.start();
- ARMARX_TRACE;
- for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
+ static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
+ if (!std::regex_match(pattern, pattern_regex))
{
- if (!ptr)
- {
- break;
- }
- ptr->print(controlThreadId);
+ throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
}
- durations.msg.stop();
+ static const std::regex reg_dot{"[.]"};
+ static const std::regex reg_star{"[*]"};
+ const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
+ const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
+ const std::regex key_regex{rpl2};
+ return std::regex_search(name, key_regex);
}
-}
-}
-bool
-Logging::MatchName(const std::string& pattern, const std::string& name)
-{
- ARMARX_TRACE;
- if (pattern.empty())
- {
- return false;
- }
- static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
- if (!std::regex_match(pattern, pattern_regex))
- {
- throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
- }
- static const std::regex reg_dot{"[.]"};
- static const std::regex reg_star{"[*]"};
- const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
- const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
- const std::regex key_regex{rpl2};
- return std::regex_search(name, key_regex);
-}
-
-void
-Logging::_postOnInitRobotUnit()
-{
- ARMARX_TRACE;
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
- ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
-
- messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
- messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
- ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
-
- messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
- messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
- ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
-
- rtLoggingBacklogRetentionTime =
- IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
- rtLoggingBacklogMaxSize = getProperty<std::size_t>("RTLogging_MaxBacklogSize");
- rtLoggingBacklogEnabled = getProperty<bool>("RTLogging_EnableBacklog");
- getProperty(numberOfEntriesToLog, "RTLogging_LogLastNMessagesOnly");
- ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
- ARMARX_IMPORTANT << "Initializing RTLogging with the following parameters: " << VAROUT(rtLoggingTimestepMs)
- << VAROUT(messageBufferSize) << VAROUT(messageBufferMaxSize)
- << VAROUT(messageBufferNumberEntries) << VAROUT(messageBufferMaxNumberEntries)
- << VAROUT(rtLoggingBacklogRetentionTime) << VAROUT(rtLoggingBacklogMaxSize)
- << VAROUT(rtLoggingBacklogEnabled) << VAROUT(numberOfEntriesToLog);
-}
-
-void
-Logging::_postFinishDeviceInitialization()
-{
- ARMARX_TRACE;
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- //init buffer
+ void
+ Logging::_postOnInitRobotUnit()
{
ARMARX_TRACE;
- std::size_t ctrlThreadPeriodUs =
- static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
- std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
- std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
-
- const auto bufferSize =
- _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
- nBuffers,
- _module<Devices>().getControlDevices(),
- _module<Devices>().getSensorDevices(),
- messageBufferSize,
- messageBufferNumberEntries,
- messageBufferMaxSize,
- messageBufferMaxNumberEntries);
- ARMARX_INFO << "RTLogging activated. Using " << nBuffers << " buffers "
- << "(buffersize = " << bufferSize << " bytes)";
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
+ ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
+
+ messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
+ messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
+
+ messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
+ messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
+
+ rtLoggingBacklogRetentionTime =
+ IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
+ rtLoggingBacklogMaxSize = getProperty<std::size_t>("RTLogging_MaxBacklogSize");
+ rtLoggingBacklogEnabled = getProperty<bool>("RTLogging_EnableBacklog");
+ getProperty(numberOfEntriesToLog, "RTLogging_LogLastNMessagesOnly");
+ ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
+ ARMARX_IMPORTANT << "Initializing RTLogging with the following parameters: "
+ << VAROUT(rtLoggingTimestepMs) << VAROUT(messageBufferSize)
+ << VAROUT(messageBufferMaxSize) << VAROUT(messageBufferNumberEntries)
+ << VAROUT(messageBufferMaxNumberEntries)
+ << VAROUT(rtLoggingBacklogRetentionTime) << VAROUT(rtLoggingBacklogMaxSize)
+ << VAROUT(rtLoggingBacklogEnabled) << VAROUT(numberOfEntriesToLog);
}
- //init logging names + field types
+
+ void
+ Logging::_postFinishDeviceInitialization()
{
ARMARX_TRACE;
- const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //init buffer
+ {
+ ARMARX_TRACE;
+ std::size_t ctrlThreadPeriodUs =
+ static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
+ std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
+ std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
+
+ const auto bufferSize =
+ _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
+ nBuffers,
+ _module<Devices>().getControlDevices(),
+ _module<Devices>().getSensorDevices(),
+ messageBufferSize,
+ messageBufferNumberEntries,
+ messageBufferMaxSize,
+ messageBufferMaxNumberEntries);
+ ARMARX_INFO << "RTLogging activated. Using " << nBuffers << " buffers "
+ << "(buffersize = " << bufferSize << " bytes)";
+ }
+ //init logging names + field types
{
- ValueMetaData data;
- const auto names = val->getDataFieldNames();
- data.fields.resize(names.size());
+ ARMARX_TRACE;
+ const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
+ {
+ ValueMetaData data;
+ const auto names = val->getDataFieldNames();
+ data.fields.resize(names.size());
+
+ for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++fieldIdx)
+ {
+ std::string const& fieldName = names[fieldIdx];
+ data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
+ data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
+ }
+ return data;
+ };
- for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++fieldIdx)
+ //sensorDevices
+ controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
+ for (const auto& cd : _module<Devices>().getControlDevices().values())
{
- std::string const& fieldName = names[fieldIdx];
- data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
- data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
+ ARMARX_TRACE;
+ controlDeviceValueMetaData.emplace_back();
+ auto& dataForDev = controlDeviceValueMetaData.back();
+ dataForDev.reserve(cd->getJointControllers().size());
+ for (auto jointC : cd->getJointControllers())
+ {
+ dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
+ "ctrl." + cd->getDeviceName() + "." +
+ jointC->getControlMode()));
+ }
}
- return data;
- };
-
- //sensorDevices
- controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
- for (const auto& cd : _module<Devices>().getControlDevices().values())
- {
- ARMARX_TRACE;
- controlDeviceValueMetaData.emplace_back();
- auto& dataForDev = controlDeviceValueMetaData.back();
- dataForDev.reserve(cd->getJointControllers().size());
- for (auto jointC : cd->getJointControllers())
+ //sensorDevices
+ sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
+ for (const auto& sd : _module<Devices>().getSensorDevices().values())
{
- dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
- "ctrl." + cd->getDeviceName() + "." +
- jointC->getControlMode()));
+ ARMARX_TRACE;
+ sensorDeviceValueMetaData.emplace_back(
+ makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
}
}
- //sensorDevices
- sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
- for (const auto& sd : _module<Devices>().getSensorDevices().values())
+ //start logging thread is done in rtinit
+ //maybe add the default log
{
ARMARX_TRACE;
- sensorDeviceValueMetaData.emplace_back(
- makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
+ const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
+ if (!loggingpath.empty())
+ {
+ defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
+ }
}
}
- //start logging thread is done in rtinit
- //maybe add the default log
+
+ void
+ Logging::DataStreamingEntry::clearResult()
{
ARMARX_TRACE;
- const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
- if (!loggingpath.empty())
+ for (auto& e : result)
{
- defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
+ entryBuffer.emplace_back(std::move(e));
}
+ result.clear();
}
-}
-void
-Logging::DataStreamingEntry::clearResult()
-{
- ARMARX_TRACE;
- for (auto& e : result)
+ RobotUnitDataStreaming::TimeStep
+ Logging::DataStreamingEntry::allocateResultElement() const
{
- entryBuffer.emplace_back(std::move(e));
+ ARMARX_TRACE;
+ RobotUnitDataStreaming::TimeStep data;
+ data.bools.resize(numBools);
+ data.bytes.resize(numBytes);
+ data.shorts.resize(numShorts);
+ data.ints.resize(numInts);
+ data.longs.resize(numLongs);
+ data.floats.resize(numFloats);
+ data.doubles.resize(numDoubles);
+ return data;
}
- result.clear();
-}
-RobotUnitDataStreaming::TimeStep
-Logging::DataStreamingEntry::allocateResultElement() const
-{
- ARMARX_TRACE;
- RobotUnitDataStreaming::TimeStep data;
- data.bools.resize(numBools);
- data.bytes.resize(numBytes);
- data.shorts.resize(numShorts);
- data.ints.resize(numInts);
- data.longs.resize(numLongs);
- data.floats.resize(numFloats);
- data.doubles.resize(numDoubles);
- return data;
-}
-
-RobotUnitDataStreaming::TimeStep
-Logging::DataStreamingEntry::getResultElement()
-{
- ARMARX_TRACE;
- if (entryBuffer.empty())
+ RobotUnitDataStreaming::TimeStep
+ Logging::DataStreamingEntry::getResultElement()
{
- return allocateResultElement();
+ ARMARX_TRACE;
+ if (entryBuffer.empty())
+ {
+ return allocateResultElement();
+ }
+ auto e = std::move(entryBuffer.back());
+ entryBuffer.pop_back();
+ return e;
}
- auto e = std::move(entryBuffer.back());
- entryBuffer.pop_back();
- return e;
-}
-void
-Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
-{
- ARMARX_TRACE;
- if (stopStreaming)
+ void
+ Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
{
- return;
- }
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
- auto& data = result.emplace_back(getResultElement());
+ auto& data = result.emplace_back(getResultElement());
- data.iterationId = e.iteration;
- data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
- data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
-}
+ data.iterationId = e.iteration;
+ data.timestampUSec =
+ armarx::mapRtTimestampToNonRtTimestamp(e.sensorValuesTimestamp).toMicroSeconds();
+ data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
+ }
-void
-WriteTo(const auto& dentr,
- const Logging::DataStreamingEntry::OutVal& out,
- const auto& val,
- std::size_t fidx,
- auto& data)
-{
- ARMARX_TRACE;
- using enum_t = Logging::DataStreamingEntry::ValueT;
- try
+ void
+ WriteTo(const auto& dentr,
+ const Logging::DataStreamingEntry::OutVal& out,
+ const auto& val,
+ std::size_t fidx,
+ auto& data)
{
ARMARX_TRACE;
- switch (out.value)
+ using enum_t = Logging::DataStreamingEntry::ValueT;
+ try
{
- case enum_t::Bool:
- bool b;
- val.getDataFieldAs(fidx, b);
- data.bools.at(out.idx) = b;
- return;
- case enum_t::Byte:
- val.getDataFieldAs(fidx, data.bytes.at(out.idx));
- return;
- case enum_t::Short:
- val.getDataFieldAs(fidx, data.shorts.at(out.idx));
- return;
- case enum_t::Int:
- val.getDataFieldAs(fidx, data.ints.at(out.idx));
- return;
- case enum_t::Long:
- val.getDataFieldAs(fidx, data.longs.at(out.idx));
- return;
- case enum_t::Float:
- val.getDataFieldAs(fidx, data.floats.at(out.idx));
- return;
- case enum_t::Double:
- val.getDataFieldAs(fidx, data.doubles.at(out.idx));
- return;
- case enum_t::Skipped:
- return;
+ ARMARX_TRACE;
+ switch (out.value)
+ {
+ case enum_t::Bool:
+ bool b;
+ val.getDataFieldAs(fidx, b);
+ data.bools.at(out.idx) = b;
+ return;
+ case enum_t::Byte:
+ val.getDataFieldAs(fidx, data.bytes.at(out.idx));
+ return;
+ case enum_t::Short:
+ val.getDataFieldAs(fidx, data.shorts.at(out.idx));
+ return;
+ case enum_t::Int:
+ val.getDataFieldAs(fidx, data.ints.at(out.idx));
+ return;
+ case enum_t::Long:
+ val.getDataFieldAs(fidx, data.longs.at(out.idx));
+ return;
+ case enum_t::Float:
+ val.getDataFieldAs(fidx, data.floats.at(out.idx));
+ return;
+ case enum_t::Double:
+ val.getDataFieldAs(fidx, data.doubles.at(out.idx));
+ return;
+ case enum_t::Skipped:
+ return;
+ }
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << GetHandledExceptionString() << "\ntype " << static_cast<int>(out.value)
+ << "\n"
+ << VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools) << "\n"
+ << VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes) << "\n"
+ << VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts) << "\n"
+ << VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts) << "\n"
+ << VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs) << "\n"
+ << VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats) << "\n"
+ << VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
+ throw;
}
}
- catch (...)
- {
- ARMARX_ERROR << GetHandledExceptionString() << "\ntype " << static_cast<int>(out.value)
- << "\n"
- << VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools) << "\n"
- << VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes) << "\n"
- << VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts) << "\n"
- << VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts) << "\n"
- << VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs) << "\n"
- << VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats) << "\n"
- << VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
- throw;
- }
-}
-void
-Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
- std::size_t didx,
- std::size_t vidx,
- std::size_t fidx)
-{
- ARMARX_TRACE;
- if (stopStreaming)
+ void
+ Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
+ std::size_t didx,
+ std::size_t vidx,
+ std::size_t fidx)
{
- return;
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
+ WriteTo(*this, o, val, fidx, data);
}
- auto& data = result.back();
- const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
- WriteTo(*this, o, val, fidx, data);
-}
-
-void
-Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
- std::size_t didx,
- std::size_t fidx)
-{
- ARMARX_TRACE;
- if (stopStreaming)
+
+ void
+ Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
+ std::size_t didx,
+ std::size_t fidx)
{
- return;
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = sensDevs.at(didx).at(fidx);
+ WriteTo(*this, o, val, fidx, data);
}
- auto& data = result.back();
- const OutVal& o = sensDevs.at(didx).at(fidx);
- WriteTo(*this, o, val, fidx, data);
-}
-void
-Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r, std::uint64_t msgId)
-{
- ARMARX_TRACE;
- const auto start_send = armarx::rtNow();
- const auto num_timesteps = result.size();
- updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
- const auto start_clear = armarx::rtNow();
- clearResult();
- const auto end = armarx::rtNow();
- ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
- << "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms ("
- << num_timesteps << " timesteps)"
- << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
-
- //now execute all ready callbacks
- std::size_t i = 0;
- for (; i < updateCalls.size(); ++i)
+ void
+ Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r,
+ std::uint64_t msgId)
{
- try
+ ARMARX_TRACE;
+ const auto start_send = armarx::rtNow();
+ const auto num_timesteps = result.size();
+ updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
+ const auto start_clear = armarx::rtNow();
+ clearResult();
+ const auto end = armarx::rtNow();
+ ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
+ << "\n update " << (start_clear - start_send).toMilliSecondsDouble()
+ << "ms (" << num_timesteps << " timesteps)"
+ << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
+
+ //now execute all ready callbacks
+ std::size_t i = 0;
+ for (; i < updateCalls.size(); ++i)
{
- if (!updateCalls.at(i)->isCompleted())
+ try
{
- break;
+ if (!updateCalls.at(i)->isCompleted())
+ {
+ break;
+ }
+ r->end_update(updateCalls.at(i));
+ connectionFailures = 0;
}
- r->end_update(updateCalls.at(i));
- connectionFailures = 0;
- }
- catch (...)
- {
- ARMARX_TRACE;
- ++connectionFailures;
- if (connectionFailures > rtStreamMaxClientErrors)
+ catch (...)
{
- stopStreaming = true;
- ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
- << connectionFailures << " iterations! Removing receiver";
- updateCalls.clear();
- break;
+ ARMARX_TRACE;
+ ++connectionFailures;
+ if (connectionFailures > rtStreamMaxClientErrors)
+ {
+ stopStreaming = true;
+ ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
+ << connectionFailures << " iterations! Removing receiver";
+ updateCalls.clear();
+ break;
+ }
}
}
+ if (!updateCalls.empty())
+ {
+ updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
+ }
}
- if (!updateCalls.empty())
- {
- updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
- }
-}
} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index 4b9a12a21520e58e87f0038c808715be940b8584..c96a83d546b858ad820cb60d0ed73493ebfe2219 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -35,6 +35,7 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/util/NonRtTiming.h>
namespace armarx::RobotUnitModule
{
@@ -632,7 +633,10 @@ namespace armarx::RobotUnitModule
const auto requestedJointControllers =
_module<ControlThreadDataBuffer>().copyRequestedJointControllers();
- lastControlThreadTimestamp = controlThreadOutputBuffer.sensorValuesTimestamp;
+ // controlThreadOutputBuffer.sensorValuesTimestamp is in MONOTONIC_RAW (not relative to epoch).
+ // We have to map it to be relative to epoch (REAL_TIME).
+ lastControlThreadTimestamp =
+ armarx::mapRtTimestampToNonRtTimestamp(controlThreadOutputBuffer.sensorValuesTimestamp);
const bool publishToObserver = !(publishIterationCount % debugObserverSkipIterations);
//publish publishing meta state
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index a160770bdd08245712d0ad193e20966cd7d2a2ce..5304a0aa8a0e6716b6c9c8266c01d9db61e9ee6a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -290,9 +290,10 @@ namespace armarx::RobotUnitModule
//try to find the correct mode (maybe the specified target was derived!
//this only works, if exactly one controller provides this mode
const JointController* selected_ctrl{nullptr};
- for (const auto ctrl : controlDevice->getJointControllers())
+ auto jointControllers = controlDevice->getJointControllers();
+ for (const auto* jointController : jointControllers)
{
- if (ctrl->getControlTarget()->isA<CtargT>())
+ if (jointController->getControlTarget()->isA<CtargT>())
{
if (selected_ctrl)
{
@@ -301,7 +302,7 @@ namespace armarx::RobotUnitModule
<< requestedControlMode << "! autoselection failed";
break;
}
- selected_ctrl = ctrl;
+ selected_ctrl = jointController;
}
}
if (selected_ctrl)
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index bedfc430f0dee9f2030ac495d2ba6dd74bfa1350..8f8738d48a54c135198d48a370d0d36006d5a129 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -362,7 +362,7 @@ namespace armarx
const auto numExcessEntries =
std::max(requiredAdditionalEntries, numEntries - entries.size());
const auto requiredSize = entries.size() + numExcessEntries;
- ARMARX_WARNING << "Iteration " << iterationCount << " required "
+ ARMARX_VERBOSE << "Iteration " << iterationCount << " required "
<< requiredAdditionalEntries << " | " << numExcessEntries
<< " additional message entries. \n"
<< "The requested total number of entries is " << requiredSize << ". \n"
@@ -371,7 +371,7 @@ namespace armarx
<< getMaximalNumberOfBufferEntries();
if (requiredSize > getMaximalNumberOfBufferEntries())
{
- ARMARX_WARNING << deactivateSpam(1, to_string(requiredSize)) << "Iteration "
+ ARMARX_VERBOSE << deactivateSpam(1, to_string(requiredSize)) << "Iteration "
<< iterationCount << " would require " << requiredSize
<< " message entries, but the maximal number of entries is "
<< getMaximalNumberOfBufferEntries();
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index 78bd7da841220341e817df9d566e4275b150eb7b..cb47d82b18e8c30d5b0f82c7923ae8f2328bf734 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -24,6 +24,7 @@
#include <vector>
+#include <ArmarXCore/core/logging/LoggingUtil.h> // THIS NEEDS TO BE INCLUDED BEFORE EXPRESSION EXCEPTION
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/PropagateConst.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
@@ -34,6 +35,7 @@
#include "../Devices/SensorDevice.h"
#include "../SensorValues/SensorValueBase.h"
#include "HeterogenousContinuousContainer.h"
+#include "RtTiming.h"
namespace armarx
{
@@ -50,7 +52,7 @@ namespace armarx::detail
{
struct RtMessageLogEntryBase
{
- RtMessageLogEntryBase() : time{IceUtil::Time::now()}
+ RtMessageLogEntryBase() : time{armarx::rtNow()}
{
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/NonRtTiming.h b/source/RobotAPI/components/units/RobotUnit/util/NonRtTiming.h
new file mode 100644
index 0000000000000000000000000000000000000000..a77f5d6291579cf373173d817b7a6ae1a7f9b22b
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/util/NonRtTiming.h
@@ -0,0 +1,66 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <time.h>
+
+#include <IceUtil/Time.h>
+
+#include "RtTiming.h"
+
+namespace armarx
+{
+ inline IceUtil::Time
+ nonRtNow()
+ {
+ using namespace rt_timing::constants;
+
+ struct timespec ts;
+ clock_gettime(CLOCK_REALTIME, &ts);
+ return IceUtil::Time::microSeconds(ts.tv_sec * seconds2MicroSeconds +
+ ts.tv_nsec / nanoSeconds2MicroSeconds);
+ }
+
+ inline IceUtil::Time
+ mapRtTimestampToNonRtTimestamp(const IceUtil::Time& time_monotic_raw)
+ {
+ // This is the "real time" clock, i.e. NTP-synchronized and relative to epoch.
+ IceUtil::Time now_real_time = armarx::nonRtNow();
+ // This is not relative to epoch and not NTP-synchronized.
+ IceUtil::Time now_monotonic_raw = armarx::rtNow();
+
+ /*
+ * Assumption for small very small time deltas (i.e. "time" is close to "now"):
+ *
+ * time_real_time - now_real_time == time_monotic_raw - now_monotonic_raw
+ * =>
+ * time_real_time = time_monotic_raw - now_monotonic_raw + now_real_time
+ */
+ //
+ IceUtil::Time time_real_time = time_monotic_raw - now_monotonic_raw + now_real_time;
+ ARMARX_DEBUG << VAROUT(time_monotic_raw) << " vs. " << VAROUT(time_real_time);
+ return time_real_time;
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
index 78c86b83633a83ccaf0d73238da632271b956889..0f5b47c8e0bfc783b34cc2080488bf5d81a9b1d6 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/RtTiming.h
@@ -23,8 +23,9 @@
*/
#pragma once
-#include <chrono>
-#include "ControlThreadOutputBuffer.h"
+#include <time.h>
+
+#include <IceUtil/Time.h>
namespace armarx
{
@@ -50,8 +51,7 @@ namespace armarx
//! \ingroup VirtualTime
//! Prints duration with comment in front of it, yet only once per second.
#define RT_TIMING_END_COMMENT(name, comment) \
- printf( \
- "%s - duration: %.3f ms \n", comment, (armarx::rtNow() - name).toMilliSecondsDouble());
+ printf("%s - duration: %.3f ms \n", comment, (armarx::rtNow() - name).toMilliSecondsDouble());
//! \ingroup VirtualTime
//! Prints duration
#define RT_TIMING_END(name) RT_TIMING_END_COMMENT(name, #name)
diff --git a/source/RobotAPI/components/units/SensorActorUnit.cpp b/source/RobotAPI/components/units/SensorActorUnit.cpp
index f93f684d72619a4ad402f8f56a5f6c285d00a866..45efd98c586e5bfa15086ddc08a80d65f7c3497a 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.cpp
+++ b/source/RobotAPI/components/units/SensorActorUnit.cpp
@@ -160,6 +160,8 @@ void SensorActorUnit::release(const Ice::Current& c)
void SensorActorUnit::onExitComponent()
{
- unitMutex.try_lock(); // try to lock, to ensure that it is locked on unlock call
- unitMutex.unlock();
+ if (unitMutex.try_lock()) // try to lock, to ensure that it is locked on unlock call
+ {
+ unitMutex.unlock();
+ }
}
diff --git a/source/RobotAPI/drivers/GamepadUnit/README.md b/source/RobotAPI/drivers/GamepadUnit/README.md
index f987b05e9a80bc59741e77b07502516f7de3d842..5def39e88a135d938e9bcbcac27bbf553285ebd2 100644
--- a/source/RobotAPI/drivers/GamepadUnit/README.md
+++ b/source/RobotAPI/drivers/GamepadUnit/README.md
@@ -1,4 +1,4 @@
-# GamepadUnit
+# GamepadUnit {#GamepadUnit}
This component is used to control the robot with a gamepad.
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 5dba06222784b7bb92ffa9236329a827973b4ba5..571b43b1344de7db6ed479f0527530537ceea660 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -1445,7 +1445,7 @@ namespace armarx
auto it = reportedJointStatuses.find(rn[i]->getName());
if (it == reportedJointStatuses.end())
{
- ARMARX_WARNING << deactivateSpam(5, rn[i]->getName()) << "Joint Status for "
+ ARMARX_VERBOSE << deactivateSpam(5, rn[i]->getName()) << "Joint Status for "
<< rn[i]->getName() << " was not reported!";
continue;
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 3e503f9a7f04ff1fdcdb4bb1988ee04f0b18666d..879422d20200f53cc0befe9b1c28f6b59220396a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -82,7 +82,6 @@ armarx_enable_aron_file_generation_for_target(
"${LIB_NAME}"
ARON_FILES
aron/ObjectID.xml
- aron/ObjectNames.xml
aron/ObjectPose.xml
aron/ObjectType.xml
aron/PoseManifoldGaussian.xml
diff --git a/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h b/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
index a0ad9297f873496e7dbe0d1c17fb3ab0b5904ac1..57528416490797728886eb08a0ff7999bb4e4887 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
@@ -138,5 +138,11 @@ namespace armarx::priorknowledge::core
virtual std::optional<FinderInfoType> find(const DatasetType& dataset,
const IDType& id) const = 0;
virtual std::vector<FinderInfoType> findAll(const DatasetType& dataset) const = 0;
+
+ // Fix hidden virtual functions
+ using Base::checkAll;
+ using Base::check;
+ using Base::find;
+ using Base::findAll;
};
} // namespace armarx::priorknowledge::core
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt b/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt
index 5e24563b59a6d4d470916e244a451a6878d07436..545faddd872d9ac16863b9471c6c876b5b705045 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt
+++ b/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt
@@ -27,5 +27,7 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+target_compile_options("${LIB_NAME}" PRIVATE -Wno-error=maybe-uninitialized)
+
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 9030bd17597399abf79e3413ec71a789f3a113ca..670fef073807ec08cf5938f77a5876f099d89f07 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -7,6 +7,7 @@ armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
ArmarXCoreInterfaces
ArmarXCore
+ DebugObserverHelper
RemoteGui
aron
aroncommon
@@ -64,8 +65,9 @@ set(LIB_FILES
client/plugins/PluginUser.cpp
client/plugins/Plugin.cpp
- client/util/SubscriptionHandle.cpp
client/util/MemoryListener.cpp
+ client/util/MemoryToDebugObserver.cpp
+ client/util/SubscriptionHandle.cpp
client/util/SimpleReaderBase.cpp
client/util/SimpleWriterBase.cpp
@@ -157,10 +159,11 @@ set(LIB_HEADERS
client/query/detail/NameSelectorOps.h
client/query/detail/SelectorOps.h
- client/util/SubscriptionHandle.h
+ client/util/MemoryToDebugObserver.h
client/util/MemoryListener.h
client/util/SimpleReaderBase.h
client/util/SimpleWriterBase.h
+ client/util/SubscriptionHandle.h
server.h
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..89e2cf20cb7d5786458ed5453ece73749e4783bd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
@@ -0,0 +1,241 @@
+/*
+ * 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::MemoryToDebugObserver
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "MemoryToDebugObserver.h"
+
+#include <RobotAPI/libraries/armem/core/error/mns.h>
+#include <RobotAPI/libraries/armem/core/json_conversions.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/data/variant/primitive/All.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+namespace armarx::armem::client::util
+{
+
+ MemoryToDebugObserver::MemoryToDebugObserver(const Properties& properties,
+ const Services& services) :
+ properties(properties), services(services)
+ {
+ services.debugObserver.setDebugObserverBatchModeEnabled(true);
+ }
+
+ class Visitor : public aron::data::ConstVariantVisitor
+ {
+ public:
+ Visitor(armarx::DebugObserverHelper& debugObserver) : debugObserver{debugObserver}
+ {
+ }
+
+ // void visitAronVariant(const data::DictPtr&) override;
+ // void visitAronVariant(const data::ListPtr&) override;
+ // void visitAronVariant(const data::NDArrayPtr&) override;
+ void
+ visitAronVariant(const aron::data::IntPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::LongPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::FloatPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::DoublePtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::BoolPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ void
+ visitAronVariant(const aron::data::StringPtr& v) override
+ {
+ setDatafield(v->getValue());
+ }
+
+ template <class ValueT>
+ void
+ setDatafield(const ValueT& value)
+ {
+ debugObserver.setDebugObserverDatafield(channelName, datafieldName, value);
+ ++count;
+ }
+
+ armarx::DebugObserverHelper& debugObserver;
+ std::string channelName;
+ std::string datafieldName;
+
+ int count = 0;
+ };
+
+ void
+ MemoryToDebugObserver::pollOnce()
+ {
+ Visitor visitor(services.debugObserver);
+
+ std::stringstream log;
+
+ // Group by memory segment to reduce number of queries.
+ std::map<MemoryID, std::vector<const MemoryValueID*>> valuesPerProviderSegment;
+ for (const MemoryValueID& valueId : properties.plottedValues)
+ {
+ valuesPerProviderSegment[valueId.entityID.getProviderSegmentID()].push_back(&valueId);
+ }
+
+ for (const auto& [providerSegmentID, values] : valuesPerProviderSegment)
+ {
+ armem::client::Reader* reader = nullptr;
+ try
+ {
+ reader = getReader(providerSegmentID);
+ }
+ catch (armem::error::CouldNotResolveMemoryServer& e)
+ {
+ log << "\n" << e.what();
+ continue;
+ }
+ ARMARX_CHECK_NOT_NULL(reader);
+
+ const QueryResult result = reader->getLatestSnapshotsIn(providerSegmentID);
+ if (not result.success)
+ {
+ log << "Query to provider segment " << providerSegmentID
+ << " failed: " << result.errorMessage;
+ continue;
+ }
+
+ for (const MemoryValueID* valueId : values)
+ {
+ const wm::Entity* entity = result.memory.findEntity(valueId->entityID);
+ if (entity == nullptr)
+ {
+ log << "\nDid not find entity " << valueId->entityID << " in provider segment "
+ << providerSegmentID << ".";
+ continue;
+ }
+
+ const wm::EntityInstance& instance = entity->getLatestInstance();
+ aron::data::VariantPtr valueVariant =
+ instance.data()->navigateAbsolute(valueId->aronPath);
+ if (not valueVariant)
+ {
+ log << "\nDid not find " << valueId->aronPath.toString()
+ << " in entity instance " << instance.id() << ".";
+ continue;
+ }
+
+ visitor.channelName = makeChannelName(valueId->entityID);
+ visitor.datafieldName = makeDatafieldName(valueId->aronPath);
+
+ aron::data::visit(visitor, valueVariant);
+ }
+ }
+
+ services.debugObserver.sendDebugObserverBatch();
+
+ if (not log.str().empty())
+ {
+ ARMARX_INFO << deactivateSpam(60)
+ << "Encountered issues while sending memory values to the debug observer "
+ "for plotting: "
+ << log.str();
+ }
+ }
+
+ std::string
+ MemoryToDebugObserver::makeChannelName(const MemoryID& memoryID)
+ {
+ return simox::alg::replace_all(memoryID.str(), "/", ">");
+ }
+
+ std::string
+ MemoryToDebugObserver::makeDatafieldName(const aron::Path& path)
+ {
+ // The first element is always "ARON->", which we can discard for readability.
+ std::string str = path.toString();
+ str = simox::alg::remove_prefix(str, path.getRootIdentifier() + path.getDelimeter());
+ str = simox::alg::replace_all(str, path.getDelimeter(), ">");
+ return str;
+ }
+
+ Reader*
+ MemoryToDebugObserver::getReader(const MemoryID& memoryID)
+ {
+ auto it = memoryReaders.find(memoryID);
+ if (it != memoryReaders.end())
+ {
+ return &it->second;
+ }
+ else
+ {
+ armem::client::Reader reader = services.memoryNameSystem.getReader(memoryID);
+
+ auto [it, _] = memoryReaders.emplace(memoryID, reader);
+ return &it->second;
+ }
+ }
+
+} // namespace armarx::armem::client::util
+
+namespace armarx::armem::client
+{
+
+ void
+ util::to_json(simox::json::json& j, const MemoryValueID& id)
+ {
+ j["entityID"] = id.entityID.getItems();
+ j["aronPath"] = id.aronPath.getPath();
+ }
+
+ void
+ util::from_json(const simox::json::json& j, MemoryValueID& id)
+ {
+ id.entityID = MemoryID::fromItems(j.at("entityID").get<std::vector<std::string>>());
+ id.aronPath = {j.at("aronPath").get<std::vector<std::string>>()};
+ }
+
+ void
+ util::to_json(simox::json::json& j, const MemoryToDebugObserver::Properties& p)
+ {
+ j["plottedValues"] = p.plottedValues;
+ }
+
+ void
+ util::from_json(const simox::json::json& j, MemoryToDebugObserver::Properties& p)
+ {
+ j.at("plottedValues").get_to(p.plottedValues);
+ }
+
+
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..f1628bc09025c99ec6b3d6c8382ad30c79973977
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.h
@@ -0,0 +1,101 @@
+/*
+ * 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::MemoryToDebugObserver
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/json/json.h>
+
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
+namespace armarx::armem::client::util
+{
+ /**
+ * @brief ID of an ARON value in the memory.
+ */
+ struct MemoryValueID
+ {
+ armem::MemoryID entityID;
+ aron::Path aronPath;
+ };
+
+ /**
+ * @brief Transfers data from memory servers to the DebugObserver.
+ *
+ * Transfers data from the memory system to the \ref Component-DebugObserver "Debug Observer",
+ * allowing to visualize them in the \ref ArmarXGui-GuiPlugins-PlotterPlugin "Live Plotter".
+ */
+ class MemoryToDebugObserver
+ {
+ public:
+ /**
+ * @brief Configuration.
+ *
+ * Can be converted to and from JSON.
+ */
+ struct Properties
+ {
+ std::vector<MemoryValueID> plottedValues;
+ };
+
+ /**
+ * @brief Required services.
+ */
+ struct Services
+ {
+ MemoryNameSystem& memoryNameSystem;
+ armarx::DebugObserverHelper& debugObserver;
+ };
+
+ /**
+ * @brief Constructor.
+ */
+ MemoryToDebugObserver(const Properties& properties, const Services& services);
+
+ /**
+ * @brief Query values from the memory and send them to the debug observer.
+ */
+ void pollOnce();
+
+ private:
+ static std::string makeChannelName(const armem::MemoryID& memoryID);
+ static std::string makeDatafieldName(const aron::Path& path);
+
+ armem::client::Reader* getReader(const armem::MemoryID& memoryID);
+
+ Properties properties;
+ Services services;
+
+ std::map<armem::MemoryID, armem::client::Reader> memoryReaders;
+ };
+
+ void to_json(simox::json::json& j, const MemoryValueID& id);
+ void from_json(const simox::json::json& j, MemoryValueID& id);
+
+ void to_json(simox::json::json& j, const MemoryToDebugObserver::Properties& p);
+ void from_json(const simox::json::json& j, MemoryToDebugObserver::Properties& p);
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index c2a59ff9cb7e653f74153ced3eb73d00ed7572c4..e150f1e044f225c73aeda5f330a50ffef1c51a16 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -1,13 +1,14 @@
#include "MemoryID.h"
-#include "error/ArMemError.h"
+#include <forward_list>
+
+#include <boost/algorithm/string.hpp>
#include <SimoxUtility/algorithm/advanced.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include "ArmarXCore/core/logging/Logging.h"
-#include <boost/algorithm/string.hpp>
-
-#include <forward_list>
+#include "error/ArMemError.h"
namespace armarx::armem
@@ -16,13 +17,14 @@ namespace armarx::armem
// This is constant, not a variable.
const std::string MemoryID::delimiter = "/";
-
MemoryID::MemoryID()
{
}
MemoryID::MemoryID(const std::string& string)
{
+ //ARMARX_INFO << "Trying to create MemoryID from string";
+ //ARMARX_INFO << VAROUT(string);
std::forward_list<std::string> items;
boost::split(items, string, boost::is_any_of(delimiter));
@@ -78,16 +80,15 @@ namespace armarx::armem
return;
}
this->instanceIndex = instanceIndexFromStr(*it);
+ //ARMARX_INFO << this->str();
}
-
- MemoryID::MemoryID(
- const std::string& memoryName,
- const std::string& coreSegmentName,
- const std::string& providerSegmentName,
- const std::string& entityName,
- Time timestamp,
- int instanceIndex) :
+ MemoryID::MemoryID(const std::string& memoryName,
+ const std::string& coreSegmentName,
+ const std::string& providerSegmentName,
+ const std::string& entityName,
+ Time timestamp,
+ int instanceIndex) :
memoryName(memoryName),
coreSegmentName(coreSegmentName),
providerSegmentName(providerSegmentName),
@@ -97,13 +98,14 @@ namespace armarx::armem
{
}
-
- std::string MemoryID::str(bool escapeDelimiters) const
+ std::string
+ MemoryID::str(bool escapeDelimiters) const
{
return str(delimiter, escapeDelimiters);
}
- std::string MemoryID::str(const std::string& delimiter, bool escapeDelimiter) const
+ std::string
+ MemoryID::str(const std::string& delimiter, bool escapeDelimiter) const
{
std::vector<std::string> items = getAllItems(escapeDelimiter);
while (items.size() > 0 && items.back().empty())
@@ -113,7 +115,8 @@ namespace armarx::armem
return simox::alg::join(items, delimiter, false, false);
}
- std::string MemoryID::getLeafItem() const
+ std::string
+ MemoryID::getLeafItem() const
{
std::vector<std::string> items = getAllItems();
for (auto it = items.rbegin(); it != items.rend(); ++it)
@@ -126,7 +129,33 @@ namespace armarx::armem
return "";
}
- bool MemoryID::hasGap() const
+ MemoryID MemoryID::cleanID() const
+ {
+ std::vector<std::string> allItems = this->getAllItems(true);
+ std::string newID = "";
+ for(int i = 0; i < allItems.size(); i++){
+ if(!allItems.at(i).empty()){
+ std::string toAppend = allItems.at(i);
+ if(allItems.at(i).find(delimiter) != std::string::npos){
+ size_t pos = 0;
+ std::string token;
+ while ((pos = toAppend.find(delimiter)) != std::string::npos) {
+ token = toAppend.substr(0, pos);
+ toAppend.erase(0, pos + delimiter.length());
+ }
+ }
+ newID.append(toAppend);
+ if(i < allItems.size() - 1 && !allItems.at(i+1).empty()){
+ newID.append(delimiter);
+ }
+ }
+ }
+ return MemoryID(newID);
+ }
+
+
+ bool
+ MemoryID::hasGap() const
{
bool emptyFound = false;
for (const std::string& item : getAllItems())
@@ -144,18 +173,59 @@ namespace armarx::armem
return false;
}
- bool MemoryID::isWellDefined() const
+ bool
+ MemoryID::isWellDefined() const
{
return !hasGap();
}
-
- MemoryID MemoryID::fromString(const std::string& string)
+ MemoryID
+ MemoryID::fromString(const std::string& string)
{
return MemoryID(string);
}
- std::vector<std::string> MemoryID::getItems(bool escapeDelimiters) const
+ MemoryID
+ MemoryID::fromItems(const std::vector<std::string>& items)
+ {
+ MemoryID id;
+
+ std::size_t i = 0;
+ if (i < items.size())
+ {
+ id.memoryName = items[i];
+ ++i;
+ }
+ if (i < items.size())
+ {
+ id.coreSegmentName = items[i];
+ ++i;
+ }
+ if (i < items.size())
+ {
+ id.providerSegmentName = items[i];
+ ++i;
+ }
+ if (i < items.size())
+ {
+ id.entityName = items[i];
+ ++i;
+ }
+ if (i < items.size())
+ {
+ id.timestamp = timestampFromStr(items[i]);
+ ++i;
+ }
+ if (i < items.size())
+ {
+ id.instanceIndex = instanceIndexFromStr(items[i]);
+ ++i;
+ }
+ return id;
+ }
+
+ std::vector<std::string>
+ MemoryID::getItems(bool escapeDelimiters) const
{
std::vector<std::string> items;
@@ -194,59 +264,69 @@ namespace armarx::armem
return items;
}
- std::vector<std::string> MemoryID::getAllItems(bool escapeDelimiters) const
+ std::vector<std::string>
+ MemoryID::getAllItems(bool escapeDelimiters) const
{
- return
- {
- escape(memoryName, escapeDelimiters), escape(coreSegmentName, escapeDelimiters),
- escape(providerSegmentName, escapeDelimiters), escape(entityName, escapeDelimiters),
- timestampStr(), instanceIndexStr()
+ return {
+ escape(memoryName, escapeDelimiters),
+ escape(coreSegmentName, escapeDelimiters),
+ escape(providerSegmentName, escapeDelimiters),
+ escape(entityName, escapeDelimiters),
+ timestampStr(),
+ instanceIndexStr(),
};
}
- MemoryID MemoryID::getMemoryID() const
+ MemoryID
+ MemoryID::getMemoryID() const
{
MemoryID id;
id.memoryName = memoryName;
return id;
}
- MemoryID MemoryID::getCoreSegmentID() const
+ MemoryID
+ MemoryID::getCoreSegmentID() const
{
MemoryID id = getMemoryID();
id.coreSegmentName = coreSegmentName;
return id;
}
- MemoryID MemoryID::getProviderSegmentID() const
+ MemoryID
+ MemoryID::getProviderSegmentID() const
{
MemoryID id = getCoreSegmentID();
id.providerSegmentName = providerSegmentName;
return id;
}
- MemoryID MemoryID::getEntityID() const
+ MemoryID
+ MemoryID::getEntityID() const
{
MemoryID id = getProviderSegmentID();
id.entityName = entityName;
return id;
}
- MemoryID MemoryID::getEntitySnapshotID() const
+ MemoryID
+ MemoryID::getEntitySnapshotID() const
{
MemoryID id = getEntityID();
id.timestamp = timestamp;
return id;
}
- MemoryID MemoryID::getEntityInstanceID() const
+ MemoryID
+ MemoryID::getEntityInstanceID() const
{
MemoryID id = getEntitySnapshotID();
id.instanceIndex = instanceIndex;
return id;
}
- MemoryID MemoryID::removeLeafItem() const
+ MemoryID
+ MemoryID::removeLeafItem() const
{
if (instanceIndex != -1)
{
@@ -268,118 +348,133 @@ namespace armarx::armem
{
return getMemoryID();
}
- return {}; // return empty if already empty. Client needs to check (Avoids using optional as additional include)
+ return {}; // return empty if already empty. Client needs to check (Avoids using optional as additional include)
}
- void MemoryID::setMemoryID(const MemoryID& id)
+ void
+ MemoryID::setMemoryID(const MemoryID& id)
{
memoryName = id.memoryName;
}
- void MemoryID::setCoreSegmentID(const MemoryID& id)
+ void
+ MemoryID::setCoreSegmentID(const MemoryID& id)
{
setMemoryID(id);
coreSegmentName = id.coreSegmentName;
}
- void MemoryID::setProviderSegmentID(const MemoryID& id)
+ void
+ MemoryID::setProviderSegmentID(const MemoryID& id)
{
setCoreSegmentID(id);
providerSegmentName = id.providerSegmentName;
}
- void MemoryID::setEntityID(const MemoryID& id)
+ void
+ MemoryID::setEntityID(const MemoryID& id)
{
setProviderSegmentID(id);
entityName = id.entityName;
}
- void MemoryID::setEntitySnapshotID(const MemoryID& id)
+ void
+ MemoryID::setEntitySnapshotID(const MemoryID& id)
{
setEntityID(id);
timestamp = id.timestamp;
}
- void MemoryID::setEntityInstanceID(const MemoryID& id)
+ void
+ MemoryID::setEntityInstanceID(const MemoryID& id)
{
setEntitySnapshotID(id);
instanceIndex = id.instanceIndex;
}
- MemoryID MemoryID::withMemoryName(const std::string& name) const
+ MemoryID
+ MemoryID::withMemoryName(const std::string& name) const
{
MemoryID id = *this;
id.memoryName = name;
return id;
}
- MemoryID MemoryID::withCoreSegmentName(const std::string& name) const
+ MemoryID
+ MemoryID::withCoreSegmentName(const std::string& name) const
{
MemoryID id = *this;
id.coreSegmentName = name;
return id;
}
- MemoryID MemoryID::withProviderSegmentName(const std::string& name) const
+ MemoryID
+ MemoryID::withProviderSegmentName(const std::string& name) const
{
MemoryID id = *this;
id.providerSegmentName = name;
return id;
}
- MemoryID MemoryID::withEntityName(const std::string& name) const
+ MemoryID
+ MemoryID::withEntityName(const std::string& name) const
{
MemoryID id = *this;
id.entityName = name;
return id;
}
- MemoryID MemoryID::withTimestamp(Time time) const
+ MemoryID
+ MemoryID::withTimestamp(Time time) const
{
MemoryID id = *this;
id.timestamp = time;
return id;
}
- MemoryID MemoryID::withInstanceIndex(int index) const
+ MemoryID
+ MemoryID::withInstanceIndex(int index) const
{
MemoryID id = *this;
id.instanceIndex = index;
return id;
}
-
- std::string MemoryID::timestampStr() const
+ std::string
+ MemoryID::timestampStr() const
{
return hasTimestamp() ? std::to_string(timestamp.toMicroSecondsSinceEpoch()) : "";
}
- std::string MemoryID::instanceIndexStr() const
+ std::string
+ MemoryID::instanceIndexStr() const
{
return hasInstanceIndex() ? std::to_string(instanceIndex) : "";
}
- Time MemoryID::timestampFromStr(const std::string& string)
+ Time
+ MemoryID::timestampFromStr(const std::string& string)
{
return Time(Duration::MicroSeconds(parseInteger(string, "timestamp")));
}
- int MemoryID::instanceIndexFromStr(const std::string& string)
+ int
+ MemoryID::instanceIndexFromStr(const std::string& string)
{
return int(parseInteger(string, "instance index"));
}
- bool MemoryID::operator==(const MemoryID& other) const
+ bool
+ MemoryID::operator==(const MemoryID& other) const
{
- return memoryName == other.memoryName
- and coreSegmentName == other.coreSegmentName
- and providerSegmentName == other.providerSegmentName
- and entityName == other.entityName
- and timestamp == other.timestamp
- and instanceIndex == other.instanceIndex;
+ return memoryName == other.memoryName and coreSegmentName == other.coreSegmentName and
+ providerSegmentName == other.providerSegmentName and
+ entityName == other.entityName and timestamp == other.timestamp and
+ instanceIndex == other.instanceIndex;
}
- bool MemoryID::operator<(const MemoryID& rhs) const
+ bool
+ MemoryID::operator<(const MemoryID& rhs) const
{
int c = memoryName.compare(rhs.memoryName);
if (c != 0)
@@ -413,7 +508,8 @@ namespace armarx::armem
return instanceIndex < rhs.instanceIndex;
}
- long long MemoryID::parseInteger(const std::string& string, const std::string& semanticName)
+ long long
+ MemoryID::parseInteger(const std::string& string, const std::string& semanticName)
{
if (string.empty())
{
@@ -433,12 +529,14 @@ namespace armarx::armem
}
}
- std::string MemoryID::escapeDelimiter(const std::string& name)
+ std::string
+ MemoryID::escapeDelimiter(const std::string& name)
{
return simox::alg::replace_all(name, delimiter, "\\" + delimiter);
}
- std::string MemoryID::escape(const std::string& name, bool escapeDelimiters)
+ std::string
+ MemoryID::escape(const std::string& name, bool escapeDelimiters)
{
if (escapeDelimiters)
{
@@ -450,24 +548,27 @@ namespace armarx::armem
}
}
- std::ostream& operator<<(std::ostream& os, const MemoryID id)
+ std::ostream&
+ operator<<(std::ostream& os, const MemoryID id)
{
return os << "'" << id.str() << "'";
}
-
- bool contains(const MemoryID& general, const MemoryID& specific)
+ bool
+ contains(const MemoryID& general, const MemoryID& specific)
{
if (!general.isWellDefined())
{
std::stringstream ss;
- ss << "\nGeneral ID " << general << " is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
+ ss << "\nGeneral ID " << general << " is not well-defined, which is required for `"
+ << __FUNCTION__ << "()`.";
throw error::InvalidMemoryID(general, ss.str());
}
if (!specific.isWellDefined())
{
std::stringstream ss;
- ss << "\nSpecific ID " << specific << " is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
+ ss << "\nSpecific ID " << specific << " is not well-defined, which is required for `"
+ << __FUNCTION__ << "()`.";
throw error::InvalidMemoryID(specific, ss.str());
}
@@ -528,4 +629,4 @@ namespace armarx::armem
return true;
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index 64850ddefa9d2e92cc924e33d765317a8895c067..e54617f4cd7d740b0b7cb95aea78c5b55a82b681 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -1,12 +1,11 @@
#pragma once
-#include <functional> // for std::hash
+#include <functional> // for std::hash
#include <string>
#include <vector>
#include "Time.h"
-
namespace armarx::armem
{
@@ -48,7 +47,6 @@ namespace armarx::armem
class MemoryID
{
public:
-
std::string memoryName = "";
std::string coreSegmentName = "";
std::string providerSegmentName = "";
@@ -58,7 +56,6 @@ namespace armarx::armem
public:
-
/// Construct a default (empty) memory ID.
MemoryID();
/// (Re-)Construct a memory ID from a string representation as returned by `str()`.
@@ -72,6 +69,13 @@ namespace armarx::armem
int instanceIndex = -1);
+ /// Alias for constructor from string.
+ static MemoryID fromString(const std::string& string);
+
+ /// Constructor memory ID from items as returned by getItems().
+ static MemoryID fromItems(const std::vector<std::string>& items);
+
+
/**
* @brief Indicate whether this ID is well-defined.
*
@@ -93,43 +97,56 @@ namespace armarx::armem
*/
bool isWellDefined() const;
-
// Checks whether a specific level is specified.
- bool hasMemoryName() const
+ bool
+ hasMemoryName() const
{
return not memoryName.empty();
}
- bool hasCoreSegmentName() const
+
+ bool
+ hasCoreSegmentName() const
{
return not coreSegmentName.empty();
}
- bool hasProviderSegmentName() const
+
+ bool
+ hasProviderSegmentName() const
{
return not providerSegmentName.empty();
}
- bool hasEntityName() const
+
+ bool
+ hasEntityName() const
{
return not entityName.empty();
}
- bool hasTimestamp() const
+
+ bool
+ hasTimestamp() const
{
return timestamp.isValid();
}
- void clearTimestamp()
+
+ void
+ clearTimestamp()
{
timestamp = Time::Invalid();
}
- bool hasInstanceIndex() const
+
+ bool
+ hasInstanceIndex() const
{
return instanceIndex >= 0;
}
- void clearInstanceIndex()
+
+ void
+ clearInstanceIndex()
{
instanceIndex = -1;
}
-
// Slice getters: Get upper part of the ID.
MemoryID getMemoryID() const;
@@ -182,8 +199,6 @@ namespace armarx::armem
/// Get the instance index as string.
std::string instanceIndexStr() const;
- /// Alias for constructor from string.
- static MemoryID fromString(const std::string& string);
/// Reconstruct a timestamp from a string as returned by `timestampStr()`.
static Time timestampFromStr(const std::string& timestamp);
/// Reconstruct an instance index from a string as returned by `instanceIndexStr()`.
@@ -203,34 +218,43 @@ namespace armarx::armem
/// Get the lowest defined level (or empty string if there is none).
std::string getLeafItem() const;
+ MemoryID cleanID() const;
+
// Operators
- bool operator ==(const MemoryID& other) const;
- inline bool operator !=(const MemoryID& other) const
+ bool operator==(const MemoryID& other) const;
+
+ inline bool
+ operator!=(const MemoryID& other) const
{
- return not (*this == other);
+ return not(*this == other);
}
- bool operator< (const MemoryID& rhs) const;
- inline bool operator> (const MemoryID& rhs) const
+ bool operator<(const MemoryID& rhs) const;
+
+ inline bool
+ operator>(const MemoryID& rhs) const
{
return rhs < (*this);
}
- inline bool operator<=(const MemoryID& rhs) const
+
+ inline bool
+ operator<=(const MemoryID& rhs) const
{
- return not operator> (rhs);
+ return not operator>(rhs);
}
- inline bool operator>=(const MemoryID& rhs) const
+
+ inline bool
+ operator>=(const MemoryID& rhs) const
{
- return not operator< (rhs);
+ return not operator<(rhs);
}
friend std::ostream& operator<<(std::ostream& os, const MemoryID id);
private:
-
static long long parseInteger(const std::string& string, const std::string& semanticName);
static std::string escapeDelimiter(const std::string& name);
static std::string escape(const std::string& name, bool escapeDelimiters);
@@ -240,10 +264,8 @@ namespace armarx::armem
// Do not allow specifying the delimiter from outside.
std::string str(const std::string& delimiter, bool escapeDelimiter) const;
-
};
-
/**
* @brief Indicates whether `general` is "less specific" than, or equal to, `specific`,
* i.e. `general` "contains" `specific`.
@@ -266,8 +288,7 @@ namespace armarx::armem
*/
bool contains(const MemoryID& general, const MemoryID& specific);
-} // namespace armarx::armem
-
+} // namespace armarx::armem
namespace std
{
@@ -275,12 +296,12 @@ namespace std
template <>
struct hash<armarx::armem::MemoryID>
{
- std::size_t operator()(const armarx::armem::MemoryID& id) const
+ std::size_t
+ operator()(const armarx::armem::MemoryID& id) const
{
const std::string sid = id.str();
return std::hash<string>()(sid);
}
};
-}
-
+} // namespace std
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index 2fb8bd3cc6ffcb217f7882e66fa2ed1e1b52d429..c1eeea7275cecc1f4dd2c76d7723720681fcead1 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -269,16 +269,7 @@ namespace armarx::armem::base
EntityInstanceT&
addInstance(EntityInstanceT&& instance)
{
- if (instance.index() > 0 &&
- static_cast<size_t>(instance.index()) < this->_container.size())
- {
- throw error::InvalidArgument(
- std::to_string(instance.index()),
- "EntitySnapshot::addInstance",
- "Cannot add an EntityInstance because its index already exists.");
- }
- if (instance.index() > 0 &&
- static_cast<size_t>(instance.index()) > this->_container.size())
+ if (instance.index() > 0 && static_cast<size_t>(instance.index()) > this->_container.size())
{
throw error::InvalidArgument(
std::to_string(instance.index()),
@@ -286,8 +277,9 @@ namespace armarx::armem::base
"Cannot add an EntityInstance because its index is too big.");
}
- int index = static_cast<int>(this->_container.size());
+ int index = instance.index();
EntityInstanceT& added = this->_container.emplace_back(std::move(instance));
+
added.id() = this->id().withInstanceIndex(index);
return added;
}
@@ -295,7 +287,8 @@ namespace armarx::armem::base
EntityInstanceT&
addInstance()
{
- int index = static_cast<int>(this->size());
+ //ARMARX_INFO << "Trying to add an instance without instance yet generated";
+ int index = static_cast<int>(this->size()); //this is the problem, because instances have names 0 and 11 (I do not know why)
EntityInstanceT& added = this->_container.emplace_back(EntityInstanceT());
added.id() = this->id().withInstanceIndex(index);
return added;
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
index 9f5190364d66b999fa9d15d64bf9c12454d05715..07930b46e931513cd42f4a6dcdaacd5a3f80a56d 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
@@ -2,6 +2,7 @@
#include <functional>
#include <type_traits>
+#include "ArmarXCore/core/logging/Logging.h"
#include <RobotAPI/libraries/armem/core/MemoryID.h>
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp
index 5856407816fe423e2f93bd0375a631863c304a89..e72b21abffa5e571ffab8437d35ce6026dd1ab62 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp
@@ -52,6 +52,7 @@ namespace armarx::armem::wm
toMemory(Memory& m, const std::vector<EntitySnapshot>& e)
{
m.clear();
+
for (const auto& s : e)
{
if (!m.hasCoreSegment(s.id().coreSegmentName))
@@ -108,4 +109,42 @@ namespace armarx::armem::wm
sn->addInstance(s);
}
}
+
+ void toMemory(Memory &m, const armarx::armem::server::wm::Memory &structure, const std::vector<EntitySnapshot> &e)
+ {
+ // create an empty working memory:
+ m.clear();
+
+ for (const auto& s : e)
+ {
+ //get name of the core segment:
+ auto coreSegmentName = s.id().coreSegmentName;
+ auto* coreStructure = structure.findCoreSegment(coreSegmentName);
+
+ //if m does not have this core segment yet add the segment but copy the structure from the server wm:
+ if (!m.hasCoreSegment(coreSegmentName))
+ {
+ m.addCoreSegment(coreSegmentName, coreStructure->aronType());
+ }
+ auto* c = m.findCoreSegment(coreSegmentName);
+
+ //get name of provider segment:
+ auto providerSegmentName = s.id().providerSegmentName;
+ auto* providerStructure = coreStructure->findProviderSegment(providerSegmentName);
+
+ if (!c->hasProviderSegment(providerSegmentName))
+ {
+ c->addProviderSegment(providerSegmentName, providerStructure->aronType());
+ }
+ auto* p = c->findProviderSegment(providerSegmentName);
+
+ if (!p->hasEntity(s.id().entityName))
+ {
+ p->addEntity(s.id().entityName);
+ }
+ auto* en = p->findEntity(s.id().entityName);
+ en->addSnapshot(s);
+ }
+ }
+
} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h
index 53ac02bfc5536150b2d0f01b9457537bb236d95a..7ea40b5f54eb2a994eb98cd5f278ee29582ce11b 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h
@@ -3,6 +3,7 @@
#include <vector>
#include "memory_definitions.h"
+#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
namespace armarx::armem::wm
{
@@ -10,5 +11,15 @@ namespace armarx::armem::wm
void toMemory(Memory& m, const std::vector<ProviderSegment>& e);
void toMemory(Memory& m, const std::vector<Entity>& e);
void toMemory(Memory& m, const std::vector<EntitySnapshot>& e);
+
+ /**
+ * @brief toMemory converts a vector of entity snapshots into a working memory and considers
+ * the structure of the memory before
+ * @param m the (empty) new memory
+ * @param structure the server working memory for structure information
+ * @param e the vector of entity snapshots
+ */
+ void toMemory(Memory& m, const armarx::armem::server::wm::Memory& structure, const std::vector<EntitySnapshot>& e);
+
void toMemory(Memory& m, const std::vector<EntityInstance>& e);
} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
index de2a911560d8d13c2516db3f22348c59cfa38365..0e2544ab302ba69802c699a238a79e71952ccacc 100644
--- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
@@ -22,6 +22,7 @@ set(LIBS
RemoteGui
RobotAPI::aron
RobotAPI::armem
+ RobotAPI::aron::similarity
# LTM
RobotAPI::aron::converter::json
@@ -60,6 +61,8 @@ set(LIB_FILES
ltm/processors/filter/Filter.cpp
ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
ltm/processors/filter/equalityFilter/EqualityFilter.cpp
+ #ltm/processors/filter/similarityFilter/SimilarityFilter.cpp
+ ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
ltm/processors/extractor/Extractor.cpp
ltm/processors/extractor/imageExtractor/ImageExtractor.cpp
@@ -116,6 +119,8 @@ set(LIB_FILES
query_proc/wm/detail/CoreSegmentQueryProcessorBase.cpp
query_proc/wm/detail/MemoryQueryProcessorBase.cpp
query_proc/wm/wm.cpp
+
+ test/ForgettingExperiments.cpp
)
set(LIB_HEADERS
@@ -149,6 +154,8 @@ set(LIB_HEADERS
ltm/processors/filter/Filter.h
ltm/processors/filter/frequencyFilter/FrequencyFilter.h
ltm/processors/filter/equalityFilter/EqualityFilter.h
+ #ltm/processors/filter/similarityFilter/SimilarityFilter.h
+ ltm/processors/filter/importanceFilter/ImportanceFilter.h
ltm/processors/extractor/Extractor.h
ltm/processors/extractor/imageExtractor/ImageExtractor.h
@@ -208,6 +215,8 @@ set(LIB_HEADERS
query_proc/wm/detail/CoreSegmentQueryProcessorBase.h
query_proc/wm/detail/MemoryQueryProcessorBase.h
query_proc/wm/wm.h
+
+ test/ForgettingExperiments.h
)
# Clear variable set by CMakeLists.txt in parent directory.
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 194785757e80ecbb9b77b06ec9224f500c85ec55..a033f89b0a490ca8f041f9fb3cfcd1bbdf4f035b 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -1,5 +1,7 @@
#include "MemoryToIceAdapter.h"
+#include <thread>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -233,15 +235,14 @@ namespace armarx::armem::server
ARMARX_DEBUG << "The id " << snapshot.id() << " was removed from wm";
}
- // Consollidate to ltm
+
+ // Consollidate to ltm(s)
if (longtermMemory->isRecording())
{
- //ARMARX_IMPORTANT << longtermMemory->id().str();
- //ARMARX_IMPORTANT << longtermMemory->getPath();
-
// convert removedSnapshots to Memory
armem::wm::Memory m(longtermMemory->name());
- armem::wm::toMemory(m, updateResult.removedSnapshots);
+ armem::wm::toMemory(m, *workingMemory, updateResult.removedSnapshots);
+ //this removes information about segments, because new memory (wm) is used
// store memory
longtermMemory->store(m);
@@ -268,6 +269,10 @@ namespace armarx::armem::server
result.success = false;
result.errorMessage = e.what();
}
+ catch (...)
+ {
+ ARMARX_INFO << "Error during LTM consollidation";
+ }
}
if (publishUpdates)
@@ -399,7 +404,7 @@ namespace armarx::armem::server
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(longtermMemory);
ARMARX_IMPORTANT << "Enabling the recording of memory " << longtermMemory->id().str();
- longtermMemory->startRecording(); // TODO: config and execution time!
+ longtermMemory->startRecording();
dto::StartRecordResult ret;
ret.success = true;
@@ -413,7 +418,21 @@ namespace armarx::armem::server
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(longtermMemory);
ARMARX_IMPORTANT << "Disabling the recording of memory " << longtermMemory->id().str();
- longtermMemory->stopRecording();
+
+ //put calling stopRecording into a seperate thread and detach to make sure GUI does not freeze
+ auto ltm = longtermMemory;
+ std::thread stopRecordingThread([<m](){
+ ltm->stopRecording();
+ });
+ std::thread savingDataUpdateThread([<m](){
+ ltm->bufferFinished();
+ });
+ stopRecordingThread.detach();
+ savingDataUpdateThread.detach();
+
+ ARMARX_IMPORTANT
+ << "Stopped all LTM recordings, please wait with stopping the component until "
+ "all files are written";
dto::StopRecordResult ret;
ret.success = true;
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index f6fdfe9eaf1a985fe3ceba8a17c2665111333b0e..0ae95a4207d86842d1b11e82cd97af6030147401 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -25,7 +25,6 @@ namespace armarx::armem::server
void setMemoryListener(client::MemoryListenerInterfacePrx memoryListenerTopic);
-
// WRITING
data::AddSegmentResult addSegment(const data::AddSegmentInput& input,
bool addCoreSegments = false);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
index 5e0f1908bc411d1270503a273d62d8508707f4be..87575f5d3005a1cf63ec906c41c404d0fb0a2837 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
@@ -25,19 +25,6 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- /*if (connected() && collectionExists())
- {
- for (const auto& doc : getAllDocuments())
- {
- std::string id_str = doc[FOREIGN_KEY];
- armem::MemoryID segment_id(id_str);
- ProviderSegment c(
- getMemoryBasePath(), getSettings(), getExportName(), segment_id, processors);
- func(c);
- }
- } else */
-
- // legacy
if (fullPathExists())
{
for (const auto& subdirName : getAllDirectories())
@@ -59,17 +46,6 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- /*if (connected() && collectionExists())
- {
- auto c = ProviderSegment(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withProviderSegmentName(name),
- processors);
-
- return (bool)c.collectionExists();
- }*/
-
if (fullPathExists())
{
auto c = ProviderSegment(getMemoryBasePath(),
@@ -106,26 +82,7 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- e.id() = id();
-
- auto& conv = processors->defaultTypeConverter;
- auto setType = [&conv, &e](const std::vector<unsigned char>& aronJson)
- {
- auto typeAron = conv.convert(aronJson, "");
- e.aronType() = aron::type::Object::DynamicCastAndCheck(typeAron);
- };
-
- /*if (connected() && collectionExists())
- {
- // TODO:
- } else */
-
- if (std::string filename = TYPE_FILENAME + conv.suffix;
- fullPathExists() && fileExists(filename))
- {
- auto typeFileContent = readDataFromFile(filename);
- setType(typeFileContent);
- }
+ e.id() = id().getCoreSegmentID().cleanID();
forEachProviderSegment(
[&e](auto& x)
@@ -195,6 +152,8 @@ namespace armarx::armem::server::ltm
}
else
{
+ ARMARX_INFO << "CoreSegment does not have aron type, so aron type information "
+ "cannot be exported";
/*writeForeignKeyToPreviousDocument();*/
}
@@ -208,7 +167,6 @@ namespace armarx::armem::server::ltm
processors);
c.store(prov);
- statistics.recordedProviderSegments++;
});
}
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
index b34d9d5167b9f505d70e3c20caf8e317b7680be3..ac9433dfb78be5a973925feb9c917a67699f1d7e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
@@ -39,20 +39,6 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- /*if (connected() && collectionExists())
- {
- for (const auto& doc : getAllDocuments())
- {
- std::string id_str = doc[ID];
-
- armem::MemoryID segment_id = id();
- segment_id.timestampFromStr(id_str);
-
- EntitySnapshot c(
- getMemoryBasePath(), getSettings(), getExportName(), segment_id, processors);
- func(c);
- }
- } else */
if (fullPathExists())
{
@@ -201,15 +187,6 @@ namespace armarx::armem::server::ltm
Entity::hasSnapshot(const Time& n) const
{
std::lock_guard l(ltm_mutex);
- /*if (connected() && collectionExists())
- {
- auto c = EntitySnapshot(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withTimestamp(n),
- processors);
- return (bool)c.documentExists();
- }*/
if (fullPathExists())
{
@@ -379,7 +356,7 @@ namespace armarx::armem::server::ltm
Entity::_loadAllReferences(armem::wm::Entity& e)
{
std::lock_guard l(ltm_mutex);
- e.id() = id();
+ e.id() = id().getEntityID().cleanID();
forEachSnapshot(
[&e](auto& x)
@@ -450,8 +427,7 @@ namespace armarx::armem::server::ltm
if (hasSnapshot(snap.id().timestamp))
{
- ARMARX_INFO << deactivateSpam()
- << "Ignoring to put an EntitiySnapshot into the LTM because "
+ ARMARX_INFO << "Ignoring to put an EntitiySnapshot into the LTM because "
"the timestamp already existed (we assume snapshots are "
"const and do not change outside the ltm).";
return;
@@ -459,12 +435,14 @@ namespace armarx::armem::server::ltm
for (auto& f : processors->snapFilters)
{
- if (!f->accept(snap))
+
+ bool accepted = f->accept(snap);
+ if (!accepted)
{
- ARMARX_INFO << deactivateSpam()
- << "Ignoring to put an EntitiySnapshot into the LTM because it "
- "got filtered.";
+ //ARMARX_INFO << "Ignoring to put an EntitiySnapshot into the LTM because it got filtered.";
return;
+ } else {
+ //ARMARX_INFO << "Storing EntitySnapshot";
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
index b5800ec11fd3cfaa5f8c3fd3fcff6e4a4b5bf599..0be6975beb6785dbb2426b74c0d70000edf0ce27 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
@@ -31,16 +31,24 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
// assure that we put the right index to the snapshot
- ARMARX_CHECK(e.size() == (size_t)id().instanceIndex);
+ //ARMARX_CHECK(e.size() == (size_t)id().instanceIndex);
+ //int index = e.getInstanceIndices().size();
+
+ //ARMARX_INFO << VAROUT(getFullPath());
+ //ARMARX_INFO << VAROUT(getMemoryID().getEntityInstanceID().getLeafItem());
+ int index = std::stoi(getMemoryID().getEntityInstanceID().getLeafItem());
+
+ armem::wm::EntityInstance i = armem::wm::EntityInstance(index, e.id());
// add instance. Do not set data, since we only return references
- e.addInstance();
+ e.addInstance(i);
}
void
EntityInstance::_resolve(armem::wm::EntityInstance& e) const
{
std::lock_guard l(ltm_mutex);
+ //ARMARX_INFO << "resolve for entity instance " << e.id().str();
auto& dictConverter = processors->defaultObjectConverter;
aron::data::DictPtr datadict = nullptr;
@@ -89,6 +97,8 @@ namespace armarx::armem::server::ltm
}
} else */
+ //ARMARX_INFO << VAROUT(getFullPath());
+
if (fullPathExists())
{
@@ -183,13 +193,30 @@ namespace armarx::armem::server::ltm
auto metadataAron = std::make_shared<aron::data::Dict>();
to_aron(metadataAron, dataAron, e);
+ std::shared_ptr<aron::data::Dict> source;
+
+ bool saveAndExtract = false; //if true the data is saved in extracted form and in data.aron.json
+ // this is needed if several LTMs are recorded at once because otherwise the data from the data.aron.json
+ // is not there anymore to extract from
+
+ if (saveAndExtract)
+ {
+ source = dataAron->clone();
+ }
+ else
+ {
+ source = dataAron;
+ }
+
+ //ARMARX_INFO << "save and extract is " << saveAndExtract;
+
+
// check special members for special converters
for (auto& c : processors->converters)
{
ARMARX_CHECK_NOT_NULL(c);
ARMARX_CHECK_NOT_NULL(c->extractor);
-
- auto dataExt = c->extractor->extract(dataAron);
+ auto dataExt = c->extractor->extract(source);
for (const auto& [memberName, var] : dataExt.extraction)
{
@@ -202,8 +229,6 @@ namespace armarx::armem::server::ltm
ensureFileExists(filename, true);
writeDataToFile(filename, memberDataVec);
}
-
- dataAron = dataExt.dataWithoutExtraction;
}
// convert dict and metadata
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
index 693f015a20130142699eb5900eac0824a1728768..2f974697368c5a375dd9a27715f06cd1cc49dfaa 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
@@ -53,18 +53,10 @@ namespace armarx::armem::server::ltm
{
for (const auto& i : getAllDirectories())
{
- if (!util::fs::detail::isNumberString(i.filename()))
- {
- ARMARX_WARNING << "Found a non-index folder inside an entity '" << id().str()
- << "' with name '" << i.filename() << "'. "
- << "Ignoring this folder, however this is a bad situation.";
- continue;
- }
-
EntityInstance c(getMemoryBasePath(),
getSettings(),
getExportName(),
- id().withInstanceIndex(std::stoi(i.filename())),
+ id().withInstanceIndex(std::stoi(i.filename())).cleanID(),
processors);
func(c);
}
@@ -121,8 +113,11 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- e.id() = id();
- forEachInstance([&e](auto& x) { x.loadAllReferences(e); });
+ e.id() = id().getEntitySnapshotID().cleanID();
+
+ forEachInstance([&e](armem::server::ltm::EntityInstance& x) {
+ x.loadAllReferences(e);
+ });
}
void
@@ -148,6 +143,7 @@ namespace armarx::armem::server::ltm
void
EntitySnapshot::_store(const armem::wm::EntitySnapshot& p)
{
+
std::lock_guard l(ltm_mutex);
if (id().timestamp.isInvalid())
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
index 06f81e22c947328998187c84d79a7e8a1c5058c8..3f3000dd7b28717547e5721d52722cbf56a676f2 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
@@ -18,7 +18,8 @@ namespace armarx::armem::server::ltm
MongoDBStorageMixin::configureMixin(json);
}
- Memory::Memory() : Memory(std::filesystem::path("/tmp"), {}, "MemoryExport", "Test")
+ Memory::Memory() :
+ Memory(std::filesystem::path("/tmp/ARMARX/LTM_Exports"), {}, "MemoryExport", "Test")
{
}
@@ -41,6 +42,7 @@ namespace armarx::armem::server::ltm
DiskMemoryItemMixin(p, exportName, MemoryID(memoryName, "")),
MongoDBStorageMixin(s, exportName, MemoryID(memoryName, ""))
{
+ ARMARX_INFO << "Creating a new memory at " << p.string() << " and " << exportName;
}
void
@@ -62,6 +64,7 @@ namespace armarx::armem::server::ltm
{
BufferedBase::stop();
MongoDBStorageMixin::stop();
+ ARMARX_IMPORTANT << "Storing of data finished";
}
void
@@ -80,27 +83,14 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- // for each
- /*if (connected() && collectionExists())
- {
- for (const auto& doc : getAllDocuments())
- {
- std::string segmentName = doc[FOREIGN_KEY];
- CoreSegment c(getMemoryBasePath(),
- getSettings(),
- getExportName(),
- id().withCoreSegmentName(segmentName),
- processors);
- func(c);
- }
- } else */
-
// legacy: check fs
if (fullPathExists())
{
for (const auto& subdirName : getAllDirectories())
{
+
std::string segmentName = util::fs::detail::unescapeName(subdirName);
+ ARMARX_INFO << VAROUT(id().withCoreSegmentName(segmentName));
CoreSegment c(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -110,6 +100,8 @@ namespace armarx::armem::server::ltm
}
}
+ ARMARX_INFO << "All CoreSegments handeled";
+
return true;
}
@@ -167,12 +159,21 @@ namespace armarx::armem::server::ltm
processors);
}
+ void Memory::createPropertyDefinitions(PropertyDefinitionsPtr &defs, const std::string &prefix)
+ {
+ MemoryBase::createPropertyDefinitions(defs, prefix);
+ DiskMemoryBase::createPropertyDefinitions(defs, prefix);
+ BufferedBase::createPropertyDefinitions(defs, prefix);
+ }
+
void
Memory::_loadAllReferences(armem::wm::Memory& m)
{
std::lock_guard l(ltm_mutex);
- m.id() = id();
+ m.id() = id().getMemoryID();
+
+ ARMARX_INFO << VAROUT(id());
forEachCoreSegment(
[&m](auto& x)
@@ -193,6 +194,7 @@ namespace armarx::armem::server::ltm
m.forEachCoreSegment(
[&](auto& e)
{
+ //ARMARX_INFO << "resolve for CoreSegment " << e.id().str();
CoreSegment c(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -246,6 +248,6 @@ namespace armarx::armem::server::ltm
});
// 4. update cache
- CachedBase::addToCache(memory);
+ //CachedBase::addToCache(memory);
}
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Memory.h b/source/RobotAPI/libraries/armem/server/ltm/Memory.h
index 30e1929dd3156dfb0f9d4412ac78e44bb62ce664..67b16811b8b066661501c782d3d73902532c6cd8 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Memory.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/Memory.h
@@ -47,10 +47,15 @@ namespace armarx::armem::server::ltm
std::unique_ptr<CoreSegment>
findCoreSegment(const std::string& coreSegmentName) const final;
+ void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix) override;
+
protected:
void _loadAllReferences(armem::wm::Memory&) final;
void _resolve(armem::wm::Memory&) final;
void _store(const armem::wm::Memory&) final;
void _directlyStore(const armem::wm::Memory&) final;
+
+ private:
+ std::time_t current_date;
};
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
index 0cb941e1fa61aaef583b8d54d3005794039b47ed..cfb613221d44b6353a63e3ea1cebc148cc964a5a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
@@ -11,7 +11,6 @@ namespace armarx::armem::server::ltm
{
ProviderSegment::ProviderSegment(const detail::mixin::Path& p,
const detail::mixin::MongoDBSettings& s,
-
const std::string& exportName,
const armem::MemoryID& id /* UNESCAPED */,
const std::shared_ptr<Processors>& filters) :
@@ -44,6 +43,7 @@ namespace armarx::armem::server::ltm
for (const auto& subdirName : getAllDirectories())
{
+
std::string segmentName = util::fs::detail::unescapeName(subdirName);
Entity c(getMemoryBasePath(),
getSettings(),
@@ -73,6 +73,7 @@ namespace armarx::armem::server::ltm
if (fullPathExists())
{
+ ARMARX_INFO << VAROUT(id().getEntityID());
auto c = Entity(getMemoryBasePath(),
getSettings(),
getExportName(),
@@ -105,7 +106,9 @@ namespace armarx::armem::server::ltm
{
std::lock_guard l(ltm_mutex);
- e.id() = id();
+ e.id() = id().getProviderSegmentID().cleanID();
+
+ /*
auto& conv = processors->defaultTypeConverter;
auto setType = [&conv, &e](const std::vector<unsigned char>& aronJson)
@@ -114,11 +117,15 @@ namespace armarx::armem::server::ltm
e.aronType() = aron::type::Object::DynamicCastAndCheck(typeAron);
};
+ */
+
/*if (connected() && collectionExists())
{
// TODO:
} else */
+ /*
+
if (std::string filename = TYPE_FILENAME + conv.suffix;
fullPathExists() && fileExists(filename))
{
@@ -126,6 +133,8 @@ namespace armarx::armem::server::ltm
setType(typeFileContent);
}
+ */
+
forEachEntity(
[&e](auto& x)
{
@@ -194,6 +203,7 @@ namespace armarx::armem::server::ltm
}
else
{
+ ARMARX_INFO << "ProviderSegment does not seem to have an aron type, so aron type information connot be exported";
//writeForeignKeyToPreviousDocument();
}
@@ -209,6 +219,6 @@ namespace armarx::armem::server::ltm
c.store(e);
statistics.recordedEntities++;
});
- }
+ }
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
index 4ca25b8cb1944e345bf6c1f8c285b84c65ad648c..d79c7b3eefa22d8d6748564a839ec5b463d97778 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
@@ -2,11 +2,11 @@
#include <functional>
+
// BaseClass
#include "MemoryItem.h"
// ChildType
-#include "CoreSegmentBase.h"
// ArmarX
#include <ArmarXCore/core/Component.h>
@@ -18,6 +18,7 @@
#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/test/ForgettingExperiments.h>
namespace armarx::armem::server::ltm::detail
{
@@ -30,6 +31,10 @@ namespace armarx::armem::server::ltm::detail
{
armarx::core::time::DateTime lastEnabled = armarx::core::time::DateTime::Invalid();
long recordedCoreSegments = 0;
+ bool firstStart = true;
+ bool firstStop = true;
+ armarx::core::time::DateTime firstStarted = armarx::core::time::DateTime::Invalid();
+ armarx::core::time::DateTime firstStopped = armarx::core::time::DateTime::Invalid();
};
public:
@@ -46,6 +51,8 @@ namespace armarx::armem::server::ltm::detail
{
bool en = p.enabled_on_startup;
ARMARX_INFO << VAROUT(p.configuration_on_startup);
+ ARMARX_INFO << VAROUT(p.export_name);
+ this->setExportName(p.export_name);
try
{
@@ -72,8 +79,14 @@ namespace armarx::armem::server::ltm::detail
void
enable()
{
- ARMARX_INFO << "Enabling LTM " << id().str();
+ auto now = armarx::core::time::DateTime::Now();
+ ARMARX_INFO << "Enabling LTM " << id().str() << " at " << now.toDateTimeString();
+ ARMARX_INFO << "Storing information at " << this->getExportName();
enabled = true;
+ if(statistics.firstStart){
+ statistics.firstStarted = armarx::core::time::DateTime::Now();
+ statistics.firstStart = false;
+ }
this->_enable();
}
@@ -81,9 +94,15 @@ namespace armarx::armem::server::ltm::detail
void
disable()
{
- ARMARX_INFO << "Disabling LTM " << id().str();
+ auto now = armarx::core::time::DateTime::Now();
+ ARMARX_INFO << "Disabling LTM " << id().str() << " at " << now.toDateTimeString();
enabled = false;
+ if(statistics.firstStop){
+ statistics.firstStopped = armarx::core::time::DateTime::Now();
+ statistics.firstStop = false;
+ }
this->_disable();
+ ARMARX_INFO << "Disabling of LTM finished";
}
/// return the full ltm as a wm::Memory with only references
@@ -137,6 +156,7 @@ namespace armarx::armem::server::ltm::detail
store(const armem::wm::Memory& memory)
{
TIMING_START(LTM_Memory_Append);
+
for (auto& f : processors->memFilters)
{
if (!f->accept(memory))
@@ -173,7 +193,8 @@ namespace armarx::armem::server::ltm::detail
createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
{
defs->optional(p.enabled_on_startup, prefix + "enabled");
- defs->optional(p.configuration_on_startup, prefix + "configuration");
+ defs->optional(p.configuration_on_startup, prefix + "configuration");
+ defs->optional(p.export_name, prefix + "exportName");
}
/// enable/disable
@@ -181,12 +202,14 @@ namespace armarx::armem::server::ltm::detail
startRecording()
{
statistics.lastEnabled = armarx::core::time::DateTime::Now();
+
enable();
}
void
stopRecording()
{
+
disable();
}
@@ -211,7 +234,20 @@ namespace armarx::armem::server::ltm::detail
return statistics;
}
- /// get level name
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics>
+ getFilterStatistics(){
+ try{
+ ARMARX_INFO << "Trying to save statistics";
+ auto stats = processors->getSnapshotFilterStatistics();
+ return stats;
+ }catch(...){
+ ARMARX_WARNING << "Saving statistics did not work";
+ }
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics> emptyStatistics;
+ return emptyStatistics;
+ }
+
+ /// get level name1
static std::string
getLevelName()
{
@@ -250,7 +286,9 @@ namespace armarx::armem::server::ltm::detail
{
bool enabled_on_startup = false;
std::string configuration_on_startup =
- "{\"SnapshotFrequencyFilter\": { \"WaitingTimeInMs\": 1000}, \"PngConverter\": {}}";
+ "{ \"SnapshotFrequencyFilter\": {\"WaitingTimeInMsForFilter\" : 1000}, \"PngConverter\": {}}";
+ std::string export_name ="MemoryExport";
+ std::string export_path = "/tmp";
} p;
protected:
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h
index bf212bb37f252afb3e848c8fe322d74a4b550b22..821f131bb10b628380359b601aac4d02ff5dbcb7 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h
@@ -23,12 +23,12 @@ namespace armarx::armem::server::ltm::detail
void setMemoryID(const MemoryID&);
void setMemoryName(const std::string& memoryName);
- std::string
- getExportName() const
+ virtual std::string getExportName() const
{
return exportName;
}
+
MemoryID
getMemoryID() const
{
@@ -54,7 +54,7 @@ namespace armarx::armem::server::ltm::detail
std::shared_ptr<Processors> processors;
private:
- std::string exportName = "MemoryExport";
+ std::string exportName = "MemoryExportMI";
MemoryID _id;
};
} // namespace armarx::armem::server::ltm::detail
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
index 65021cdbb9a7487dc20f2ea930cb7a37d109e8fc..86f6306c700b9888636c6d7087cc849558a59417 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
@@ -70,8 +70,8 @@ namespace armarx::armem::server::ltm::detail
/// find entity segment
virtual std::shared_ptr<EntityT> findEntity(const std::string&) const = 0;
- aron::type::ObjectPtr
- aronType() const
+ ///get aron type
+ aron::type::ObjectPtr aronType() const
{
return nullptr;
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
index 0bbf62f24ab3dd9427a9854bfadf8fe048c98bb1..bc94ec838a5afda6a2d26740f068592bbf49a607 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
@@ -1,5 +1,9 @@
#pragma once
+#include <atomic>
+#include <chrono>
+#include <thread>
+
#include <SimoxUtility/json.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
@@ -31,6 +35,12 @@ namespace armarx::armem::server::ltm::detail::mixin
TIMING_END_STREAM(LTM_Memory_DirectlyStore, ARMARX_DEBUG);
}
+ void
+ bufferFinished()
+ {
+ //use this to count how much work is still left
+ }
+
protected:
void
setMixinMemoryID(const MemoryID& id)
@@ -51,6 +61,8 @@ namespace armarx::armem::server::ltm::detail::mixin
task = new armarx::PeriodicTask<BufferedMemoryMixin>(
this, &BufferedMemoryMixin::storeBuffer, waitingTimeMs);
task->start();
+ task->setDelayWarningTolerance(
+ waitingTimeMs); //a warning will be issued if the task takes longer than the waitingTime
}
}
@@ -62,6 +74,7 @@ namespace armarx::armem::server::ltm::detail::mixin
task->stop();
task = nullptr;
}
+
}
armem::wm::Memory
@@ -87,7 +100,12 @@ namespace armarx::armem::server::ltm::detail::mixin
return;
}
+ while (storeFlag.test_and_set(std::memory_order_acquire))
+ {
+ std::this_thread::yield();
+ }
this->directlyStore(*to_store);
+ storeFlag.clear(std::memory_order_release);
}
/// configuration
@@ -97,9 +115,17 @@ namespace armarx::armem::server::ltm::detail::mixin
if (json.find("BufferedMemory.storeFrequency") != json.end())
{
storeFrequency = json.at("BufferedMemory.storeFrequency");
+ ARMARX_INFO << "Setting store frequency from configuration json to "
+ << storeFrequency;
}
}
+ void
+ createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
+ {
+ defs->optional(storeFrequency, prefix + "storeFrequency");
+ }
+
virtual void _directlyStore(const armem::wm::Memory& memory) = 0;
void
@@ -109,6 +135,7 @@ namespace armarx::armem::server::ltm::detail::mixin
buffer->append(memory);
}
+
protected:
/// Internal memory for data consolidated from wm to ltm (double-buffer)
/// The to-put-to-ltm buffer (contains data in plain text)
@@ -116,6 +143,7 @@ namespace armarx::armem::server::ltm::detail::mixin
/// This means that it is not guaranteed that all data in the buffer will be stored in the ltm
std::unique_ptr<armem::wm::Memory> buffer;
std::unique_ptr<armem::wm::Memory> to_store;
+ std::atomic_flag storeFlag = ATOMIC_FLAG_INIT;
/// The frequency (Hz) to store data to the ltm
float storeFrequency = 10;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
index 8ceaf84eb19deaa0db3efdd5595179ed8d4bcbd4..19bfcd18bdb28870df2a9e69b1d0c74e387e07ba 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
@@ -23,15 +23,36 @@ namespace armarx::armem::server::ltm::detail::mixin
{
}
- void
- DiskMemoryItemMixin::setMemoryBasePath(const Path& n)
- {
- memoryBasePath = n;
+ Path
+ DiskMemoryItemMixin::addDateToMemoryBasePath(const Path& n) const
+ {
+ //set path to include date of creation as prefix:
+ auto current_date = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
+ std::tm* localTime = std::localtime(¤t_date);
+
+ // convert current time into string:
+ std::stringstream ss;
+ ss << std::put_time(localTime, "%Y_%m_%d");
+ std::string dateString = ss.str();
+
+ // change memory base path to include current date (date of memory creation)
+ std::filesystem::path current_base_path = n;
+ current_base_path.append(dateString);
+
+ //inform user about change:
+ ARMARX_DEBUG << "Changed memory base path to include current date of "
+ << dateString
+ << " to "
+ << this->getMemoryBasePath()
+ ;
+
+ return current_base_path;
}
void
DiskMemoryItemMixin::setMixinExportName(const std::string& n)
{
+ ARMARX_INFO << "Currently setting export name to: " << n;
exportName = n;
}
@@ -48,21 +69,31 @@ namespace armarx::armem::server::ltm::detail::mixin
{
if (json.find("DiskMemory.memoryParentPath") != json.end())
{
- memoryBasePath = Path(json.at("DiskMemory.memoryParentPath"));
+ //memoryBasePath = Path(json.at("DiskMemory.memoryParentPath"));
}
}
Path
DiskMemoryItemMixin::getMemoryBasePath() const
{
- return memoryBasePath;
+ if(memoryBasePathString.empty()){
+ return memoryBasePath;
+ } else {
+ std::filesystem::path newPath;
+ newPath.assign(memoryBasePathString);
+ return this->addDateToMemoryBasePath(newPath);
+ }
}
Path
DiskMemoryItemMixin::getFullPath() const
{
auto p = getMemoryBasePath() / exportName;
- return util::fs::toPath(p, _id);
+ //ARMARX_INFO << VAROUT(_id);
+ //ARMARX_INFO << VAROUT(_id.cleanID());
+ auto cleanID = _id.cleanID(); //somehow, the iDs are jumbled when loading the LTM from disk, this solves it for now
+
+ return util::fs::toPath(p, cleanID);
}
bool
@@ -134,4 +165,9 @@ namespace armarx::armem::server::ltm::detail::mixin
auto p = getFullPath();
return util::fs::getAllFiles(p);
}
+
+ void DiskMemoryItemMixin::createPropertyDefinitions(PropertyDefinitionsPtr &defs, const std::string &prefix)
+ {
+ defs->optional(memoryBasePathString, prefix + "exportPath");
+ }
} // namespace armarx::armem::server::ltm::detail::mixin
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.h
index b9550d4117a0c06675b3788e3c0065c040677fd3..bf02aa5dfdae044416a0d15e91cbdf18f6b1dd83 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.h
@@ -44,16 +44,20 @@ namespace armarx::armem::server::ltm::detail::mixin
std::vector<Path> getAllDirectories() const;
std::vector<Path> getAllFiles() const;
+ void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix);
+
protected:
// setter
void setMixinMemoryID(const MemoryID& n);
- void setMemoryBasePath(const std::filesystem::path& n);
void setMixinExportName(const std::string& n);
- /// configuration
+ // configuration
void configureMixin(const nlohmann::json& json);
+ //construct correct memory base path
+ Path addDateToMemoryBasePath(const std::filesystem::path& n) const;
+
public:
static const int DEPTH_TO_DATA_FILES =
7; // from memory folder = 1 (cseg) + 1 (pseg) + 1 (ent) + 3 (snap) + 1 (inst)
@@ -67,6 +71,7 @@ namespace armarx::armem::server::ltm::detail::mixin
private:
std::filesystem::path memoryBasePath;
+ std::string memoryBasePathString;
std::string exportName;
armem::MemoryID _id;
};
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h
index 9908e6c5b7f379be2ab7207a28610183c87395f5..5cb419cc05ccc2475bc1ae414b9303dbef25647c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h
@@ -8,15 +8,16 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <bsoncxx/builder/stream/array.hpp>
-#include <bsoncxx/builder/stream/document.hpp>
-#include <bsoncxx/builder/stream/helpers.hpp>
-#include <bsoncxx/json.hpp>
+
#include <mongocxx/client.hpp>
#include <mongocxx/instance.hpp>
#include <mongocxx/pool.hpp>
#include <mongocxx/stdx.hpp>
#include <mongocxx/uri.hpp>
+#include <bsoncxx/builder/stream/array.hpp>
+#include <bsoncxx/builder/stream/document.hpp>
+#include <bsoncxx/builder/stream/helpers.hpp>
+#include <bsoncxx/json.hpp>
namespace armarx::armem::server::ltm::util::mongodb
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
index 2b4f5afa549ab239b91d05ceafe3e49e13c2b36d..a1e16d2e35d2b6a77956799e6eb3545f8324e046 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
@@ -8,6 +8,7 @@
#include "extractor/imageExtractor/ImageExtractor.h"
#include "filter/equalityFilter/EqualityFilter.h"
#include "filter/frequencyFilter/FrequencyFilter.h"
+#include "filter/importanceFilter/ImportanceFilter.h"
namespace armarx::armem::server::ltm
{
@@ -15,21 +16,51 @@ namespace armarx::armem::server::ltm
Processors::configure(const nlohmann::json& config)
{
// Filters:
+ snapFilters.clear();
if (config.contains(processor::filter::SnapshotFrequencyFilter::NAME))
{
- ARMARX_IMPORTANT << "ADDING SNAPSHOT FILTER";
+ ARMARX_IMPORTANT << "ADDING SNAPSHOT FREQUENCY FILTER";
auto f = std::make_unique<processor::filter::SnapshotFrequencyFilter>();
f->configure(config[processor::filter::SnapshotFrequencyFilter::NAME]);
snapFilters.push_back(std::move(f));
}
+ if(config.contains(processor::filter::SnapshotSimilarityFilter::NAME)){
+ ARMARX_IMPORTANT << "ADDING SNAPSHOT SIMILARITY FILTER";
+ auto f = std::make_unique<processor::filter::SnapshotSimilarityFilter>();
+ f->configure(config[processor::filter::SnapshotSimilarityFilter::NAME]);
+ snapFilters.push_back(std::move(f));
+ }
+ if(config.contains(processor::filter::SnapshotImportanceFilter::NAME)){
+ ARMARX_IMPORTANT << "ADDING SNAPSHOT IMPORTANCE FILTER";
+ auto f = std::make_unique<processor::filter::SnapshotImportanceFilter>();
+ f->configure(config[processor::filter::SnapshotImportanceFilter::NAME]);
+ snapFilters.push_back(std::move(f));
+ }
// Converters
if (config.contains(processor::converter::data::image::PngConverter::NAME))
{
- ARMARX_IMPORTANT << "ADDING IMG CONVERTER";
+ ARMARX_IMPORTANT << "ADDING IMG CONVERTER PNG";
auto f = std::make_unique<processor::converter::data::image::PngConverter>();
f->configure(config[processor::converter::data::image::PngConverter::NAME]);
converters.push_back(std::move(f));
}
+ if (config.contains(processor::converter::data::image::ExrConverter::NAME))
+ {
+ ARMARX_IMPORTANT << "ADDING IMG CONVERTER EXR";
+ auto f = std::make_unique<processor::converter::data::image::ExrConverter>();
+ f->configure(config[processor::converter::data::image::ExrConverter::NAME]);
+ converters.push_back(std::move(f));
+ }
+ }
+
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics> Processors::getSnapshotFilterStatistics()
+ {
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics> stats;
+ ARMARX_INFO << "Number of active filters: " << snapFilters.size();
+ for(int i = 0; i < snapFilters.size(); i++){
+ stats[snapFilters.at(i)->getName()] = snapFilters.at(i)->getFilterStatistics();
+ }
+ return stats;
}
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h
index 0f4186a2c872f30d6ef1bb9b3ce7652232e0c669..b5dc5c618bb8eac2258fdac4f626c113959552ad 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h
@@ -23,6 +23,8 @@ namespace armarx::armem::server::ltm
void configure(const nlohmann::json& config);
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics> getSnapshotFilterStatistics();
+
public:
// Unique Memory Filters
std::vector<std::unique_ptr<processor::MemoryFilter>> memFilters;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h
index ce9cc7ab220fd794442b69c4b826b647122d022e..b65d01925c32dd8d45345c6dba78195713a83be5 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h
@@ -9,6 +9,9 @@ namespace armarx::armem::server::ltm::processor::converter::data::image
class ExrConverter : public ImageConverter
{
public:
+
+ static const constexpr char* NAME = "ExrConverter";
+
ExrConverter() :
ImageConverter(ConverterType::Binary,
"depthimage",
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.h
index 8fb622d281147068c2c01d4902258867ce3545e3..34904df5c275158b25f9aebd64e60a3ef4f96499 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/png/PngConverter.h
@@ -21,6 +21,7 @@ namespace armarx::armem::server::ltm::processor::converter::data::image
void configure(const nlohmann::json& json) override;
+
protected:
ConversionResult _convert(const aron::data::NDArrayPtr& data) final;
aron::data::NDArrayPtr _convert(const ConversionResult& data,
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp
index c4c53c68888b5fe576572ef8166f9a20807d27ba..c947d7896c1ff08d04389857eeab3b18e5e79fed 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp
@@ -11,4 +11,16 @@ namespace armarx::armem::server::ltm::processor
SnapshotFilter::configure(const nlohmann::json& json)
{
}
+
+ SnapshotFilter::FilterStatistics SnapshotFilter::getFilterStatistics()
+ {
+ return stats;
+ }
+
+ std::string SnapshotFilter::getName()
+ {
+ return "Base_Filter";
+ }
+
+
} // namespace armarx::armem::server::ltm::processor
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
index 9d84c0a0ffdd976b54deb4585279d28161223da6..9b32dc78511a9a9b1ae7bd819558ba43c963ee3c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
@@ -7,6 +7,7 @@
#include <SimoxUtility/json.h>
// ArmarX
+#include "RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h"
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
@@ -30,5 +31,20 @@ namespace armarx::armem::server::ltm::processor
virtual bool accept(const armem::wm::EntitySnapshot& e) = 0;
virtual void configure(const nlohmann::json& json);
+
+ struct FilterStatistics {
+ double accepted = 0;
+ double rejected = 0;
+ std::chrono::duration<double> additional_time = std::chrono::duration<double>::zero();
+ std::string additional_info = "";
+ aron::similarity::NDArraySimilarity::Type similarity_type;
+ std::chrono::high_resolution_clock::time_point start_time;
+ std::chrono::high_resolution_clock::time_point end_time;
+ int number_of_compared_objects = 2;
+ std::string importance_type = "";
+ } stats;
+
+ virtual FilterStatistics getFilterStatistics();
+ virtual std::string getName();
};
} // namespace armarx::armem::server::ltm::processor
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
index 92c8bb26ecf818492401e938b1c95cecb17519e1..715520f7b51d362cfab9df471ef677e5d926e464 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
@@ -1,51 +1,183 @@
#include "EqualityFilter.h"
+#include <list>
#include <IceUtil/Time.h>
+#include "RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h"
+#include "RobotAPI/libraries/aron/core/data/variant/primitive/Float.h"
+
namespace armarx::armem::server::ltm::processor::filter
{
bool
SnapshotSimilarityFilter::accept(const armem::wm::EntitySnapshot& e)
{
- auto entityID = e.id().getEntityID();
- auto genMs = e.time().toMilliSecondsSinceEpoch();
+ auto start = std::chrono::high_resolution_clock::now();
- long lastMs = 0;
- std::vector<aron::data::DictPtr> lastData;
- if (timestampLastCommitInMs.count(entityID) > 0)
- {
- lastData = dataLastCommit.at(entityID);
- lastMs = timestampLastCommitInMs.at(entityID);
+ int num_instances = 0;
+ std::vector<armarx::aron::data::NDArrayPtr> images_snapshot;
+ std::vector<armarx::aron::data::FloatPtr> floats_snapshot;
+ std::vector<float> distances;
+
+ e.forEachInstance([&num_instances, &images_snapshot, &floats_snapshot](armem::wm::EntityInstance& i){
+ auto data = aron::data::Dict::DynamicCastAndCheck(i.data());
+ for(auto key: data->getAllKeys()){
+ aron::data::Descriptor img_desc;
+ try{
+ auto d = data->at(key);
+; img_desc = data->at(key)->getDescriptor();
+ } catch (...){
+ ARMARX_INFO << "Problem with accessing image description";
+ img_desc = aron::data::Descriptor::NDARRAY;
+ }
+ if (img_desc == aron::data::Descriptor::NDARRAY){
+ auto img_nd = aron::data::NDArray::DynamicCastAndCheck(data->at(key));
+ images_snapshot.insert(images_snapshot.end(), img_nd);
+ num_instances++;
+ } else if (img_desc == aron::data::Descriptor::FLOAT){
+ auto fl = aron::data::Float::DynamicCastAndCheck(data->at(key));
+ floats_snapshot.push_back(fl);
+ num_instances++;
+ } else {
+ ARMARX_INFO << "data-type not yet supported. \n"
+ << "Only ndarray and float data types are supported for equality filters yet.";
+ }
+ }
+ });
+
+ if(images.size() < 2){
+ ARMARX_INFO << "Adding first images, because nothing to compare";
+ images.push_back(images_snapshot);
+ this->stats.accepted += 1;
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
+ return true;
+ } else if(images.size() < max_images){
+ ARMARX_INFO << "Not enough elements yet to do full comparison of last " << max_images
+ << " elements";
+ images.push_back(images_snapshot);
+ this->stats.accepted += 1;
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
+ return true;
}
- bool accept = false;
- std::vector<aron::data::DictPtr> genData;
- for (unsigned int i = 0; i != e.size(); ++i)
- {
- const auto& d = e.getInstance(i).data();
- genData.push_back(d);
-
- if (lastMs == 0 ||
- e.size() != lastData.size()) // nothing stored yet or we cannot compare
- {
- accept = true;
- break;
+ std::vector<armarx::aron::data::NDArrayPtr> lastCommittedImages;
+ int sizeOfCommited = 0;
+ for(int i = 0; i < max_images; i++){
+ std::vector<armarx::aron::data::NDArrayPtr> lastCommitImages = images.at(images.size() - i - 1);
+ sizeOfCommited = lastCommitImages.size();
+ for(int j = 0; j < lastCommitImages.size(); j++){
+ lastCommittedImages.push_back(lastCommitImages.at(j));
}
+ // we just concatenate all images (instances)
+ }
- const auto& el = lastData.at(i);
- if ((!d and el) || (d and !el) || (d && el && !(*d == *el))) // data unequal?
- {
- accept = true;
- break;
+ ARMARX_CHECK(sizeOfCommited == images_snapshot.size()); //make sure we have enough instances
+
+ for(int i= 0; i < images_snapshot.size(); i++){
+ armarx::aron::data::NDArrayPtr new_image = images_snapshot.at(i);
+ std::vector<armarx::aron::data::NDArrayPtr> commited_images;
+ for(int j = 0; j < max_images; j++){
+ int index = i + 2*j;
+ auto image = lastCommittedImages.at(index);
+ commited_images.emplace_back(image);
}
+
+ float distance = aron::similarity::NDArraySimilarity::calculate_similarity_multi(commited_images, new_image, this->similarity_type);
+
+ distances.insert(distances.end(), distance);
+ }
+
+ //check for criterion:
+ float sum_distances = 0;
+ float max_distance = 0;
+ for(auto d: distances){
+ sum_distances += d;
+ if(d > max_distance){
+ max_distance = d;
+ }
+ }
+
+ bool max = true; //set true if only maximum distance value is important and false if sum of distances is important
+ bool accept = false;
+
+ if(max){
+ accept = (max_distance > this->threshold);
+ } else {
+ accept = (sum_distances > this->threshold);
}
- if (!accept)
+ if(accept){
+ images.pop_front(); //delete first element
+ images.push_back(images_snapshot);
+ this->stats.accepted += 1;
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.additional_time += (end - start);
+ stats.end_time = end;
+ return true;
+ } else {
+ this->stats.rejected += 1;
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.additional_time += (end - start);
+ stats.end_time = end;
return false;
+ }
+ }
+
+ void SnapshotSimilarityFilter::configure(const nlohmann::json &json)
+ {
+ if (json.find(PARAM_THRESHOLD) != json.end())
+ {
+ threshold = json.at(PARAM_THRESHOLD);
+ ARMARX_INFO << VAROUT(threshold);
+ stats.additional_info += "Threshold-Parameter: ";
+ stats.additional_info += std::to_string(threshold);
+ }
+ if(json.find(PARAM_SIM_MEASURE) != json.end()){
+ std::string type_string = json.at(PARAM_SIM_MEASURE);
+ if(type_string == "MSE"){
+ this->similarity_type = aron::similarity::NDArraySimilarity::Type::MSE;
+ this->float_similarity_type = aron::similarity::FloatSimilarity::Type::MSE;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::MSE;
+ } else if (type_string == "MAE"){
+ this->similarity_type = aron::similarity::NDArraySimilarity::Type::MAE;
+ this->float_similarity_type = aron::similarity::FloatSimilarity::Type::MAE;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::MAE;
+ } else if (type_string == "Chernoff"){
+ this->similarity_type = aron::similarity::NDArraySimilarity::Type::CHERNOFF;
+ this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::CHERNOFF;
+ } else if (type_string == "Cosine"){
+ this->similarity_type = aron::similarity::NDArraySimilarity::Type::COSINE;
+ this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::COSINE;
+ } else {
+ ARMARX_WARNING << "Undefined similarity measure detected in JSON file";
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE;
+ this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
+ }
+ ARMARX_INFO << VAROUT(this->similarity_type);
+ }
+ if(json.find(PARAM_MAX_OBJECTS) != json.end()){
+ max_images = json.at(PARAM_MAX_OBJECTS);
+ ARMARX_INFO << VAROUT(max_images);
+ stats.number_of_compared_objects = max_images;
+ }
+
+ stats.start_time = std::chrono::high_resolution_clock::now();
+ }
+
+ SnapshotFilter::FilterStatistics SnapshotSimilarityFilter::getFilterStatistics()
+ {
+ return stats;
+ }
- dataLastCommit[entityID] = genData;
- timestampLastCommitInMs[entityID] = genMs;
- return true;
+ std::string SnapshotSimilarityFilter::getName()
+ {
+ return this->NAME;
}
+
} // namespace armarx::armem::server::ltm::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
index 203b1fe3fa87108b144c604a59ebf46e58ab350f..67448db2dba914f0d7de8f392433e55f03725026 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
@@ -8,6 +8,10 @@
// Aron
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include "RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h"
+#include "RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h"
+
+#include <chrono>
namespace armarx::armem::server::ltm::processor::filter
{
@@ -15,16 +19,28 @@ namespace armarx::armem::server::ltm::processor::filter
{
public:
static const constexpr char* NAME = "SnapshotSimilarityFilter";
+ static const constexpr char* PARAM_THRESHOLD = "Threshold";
+ static const constexpr char* PARAM_SIM_MEASURE = "SimilarityMeasure";
+ static const constexpr char* PARAM_MAX_OBJECTS = "NumberOfObjectsToCompare";
SnapshotSimilarityFilter() = default;
virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+ void configure(const nlohmann::json& json) override;
- public:
- // TODO link aron/similarity
+ FilterStatistics getFilterStatistics() override;
+ std::string getName() override;
private:
- std::map<MemoryID, std::vector<aron::data::DictPtr>> dataLastCommit;
+ //std::map<MemoryID, std::vector<aron::data::DictPtr>> dataLastCommit;
+ std::deque<std::vector<armarx::aron::data::NDArrayPtr>> images;
+ std::deque<std::vector<armarx::aron::data::FloatPtr>> floats;
std::map<MemoryID, long> timestampLastCommitInMs;
+ std::double_t threshold;
+ FilterStatistics stats;
+ int max_images = 2;
+ aron::similarity::NDArraySimilarity::Type similarity_type;
+ aron::similarity::FloatSimilarity::Type float_similarity_type;
+
};
} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
index 56c7250c37988efd1b5fc824aec8aabd2e040a17..aa773fc1faf83bb36a43801620def4508cf604cd 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
@@ -1,51 +1,49 @@
#include "FrequencyFilter.h"
-#include <IceUtil/Time.h>
+#include <chrono>
namespace armarx::armem::server::ltm::processor::filter
{
+
bool
- MemoryFrequencyFilter::accept(const armem::wm::Memory& e)
+ SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot &e)
{
- auto now = armem::Time::Now().toMilliSecondsSinceEpoch();
- if (waitingTimeInMs < 0 || (now - timestampLastCommitInMs) > waitingTimeInMs)
- {
- timestampLastCommitInMs = now;
- return true;
- }
- return false;
- }
- void
- MemoryFrequencyFilter::configure(const nlohmann::json& json)
- {
- if (json.find(PARAM_WAITING_TIME) != json.end())
- {
- waitingTimeInMs = json.at(PARAM_WAITING_TIME);
+ //accepting to many elements makes the filter slow and brings problems with the buffer with itself.
+ auto start = std::chrono::high_resolution_clock::now();
+ bool instances_accepted = false;
+ std::int64_t current;
+
+ if(this->lastTime < 1){ //this is the first instance to be saved
+ lastTime = e.time().toMilliSecondsSinceEpoch();
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
+ stats.accepted += 1;
+ return true; //the first one is always accepted
}
- }
- bool
- SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot& e)
- {
- auto entityID = e.id().getEntityID();
- auto genMs = e.time().toMilliSecondsSinceEpoch();
+ e.forEachInstance([this, &instances_accepted, ¤t](armem::wm::EntityInstance& i){
+ int difference = std::abs(i.metadata().sentTime.toMilliSecondsSinceEpoch() - lastTime);
+ if(difference > this->maxDifference){ //at least one instance is older than the last saved instance
+ instances_accepted = true;
+ current = i.metadata().sentTime.toMilliSecondsSinceEpoch();
+ }
+ });
- long lastMs = 0;
- if (timestampLastCommitInMs.count(entityID) > 0)
- {
- lastMs = timestampLastCommitInMs.at(entityID);
- }
+ //set stats:
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
- if (waitingTimeInMs < 0 || (genMs - lastMs) > waitingTimeInMs)
- {
- /*std::cout << "diff: " << (dataGeneratedInMs - timestampLastCommitInMs) << std::endl;
- std::cout << "gen: " << (dataGeneratedInMs) << std::endl;
- std::cout << "last: " << (timestampLastCommitInMs) << std::endl;*/
- timestampLastCommitInMs[entityID] = genMs;
- return true;
+ if(instances_accepted){
+ lastTime = current;
+ this->stats.accepted += 1;
+ } else {
+ this->stats.rejected += 1;
}
- return false;
+
+ return instances_accepted;
}
void
@@ -53,7 +51,23 @@ namespace armarx::armem::server::ltm::processor::filter
{
if (json.find(PARAM_WAITING_TIME) != json.end())
{
- waitingTimeInMs = json.at(PARAM_WAITING_TIME);
+ this->maxDifference = json.at(PARAM_WAITING_TIME);
+ ARMARX_INFO << VAROUT(maxDifference);
+ stats.additional_info = "Max Difference in ms: " + std::to_string(this->maxDifference);
}
+ stats.start_time = std::chrono::high_resolution_clock::now();
+ stats.number_of_compared_objects = 1;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE; //information for statistics export
+ }
+
+ SnapshotFilter::FilterStatistics SnapshotFrequencyFilter::getFilterStatistics()
+ {
+ stats.end_time = std::chrono::high_resolution_clock::now();
+ return this->stats;
+ }
+
+ std::string SnapshotFrequencyFilter::getName()
+ {
+ return this->NAME;
}
} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
index 00ba1e9db0162d4d866a359c920008838ad5e279..1b778528ebd0812c91dada5f1e275f9bfcfa44ca 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
@@ -1,45 +1,28 @@
#pragma once
-#include <map>
-
// Base Class
#include "../Filter.h"
namespace armarx::armem::server::ltm::processor::filter
{
- class MemoryFrequencyFilter : public MemoryFilter
- {
- public:
- static const constexpr char* NAME = "MemoryFrequencyFilter";
- static const constexpr char* PARAM_WAITING_TIME = "WaitingTimeInMs";
-
- MemoryFrequencyFilter() = default;
-
- virtual bool accept(const armem::wm::Memory& e) override;
- void configure(const nlohmann::json& json) override;
-
- public:
- int waitingTimeInMs = -1;
-
- private:
- long timestampLastCommitInMs = 0;
- };
class SnapshotFrequencyFilter : public SnapshotFilter
{
public:
static const constexpr char* NAME = "SnapshotFrequencyFilter";
- static const constexpr char* PARAM_WAITING_TIME = "WaitingTimeInMs";
+ static const constexpr char* PARAM_WAITING_TIME = "WaitingTimeInMsForFilter";
SnapshotFrequencyFilter() = default;
- virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+ virtual bool accept(const armem::wm::EntitySnapshot &e) override;
void configure(const nlohmann::json& json) override;
- public:
- int waitingTimeInMs = -1;
+ FilterStatistics getFilterStatistics() override;
+ std::string getName() override;
private:
- std::map<MemoryID, long> timestampLastCommitInMs;
+ int maxDifference = 0;
+ std::int64_t lastTime = 0;
+
};
} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ae255a4d4488048c7786aa1fafa3d67f2538f77
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
@@ -0,0 +1,84 @@
+#include "ImportanceFilter.h"
+
+
+namespace armarx::armem::server::ltm::processor::filter
+{
+
+bool SnapshotImportanceFilter::accept(const armem::wm::EntitySnapshot &e)
+{
+ auto start = std::chrono::high_resolution_clock::now();
+ bool instances_accepted = false;
+
+ e.forEachInstance([this, &instances_accepted](armem::wm::EntityInstance& i){
+ if(!instances_accepted){ //if no instance was accepted yet
+ instances_accepted = important(i); //see if another instance triggers acceptance
+ }
+ });
+
+ //set stats:
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
+ if(instances_accepted){
+ stats.accepted += 1;
+ } else {
+ stats.rejected += 1;
+ }
+
+ return instances_accepted;
+}
+
+void SnapshotImportanceFilter::configure(const nlohmann::json &json)
+{
+ if(json.find(PARAM_THRESHOLD) != json.end()){
+ this->threshold = json.at(PARAM_THRESHOLD);
+ ARMARX_INFO << VAROUT(this->threshold);
+ stats.additional_info += "Threshold for importance: ";
+ stats.additional_info += std::to_string(this->threshold);
+ }
+ if(json.find(PARAM_TYPE) != json.end()){
+ auto t = json.at(PARAM_TYPE);
+ if(t == "Confidence"){
+ this->type = ImportanceType::CONFIDENCE;
+ }
+ if(t == "Accesses"){
+ this->type = ImportanceType::ACCESSES;
+ }
+ stats.importance_type = t;
+ ARMARX_INFO << VAROUT(stats.importance_type);
+ }
+
+ stats.start_time = std::chrono::high_resolution_clock::now();
+ stats.number_of_compared_objects = 1;
+ stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE; //information for statistics export
+}
+
+SnapshotFilter::FilterStatistics SnapshotImportanceFilter::getFilterStatistics()
+{
+ return this->stats;
+}
+
+std::string SnapshotImportanceFilter::getName()
+{
+ return this->NAME;
+}
+
+bool SnapshotImportanceFilter::important(armem::wm::EntityInstance &i)
+{
+ if(this->type == ImportanceType::CONFIDENCE){
+ auto c = i.metadata().confidence;
+ if ( c > this->threshold){
+ //for now the whole entity snapshot is accepted if at least one instance has a confidence higher than the threshold
+ return true;
+ }
+ } else if (this->type == ImportanceType::ACCESSES){
+ auto a = i.metadata().numAccessed;
+ if (a > this->threshold){
+ return true;
+ }
+ }
+ return false;
+}
+
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..929bb2e2ac6b03b31f9225cbda74b4a4b34f8e8d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h
@@ -0,0 +1,41 @@
+#pragma once
+
+// Base Class
+#include "../Filter.h"
+
+#include <chrono>
+
+namespace armarx::armem::server::ltm::processor::filter
+{
+
+enum ImportanceType{
+ CONFIDENCE,
+ ACCESSES
+};
+
+class SnapshotImportanceFilter : public SnapshotFilter
+{
+public:
+
+ static const constexpr char* NAME = "SnapshotImportanceFilter";
+ static const constexpr char* PARAM_THRESHOLD = "Threshold";
+ static const constexpr char* PARAM_TYPE = "Type";
+
+ SnapshotImportanceFilter() = default;
+
+ virtual bool accept(const armem::wm::EntitySnapshot& e) override;
+ void configure(const nlohmann::json& json) override;
+
+ FilterStatistics getFilterStatistics() override;
+ std::string getName() override;
+
+private:
+ double threshold = 1.0;
+ FilterStatistics stats;
+ ImportanceType type;
+
+ bool important(armem::wm::EntityInstance& i);
+
+};
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index d379cf033d76392d07417b0345aa09a3cca900bf..665d49ca87dbb6596e70f97530ff589da9b34338 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -8,6 +8,8 @@
#include <RobotAPI/libraries/armem/client/util/MemoryListener.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include <chrono>
+
namespace armarx::armem::server::plugins
{
@@ -41,7 +43,6 @@ namespace armarx::armem::server::plugins
workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
}
- // stuff for ltm
longtermMemory.createPropertyDefinitions(properties, prefix + "ltm.");
}
@@ -93,10 +94,35 @@ namespace armarx::armem::server::plugins
void
Plugin::preOnDisconnectComponent()
{
+ ARMARX_INFO << "Preparing to save statistics for " << this->workingMemory.name();
+ try{
+ auto first_stats = longtermMemory.getFilterStatistics();
+ std::map<std::string, std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>> information;
+ std::map<std::string, armarx::core::time::DateTime> times;
+
+ try{
+ times["Started LTM1"] = longtermMemory.getStatistics().firstStarted;
+ times["Stopped LTM1"] = longtermMemory.getStatistics().firstStopped;
+ information["LTM"] = first_stats;
+ } catch(...){
+ ARMARX_INFO << "Something went wrong after getting the statistics";
+ }
+ auto exportPath = longtermMemory.getMemoryBasePath();
+ auto exportName = longtermMemory.getExportName();
+ auto recording_started = longtermMemory.getStatistics().firstStarted;
+ auto recording_stopped = longtermMemory.getStatistics().firstStopped;
+ test::save_statistics(information, times, recording_started, recording_stopped,
+ exportPath, exportName, longtermMemory.name());
+ }catch (...){
+ ARMARX_INFO << "Something went wrong with the statistics saving process";
+ }
+
+ statistics_saved = true;
+
if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
{
removeServer();
- }
+ }
}
void
@@ -114,6 +140,7 @@ namespace armarx::armem::server::plugins
mns::dto::RegisterServerResult
Plugin::registerServer(armarx::Component& parent)
{
+ ARMARX_INFO << "Registering server for " << workingMemory.name();
ARMARX_TRACE;
MemoryID id = MemoryID().withMemoryName(workingMemory.name());
@@ -137,6 +164,8 @@ namespace armarx::armem::server::plugins
result.errorMessage = e.what();
ARMARX_WARNING << e.what();
}
+
+
return result;
}
@@ -152,6 +181,7 @@ namespace armarx::armem::server::plugins
result.success = true;
ARMARX_DEBUG << "Removed memory server for " << id
<< " from the Memory Name System (MNS).";
+
}
catch (const armem::error::ServerRegistrationOrRemovalFailed& e)
{
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
index e37c5e093232af4aae66114703a7debb8f122ba4..0712e4d9f2281adf2c435a7585a019b97907c9d7 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
@@ -85,6 +85,7 @@ namespace armarx::armem::server::plugins
std::atomic_bool initialized = false;
std::atomic_bool connected = false;
+ std::atomic_bool statistics_saved = false;
};
} // namespace armarx::armem::server::plugins
diff --git a/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0025f3e20582dea2ab6891cf11ec9b8b5584a989
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp
@@ -0,0 +1,280 @@
+#include "ForgettingExperiments.h"
+
+#include <iostream>
+#include <fstream>
+#include <chrono>
+#include <filesystem>
+
+namespace armarx::armem::server::test
+{
+
+void get_information_single_ltm(
+ std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>& filter
+ )
+{
+ /*
+ nlohmann::json info;
+ for(const auto& pair: filter.second){
+ if(pair.second.accepted + pair.second.rejected < 1){
+ ended_without_recording = true;
+ ARMARX_INFO << "Trying to not add JSON file because accepted + rejected < 1";
+ }
+ std::map<std::string, std::string> information;
+ information["Memory Name: "] = memoryName;
+ information["additional time needed: "] = (std::to_string(pair.second.additional_time.count()) + " sec");
+ information["number of accepted elements: "] = std::to_string(pair.second.accepted);
+ information["number of rejected elements: "] = std::to_string(pair.second.rejected);
+ information["Additional information: "] = pair.second.additional_info;
+ information["Similarity-Type: "] = std::to_string(pair.second.similarity_type);
+ information["Number of objects compared each time: "] = std::to_string((pair.second.number_of_compared_objects));
+ information["Importance type: "] = pair.second.importance_type;
+ auto time = std::chrono::high_resolution_clock::to_time_t(pair.second.start_time);
+ auto t = localtime(&time);
+ std::stringstream ss;
+ ss << std::put_time(t, "%Y-%m-%d %H:%M:%S");
+ std::string timeString = ss.str();
+ information["Filter instance created at: "] = timeString;
+ time = std::chrono::high_resolution_clock::to_time_t(pair.second.end_time);
+ t = localtime(&time);
+ std::stringstream s;
+ s << std::put_time(t, "%Y-%m-%d %H:%M:%S");
+ timeString = s.str();
+ information["Filter instance last used at: "] = timeString;
+ info[pair.first] = information;
+
+ armarx::core::time::DateTime start_time = time_stats["Started " + filter.first];
+ auto started = start_time.toDateTimeString();
+ info["Started"] = started;
+ ARMARX_INFO << started;
+
+ armarx::core::time::DateTime stop_time = time_stats["Stopped " + filter.first];
+ auto stopped = stop_time.toDateTimeString();
+ info["Stopped"] = stopped;
+ ARMARX_INFO << stopped;
+ }
+ jsonData[filter.first] = info;
+ info.clear();
+ */
+}
+
+void save_statistics(
+ std::map<std::string, std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>> stats,
+ std::map<std::string, armarx::core::time::DateTime> time_stats,
+ armarx::core::time::DateTime firstStartedRecording,
+ armarx::core::time::DateTime firstStoppedRecording,
+ std::filesystem::path exportPath,
+ std::string exportName,
+ std::string memoryName,
+ std::string sim_json_data,
+ std::string object_memory_json_data)
+{
+ ARMARX_INFO << "Saving statistics";
+
+ std::filesystem::path d_p;
+ try{
+ d_p = test::getStatisticsDirectory(exportPath, exportName, memoryName);
+ std::filesystem::create_directories(d_p);
+ ARMARX_INFO << "Experiments will be saved at: " << d_p.string();
+ } catch(...){
+ ARMARX_WARNING << "Creating needed directories for saving statistics did not work";
+ }
+
+ //json object for statistics:
+ nlohmann::json jsonData;
+
+ //get current date:
+ auto now = std::chrono::high_resolution_clock::now();
+ auto now_time = std::chrono::high_resolution_clock::to_time_t(now);
+ auto time = localtime(&now_time);
+
+ std::stringstream ss;
+ ss << std::put_time(time, "%Y-%m-%d %H:%M:%S");
+ std::string timeString = ss.str();
+ std::stringstream s;
+ s << std::put_time(time, "%Y%m%d%H%M%S");
+ std::string timeID = s.str();
+ jsonData["Statistics saved at"] = timeString;
+
+ bool ended_without_recording = false;
+
+ for(const auto& filter: stats){
+ filter_statistics(filter.second, memoryName,firstStartedRecording, firstStoppedRecording, &jsonData);
+ }
+
+ if(ended_without_recording){
+ return;
+ }
+
+ /*
+ if(sim_json_data.empty() || object_memory_json_data.empty()){
+ jsonData["Scene-Information for Simulation"] = load_scene_information("sim");
+ jsonData["Scene-Information for ObjectMemory"] = load_scene_information("om");
+ jsonData["Additional object in simulation"] = find_difference(jsonData["Scene-Information for ObjectMemory"], jsonData["Scene-Information for Simulation"]);
+ }else {
+ jsonData["Scene-Information for Simulation"] = sim_json_data;
+ jsonData["Scene-Information for ObjectMemory"] = object_memory_json_data;
+ }
+ */
+
+ std::string path = d_p.string();
+ path += "/tmp_wm_";
+ path += timeID;
+ path += ".json";
+ try{
+ std::ofstream outputFile;
+ outputFile.open(path);
+ if(outputFile.is_open()){
+ outputFile << jsonData.dump(4);
+ outputFile.close();
+ } else {
+ if(outputFile.fail()){
+ ARMARX_INFO << outputFile.rdstate();
+ outputFile.clear();
+ }
+ ARMARX_INFO << "File is not open";
+ }
+ } catch(...){
+ ARMARX_WARNING << "std::ofstream failed";
+ }
+
+ ARMARX_INFO << "Saving statistics completed";
+}
+
+nlohmann::json load_scene_information(std::string om_or_sim)
+{
+ std::string home_dir = getenv("HOME");
+ if(home_dir.empty()){
+ ARMARX_WARNING << "Trying to open home directory but env variable HOME is not set";
+ }
+ std::filesystem::path path = home_dir;
+ path /= "code";
+ path /= "h2t" ;
+ path /= "PriorKnowledgeData";
+ path /= "data";
+ path /= "PriorKnowledgeData";
+ path /= "scenes";
+ std::string name_for_current_scene = "ARMAR-III-kitchen"; //this can be changed later, maybe to something that indicates better that these are the changed files
+ std::string file_name = name_for_current_scene + "_" + om_or_sim + ".json";
+ path /= file_name;
+
+ std::ifstream ifs(path);
+ nlohmann::json json_data = nlohmann::json::parse(ifs);
+
+ return json_data;
+
+}
+
+nlohmann::json find_difference(nlohmann::json om, nlohmann::json sim)
+{
+ //the new object is at the last position of sim at the moment
+ auto objects = sim.at("objects");
+ int num_elements = objects.size();
+ auto object = objects[num_elements - 1];
+ return object;
+}
+
+/**
+ * @brief getStatisticsDirectory returns the path to where the statistics for this forgetting process
+ * should be saved
+ * @param memoryName name of the memory, e.g. VisionMemory
+ * @param exportName name of the export
+ * @param exportPath path to the export folder with date for this LTM
+ * @return path
+ */
+std::filesystem::path getStatisticsDirectory(
+ std::filesystem::path exportPath,
+ std::string exportName,
+ std::string memoryName)
+{
+ //path consists of exportPath/Date/exportName/memoryName with the date being the start date!
+ std::filesystem::path statisticsPath = exportPath;
+
+ //add parts to exportPath
+ statisticsPath /= exportName;
+ statisticsPath /= memoryName;
+
+ //add Statistics Directory:
+ statisticsPath /= "Statistics";
+
+ return statisticsPath;
+}
+
+void filter_statistics(
+ std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics> statistics_processors,
+ std::string memoryName,
+ armarx::core::time::DateTime firstStartedRecording,
+ armarx::core::time::DateTime firstStoppedRecording,
+ nlohmann::json *statistics)
+{
+ //TODO: start time of processor should be when enabling recording for filters... not creation
+ //TODO: make sure order stays the way it was when added
+ nlohmann::json ltm_information;
+
+ for(const auto& statistics_processor: statistics_processors){
+ nlohmann::json processor_information;
+
+ //get start time of the processor:
+ auto time = std::chrono::high_resolution_clock::to_time_t(statistics_processor.second.start_time);
+ auto t = localtime(&time);
+ std::stringstream ss;
+ ss << std::put_time(t, "%Y-%m-%d %H:%M:%S");
+ std::string timeStringStart = ss.str();
+ processor_information["Processor instance created at"] = timeStringStart;
+
+ //get end time of the processor:
+ auto timeEnd = std::chrono::high_resolution_clock::to_time_t(statistics_processor.second.end_time);
+ auto t_end = localtime(&timeEnd);
+ std::stringstream s;
+ s << std::put_time(t_end, "%Y-%m-%d %H:%M:%S");
+ std::string timeStringEnd = s.str();
+ processor_information["Processor instance last used at"] = timeStringEnd;
+
+ //get time the processor ran for in seconds:
+ long seconds = timeEnd - time;
+ processor_information["Lifetime of processor in sec"] = seconds;
+
+ //actual runtime in first recording:
+ processor_information["Processor started at"] = firstStartedRecording.toDateTimeString();
+ processor_information["Processor stopped at"] = firstStoppedRecording.toDateTimeString();
+ auto runtime = firstStoppedRecording.toMilliSecondsSinceEpoch() - firstStartedRecording.toMilliSecondsSinceEpoch();
+ auto seconds_runtime = runtime / 1000.0;
+ processor_information["Duration of recording in seconds"] = seconds_runtime;
+
+ //add additional information:
+ processor_information["Additional information"] = statistics_processor.second.additional_info;
+
+ //add number of accepted elements if this is a filter:
+ processor_information["Accepted elements"] = statistics_processor.second.accepted;
+
+ //add number of rejected elements if this is a filter:
+ processor_information["Rejected elements"] = statistics_processor.second.rejected;
+
+ //add additional time needed:
+ processor_information["Additional time needed in sec"] = statistics_processor.second.additional_time.count();
+
+ //add importance or similarity type if this type of filter:
+ auto importance_type = statistics_processor.second.importance_type;
+ if (!importance_type.empty()){
+ processor_information["Importance type"] = importance_type;
+ }
+ auto similarity_type = statistics_processor.second.similarity_type;
+ if(similarity_type != aron::similarity::NDArraySimilarity::Type::NONE){
+ processor_information["Similarity type"] = std::to_string(similarity_type);
+ //add number of objects compared each time if similarity filter:
+ processor_information["Number of objects compared each time"] =
+ statistics_processor.second.number_of_compared_objects;
+ }
+
+ //add memory name as a sanity check:
+ processor_information["Memory name"] = memoryName;
+
+ // add information about this specific processor to the information about the ltm:
+ ltm_information[statistics_processor.first] = processor_information;
+ }
+
+
+ // add the ltm information to the statistics json object:
+ (*statistics)["Episodic Memory Information"] = ltm_information;
+}
+
+}
diff --git a/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.h b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.h
new file mode 100644
index 0000000000000000000000000000000000000000..ce929849a4d05cbd343b915d7bad6279aeb0e057
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.h
@@ -0,0 +1,63 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Joana Plewnia ( uhfpm at student dot kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <map>
+#include <vector>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h>
+
+namespace armarx::armem::server::test
+{
+ void save_statistics(
+ std::map<std::string, std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>> stats,
+ std::map<std::string, armarx::core::time::DateTime> time_stats,
+ armarx::core::time::DateTime firstStartedRecording,
+ armarx::core::time::DateTime firstStoppedRecording,
+ std::filesystem::path exportPath = "/tmp",
+ std::string exportName ="MemoryExports",
+ std::string memoryName = "",
+ std::string sim_json_data = "",
+ std::string object_memory_json_data = ""
+ );
+
+ nlohmann::json load_scene_information(std::string om_or_sim);
+
+ nlohmann::json find_difference(nlohmann::json om, nlohmann::json sim);
+
+ std::filesystem::path getStatisticsDirectory(
+ std::filesystem::path exportPath = "/tmp",
+ std::string exportName ="MemoryExports",
+ std::string memoryName = "NoMemoryName");
+
+ nlohmann::json filter_statistics_comparison();
+
+ void filter_statistics(
+ std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics> statistics_processors,
+ std::string memoryName,
+ armarx::core::time::DateTime firstStartedRecording,
+ armarx::core::time::DateTime firstStoppedRecording,
+ nlohmann::json* statistics);
+
+}
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index 2f45b843a0419a639e000cc2594b59783df06267..050df3f880d715e44f2e07f37e06bf9b6708d760 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -88,18 +88,15 @@ namespace armarx::armem
// loop over all entities and their snapshots
for (const auto &[s, entity] : entities)
{
- for (const auto &[ss, entitySnapshot] : entity.history())
+ entity.forEachInstance([&outV](const wm::EntityInstance& entityInstance)
{
- for (const auto& entityInstance : entitySnapshot.instances())
- {
- const auto o = tryCast<AronClass>(entityInstance);
+ const auto o = tryCast<AronClass>(entityInstance);
- if (o)
- {
- outV.push_back(*o);
- }
+ if (o)
+ {
+ outV.push_back(*o);
}
- }
+ });
}
return outV;
@@ -139,25 +136,15 @@ namespace armarx::armem
// loop over all entities and their snapshots
for (const auto &[s, entity] : entities)
{
- if (entity.history().empty())
+ entity.forEachInstance([&outV, &pred](const wm::EntityInstance& entityInstance)
{
- ARMARX_WARNING << "Empty history for " << s;
- }
-
- ARMARX_DEBUG << "History size: " << entity.history().size();
+ const auto o = tryCast<AronClass>(entityInstance);
- for (const auto &[ss, entitySnapshot] : entity.history())
- {
- for (const auto& entityInstance : entitySnapshot.instances())
+ if (o)
{
- const auto o = tryCast<AronClass>(entityInstance);
-
- if (o)
- {
- outV.push_back(pred(*o));
- }
+ outV.push_back(pred(*o));
}
- }
+ });
}
return outV;
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
index 019987f5054e844478066d377ad0bf1e948b6db7..190e105b4d17d222543ff72db8e2feada3db7b5d 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -8,7 +8,7 @@ Core segment type of Object/Class.
<PackagePath package="RobotAPI" path="libraries/aron/common/aron/OrientedBox.xml" />
<PackagePath package="RobotAPI" path="libraries/aron/common/aron/PackagePath.xml" />
<PackagePath package="RobotAPI" path="libraries/ArmarXObjects/aron/ObjectID.xml" />
- <PackagePath package="RobotAPI" path="libraries/ArmarXObjects/aron/ObjectNames.xml" />
+ <PackagePath package="RobotAPI" path="libraries/aron/common/aron/Names.xml" />
<PackagePath package="RobotAPI" path="libraries/armem/aron/MemoryID.xml" />
</AronIncludes>
<GenerateTypes>
@@ -83,7 +83,7 @@ Core segment type of Object/Class.
</ObjectChild>
<ObjectChild key="names">
- <armarx::arondto::ObjectNames />
+ <armarx::arondto::Names />
</ObjectChild>
<ObjectChild key="ivtFeatures">
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index da32c2771a2149f58e5ba3a1a01db93ecbe71028..62ee858c9a198e2141228d56fd5af09e1c30f1af 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -4,50 +4,53 @@
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-namespace armarx::armem
+namespace armarx
{
void
- fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
+ armem::fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
{
bo = dto.pose;
}
void
- toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
+ armem::toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
{
dto.pose = bo;
}
void
- fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
+ armem::fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
{
objpose::fromAron(dto.pose, bo);
}
void
- toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
+ armem::toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
{
objpose::toAron(dto.pose, bo);
}
/* Attachments */
void
- fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo)
+ armem::fromAron(const arondto::attachment::AgentDescription& dto,
+ attachment::AgentDescription& bo)
{
fromAron(dto.id, bo.id);
aron::fromAron(dto.frame, bo.frame);
}
void
- toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo)
+ armem::toAron(arondto::attachment::AgentDescription& dto,
+ const attachment::AgentDescription& bo)
{
toAron(dto.id, bo.id);
aron::toAron(dto.frame, bo.frame);
}
void
- fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo)
+ armem::fromAron(const arondto::attachment::ObjectAttachment& dto,
+ attachment::ObjectAttachment& bo)
{
fromAron(dto.agent, bo.agent);
aron::fromAron(dto.transformation, bo.transformation);
@@ -57,7 +60,8 @@ namespace armarx::armem
}
void
- toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo)
+ armem::toAron(arondto::attachment::ObjectAttachment& dto,
+ const attachment::ObjectAttachment& bo)
{
toAron(dto.agent, bo.agent);
aron::toAron(dto.transformation, bo.transformation);
@@ -67,8 +71,8 @@ namespace armarx::armem
}
void
- fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
- attachment::ArticulatedObjectAttachment& bo)
+ armem::fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
+ attachment::ArticulatedObjectAttachment& bo)
{
fromAron(dto.agent, bo.agent);
aron::fromAron(dto.transformation, bo.transformation);
@@ -78,8 +82,8 @@ namespace armarx::armem
}
void
- toAron(arondto::attachment::ArticulatedObjectAttachment& dto,
- const attachment::ArticulatedObjectAttachment& bo)
+ armem::toAron(arondto::attachment::ArticulatedObjectAttachment& dto,
+ const attachment::ArticulatedObjectAttachment& bo)
{
toAron(dto.agent, bo.agent);
aron::toAron(dto.transformation, bo.transformation);
@@ -89,7 +93,7 @@ namespace armarx::armem
}
void
- toAron(arondto::Marker& dto, const marker::Marker& bo)
+ armem::toAron(arondto::Marker& dto, const marker::Marker& bo)
{
dto.name = bo.name;
armarx::toAron(dto.robotGlobal, bo.robotGlobal);
@@ -100,7 +104,7 @@ namespace armarx::armem
}
void
- fromAron(const arondto::Marker& dto, marker::Marker& bo)
+ armem::fromAron(const arondto::Marker& dto, marker::Marker& bo)
{
bo.name = dto.name;
armarx::fromAron(dto.robotGlobal, bo.robotGlobal);
@@ -109,13 +113,13 @@ namespace armarx::armem
armarx::fromAron(dto.markerPose, bo.markerPose);
}
-} // namespace armarx::armem
+} // namespace armarx
-namespace armarx::armem::clazz
+namespace armarx::armem
{
void
- fromAron(const arondto::Feature& dto, Feature& bo)
+ clazz::fromAron(const arondto::Feature& dto, Feature& bo)
{
bo.angle = dto.angle;
bo.scale = dto.scale;
@@ -125,7 +129,7 @@ namespace armarx::armem::clazz
}
void
- toAron(arondto::Feature& dto, const Feature& bo)
+ clazz::toAron(arondto::Feature& dto, const Feature& bo)
{
dto.angle = bo.angle;
dto.scale = bo.scale;
@@ -135,7 +139,7 @@ namespace armarx::armem::clazz
}
void
- fromAron(const arondto::ObjectClass& dto, ObjectClass& bo)
+ clazz::fromAron(const arondto::ObjectClass& dto, ObjectClass& bo)
{
armarx::fromAron(dto.id, bo.id);
armarx::fromAron(dto.simoxXmlPath, bo.simoxXmlPath);
@@ -147,6 +151,7 @@ namespace armarx::armem::clazz
armarx::fromAron(dto.meshObjPath, bo.meshObjPath);
armarx::fromAron(dto.aabb, bo.aabb);
armarx::fromAron(dto.oobb, bo.oobb);
+ fromAron(dto.names, bo.names);
bo.ivtFeatures.clear();
for (const auto& i : dto.ivtFeatures)
{
@@ -155,7 +160,7 @@ namespace armarx::armem::clazz
}
void
- toAron(arondto::ObjectClass& dto, const ObjectClass& bo)
+ clazz::toAron(arondto::ObjectClass& dto, const ObjectClass& bo)
{
armarx::toAron(dto.id, bo.id);
armarx::toAron(dto.simoxXmlPath, bo.simoxXmlPath);
@@ -167,6 +172,7 @@ namespace armarx::armem::clazz
armarx::toAron(dto.meshObjPath, bo.meshObjPath);
armarx::toAron(dto.aabb, bo.aabb);
armarx::toAron(dto.oobb, bo.oobb);
+ toAron(dto.names, bo.names);
dto.ivtFeatures.clear();
for (const auto& i : bo.ivtFeatures)
{
@@ -174,7 +180,7 @@ namespace armarx::armem::clazz
}
}
-} // namespace armarx::armem::clazz
+} // namespace armarx::armem
armarx::armem::MemoryID
armarx::armem::obj::makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose)
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index b61164a5d6a7de07bd8ba3761117e263ac714949..a41c7ccf43a9d5050cad2488c6369d0c28b3772d 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -1,6 +1,7 @@
#pragma once
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
@@ -32,6 +33,7 @@ namespace armarx::armem
void fromAron(const arondto::Marker& dto, marker::Marker& bo);
void toAron(arondto::Marker& dto, const marker::Marker& bo);
+
} // namespace armarx::armem
namespace armarx::armem::clazz
@@ -44,10 +46,9 @@ namespace armarx::armem::clazz
} // namespace armarx::armem::clazz
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
namespace armarx::armem::obj
{
/// Make a Memory ID for the object instance snapshot representing this pose.
MemoryID makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose);
+
} // namespace armarx::armem::obj
diff --git a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp
index bceba7d960ffbe41a620c36d47a5d8fc0897c157..ca365080d21ef1ca6eab5c779d50231e4f1b007a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp
@@ -1,5 +1,9 @@
#include "ClassReader.h"
+#include <RobotAPI/libraries/armem/client/query.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
namespace armarx::armem::obj::clazz
{
std::optional<armem::clazz::ObjectClass>
@@ -27,6 +31,42 @@ namespace armarx::armem::obj::clazz
return std::nullopt;
}
+ std::map<ObjectID, armem::clazz::ObjectClass>
+ ClassReader::getObjectClasses(const std::vector<ObjectID>& objectIDs)
+ {
+ armem::client::query::Builder builder;
+ auto& entities = builder.coreSegments()
+ .withName(properties().coreSegmentName)
+ .providerSegments()
+ .all()
+ .entities();
+
+ for (const ObjectID& objectID : objectIDs)
+ {
+ entities.withName(objectID.getClassID().str()).snapshots().latest();
+ }
+
+ const armem::client::QueryResult result = memoryReader().query(builder);
+ if (not result.success)
+ {
+ throw armem::error::QueryFailed(result.errorMessage);
+ }
+
+ std::map<ObjectID, armem::clazz::ObjectClass> objectClasses;
+
+ result.memory.forEachInstance(
+ [&objectClasses](const armem::wm::EntityInstance& instance) -> bool
+ {
+ const ObjectID classID = ObjectID::FromString(instance.id().entityName);
+ auto aron = instance.dataAs<armarx::armem::arondto::ObjectClass>();
+ armarx::armem::clazz::fromAron(aron, objectClasses[classID]);
+
+ return true;
+ });
+
+ return objectClasses;
+ }
+
std::string
ClassReader::propertyPrefix() const
{
diff --git a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h
index 4d2805b4f03e81bb6b58b65d73997eeae56755ab..8dd50018a7483d7ce645475dd49f3dd840fe8f75 100644
--- a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h
@@ -21,11 +21,10 @@
#pragma once
-#include <mutex>
+#include <map>
#include <optional>
#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem::obj::clazz
@@ -38,11 +37,18 @@ namespace armarx::armem::obj::clazz
std::optional<armem::clazz::ObjectClass> getObjectClass(const std::string& providerName,
const armarx::ObjectID& id);
+ /**
+ * @brief Get object class information for object class IDs.
+ * @param objectIDs The object class IDs.
+ * @return The corresponding object classes.
+ * @throw armarx::armem::error::QueryFailed If the memory query failed.
+ */
+ std::map<armarx::ObjectID, armem::clazz::ObjectClass>
+ getObjectClasses(const std::vector<armarx::ObjectID>& objectIDs);
+
protected:
std::string propertyPrefix() const final;
Properties defaultProperties() const final;
-
- private:
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index c841afc4dc6afe2629802871399944a0ef39da04..5506ddbf598ed92ac11522769b9e7b0734d20162 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -1,51 +1,55 @@
#include "Segment.h"
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/memory_ids.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
+#include <filesystem>
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
-#include <filesystem>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
namespace armarx::armem::server::obj::clazz
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- SpecializedCoreSegment(memoryToIceAdapter, objects::classSegmentID.coreSegmentName, arondto::ObjectClass::ToAronType(), -1)
+ SpecializedCoreSegment(memoryToIceAdapter,
+ objects::classSegmentID.coreSegmentName,
+ arondto::ObjectClass::ToAronType(),
+ -1)
{
}
-
Segment::~Segment()
{
}
-
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
SpecializedCoreSegment::defineProperties(defs, prefix);
- defs->optional(p.objectsPackage, prefix + "ObjectsPackage", "Name of the objects package to load from.");
- defs->optional(p.loadFromObjectsPackage, prefix + "LoadFromObjectsPackage",
+ defs->optional(p.objectsPackage,
+ prefix + "ObjectsPackage",
+ "Name of the objects package to load from.");
+ defs->optional(p.loadFromObjectsPackage,
+ prefix + "LoadFromObjectsPackage",
"If true, load the objects from the objects package on startup.");
floorVis.defineProperties(defs, prefix + "Floor.");
}
-
- void Segment::init()
+ void
+ Segment::init()
{
SpecializedCoreSegment::init();
@@ -55,8 +59,8 @@ namespace armarx::armem::server::obj::clazz
}
}
-
- void Segment::connect(viz::Client arviz)
+ void
+ Segment::connect(viz::Client arviz)
{
this->arviz = arviz;
@@ -64,28 +68,29 @@ namespace armarx::armem::server::obj::clazz
floorVis.updateFloorObject(*segmentPtr);
}
-
- void Segment::loadByObjectFinder(const std::string& objectsPackage)
+ void
+ Segment::loadByObjectFinder(const std::string& objectsPackage)
{
loadByObjectFinder(ObjectFinder(objectsPackage));
}
-
- void Segment::loadByObjectFinder(const ObjectFinder& finder)
+ void
+ Segment::loadByObjectFinder(const ObjectFinder& finder)
{
this->objectFinder = finder;
loadByObjectFinder();
}
-
- void Segment::loadByObjectFinder()
+ void
+ Segment::loadByObjectFinder()
{
const Time now = Time::Now();
const bool checkPaths = false;
std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
- const MemoryID providerID = segmentPtr->id().withProviderSegmentName(objectFinder.getPackageName());
+ const MemoryID providerID =
+ segmentPtr->id().withProviderSegmentName(objectFinder.getPackageName());
ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
<< objectFinder.getPackageName() << "' ...";
Commit commit;
@@ -98,18 +103,15 @@ namespace armarx::armem::server::obj::clazz
update.arrivedTime = update.referencedTime = update.sentTime = now;
arondto::ObjectClass objectClass = objectClassFromInfo(info);
- update.instancesData =
- {
- objectClass.toAron()
- };
+ update.instancesData = {objectClass.toAron()};
}
ARMARX_INFO << "Loaded " << commit.updates.size() << " object classes from '"
<< objectFinder.getPackageName() << "'.";
iceMemory.commitLocking(commit);
}
-
- void Segment::visualizeClass(const MemoryID& entityID, bool showAABB, bool showOOBB)
+ void
+ Segment::visualizeClass(const MemoryID& entityID, bool showAABB, bool showOOBB)
{
const Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
@@ -124,10 +126,11 @@ namespace armarx::armem::server::obj::clazz
{
try
{
- std::optional<arondto::ObjectClass> aron = doLocked([this, &entityID]()
- {
- return segmentPtr->findLatestInstanceDataAs<arondto::ObjectClass>(entityID, 0);
- });
+ std::optional<arondto::ObjectClass> aron = doLocked(
+ [this, &entityID]() {
+ return segmentPtr->findLatestInstanceDataAs<arondto::ObjectClass>(entityID,
+ 0);
+ });
if (not aron.has_value())
{
return;
@@ -136,23 +139,24 @@ namespace armarx::armem::server::obj::clazz
if (not aron->simoxXmlPath.package.empty())
{
layerObject.add(viz::Object(entityID.str())
- .file(aron->simoxXmlPath.package, aron->simoxXmlPath.path)
- .pose(pose));
+ .file(aron->simoxXmlPath.package, aron->simoxXmlPath.path)
+ .pose(pose));
}
if (showAABB)
{
layerAABB.add(viz::Box("AABB")
- .pose(pose * simox::math::pose(aron->aabb.center))
- .size(aron->aabb.extents)
- .color(simox::Color::cyan(255, 64)));
+ .pose(pose * simox::math::pose(aron->aabb.center))
+ .size(aron->aabb.extents)
+ .color(simox::Color::cyan(255, 64)));
}
if (showOOBB)
{
layerOOBB.add(viz::Box("OOBB")
- .pose(pose * simox::math::pose(aron->oobb.center, aron->oobb.orientation))
- .size(aron->oobb.extents)
- .color(simox::Color::lime(255, 64)));
+ .pose(pose * simox::math::pose(aron->oobb.center,
+ aron->oobb.orientation))
+ .size(aron->oobb.extents)
+ .color(simox::Color::lime(255, 64)));
}
}
catch (const armem::error::ArMemError& e)
@@ -170,16 +174,16 @@ namespace armarx::armem::server::obj::clazz
arviz.commit({layerObject, layerOrigin, layerAABB, layerOOBB});
}
-
- arondto::ObjectClass Segment::objectClassFromInfo(const ObjectInfo& info)
+ arondto::ObjectClass
+ Segment::objectClassFromInfo(const ObjectInfo& info)
{
namespace fs = std::filesystem;
arondto::ObjectClass data;
toAron(data.id, info.id());
- auto setPathIfExists = [](armarx::arondto::PackagePath & aron,
- const PackageFileLocation & location)
+ auto setPathIfExists =
+ [](armarx::arondto::PackagePath& aron, const PackageFileLocation& location)
{
if (fs::is_regular_file(location.absolutePath))
{
@@ -206,18 +210,18 @@ namespace armarx::armem::server::obj::clazz
if (auto recogNames = info.loadRecognizedNames())
{
- data.names.recognizedNames = recogNames.value();
+ data.names.recognized = recogNames.value();
}
if (auto spokenNames = info.loadSpokenNames())
{
- data.names.spokenNames = spokenNames.value();
+ data.names.spoken = spokenNames.value();
}
return data;
}
-
- void Segment::RemoteGui::setup(const Segment& segment)
+ void
+ Segment::RemoteGui::setup(const Segment& segment)
{
using namespace armarx::RemoteGui::Client;
@@ -232,15 +236,15 @@ namespace armarx::armem::server::obj::clazz
group.addChildren({layout, VSpacer()});
}
-
- void Segment::RemoteGui::update(Segment& segment)
+ void
+ Segment::RemoteGui::update(Segment& segment)
{
data.update(segment);
visu.update(segment);
}
-
- void Segment::RemoteGui::Data::setup(const Segment& segment)
+ void
+ Segment::RemoteGui::Data::setup(const Segment& segment)
{
using namespace armarx::RemoteGui::Client;
@@ -264,8 +268,8 @@ namespace armarx::armem::server::obj::clazz
group.addChild(grid);
}
-
- void Segment::RemoteGui::Data::update(Segment& segment)
+ void
+ Segment::RemoteGui::Data::update(Segment& segment)
{
if (reloadButton.wasClicked())
{
@@ -275,31 +279,35 @@ namespace armarx::armem::server::obj::clazz
}
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- segment.doLocked([this, &segment]()
- {
- segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (segment.segmentPtr)
+ segment.doLocked(
+ [this, &segment]()
{
- segment.segmentPtr->setMaxHistorySize(long(segment.properties.maxHistorySize));
- }
- });
+ segment.properties.maxHistorySize =
+ infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.segmentPtr)
+ {
+ segment.segmentPtr->setMaxHistorySize(
+ long(segment.properties.maxHistorySize));
+ }
+ });
}
}
-
- void Segment::RemoteGui::Visu::setup(const Segment& segment)
+ void
+ Segment::RemoteGui::Visu::setup(const Segment& segment)
{
using namespace armarx::RemoteGui::Client;
showComboBox = {};
showOptionsIndex.clear();
- segment.segmentPtr->forEachEntity([this](const wm::Entity & entity)
- {
- std::stringstream option;
- option << entity.id().entityName << " (" << entity.id().providerSegmentName << ")";
- showComboBox.addOption(option.str());
- showOptionsIndex.push_back(entity.id());
- });
+ segment.segmentPtr->forEachEntity(
+ [this](const wm::Entity& entity)
+ {
+ std::stringstream option;
+ option << entity.id().entityName << " (" << entity.id().providerSegmentName << ")";
+ showComboBox.addOption(option.str());
+ showOptionsIndex.push_back(entity.id());
+ });
if (showOptionsIndex.empty())
{
showComboBox.addOption("<none>");
@@ -318,8 +326,8 @@ namespace armarx::armem::server::obj::clazz
group.addChild(grid);
}
-
- void Segment::RemoteGui::Visu::update(Segment& segment)
+ void
+ Segment::RemoteGui::Visu::update(Segment& segment)
{
if (showButton.wasClicked())
{
@@ -331,4 +339,4 @@ namespace armarx::armem::server::obj::clazz
}
}
-}
+} // namespace armarx::armem::server::obj::clazz
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index d6635696acd4f72d3c3342423233a459baf89af0..62dec875ea7ccb106b762edec6ae79e2341e27b0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -226,107 +226,121 @@ namespace armarx::armem::server::obj::instance
}
}
- if (!discard)
+ if (discard)
{
- // Update the entity.
- stats.numUpdated++;
+ continue;
+ }
- VirtualRobot::RobotPtr robot =
- robots.get(provided.robotName, provided.providerName);
+ // Update the entity.
+ stats.numUpdated++;
- if (not robot)
- {
- ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
- << provided.robotName << "`.";
- }
+ VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName);
+ if (not robot)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
+ << provided.robotName << "`.";
+ }
- // robot may be null!
+ // robot may be null!
- // Update the robot to obtain correct local -> global transformation
- if (robot and robotSyncTimestamp != timestamp)
- {
- ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
- << "Failed to synchronize robot to timestamp " << timestamp << ". This is "
- << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
+ bool synchronized = false;
- robotSyncTimestamp = timestamp;
+ // Update the robot to obtain correct local -> global transformation
+ if (robot and robotSyncTimestamp != timestamp)
+ {
+ synchronized = robots.reader->synchronizeRobot(*robot, timestamp);
+ if (not synchronized)
+ {
+ ARMARX_VERBOSE << "Failed to synchronize robot to timestamp " << timestamp
+ << " of provided object pose (" << provided.objectID
+ << "). This is " << (Clock::Now() - timestamp).toSecondsDouble()
+ << " seconds in the past.";
+ }
+ robotSyncTimestamp = timestamp;
- // Apply calibration offset after synchronizing the robot.
+ // Apply calibration offset after synchronizing the robot.
+ {
+ if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
{
- if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
- {
- VirtualRobot::RobotNodePtr robotNode =
- robot->getRobotNode(calibration.robotNode);
+ VirtualRobot::RobotNodePtr robotNode =
+ robot->getRobotNode(calibration.robotNode);
- float value = robotNode->getJointValue();
- float newValue = value + calibration.offset;
+ float value = robotNode->getJointValue();
+ float newValue = value + calibration.offset;
- /*
+ /*
* If newValue is out of the joint's limits, then the calibration would be ignored.
* In that case, we expand the joint value limits of the local robot model to allow
* for the calibrated value.
* As this is just for perception (and not for controlling the robot), this should be fine^TM.
*/
- VirtualRobot::RobotNode::JointLimits limits =
- robotNode->getJointLimits();
- bool limitsChanged = false;
- if (newValue < limits.low)
- {
- limits.low = newValue;
- limitsChanged = true;
- }
- if (newValue > limits.high)
- {
- limits.high = newValue;
- limitsChanged = true;
- }
- if (limitsChanged)
- {
- robotNode->setJointLimits(limits.low, limits.high);
- }
-
- robotNode->setJointValue(newValue);
+ VirtualRobot::RobotNode::JointLimits limits = robotNode->getJointLimits();
+ bool limitsChanged = false;
+ if (newValue < limits.low)
+ {
+ limits.low = newValue;
+ limitsChanged = true;
}
+ if (newValue > limits.high)
+ {
+ limits.high = newValue;
+ limitsChanged = true;
+ }
+ if (limitsChanged)
+ {
+ robotNode->setJointLimits(limits.low, limits.high);
+ }
+
+ robotNode->setJointValue(newValue);
}
}
+ }
- objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
- if (provided.objectPoseFrame.empty())
- {
- objpose::data::ProvidedObjectPose copy = provided;
- copy.objectPoseFrame = armarx::GlobalFrame;
- newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
- }
- else
- {
- newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
- }
+ if (not synchronized)
+ {
+ /*
+ * We know nothing about the current robot configuration. So the behvaiour of the
+ * following code is the same as if we have no robot model at all.
+ */
+ robot = nullptr;
+ }
- if (previousPose && previousPose->attachment)
- {
- // Keep current attachment.
- ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
- newPose.attachment = previousPose->attachment;
- }
+ objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
+ if (provided.objectPoseFrame.empty())
+ {
+ objpose::data::ProvidedObjectPose copy = provided;
+ copy.objectPoseFrame = armarx::GlobalFrame;
+ newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
+ }
+ else
+ {
+ newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
+ }
- if (newPose.objectID.dataset().empty())
- {
- // Try to find the data set.
- const std::string dataset =
- classNameToDatasetCache.get(newPose.objectID.className());
- if (!dataset.empty())
- {
- newPose.objectID = {
- dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
- }
- }
- if (!provided.localOOBB)
+ if (previousPose && previousPose->attachment)
+ {
+ // Keep current attachment.
+ ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
+ newPose.attachment = previousPose->attachment;
+ }
+
+ if (newPose.objectID.dataset().empty())
+ {
+ // Try to find the data set.
+ const std::string dataset =
+ classNameToDatasetCache.get(newPose.objectID.className());
+ if (!dataset.empty())
{
- // Try to load oobb from disk.
- newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ newPose.objectID = {
+ dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
}
}
+ if (!provided.localOOBB)
+ {
+ // Try to load oobb from disk.
+ newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ }
}
commitObjectPoses(newObjectPoses, providerName);
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 598c78ca9c6bc5f4fc007059352ee94fc28b2fec..564bf3b460a06e036cd33b481541f4d41119cf25 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -30,6 +30,7 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Names.h>
#include "aron_forward_declarations.h"
@@ -67,7 +68,7 @@ namespace armarx::armem::clazz
armarx::PackagePath meshObjPath;
simox::AxisAlignedBoundingBox aabb;
simox::OrientedBoxf oobb;
- // TODO NAMES
+ armarx::Names names;
std::vector<Feature> ivtFeatures;
};
} // namespace armarx::armem::clazz
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 9caf84f24574be547036e94fc0f07e50680d0fec..f497c3e2ddcbec47b26585202f3f97d0dedbf957 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -25,8 +25,8 @@ namespace armarx::armem::robot
struct Twist
{
- Eigen::Vector3f linear;
- Eigen::Vector3f angular;
+ Eigen::Vector3f linear{Eigen::Vector3f::Zero()};
+ Eigen::Vector3f angular{Eigen::Vector3f::Zero()};
};
struct PlatformState
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
index 29a009714506d069ebd05dcbc988acc64eafe1de..0bae7b6e9d12bf287307ec5f774bc95389ca216c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
@@ -54,6 +54,8 @@ namespace armarx::armem::robot_state
[[nodiscard]] bool storeState(const VirtualRobot::Robot& robot,
const armem::Time& timestamp);
+ using RobotWriter::storeState;
+ using RobotWriter::storeDescription;
};
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index e1469a80d35feecd9cb12914a62b944cabb4ba81..5c4580c4c8c5d4540b0867365c657807d0502a7d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -40,7 +40,6 @@
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-
namespace armarx::armem::server::robot_state::proprioception
{
@@ -52,7 +51,6 @@ namespace armarx::armem::server::robot_state::proprioception
DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
}
-
static float
toDurationMs(std::chrono::high_resolution_clock::time_point start,
std::chrono::high_resolution_clock::time_point end)
@@ -61,7 +59,6 @@ namespace armarx::armem::server::robot_state::proprioception
return duration.count() / 1000.f;
}
-
void
RobotStateWriter::run(float pollFrequency,
Queue& dataBuffer,
@@ -70,10 +67,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
while (isRunning())
{
- // if (debugObserver)
- // {
- // debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", dataBuffer.size());
- // }
+ if (debugObserver)
+ {
+ // This locks the queue, but I did not find an interface to lock the queue,
+ // get the size and wait_pull().
+ size_t dataBufferSize = dataBuffer.size();
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size",
+ static_cast<long>(dataBufferSize));
+ }
RobotUnitData robotUnitData;
if (const auto status = dataBuffer.wait_pull(robotUnitData);
@@ -88,10 +89,12 @@ namespace armarx::armem::server::robot_state::proprioception
// Commits lock the core segments.
// Proprioception + Exteroception
- armem::CommitResult resultProprioception = memory.commitLocking(update.proprioception);
+ armem::CommitResult resultProprioception =
+ memory.commitLocking(update.proprioception);
ARMARX_DEBUG << deactivateSpam(1) << VAROUT(update.exteroception.updates.size());
- armem::CommitResult resultExteroception = memory.commitLocking(update.exteroception);
+ armem::CommitResult resultExteroception =
+ memory.commitLocking(update.exteroception);
endProprioception = std::chrono::high_resolution_clock::now();
// Localization
@@ -105,13 +108,15 @@ namespace armarx::armem::server::robot_state::proprioception
if (not resultProprioception.allSuccess())
{
- ARMARX_WARNING << "Could not commit data to proprioception segment in memory. Error message: "
+ ARMARX_WARNING << "Could not commit data to proprioception segment in memory. "
+ "Error message: "
<< resultProprioception.allErrorMessages();
}
if (not resultExteroception.allSuccess())
{
- ARMARX_WARNING << "Could not commit data to exteroception segment in memory. Error message: "
+ ARMARX_WARNING << "Could not commit data to exteroception segment in memory. "
+ "Error message: "
<< resultExteroception.allErrorMessages();
}
@@ -158,21 +163,26 @@ namespace armarx::armem::server::robot_state::proprioception
// Send batch to memory
Update update;
- if(data.proprioception){
+ if (data.proprioception)
+ {
armem::EntityUpdate& up = update.proprioception.add();
up.entityID = properties.robotUnitProviderID.withEntityName(
properties.robotUnitProviderID.providerSegmentName);
- up.entityID.coreSegmentName =::armarx::armem::robot_state::constants::proprioceptionCoreSegment;
+ up.entityID.coreSegmentName =
+ ::armarx::armem::robot_state::constants::proprioceptionCoreSegment;
up.referencedTime = data.timestamp;
+ up.arrivedTime = data.timestampArrived;
up.instancesData = {data.proprioception};
}
// Exteroception
- if(data.exteroception){
+ if (data.exteroception)
+ {
armem::EntityUpdate& up = update.exteroception.add();
up.entityID = properties.robotUnitProviderID.withEntityName(
properties.robotUnitProviderID.providerSegmentName);
- up.entityID.coreSegmentName = ::armarx::armem::robot_state::constants::exteroceptionCoreSegment;
+ up.entityID.coreSegmentName =
+ ::armarx::armem::robot_state::constants::exteroceptionCoreSegment;
up.referencedTime = data.timestamp;
up.instancesData = {data.exteroception};
}
@@ -200,7 +210,6 @@ namespace armarx::armem::server::robot_state::proprioception
return update;
}
-
armem::robot_state::Transform
RobotStateWriter::getTransform(const aron::data::DictPtr& platformData,
const Time& timestamp) const
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
index 8d904f249029cf9fd5ce010657cb4324f3ba5551..2cd2307421175d4e1feed29be5096be99d56d355 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
@@ -13,6 +13,7 @@ namespace armarx::armem::server::robot_state::proprioception
struct RobotUnitData
{
Time timestamp;
+ Time timestampArrived;
aron::data::DictPtr proprioception;
aron::data::DictPtr exteroception;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index 34260f51e36951f7b9a0a68369cf8317e8a4dedc..ec7a56751f1ca8393f218941a27e4041190d1c96 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -56,10 +56,11 @@ namespace armarx::armem::server::robot_state::proprioception
if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
- // will lock a mutex
- debugObserver->setDebugObserverDatafield("RobotUnitReader | t commitTimestamp [us]",
- commit->timestamp.toMicroSecondsSinceEpoch());
+ debugObserver->setDebugObserverDatafield(
+ "RobotUnitReader | t commitTimestamp [us]",
+ commit->timestamp.toMicroSecondsSinceEpoch());
+ // will lock a mutex
dataBuffer.push(std::move(commit.value()));
}
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
@@ -80,11 +81,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
ARMARX_CHECK_NOT_NULL(converterProprioception);
+ RobotUnitData result;
std::optional<RobotUnitDataStreaming::TimeStep> data;
{
auto start = std::chrono::high_resolution_clock::now();
data = fetchLatestData();
+ result.timestampArrived = armarx::DateTime::Now();
+
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
std::chrono::high_resolution_clock::now() - start);
if (debugObserver)
@@ -101,7 +105,6 @@ namespace armarx::armem::server::robot_state::proprioception
ARMARX_DEBUG << "RobotUnitReader: Converting data current timestep to commit";
auto start = std::chrono::high_resolution_clock::now();
- RobotUnitData result;
if (converterProprioception != nullptr)
{
@@ -143,6 +146,9 @@ namespace armarx::armem::server::robot_state::proprioception
debugObserver->setDebugObserverDatafield(
"RobotUnitReader | RT Timestamp Since Last Iteration",
data.back().timesSinceLastIterationUSec);
+ debugObserver->setDebugObserverDatafield(
+ "RobotUnitReader | Timestamp Arrived in RSM",
+ armarx::DateTime::Now().toMicroSecondsSinceEpoch());
}
}
if (data.empty())
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp
index 7d86ee5491dc5e0999e56a4089b3b468061e0909..ab76f86ff23c9b47c402bc7797c7a6d72b99a640 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp
@@ -9,11 +9,11 @@
#pragma once
-
-#include <tuple>
+#include <cstdint>
#include <map>
-#include <unordered_map>
#include <string>
+#include <tuple>
+#include <unordered_map>
class linuxProcessLoad {
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
index 6ace1783e7b6cedbd7dc8312bb3c38265450c216..404373a0a4b1c64778d597c15304ba8f30c62d49 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
@@ -69,19 +69,19 @@ namespace armarx::aron::codegenerator::cpp::generator
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
Matrix::getCtorInitializers(const std::string& name) const
{
- if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT)
+ if (type.getCols() == -1 || type.getRows() == -1)
{
return {{}, false};
}
- if (type.getDefaultValue() == aron::type::matrix::default_value::IDENTITY)
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::IDENTITY)
{
return {{{name, getInstantiatedCppTypename() + "::Identity()"}}, true};
}
- if (type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT || type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
{
return {{{name, getInstantiatedCppTypename() + "::Zero()"}}, true};
}
- if (type.getDefaultValue() == aron::type::matrix::default_value::ONES)
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::ONES)
{
return {{{name, getInstantiatedCppTypename() + "::One()"}}, true};
}
@@ -93,6 +93,36 @@ namespace armarx::aron::codegenerator::cpp::generator
}
}
+ CppBlockPtr
+ Matrix::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+
+ if (type.getCols() == -1 || type.getRows() == -1)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "();");
+ }
+
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::IDENTITY)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::Identity();");
+ }
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT || type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::Zero();");
+ }
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::ONES)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::One();");
+ }
+ else if (not type.getDefaultValue().empty())
+ {
+ // try to parse num. We ensure from typereader that defaultValue is valid number
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::One() * " + type.getDefaultValue() + ";");
+ }
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
CppBlockPtr
Matrix::getResetSoftBlock(const std::string& cppAccessor) const
{
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
index 452ac7b53e5e9d996bc8cbc7ffa41555d7275e65..69790642abf5681ec78b9aa25a6fc3eac9163a0d 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
@@ -42,6 +42,7 @@ namespace armarx::aron::codegenerator::cpp::generator
std::vector<std::string> getRequiredIncludes() const final;
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
getCtorInitializers(const std::string&) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
const std::string& cppAccessor,
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
index 365b3c8824d6c38b60ccde7c4381aa41bb0945b7..b4d7c92cca8521e945eb6e35d1b819c53b154ac6 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
@@ -62,13 +62,13 @@ namespace armarx::aron::codegenerator::cpp::generator
{
if (type.getDefaultValue() == aron::type::quaternion::default_value::DEFAULT)
{
- return {{}, false};
+ return {{{name, getInstantiatedCppTypename() + "::Identity()"}}, true};
}
- if (type.getDefaultValue() == aron::type::quaternion::default_value::ZEROS)
+ else if (type.getDefaultValue() == aron::type::quaternion::default_value::ZEROS)
{
return {{{name, getInstantiatedCppTypename() + "(0, 0, 0, 0)"}}, true};
}
- if (type.getDefaultValue() == aron::type::quaternion::default_value::ONES)
+ else if (type.getDefaultValue() == aron::type::quaternion::default_value::ONES)
{
return {{{name, getInstantiatedCppTypename() + "(1, 1, 1, 1)"}}, true};
}
@@ -80,6 +80,31 @@ namespace armarx::aron::codegenerator::cpp::generator
}
}
+ CppBlockPtr
+ Quaternion::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+
+ if (type.getDefaultValue() == aron::type::quaternion::default_value::DEFAULT)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::Identity();");
+ }
+ else if (type.getDefaultValue() == aron::type::quaternion::default_value::ZEROS)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(0, 0, 0, 0);");
+ }
+ else if (type.getDefaultValue() == aron::type::quaternion::default_value::ONES)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(1, 1, 1, 1);");
+ }
+ else if (not type.getDefaultValue().empty())
+ {
+ // try to parse num. We ensure from typereader that defaultValue is valid number
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(" + type.getDefaultValue() + ");");
+ }
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
CppBlockPtr
Quaternion::getResetSoftBlock(const std::string& cppAccessor) const
{
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
index 4fd41b7dbd382c0231337ea30ba661f9b1e9eab3..70f8e890e8ed9f38ed75074fab8d7113dcec1d97 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
@@ -42,6 +42,7 @@ namespace armarx::aron::codegenerator::cpp::generator
std::vector<std::string> getRequiredIncludes() const final;
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
getCtorInitializers(const std::string&) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
const std::string& cppAccessor,
diff --git a/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml b/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml
index ec5aa0c961a11530ad1d6a08372fe2c42cbf10e8..9ec219210ed4aa3944d23a2b30868ca49061d193 100644
--- a/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/FramedPose.xml
@@ -1,6 +1,11 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
+ <!--
+ The types defined here are deprecated.
+ Use <Include include="RobotAPI/libraries/aron/common/aron/framed.xml" /> instead.
+ -->
+
<GenerateTypes>
<Object name="armarx::arondto::FrameHeader">
diff --git a/source/RobotAPI/libraries/aron/common/aron/framed.xml b/source/RobotAPI/libraries/aron/common/aron/framed.xml
index 1fb5025dfd26637f542f05606bbe41777fc0a903..0cb7d5e0487b1a5a30c512d85c6eb6202df96cba 100644
--- a/source/RobotAPI/libraries/aron/common/aron/framed.xml
+++ b/source/RobotAPI/libraries/aron/common/aron/framed.xml
@@ -43,6 +43,19 @@
</ObjectChild>
</Object>
+ <Object name="armarx::arondto::FramedPositionAndOrientation">
+ <ObjectChild key='header'>
+ <armarx::arondto::FrameID />
+ </ObjectChild>
+ <ObjectChild key='position'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='orientation'>
+ <Orientation />
+ </ObjectChild>
+ </Object>
+
+
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/aron/core/CMakeLists.txt b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
index 6e3ef85cf5ef086450a9896003476d788bcfa69f..863fc5e6efcbad4b7661487d653113199cb4012b 100644
--- a/source/RobotAPI/libraries/aron/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/core/CMakeLists.txt
@@ -21,7 +21,6 @@ set(LIB_FILES
aron_conversions.cpp
rw.cpp
-
data/variant/Variant.cpp
data/variant/detail/SpecializedVariant.cpp
data/variant/detail/ContainerVariant.cpp
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index 3e9f067c989d57087dc7311c73c8140cc7c1828a..19dd673bd2eeaec02f4be8fb49081d5edeb846c6 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -261,7 +261,8 @@ namespace armarx::aron::data
const auto& p = data->getPath();
if (not p.hasDirectPrefix(this->getPath()))
{
- ARMARX_DEBUG << "An element added to a dict does not have a correct path set. This "
+ ARMARX_DEBUG << deactivateSpam(5)
+ << "An element added to a dict does not have a correct path set. This "
"may cause errors. Please use setElemetCopy() instead.";
}
}
diff --git a/source/RobotAPI/libraries/aron/similarity/CMakeLists.txt b/source/RobotAPI/libraries/aron/similarity/CMakeLists.txt
index 92661f2d2ecd6994a9978e6c8d1b2aad2ffb7e9a..f85c7d514e045c8a0dbd5a31a8b918bb5afafcdb 100644
--- a/source/RobotAPI/libraries/aron/similarity/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/similarity/CMakeLists.txt
@@ -1,2 +1,50 @@
+set(LIB_NAME aronsimilarity)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
#add_subdirectory(data/aron)
#add_subdirectory(data/image)
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox not available")
+
+set(LIBS
+ aron
+)
+
+set(LIB_FILES
+
+ data/image/NDArraySimilarity.cpp
+ data/image/mse.cpp
+ data/image/mae.cpp
+ data/image/chernoff.cpp
+ data/image/FloatSimilarity.cpp
+ cosine.cpp
+
+)
+
+set(LIB_HEADERS
+
+ data/image/NDArraySimilarity.h
+ data/image/mse.h
+ data/image/mae.h
+ data/image/chernoff.h
+ data/image/FloatSimilarity.h
+ cosine.h
+
+)
+
+
+armarx_add_library(
+ LIB_NAME
+ "${LIB_NAME}"
+ SOURCES
+ "${LIB_FILES}"
+ HEADERS
+ "${LIB_HEADERS}"
+ LIBS
+ "${LIBS}"
+)
+
+add_library(RobotAPI::aron::similarity ALIAS aronsimilarity)
diff --git a/source/RobotAPI/libraries/aron/similarity/cosine.cpp b/source/RobotAPI/libraries/aron/similarity/cosine.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..396ea66133cf66e6dc1f268c8f3c5bc89c47de05
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/cosine.cpp
@@ -0,0 +1,72 @@
+#include "cosine.h"
+
+#include <SimoxUtility/algorithm/string.h>
+#include <cmath>
+
+namespace armarx::aron::similarity
+{
+ double
+ cosine::compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2)
+ {
+ //this does only work for RGB images not RGBDepth at the moment!
+ int width = p1->getShape().at(0);
+ int height = p1->getShape().at(1);
+ int dimensions = p1->getShape().at(2);
+ ARMARX_CHECK(dimensions == p2->getShape().at(2));
+ if(dimensions > 3){
+ ARMARX_INFO << "Trying to calculate cosine similarity for more than 3 channels, only first three channels will be looked at";
+ }
+ Eigen::MatrixXf m1r(width, height);
+ Eigen::MatrixXf m2r(width, height);
+ Eigen::MatrixXf m1g(width, height);
+ Eigen::MatrixXf m2g(width, height);
+ Eigen::MatrixXf m1b(width, height);
+ Eigen::MatrixXf m2b(width, height);
+
+ auto image1 = p1->getDataAsVector();
+ auto image2 = p2->getDataAsVector();
+
+
+ for(int x = 0; x < width; x++){
+ for(int y = 0; y < height; y++){
+ //R-value matices:
+ int index = x + width * (y + 0 * height);
+ double element1 = image1.at(index);
+ double element2 = image2.at(index);
+ m1r(x, y) = element1;
+ m2r(x, y) = element2;
+ //G-value matices:
+ index = x + width * (y + 1 * height);
+ element1 = image1.at(index);
+ element2 = image2.at(index);
+ m1g(x, y) = element1;
+ m2g(x, y) = element2;
+ //B-value matices:
+ index = x + width * (y + 2 * height);
+ element1 = image1.at(index);
+ element2 = image2.at(index);
+ m1b(x, y) = element1;
+ m2b(x, y) = element2;
+ }
+ }
+
+
+ double dotProductR = (m1r.array() * m2r.array()).sum();
+ double normProductR = m1r.norm() * m2r.norm();
+ double cosineR = 1.0 - (dotProductR / normProductR);
+ //ARMARX_INFO << VAROUT(cosineR);
+
+ double dotProductG = (m1g.array() * m2g.array()).sum();
+ double normProductG = m1g.norm() * m2g.norm();
+ double cosineG = 1.0 - (dotProductG / normProductG);
+ //ARMARX_INFO << VAROUT(cosineG);
+
+ double dotProductB = (m1b.array() * m2b.array()).sum();
+ double normProductB = m1b.norm() * m2b.norm();
+ double cosineB = 1.0 - (dotProductB / normProductB);
+ //ARMARX_INFO << VAROUT(cosineB);
+
+ return (cosineR + cosineG + cosineB)/ 3;
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/cosine.h b/source/RobotAPI/libraries/aron/similarity/cosine.h
new file mode 100644
index 0000000000000000000000000000000000000000..8a5e776ff4b0c13a2fd9c36d1c0a9dfc19b8f51a
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/cosine.h
@@ -0,0 +1,13 @@
+#pragma once
+
+#include <algorithm>
+#include <map>
+#include <vector>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+namespace armarx::aron::similarity::cosine
+{
+ double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2);
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2724dc3402316b4bb0c261dd67de788dd6fd1b6d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.cpp
@@ -0,0 +1,33 @@
+#include "FloatSimilarity.h"
+#include <cmath>
+#include "ArmarXCore/core/logging/Logging.h"
+
+namespace armarx::aron::similarity{
+
+double FloatSimilarity::calculate_similarity(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2, Type t)
+{
+ switch (t) {
+ case Type::MAE:
+ return calculateMAE(f1, f2);
+ case Type::MSE:
+ return calculateMSE(f1, f2);
+ default:
+ ARMARX_INFO << "Trying to calculate similarity with unknown similarity type";
+ return -1;
+ }
+}
+
+double FloatSimilarity::calculateMAE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2)
+{
+
+ return std::abs(f1->getValue() - f2->getValue());
+}
+
+double FloatSimilarity::calculateMSE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2)
+{
+ return std::pow(f1->getValue() - f2->getValue(), 2);
+}
+
+
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h
new file mode 100644
index 0000000000000000000000000000000000000000..d325c6d61f2a5dc77ad53eb78a456da2dc07acf2
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include <vector>
+#include "RobotAPI/libraries/aron/core/data/variant/primitive/Float.h"
+
+namespace armarx::aron::similarity::FloatSimilarity{
+
+ enum Type {
+ MSE,
+ MAE,
+ NONE
+ };
+
+ double calculate_similarity(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2, Type t);
+
+ double calculate_similarity_multi(std::vector<armarx::aron::data::FloatPtr>& images, armarx::aron::data::FloatPtr p, Type type);
+
+ double calculateMAE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2);
+
+ double calculateMSE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2);
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5fb38a7f8859e804e4ea8a71642402a08ce01e2b
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
@@ -0,0 +1,57 @@
+#include "NDArraySimilarity.h"
+
+#include <cmath>
+#include "mse.h"
+#include "mae.h"
+#include "chernoff.h"
+#include "../../cosine.h"
+
+namespace armarx::aron::similarity{
+
+
+ double NDArraySimilarity::calculate_similarity(data::NDArrayPtr p1, data::NDArrayPtr p2, Type type)
+ {
+ switch(type){
+ case Type::MSE:
+ //ARMARX_INFO << "Calculate MSE";
+ return armarx::aron::similarity::mse::compute_similarity(p1, p2);
+ case Type::MAE:
+ return armarx::aron::similarity::mae::compute_similarity(p1, p2);
+ case Type::CHERNOFF:
+ return armarx::aron::similarity::chernoff::compute_similarity(p1, p2);
+ case Type::COSINE:
+ return armarx::aron::similarity::cosine::compute_similarity(p1, p2);
+ default:
+ ARMARX_WARNING << "Trying to calculate similarity with unspecified similarity measurement";
+ return -1;
+ }
+ }
+
+ std::string NDArraySimilarity::to_string(Type t)
+ {
+ switch(t){
+ case MSE:
+ return "MSE";
+ case MAE:
+ return "MAE";
+ case CHERNOFF:
+ return "Chernoff";
+ case COSINE:
+ return "Cosine";
+ default:
+ return "No similarity type information";
+ }
+ }
+
+ double NDArraySimilarity::calculate_similarity_multi(std::vector<data::NDArrayPtr> images, armarx::aron::data::NDArrayPtr p, Type type)
+ {
+ double sim = 0;
+ for(auto& image: images){
+ //ARMARX_INFO << "Before calculation";
+ sim += calculate_similarity(image, p, type);
+ //ARMARX_INFO << "Sim is currently: " << sim;
+ }
+ return sim / (images.size() + 1); // to average it over the distances, makes it easier to find good parameters
+ }
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h
new file mode 100644
index 0000000000000000000000000000000000000000..70413392f2cc8f2ee50a2c78d019607c3376feef
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <vector>
+#include "RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h"
+
+namespace armarx::aron::similarity::NDArraySimilarity{
+
+ enum Type {
+ MSE,
+ MAE,
+ CHERNOFF,
+ COSINE,
+ NONE
+ };
+
+ std::string to_string(Type t);
+
+ double calculate_similarity(armarx::aron::data::NDArrayPtr p1, armarx::aron::data::NDArrayPtr p2, Type type);
+
+ /**
+ * @brief calculate_similarity_multi compares the image p with all images from the images vector, the dissimilarity values are simply summed up
+ * @param images vector of images that are compared to p
+ * @param p main image that you want to know the dissimilarity from
+ * @param type Type of dissimilarity measure used
+ * @return dissimilarity
+ */
+ double calculate_similarity_multi(std::vector<armarx::aron::data::NDArrayPtr> images, armarx::aron::data::NDArrayPtr p, Type type);
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0b4d26bc8dd4adb15a612fcc627eed41ed392cc1
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
@@ -0,0 +1,180 @@
+#include "chernoff.h"
+
+#include <cmath>
+#include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+
+namespace armarx::aron::similarity::chernoff
+{
+
+ double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2){
+ //TODO: there seems to be an error that leads to the mean vectors always having the same
+ //value, which leads to the diff being zero which leads to distance being 0/inf/nan
+ //aron::data::NDArray image1 = *p1;
+ //aron::data::NDArray image2 = *p2;
+
+ //then normalize them to have values between 0 and 1
+ //TODO: do not rturn new array but change existing one
+ auto f = normalize_ndarray(p1, 255);
+ auto s = normalize_ndarray(p2, 255);
+ //ARMARX_INFO << VAROUT(f);
+ //ARMARX_INFO << VAROUT(s);
+ //then calculate the mean vectors
+ std::vector<double> mean_one = calculate_mean_values(f);
+ std::vector<double> mean_two = calculate_mean_values(s);
+ //ARMARX_INFO << VAROUT(mean_one);
+ //then calculate the covariance matrices
+ std::vector<std::vector<double>> cov_one = calculate_covariance_matrix(f, mean_one);
+ std::vector<std::vector<double>> cov_two = calculate_covariance_matrix(s, mean_two);
+ //then calculate the Bhattacharyya distance and return
+ double distance = calculate_bhattacharyya_distance(mean_one, mean_two, cov_one, cov_two);
+ ARMARX_INFO << "Chernoff distance: " << std::to_string(distance);
+ return distance;
+ }
+
+ std::vector<double> calculate_mean_values(data::NDArray array)
+ {
+ std::vector<double> mean(3, 0.0);
+ int width = array.getShape().at(0);
+ int height = array.getShape().at(1);
+ int colors = array.getShape().at(2);
+ auto data = array.getDataAsVector();
+
+ for (int row = 0; row < height; row++){
+ for (int col = 0; col < width; col++){
+ double red = data.at(row * width * colors + col * colors);
+ double green = data.at(row * width * colors + col * colors + 1);
+ double blue = data.at(row * width * colors + col * colors + 2);
+ mean[0] += red;
+ mean[1] += green;
+ mean[2] += blue;
+ }
+ }
+ //ARMARX_INFO << VAROUT(mean);
+ int numPixels = width * height; //only per color
+ mean[0] /= numPixels;
+ mean[1] /= numPixels;
+ mean[2] /= numPixels;
+
+ return mean;
+ }
+
+ data::NDArray normalize_ndarray(data::NDArrayPtr array, int j)
+ {
+ std::vector<unsigned char> data = array->getDataAsVector();
+ std::vector<unsigned char> new_data(data.size());
+
+ for(int i = 0; i < data.size(); i++){
+ double a = data.at(i);
+ double n = a/j;
+ new_data.at(i) = n;
+ }
+
+ armarx::aron::data::NDArray n(array->getShape(), array->getType(),new_data, array->getPath());
+ return n;
+ }
+
+ std::vector<std::vector<double> > calculate_covariance_matrix(data::NDArray array, std::vector<double> mean = std::vector<double>())
+ {
+ int width = array.getShape().at(0);
+ int height = array.getShape().at(1);
+ int colors = array.getShape().at(2);
+ auto data = array.getDataAsVector();
+
+ std::vector<double> mean_vec(3, 0.0);
+ if(mean.empty()){
+ mean_vec = calculate_mean_values(array);
+ } else {
+ mean_vec = mean;
+ }
+
+ int numPixels = width * height; //only per color
+
+ std::vector<std::vector<double>> covariance(3, std::vector<double>(3, 0.0));
+
+ for(int row = 0; row < height; row++){
+ for(int col = 0; col < width; col++){
+ double red = data.at(row * width * colors + col * colors);
+ double green = data.at(row * width * colors + col * colors + 1);
+ double blue = data.at(row * width * colors + col * colors + 2);
+
+ double redDeviation = red - mean[0];
+ double greenDeviation = green - mean[1];
+ double blueDeviation = blue - mean[2];
+
+ covariance[0][0] += redDeviation * redDeviation;
+ covariance[0][1] += redDeviation * greenDeviation;
+ covariance[0][2] += redDeviation * blueDeviation;
+ covariance[1][1] += greenDeviation * greenDeviation;
+ covariance[1][2] += greenDeviation * blueDeviation;
+ covariance[2][2] += blueDeviation * blueDeviation;
+ }
+ }
+
+ for(int i = 0; i < 3; i++){
+ for (int j = 0; j < i; j++){
+ covariance[i][j] /= numPixels;
+ covariance[j][i] = covariance[i][j];
+ }
+ covariance[i][i] /= numPixels;
+ }
+
+ return covariance;
+ }
+
+ double calculate_bhattacharyya_distance(std::vector<double> mean_one, std::vector<double> mean_two, std::vector<std::vector<double> > covariance_one, std::vector<std::vector<double> > covariance_two)
+ {
+ //first make mean vectors and covariance matrices in Eigen:: objects
+ Eigen::VectorXd meanOne(3);
+ meanOne << mean_one[0], mean_one[1], mean_one[2];
+ Eigen::VectorXd meanTwo(3);
+ meanTwo << mean_two[0], mean_two[1], mean_two[2];
+
+ //ARMARX_INFO << VAROUT(meanOne);
+ //ARMARX_INFO << VAROUT(meanTwo);
+
+ //these are column-major instead or row-major, but we have a quadratic, symmetric matrix, so it does not matter
+ //this conversion does not work correctly!!
+ //Eigen::MatrixXd cov_one = Eigen::Map<Eigen::MatrixXd>(covariance_one[0].data(), covariance_one.size(), covariance_one[0].size());
+ //Eigen::MatrixXd cov_two = Eigen::Map<Eigen::MatrixXd>(covariance_two[0].data(), covariance_two.size(), covariance_two[0].size());
+
+ Eigen::Matrix3d cov_one;
+ cov_one << covariance_one[0][0], covariance_one[0][1], covariance_one[0][2],
+ covariance_one[1][0], covariance_one[1][1], covariance_one[1][2],
+ covariance_one[2][0], covariance_one[2][1], covariance_one[2][2];
+
+ Eigen::Matrix3d cov_two;
+ cov_two << covariance_two[0][0], covariance_two[0][1], covariance_two[0][2],
+ covariance_two[1][0], covariance_two[1][1], covariance_two[1][2],
+ covariance_two[2][0], covariance_two[2][1], covariance_two[2][2];
+
+
+ //ARMARX_INFO << VAROUT(cov_one);
+
+ Eigen::VectorXd meanDiff = meanOne - meanTwo;
+ //ARMARX_INFO << VAROUT(meanDiff);
+
+ Eigen::MatrixXd sigma = 0.5 * (cov_one + cov_two);
+ Eigen::MatrixXd sigma_inverse = sigma.inverse();
+
+ double det_cov_one = cov_one.determinant();
+ double det_cov_two = cov_two.determinant();
+ double det_sigma = sigma.determinant();
+
+ //formula from https://www.kaggle.com/code/debanga/statistical-distances
+ double bigger = meanDiff.transpose() * (sigma_inverse) * meanDiff;
+ //ARMARX_INFO << "Bigger: " << std::to_string(bigger);
+ double term_one = 0.125 * bigger;
+ double sqr = std::sqrt(det_cov_one * det_cov_two);
+ //ARMARX_INFO << "Sqr: " << std::to_string(sqr);
+ double before_log = det_sigma / (sqr);
+ //ARMARX_INFO << "Before log: " << std::to_string(before_log);
+ double term_two = 0.5 * std::log(before_log);
+
+ double distance = term_one + term_two;
+
+ return distance;
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.h b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.h
new file mode 100644
index 0000000000000000000000000000000000000000..9c8dc173af489379e00435d9f1c98dda5e2e5f69
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.h
@@ -0,0 +1,44 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Joana Plewnia ( uhfpm at student dot kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <map>
+#include <vector>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+namespace armarx::aron::similarity::chernoff
+{
+ double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2);
+
+ armarx::aron::data::NDArray normalize_ndarray(armarx::aron::data::NDArrayPtr array, int i);
+
+ std::vector<double> calculate_mean_values(armarx::aron::data::NDArray array);
+
+ std::vector<std::vector<double>> calculate_covariance_matrix(armarx::aron::data::NDArray array,
+ std::vector<double> mean);
+
+ double calculate_bhattacharyya_distance(std::vector<double> mean_one, std::vector<double> mean_two,
+ std::vector<std::vector<double>> covariance_one,
+ std::vector<std::vector<double>> covariance_two);
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mae.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/mae.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3be0d5ef20e5a3325c90a69dbbca0083c7ebfb89
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mae.cpp
@@ -0,0 +1,31 @@
+#include "mae.h"
+
+namespace armarx::aron::similarity::mae
+{
+
+ double compute_similarity(const data::NDArrayPtr p1, const data::NDArrayPtr p2)
+ {
+ double sum = 0;
+ int width = p1->getShape().at(0);
+ int height = p1->getShape().at(1);
+ int colors = p1->getShape().at(2);
+
+ auto first_image = p1->getDataAsVector();
+ auto second_image = p2->getDataAsVector();
+
+ ARMARX_CHECK(first_image.size() == second_image.size());
+
+ for (int w = 0; w < width; w++){
+ for (int h = 0; h < height; h++){
+ for (int c = 0; c < colors; c++){
+ int k = h * width * colors + w * colors + c;
+ sum += std::abs(first_image.at(k) - second_image.at(k));
+ }
+ }
+ }
+ return sum / (width * height * colors);
+ }
+
+
+}
+
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mae.h b/source/RobotAPI/libraries/aron/similarity/data/image/mae.h
new file mode 100644
index 0000000000000000000000000000000000000000..cb68569c32f4a1ceaef11727ea85f6e5ea12ae7e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mae.h
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Joana Plewnia ( uhfpm at student dot kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <map>
+#include <vector>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+namespace armarx::aron::similarity::mae
+{
+ double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2);
+}
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
index 0f26147604663e1e752fa8d7a8498faae480d9b3..cb9dcf9d3f7ef6ca4ee31e514e3aa7c122605dee 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
@@ -1,12 +1,63 @@
#include "mse.h"
#include <SimoxUtility/algorithm/string.h>
+#include <cmath>
namespace armarx::aron::similarity
{
double
- mse::compute_similarity(const aron::data::NDArrayPtr& p1, const aron::data::NDArrayPtr& p2)
+ mse::compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2)
{
- return 0;
+ //ARMARX_INFO << "Begin MSE";
+ //auto start = std::chrono::high_resolution_clock::now();
+ double sum = 0;
+
+ //TODO: check shapes have same size
+ //ARMARX_INFO << "One";
+ if(p1 != nullptr && p2 != nullptr){
+ p1->getShortName();
+ } else {
+ if(p1 == nullptr){
+ ARMARX_INFO << "P1 is nullptr";
+ }else{
+ ARMARX_INFO << "P2 is Nullpointer";
+ }
+ }
+ //ARMARX_INFO << "Two";
+
+ //return 0;
+
+ std::vector<unsigned char> first_image = p1->getDataAsVector();
+ std::vector<unsigned char> second_image = p2->getDataAsVector();
+ int size = first_image.size();
+
+ //ARMARX_INFO << "Image size: " << std::to_string(int(first_image.size()));
+ //ARMARX_INFO << "Image size: " << std::to_string(int(second_image.size()));
+ ARMARX_CHECK(first_image.size() == second_image.size());
+
+ //auto start = std::chrono::high_resolution_clock::now();
+ int rolling_number = 1; //TODO: make sure that no elements will be left over
+
+ for(int i = 0; i < int(first_image.size()); i+= rolling_number){
+ //loop unrolling to shorten the needed computation time:
+ sum += std::pow(first_image.at(i) - second_image.at(i), 2);
+ /*
+ sum += std::pow(first_image.at(i + 1) - second_image.at(i + 1), 2);
+ sum += std::pow(first_image.at(i + 2) - second_image.at(i + 2), 2);
+ sum += std::pow(first_image.at(i + 3) - second_image.at(i + 3), 2);
+ */
+ }
+
+
+ //auto end = std::chrono::high_resolution_clock::now();
+ //auto additional = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+ //ARMARX_INFO << "Time with unroll factor " << rolling_number << " MSE: " << std::to_string(additional.count());
+ first_image.clear();
+ second_image.clear();
+
+ //ARMARX_INFO << "MSE is: " << std::to_string(sum / size);
+
+ return sum / (size);
}
+
} // namespace armarx::aron::similarity
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mse.h b/source/RobotAPI/libraries/aron/similarity/data/image/mse.h
index 461768d99d4624a5a5d92d82ed749845ceef94f1..892a12dd06969404eba03fe21540d53ee4d3df81 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/mse.h
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mse.h
@@ -29,5 +29,6 @@
namespace armarx::aron::similarity::mse
{
- double compute_similarity(const aron::data::NDArrayPtr& p1, const aron::data::NDArrayPtr& p2);
+ double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2);
+
}
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index e309cc113c75084e5c8853d9037c7259ac2efa9d..0f18dcad581ed5ff28b27d8a8edd512a9307b6f3 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -382,6 +382,14 @@ namespace armarx
this->agent = agent;
}
+ FramedPose::FramedPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const std::string& frame, const std::string& agent) :
+ Pose(pos, ori)
+ {
+ this->frame = frame;
+ this->agent = agent;
+ }
+
+
std::string FramedPose::getFrame() const
{
return frame;
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 0abe9ab2a73614542c830e0bd9343d60d576eb60..8538d6ba250a8ee543b3cf1484ce4d810b092c6c 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -264,6 +264,7 @@ namespace armarx
FramedPose(const FramedPose& pose);
FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& frame, const std::string& agent);
FramedPose(const Eigen::Matrix4f& m, const std::string& frame, const std::string& agent);
+ FramedPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const std::string& frame, const std::string& agent);
FramedPose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori, const std::string& frame, const std::string& agent);
FramedPose& operator=(const armarx::FramedPose&) = default;
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index 324ac1afa5153251ae397de56903a53b11481d75..496affc85693ee05dae8917d905285792931c5c9 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -315,6 +315,13 @@ namespace armarx
init();
}
+ Pose::Pose(const Eigen::Vector3f& position, const Eigen::Quaternionf& quaternion)
+ {
+ this->position = new Vector3(position);
+ this->orientation = new Quaternion(quaternion);
+ init();
+ }
+
void
Pose::operator=(const Eigen::Matrix4f& matrix)
{
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index 18332395d12d9992118c32e98823974dded3b402..42309c6576246d610d25b812d081961499282bd6 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -246,6 +246,7 @@ namespace armarx
Pose(const Pose& source);
Pose(const Eigen::Matrix4f&);
Pose(const Eigen::Matrix3f&, const Eigen::Vector3f&);
+ Pose(const Eigen::Vector3f&, const Eigen::Quaternionf&);
Pose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori);
Pose& operator=(const Pose&) = default;
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
index 599126041d81ffecd9485285a785acd667cc1e1a..39220a32bc28269232f506d8368cb5394980b87b 100644
--- a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
@@ -371,6 +371,7 @@ namespace armarx
.executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
.parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
.callbackInterface = update.callbackInterface,
+ .result = armarx::aron::data::Dict::FromAronDictDTO(update.result)
}};
skills::fromIce(update.status, ret.status);
setResultFromIce(ret, update);
diff --git a/source/RobotAPI/libraries/skills/provider/CMakeLists.txt b/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
index 9cb59c319fdf937ee9eb341610071f44114de3c1..83d51089a1298c56819a820b02e4db8eb3b0ff5a 100644
--- a/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
+++ b/source/RobotAPI/libraries/skills/provider/CMakeLists.txt
@@ -46,6 +46,8 @@ armarx_add_library(
detail/SkillImplementationWrapper.h
SkillContext.h
+ blueprints/SkillWithContextBlueprint.h
+
mixins/All.h
mixins/ArvizSkillMixin.h
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index 3d775f587834aa3a62fd42d1be353eb23a8fc0d4..730ce95a67671fad8464f4b886431a544a6410e3 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -210,7 +210,7 @@ namespace armarx::plugins
auto it =
skillExecutions.emplace(std::piecewise_construct,
std::make_tuple(executionId),
- std::make_tuple(*fac,
+ std::make_tuple(fac,
executionId,
executionRequest.parameters,
executionRequest.callbackInterface));
@@ -265,7 +265,7 @@ namespace armarx::plugins
auto it =
skillExecutions.emplace(std::piecewise_construct,
std::make_tuple(executionId),
- std::make_tuple(*fac,
+ std::make_tuple(fac,
executionId,
executionRequest.parameters,
executionRequest.callbackInterface));
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index 20ffe1fbdb75ee9f4b5831dbd406180f7acf22fc..858ee511de6fa042e257b202b6cee3bf94a446a1 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -72,7 +72,7 @@ namespace armarx::plugins
{
auto fac = std::make_unique<FactoryT>(std::forward<Args>(args)...);
auto* facPtr = fac.get();
- addCustomSkillFactory(std::move(fac));
+ addSkillFactory(std::move(fac));
return static_cast<FactoryT*>(facPtr);
}
@@ -189,7 +189,7 @@ namespace armarx
requires skills::isSkillBlueprint<FacT> FacT*
addCustomSkillFactory(Args&&... args)
{
- return plugin->addSkillFactory<FacT>(std::forward<Args>(args)...);
+ return plugin->addCustomSkillFactory<FacT>(std::forward<Args>(args)...);
}
private:
diff --git a/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h b/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h
new file mode 100644
index 0000000000000000000000000000000000000000..db852e076274c6f8b479a25d4b7d65c474912fae
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h
@@ -0,0 +1,82 @@
+/*
+ * 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
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 05.12.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/skills/provider/SkillFactory.h>
+
+namespace armarx
+{
+ namespace skills
+ {
+// template <typename T>
+// concept takesContext = requires(typename T::Context cntxt) {
+// { T::connectTo(cntxt) } -> void;
+// };
+
+
+ template <typename SkillT>
+ class SkillWithContextBlueprint : public SkillBlueprint
+ {
+ public:
+ using ContextT = typename SkillT::Context;
+
+ template <typename... Args>
+ SkillWithContextBlueprint(Args&&... args) :
+ SkillBlueprint(*SkillBlueprint::ForSkill<SkillT>(std::forward<Args>(args)...))
+ {
+ }
+
+ virtual std::unique_ptr<Skill>
+ createSkill(const ProviderID& pid) const override
+ {
+ if (not isConnected_)
+ {
+ return nullptr;
+ }
+ auto s = _createSkill();
+ s->setProviderId(pid);
+ static_cast<SkillT*>(s.get())->connectTo(context_);
+ return s;
+ }
+
+ virtual SkillDescription
+ createSkillDescription(const skills::ProviderID& pid) const
+ {
+ auto s = _createSkill();
+ s->setProviderId(pid);
+ return s->getSkillDescription();
+ }
+
+ void
+ connectTo(const ContextT& context)
+ {
+ context_ = context;
+ isConnected_ = true;
+ }
+
+ private:
+ std::atomic_bool isConnected_ = false;
+ ContextT context_;
+ };
+ } // namespace skills
+} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 55f9d80c01a78154aa95780e2cc6f549be94f843..773fc5af86c525950cd84846b6a641deabcb1c29 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -5,7 +5,7 @@ namespace armarx
namespace skills::detail
{
SkillRuntime::SkillRuntime(
- const skills::SkillBlueprint& fac,
+ const skills::SkillBlueprint* fac,
const skills::SkillExecutionID& execId,
const aron::data::DictPtr& initial_parameters,
const skills::callback::dti::SkillProviderCallbackInterfacePrx& callbackInterface) :
@@ -120,7 +120,8 @@ namespace armarx
ARMARX_INFO_S << "Construct skill: " << skillName;
updateStatus(SkillStatus::Constructing);
- this->skill = this->factory.createSkill(providerId);
+ ARMARX_CHECK_NOT_NULL(factory) << "Skill Factory is a nullptr";
+ this->skill = this->factory->createSkill(providerId);
this->skill->setExecutorName(executorName);
this->skill->setManager(manager);
this->skill->setCallback([&](const SkillStatus s, const armarx::aron::data::DictPtr& d)
@@ -242,6 +243,7 @@ namespace armarx
{
std::string message = "SkillError 201: The prepare method of skill '" +
skillName + "' did not succeed.";
+ ARMARX_ERROR_S << message;
return exitAndMakeFailedResult(message);
}
}
@@ -340,7 +342,7 @@ namespace armarx
// Exit succeeded!
// All succeeded!
{
- updateStatus(SkillStatus::Succeeded);
+ updateStatus(SkillStatus::Succeeded, mainRet.data);
// return result of main method
std::unique_lock l(skillStatusesMutex);
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
index e5a5b1f82c3db4e5939fd4c880a738373967402b..935fa227bf13831d3bcc789a124b36d12f1ba423 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.h
@@ -1,5 +1,6 @@
#pragma once
+#include <experimental/memory>
#include <shared_mutex>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
@@ -18,7 +19,7 @@ namespace armarx
class SkillRuntime
{
private:
- const skills::SkillBlueprint factory;
+ const std::experimental::observer_ptr<const skills::SkillBlueprint> factory;
std::unique_ptr<Skill> skill;
mutable std::mutex executionMutex;
@@ -33,7 +34,7 @@ namespace armarx
std::thread execution;
// ctor
- SkillRuntime(const skills::SkillBlueprint& fac,
+ SkillRuntime(const skills::SkillBlueprint* fac,
const skills::SkillExecutionID&,
const aron::data::DictPtr&,
const skills::callback::dti::SkillProviderCallbackInterfacePrx&);
diff --git a/source/RobotAPI/libraries/ukfm/CMakeLists.txt b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
index 798d6767bec35847f7f86e3cd64a5db5083ed230..08ef9a9c8344ef4c5346293b910f4a26a9883c06 100644
--- a/source/RobotAPI/libraries/ukfm/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
@@ -34,6 +34,8 @@ armarx_add_library(
"${LIBS}"
)
+target_compile_options("${LIB_NAME}" PRIVATE -Wno-error=array-bounds)
+
if(Eigen3_FOUND)
target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR} ${Eigen3_INCLUDE_DIRS})
endif()
@@ -41,3 +43,4 @@ endif()
if(manif_FOUND)
target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
endif()
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
index 6be767cbd80f870ff1ba98fef2546fc29215a997..e21c2c1785fb690f1b26d7e8a9c52e9aafc558ac 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
@@ -46,7 +46,7 @@ void DetectForceSpike::run()
DatafieldRefPtr forceDf = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield(in.getFTDatafieldName()));
const Eigen::Vector3f weights = in.getForceWeights()->toEigen();
const Eigen::Vector3f axis = in.getAxis()->toEigen().normalized();
- auto getForceAlongAxis = [&]
+ auto getForceAlongAxis = [&]() -> float
{
return forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).transpose() * axis;
};