diff --git a/.gitlab/CODEOWNERS b/.gitlab/CODEOWNERS
index af86e2ce7810f25704e7afbbbb3072e53e36b487..2a5d5735c0809d89c455cfced4ecd1ca96bc961d 100644
--- a/.gitlab/CODEOWNERS
+++ b/.gitlab/CODEOWNERS
@@ -1,17 +1,17 @@
# ARON
-/source/RobotAPI/libraries/aron/ @fratty @dreher
+/source/RobotAPI/libraries/aron/ @peller @dreher
-/source/RobotAPI/interface/aron/ @fratty @dreher
-/source/RobotAPI/interface/aron.ice @fratty @dreher
+/source/RobotAPI/interface/aron/ @peller @dreher
+/source/RobotAPI/interface/aron.ice @peller @dreher
# ArMem
-/source/RobotAPI/components/armem/ @fratty @RainerKartmann @dreher
+/source/RobotAPI/components/armem/ @peller @kartmann @dreher
-/source/RobotAPI/libraries/armem/ @fratty @RainerKartmann @dreher
-/source/RobotAPI/libraries/armem_robot_localization/ @FabianReister
-/source/RobotAPI/libraries/armem_robot_mapping/ @FabianReister
+/source/RobotAPI/libraries/armem/ @peller @kartmann @dreher
+/source/RobotAPI/libraries/armem_robot_localization/ @reister
+/source/RobotAPI/libraries/armem_robot_mapping/ @reister
/source/RobotAPI/interface/armem/ @fratty @RainerKartmann @dreher
/source/RobotAPI/interface/armem.ice @fratty @RainerKartmann @dreher
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3a4902c6d8abc0c4f29442364abd8d820d95ebfb..2185d2cc3a7ead117712b3c0ba01f57eb76ee6dc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,6 +20,7 @@ depends_on_armarx_package(ArmarXGui)
find_package(DMP QUIET)
find_package(OpenCV QUIET)
find_package(IVT COMPONENTS ivt ivtopencv QUIET)
+find_package(manif QUIET)
add_subdirectory(source)
diff --git a/README.md b/README.md
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..e355791ca5849b535672904a4c313e47d16c14f5 100644
--- a/README.md
+++ b/README.md
@@ -0,0 +1,5 @@
+# RobotAPI
+
+## Documentation
+
+[Wiki](docs)
diff --git a/docs/README.md b/docs/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..090ef176fc179e5adb7d27857b0fe9933928c0b0
--- /dev/null
+++ b/docs/README.md
@@ -0,0 +1,3 @@
+[Aron](aron)
+
+[ArMem](armem)
diff --git a/docs/armem/README.md b/docs/armem/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..1742eec0abd08fe86b0724828722338718348fd5
--- /dev/null
+++ b/docs/armem/README.md
@@ -0,0 +1,3 @@
+* [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
new file mode 100644
index 0000000000000000000000000000000000000000..edc04ab4a3eda6652d59f2356687aa70d34a3db1
--- /dev/null
+++ b/docs/armem/existing_memory_servers_and_segments/README.md
@@ -0,0 +1,96 @@
+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
new file mode 100644
index 0000000000000000000000000000000000000000..fe8ac7af138bca553c7a80c3683ff74db0156c90
--- /dev/null
+++ b/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/README.md
@@ -0,0 +1,36 @@
+# 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/Localization/res/frames.png b/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/res/frames.png
new file mode 100644
index 0000000000000000000000000000000000000000..35125768524bef2ae285af2bbfedf1b15cfebbb7
Binary files /dev/null and b/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/res/frames.png differ
diff --git a/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/res/localization-armem.png b/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/res/localization-armem.png
new file mode 100644
index 0000000000000000000000000000000000000000..14240f9f9c78b34a65f70b85f6ae7e38e98bf992
Binary files /dev/null and b/docs/armem/existing_memory_servers_and_segments/RobotState/Localization/res/localization-armem.png differ
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
new file mode 100644
index 0000000000000000000000000000000000000000..167e058d44b78ebec0f59d97c2dc4f7ffc7eef72
--- /dev/null
+++ b/docs/armem/existing_memory_servers_and_segments/RobotState/Mapping/README.md
@@ -0,0 +1,9 @@
+# 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
new file mode 100644
index 0000000000000000000000000000000000000000..65e856849ff9fc62016d27b1631f8d456b5a22b1
Binary files /dev/null and b/docs/armem/existing_memory_servers_and_segments/RobotState/Mapping/res/transformations_frames-Page-2.png differ
diff --git a/docs/armem/how_to_create_a_new_core_segment_or_memory_server/README.md b/docs/armem/how_to_create_a_new_core_segment_or_memory_server/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..9aaf1d9795a9ea38ae6414dc39ad03566f9f13da
--- /dev/null
+++ b/docs/armem/how_to_create_a_new_core_segment_or_memory_server/README.md
@@ -0,0 +1,393 @@
+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
+
+Consider you want to use a data type called `MyData` (in the namespace `armarx::mydata`) in the memory system. First, you need to describe it in ARON.
+
+- 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:
+
+```plaintext
+.../libraries/
+- MyDataLib/
+ - CMakeLists.txt
+ - ...
+ - aron/
+ - MyData.xml
+```
+
+- 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(
+ TARGET_NAME
+ ${LIB_NAME}
+ ARON_FILES
+ aron/MyData.xml
+)
+```
+
+- Define your data in ARON. A hello world example could look like this:
+
+```xml
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::mydata::arondto::MyData">
+
+ <ObjectChild key="helloWorld">
+ <String />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+```
+
+Note:
+
+- `<Object>` defines a new ARON object type (generating a C++ class) containing a number of children.
+- `<ObjectChild>` defines a child (generating a public member variable of the generated C++ class, with `key` specifying the member's name).
+- `<String />` inside a `<ObjectChild>` defines the type of the child. It can be a number of primitive types, or fully qualified names of other ARON objects.
+
+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`).
+
+Here is an example of an ARON object representing the business object (BO) type `simox::OrientedBox`:
+
+```xml
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ <Include include="<Eigen/Geometry>" />
+ </CodeIncludes>
+ <GenerateTypes>
+
+ <Object name="simox::arondto::OrientedBox">
+ <ObjectChild key='center'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='orientation'>
+ <Orientation />
+ </ObjectChild>
+ <ObjectChild key='extents'>
+ <Position />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+```
+
+Note:
+
+- (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++).
+
+If you want to use an ARON object type defined in another file, this could look like this:
+
+```xml
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.aron.generated.h>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>" />
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/PackagePath.xml>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectID.xml>" />
+ <Include include="<RobotAPI/libraries/ArmarXObjects/aron/ObjectNames.xml>" />
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ </AronIncludes>
+ <GenerateTypes>
+
+ <Object name="armarx::armem::arondto::ObjectClass">
+
+ <ObjectChild key="id">
+ <armarx::arondto::ObjectID />
+ </ObjectChild>
+
+ <ObjectChild key="simoxXmlPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="meshWrlPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="meshObjPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="aabb">
+ <simox::arondto::AxisAlignedBoundingBox />
+ </ObjectChild>
+ <ObjectChild key="oobb">
+ <simox::arondto::OrientedBox />
+ </ObjectChild>
+
+ <ObjectChild key="names">
+ <armarx::arondto::ObjectNames />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
+```
+
+Note:
+
+- You need to include other ARON files in `<AronIncludes>`. The paths have the same form as usual C++ includes and point to the ARON XML files.
+- At the moment, you also have to `<CodeInclude>` the respective generated headers (`.../MyData.aron.generated.h`). These should be generated automatically in the future.
+
+# (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).
+
+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 the header, declare functions following this form:
+
+```cpp
+#pragma once
+
+#include <SimoxUtility/shapes/OrientedBox.h>
+#include <RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>
+
+
+namespace simox
+{
+ void fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo);
+ void toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo);
+}
+```
+
+Note:
+
+- Both functions take the DTO first and the BO second.
+- `fromAron()` takes a `const` DTO and a `non-const` BO.
+- `toAron()` takes a `non-const` DTO and a `const` BO.
+- All arguments are passed as reference (especially the `non-const` arguments).
+- The functions are defined in the BO's namespace (`simox` in this example, `armarx::mydata` in the running example).
+
+
+* In the cpp file, define the functions like this:
+
+```cpp
+#include "aron_conversions.h"
+
+// Include this and use armarx::aron::to/fromAron() for std::vector, std::map.
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+// Include other aron_conversions.h files for other ARON types
+
+
+void simox::fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo)
+{
+ bo = OrientedBoxf(dto.center, dto.orientation, dto.extents);
+}
+
+void simox::toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo)
+{
+ dto.center = bo.center();
+ dto.orientation = bo.rotation();
+ dto.extents = bo.dimensions();
+}
+```
+
+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.
+
+-->
+
+# Create a new memory server
+
+> **Note:** If you plan to extend an existing memory server, you can skip this step.
+
+If your data should be stored in a new memory server, follow these steps:
+
+Create a new component, let's say `MyMemory`:
+
+```plaintext
+# In the root directory of your ArmarX package:
+armarx-package add component MyMemory
+```
+
+Then, extend the new component by the respective component plugin.
+
+Add the include:
+
+```cpp
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+
+// Or:
+#include <RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.h>
+```
+
+Make the component derive from the component plugin user:
+
+```cpp
+ class MyMemory :
+ virtual public armarx::Component
+ // v Add this line ... v
+ , virtual public armarx::armem::server::ReadWritePluginUser
+ // v ... or this line ... v
+ , virtual public armem::server::ReadOnlyPluginUser
+```
+
+Don't forget to add the respective library to your `CMakeLists.txt`:
+
+```cmake
+set(COMPONENT_LIBS
+ ...
+ # Add this:
+ armem
+ ...
+)
+```
+
+**If the component already implements an ice interface,** you have to add the memory server into face to it (otherwise, you will receive `no unique final overrider for 'virtual function ...'` errors by the compiler):
+```cpp
+// memory_tutorial/source/memory_tutorial/components/object_memory/ComponentInterface.ice
+
+#pragma once
+
+// v either include this one (read-write) v
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
+// v or this one (read-only) v
+// #include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
+
+
+module memory_tutorial { module components { module object_memory
+{
+
+ interface ComponentInterface
+ // v either extend from this one (read-write) v
+ extends armarx::armem::server::MemoryInterface
+ // v or this one (read-only) v
+ // extends armarx::armem::server::ReadingMemoryInterface
+ {
+ ...
+ };
+
+};};};
+```
+
+In this case, make sure `RobotAPIInterfaces` is in the Ice dependencies of your ice library:
+```cmake
+armarx_add_component(object_memory
+ ICE_FILES
+ ComponentInterface.ice
+ ICE_DEPENDENCIES
+ ...
+ RobotAPIInterfaces
+ ...
+```
+
+
+You can set the memory name of the memory server in the `createPropertyDefinitions()`:
+
+```cpp
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+...
+
+ armarx::PropertyDefinitionsPtr MyMemory::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ ...
+
+ // Set the memory name:
+ workingMemory().name() = "My";
+
+ ...
+ return defs;
+ }
+```
+
+**Note:**
+
+* The `ReadWritePluginUser` implements the ice interface `MemoryInterface`, which combines, among others, the `ReadingMemoryInterface` and the `WritingMemoryInterface`.
+* The `ReadOnlyPluginUser` only implements the `ReadingMemoryInterface`.
+* Both provide a working memory data storage `workingMemory()` (of type `armem::server::wm::Memory`) and an `iceAdapter()` (`armem::server::MemoryToIceAdapter`).
+
+That's it: Your component is now a memory server.
+
+# Add a core segment with your new data type in a memory server
+
+> Wait, how does the new memory server know of my specially written ARON type?
+
+Oh right, it doesn't. At least not yet. But this is easy to fix: We just have to add a core segment.
+
+> What on earth is a core segment?
+
+A _segment_ is a section of a memory (server) containing data of a specific type, i.e. they are homogeneous data containers (in contrast to heterogeneous, which would mean they contain different kinds of data) (see the [Introduction](ArMem/Introduction)).
+
+There are two kinds of segments: _core_ segments, and _provider_ segments:
+
+* **Core segments** are usually added by the memory server itself with a name and the ARON data type the core segment is going to keep.
+* **Provider segments** are usually added by clients committing data to the memory. You can think of them as "sub-segments" of a (core) segment, providing a namespace for the provider's entities and some additional features.
+
+> ... what?
+
+Just stick with me, you'll get used to it.
+
+> Okay ... but how does the memory add the ... core segment?
+
+A fine question! Luckily, that's very simple. All we need is your ARON type, a name and the working memory data structure.
+
+> ...
+
+Okay, okay, I'll come to the code. Adding a core segment merely alters a local data structure (it is not an ice/network operation). Thus, we can do it in `onInitComponent()`:
+
+```cpp
+ void MyMemory::onInitComponent()
+ {
+ workingMemory().addCoreSegment("Data", armarx::mydata::arondto::MyData::toAronType());
+ }
+```
+
+* `workingMemory()` is a function provided by the component plugin user (`ReadWritePluginUser` or `ReadOnlyPluginUser`) returning a reference to the working memory data of the type `armem::server::wm::Memory`.
+* `addCoreSegment()` adds a core segment. It takes a name and (optionally) an ARON type.
+* `"Data"` is the name of the core segment.
+ * It should refer to a modality or concept, not to its elements (i.e. prefer "Instance" over "Instances" and "Location" over "Locations").
+ * Also, try to avoid repeating the memory name, as the core segment's ID is prepended by the memory name anyway (e.g. "Object/Instance" over "Object/ObjectInstance" and "Navigation/Location" over "Navigation/NavigationLocation").
+* `armarx::mydata::arondto::MyData` is the C++ class generated from your `MyData.xml` ARON XML description file.
+ * `::toAronType()` creates a runtime type object describing the ARON type. This allows, e.g., the `MemoryViewer` GUI to show the data in a suitable manner (e.g. a pose as an affine transformation instead of a 4x4 matrix).
+ * Of course, you need to include the respective header: It's located at the same path where your XML is. For example, for the `ObjectInstance.xml` located at `RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml`, the include would be:
+
+```cpp
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.h>
+```
+
+Note that you need to include the `.h` file, not the `.xml`. As the generated class is header-only, you do not need to link any libraries in your `CMakeLists.txt`.
+
+When you now start your new memory server and the MemoryNameSystem (MNS) (e.g. using the scenario _ArMemCore_ in RobotAPI), and open the _MemoryViewer_ gui plugin, you should see an entry for your memory (`My`) and the core segment (`Data` under `My`).
+
+# Conclusion
+
+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
diff --git a/docs/armem/introduction/README.md b/docs/armem/introduction/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..43f2e3ac5e1b169c900f5793875bc5788a79c37a
--- /dev/null
+++ b/docs/armem/introduction/README.md
@@ -0,0 +1,42 @@
+# 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
new file mode 100644
index 0000000000000000000000000000000000000000..daff059aabe7c576936c41e30789b87fc63c6b1c
--- /dev/null
+++ b/docs/aron/README.md
@@ -0,0 +1,4 @@
+* [Introduction](introduction)
+* [Code Generation](code_generation)
+* [Visitors](visitors)
+* [Readers, Writers and Conversion](conversion)
diff --git a/docs/aron/code_generation/README.md b/docs/aron/code_generation/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..a48594a45348e392a3d074ccb752c94c7ead9b2d
--- /dev/null
+++ b/docs/aron/code_generation/README.md
@@ -0,0 +1,733 @@
+[[_TOC_]]
+
+# 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 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.
+
+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
+
+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
+
+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:
+
+```
+.../libraries/
+- MyDataLib/
+ - CMakeLists.txt
+ - ...
+ - aron/
+ - MyData.xml
+```
+
+Then start defining your data in ARON. A hello world example could look like this:
+```xml
+<!-- MyData containing a helloWorld member -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+ <GenerateTypes>
+ <Object name="armarx::mydata::arondto::MyData">
+ <ObjectChild key="helloWorld">
+ <String />
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</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:
+```xml
+ <ObjectChild key="helloWorld">
+ <String optional="true" />
+ </ObjectChild>
+```
+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`).
+
+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>
+```
+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>
+```
+
+(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
+
+In the following we define a class that uses all more complex types once:
+```xml
+<!-- MyData containing all accepted members once -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <Include include="<Eigen/Core>" />
+ <Include include="<Eigen/Geometry>" />
+ <Include include="<pcl/point_cloud.h>" />
+ <Include include="<pcl/point_types.h>" />
+ <Include include="<opencv2/core/core.hpp>" />
+ </CodeIncludes>
+ <GenerateTypes>
+ <IntEnum name="TheIntEnum">
+ <EnumValue key="INT_ENUM_VALUE_0" value="0" />
+ <EnumValue key="INT_ENUM_VALUE_1" value="1" />
+ <EnumValue key="INT_ENUM_VALUE_2" value="2" />
+ <EnumValue key="INT_ENUM_VALUE_3" value="3" />
+ <EnumValue key="INT_ENUM_VALUE_4" value="4" />
+ <EnumValue key="INT_ENUM_VALUE_5" value="5" />
+ <EnumValue key="INT_ENUM_VALUE_6" value="6" />
+ <EnumValue key="INT_ENUM_VALUE_7" value="7" />
+ <EnumValue key="INT_ENUM_VALUE_8" value="8" />
+ </IntEnum>
+
+ <Object name="armarx::mydata::arondto::MyData">
+ <objectchild key="the_int_enum">
+ <TheIntEnum />
+ </objectchild>
+
+ <ObjectChild key="the_dict">
+ <Dict>
+ <Float />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key='the_list'>
+ <List>
+ <Float />
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='the_short_matrix'>
+ <Matrix rows="5" cols="7" type="int16" />
+ </ObjectChild>
+
+ <ObjectChild key='the_double_quaternion'>
+ <Quaternion type="float64" />
+ </ObjectChild>
+
+ <ObjectChild key='the_position'>
+ <Position />
+ </ObjectChild>
+
+ <ObjectChild key='the_orientation'>
+ <Orientation />
+ </ObjectChild>
+
+ <ObjectChild key='the_pose'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='the_rgb24_image'>
+ <Image type="rgb24" />
+ </ObjectChild>
+
+ <ObjectChild key='the_xyzrgb_pointcloud'>
+ <PointCloud type="PointXYZRGB" />
+ </ObjectChild>
+
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
+```
+
+### CMake specification
+
+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(
+ TARGET_NAME
+ ${LIB_NAME}
+ ARON_FILES
+ aron/MyData.xml
+)
+```
+
+### 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.:
+```xml
+<!-- MyData containing a helloWorld member -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+ <Object name="armarx::mydata::arondto::MyData">
+ <ObjectChild key="helloWorld">
+ <Object name="armarx::mydata::arondto::MyOtherData">
+ ...
+ </Object>
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
+```
+This is not supported anymore!
+
+## Full example
+
+Say you have the following Aron XML type description:
+```xml
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+ <IntEnum name="armarx::NaturalIKControlMode">
+ <EnumValue key="CONTROL_MODE_0" value="0" />
+ <EnumValue key="CONTROL_MODE_1" value="1" />
+ <EnumValue key="CONTROL_MODE_2" value="2" />
+ </IntEnum>
+
+ <Object name='armarx::NaturalIKResult'>
+ <ObjectChild key='control_mode'>
+ <armarx::NaturalIKControlMode />
+ </ObjectChild>
+
+ <ObjectChild key='reached'>
+ <Bool />
+ </ObjectChild>
+
+ <ObjectChild key='jointValues'>
+ <List>
+ <Float />
+ </List>
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
+```
+
+The generated c++ file looks like:
+```cpp
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <map>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/codegenerator/codewriter/cpp/AronCppClass.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h>
+#include <RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>
+#include <RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h>
+namespace armarx
+{
+ /**
+ ******************************************
+ * AUTOGENERATED CLASS. Please do NOT edit.
+ ******************************************
+ */
+ class NaturalIKControlMode
+ : public armarx::aron::codegenerator::cpp::AronCppClass
+ {
+ public:
+ /**
+ * The internal enum definition of the enum of this autogenerated class.
+ */
+ enum class ImplEnum
+ {
+ CONTROL_MODE_0,
+ CONTROL_MODE_1,
+ CONTROL_MODE_2,
+ }; // enum class ImplEnum
+
+ public:
+ using This = armarx::NaturalIKControlMode;
+ static constexpr ImplEnum CONTROL_MODE_0 = ImplEnum::CONTROL_MODE_0;
+ static constexpr ImplEnum CONTROL_MODE_1 = ImplEnum::CONTROL_MODE_1;
+ static constexpr ImplEnum CONTROL_MODE_2 = ImplEnum::CONTROL_MODE_2;
+ /// Mapping enum values to readable strings
+ const std::map<ImplEnum, std::string> EnumToStringMap = {
+ {ImplEnum::CONTROL_MODE_0, "CONTROL_MODE_0"},
+ {ImplEnum::CONTROL_MODE_1, "CONTROL_MODE_1"},
+ {ImplEnum::CONTROL_MODE_2, "CONTROL_MODE_2"},
+ };
+ /// Mapping readable strings to enum values
+ const std::map<std::string, ImplEnum> StringToEnumMap = {
+ {"CONTROL_MODE_0", ImplEnum::CONTROL_MODE_0},
+ {"CONTROL_MODE_1", ImplEnum::CONTROL_MODE_1},
+ {"CONTROL_MODE_2", ImplEnum::CONTROL_MODE_2},
+ };
+ /// Mapping enum values to a int value
+ const std::map<ImplEnum, int> EnumToValueMap = {
+ {ImplEnum::CONTROL_MODE_0, 0},
+ {ImplEnum::CONTROL_MODE_1, 1},
+ {ImplEnum::CONTROL_MODE_2, 2},
+ };
+ /// Mapping int values to a enum
+ const std::map<int, ImplEnum> ValueToEnumMap = {
+ {0, ImplEnum::CONTROL_MODE_0},
+ {1, ImplEnum::CONTROL_MODE_1},
+ {2, ImplEnum::CONTROL_MODE_2},
+ };
+ /// The current value of the enum object
+ ImplEnum value;
+
+ public:
+ NaturalIKControlMode()
+ {
+ resetHard();
+ }
+ NaturalIKControlMode(const armarx::NaturalIKControlMode& i)
+ : value(i.value)
+ {
+ }
+ NaturalIKControlMode(const ImplEnum e)
+ : value(e)
+ {
+ }
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const armarx::NaturalIKControlMode& i) const
+ {
+ if (not (value == i.value))
+ {
+ return false;
+ }
+ return true;
+ }
+
+ /**
+ * @brief resetHard() - This method resets member variables according to the XML type description.
+ * @return - nothing
+ */
+ virtual void resetHard() override
+ {
+ value = {};
+ }
+
+ /**
+ * @brief resetSoft() - This method resets all member variables with respect to the current parameterization.
+ * @return - nothing
+ */
+ virtual void resetSoft() override
+ {
+ value = {};
+ }
+
+ /**
+ * @brief writeType() - This method returns a new type from the class structure using a type writer implementation. This function is static.
+ * @return - the result of the writer implementation
+ */
+ template<class T>
+ static T writeType(armarx::aron::type::WriterInterface<T>& aron_w, armarx::aron::type::Maybe aron_maybeType = armarx::aron::type::Maybe::eNone, const armarx::aron::Path& aron_p = armarx::aron::Path())
+ {
+ //TODO:
+ std::map<std::string, int> aron_str2ValueMap;
+ return aron_w.writeIntEnum("armarx::NaturalIKControlMode", aron_str2ValueMap, aron_maybeType, aron_p);
+ }
+
+ /**
+ * @brief write() - This method returns a new type from the member data types using a data writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ template<class T>
+ T write(armarx::aron::data::WriterInterface<T>& aron_w, const armarx::aron::Path& aron_p = armarx::aron::Path()) const
+ {
+ return aron_w.writePrimitive(EnumToValueMap.at(value), aron_p); // of top level enum armarx::NaturalIKControlMode
+ }
+
+ /**
+ * @brief read() - This method sets the struct members to new values given in a data reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ template<class T>
+ void read(armarx::aron::data::ReaderInterface<T>& aron_r, T& input)
+ {
+ using TNonConst = typename std::remove_const<T>::type;
+ {
+ const TNonConst* _suppressUnusedWarning;
+ (void) _suppressUnusedWarning;
+ }
+ this->resetSoft();
+ if (aron_r.readNull(input))
+ {
+ throw armarx::aron::error::AronException(__PRETTY_FUNCTION__, "The input to the read method must not be null.");
+ }
+ int aron_tmpValue;
+ aron_r.readPrimitive(input, aron_tmpValue); // of top level enum armarx::NaturalIKControlMode
+ value = ValueToEnumMap.at(aron_tmpValue);
+ }
+
+ /**
+ * @brief toString() - Converts the internally stored value to string
+ * @return - the name of the enum
+ */
+ std::string toString() const
+ {
+ return EnumToStringMap.at(value);
+ }
+
+ /**
+ * @brief fromString() - sets the internally stored value to the corrsponding enum of the input str
+ * @return - nothing
+ */
+ void fromString(const std::string& str)
+ {
+ if (auto it = StringToEnumMap.find(str); it == StringToEnumMap.end())
+ {
+ throw armarx::LocalException("The input name is not valid. Could net set the enum to value '" + str + "'");
+ }
+ else
+ {
+ value = it->second;
+ }
+ }
+
+ /**
+ * @brief int() - Converts the internally stored value to int representation
+ * @return - the int representation
+ */
+ operator int() const
+ {
+ return EnumToValueMap.at(value);
+ }
+
+ /**
+ * @brief operator=() - Assignment operator for the internally defined enum
+ * @return - nothing
+ */
+ armarx::NaturalIKControlMode& operator=(ImplEnum v)
+ {
+ value = v;
+ return *this;
+ }
+
+ /**
+ * @brief operator=() - Assignment operator for copy
+ * @return - nothing
+ */
+ armarx::NaturalIKControlMode& operator=(const armarx::NaturalIKControlMode& c)
+ {
+ value = c.value;
+ return *this;
+ }
+
+ /**
+ * @brief operator=() - Assignment operator for the internally defined enum
+ * @return - nothing
+ */
+ armarx::NaturalIKControlMode& operator=(int v)
+ {
+ if (auto it = ValueToEnumMap.find(v); it == ValueToEnumMap.end())
+ {
+ throw armarx::LocalException("The input int is not valid. Could net set the enum to value '" + std::to_string(v) + "'");
+ }
+ else
+ {
+ value = it->second;
+ }
+ return *this;
+ }
+
+ }; // class NaturalIKControlMode
+} // namespace armarx
+
+namespace armarx
+{
+ /**
+ ******************************************
+ * AUTOGENERATED CLASS. Please do NOT edit.
+ ******************************************
+ */
+ class NaturalIKResult
+ : public armarx::aron::codegenerator::cpp::AronCppClass
+ {
+ public:
+ using This = armarx::NaturalIKResult;
+ armarx::NaturalIKControlMode control_mode;
+ std::vector<float> jointValues;
+ bool reached;
+
+ public:
+ NaturalIKResult()
+ {
+ resetHard();
+ }
+
+ public:
+ /**
+ * @brief operator==() - This method checks whether all values equal another instance.
+ * @param i - The other instance
+ * @return - true, if all members are the same, false otherwise
+ */
+ bool operator==(const armarx::NaturalIKResult& i) const
+ {
+ if (not (control_mode == i.control_mode))
+ {
+ return false;
+ }
+ if (not (jointValues == i.jointValues))
+ {
+ return false;
+ }
+ if (not (reached == i.reached))
+ {
+ return false;
+ }
+ return true;
+ }
+
+ /**
+ * @brief resetHard() - This method resets member variables according to the XML type description.
+ * @return - nothing
+ */
+ virtual void resetHard() override
+ {
+ control_mode.resetHard();
+ jointValues = std::vector<float>();
+ reached = bool();
+ }
+
+ /**
+ * @brief resetSoft() - This method resets all member variables with respect to the current parameterization.
+ * @return - nothing
+ */
+ virtual void resetSoft() override
+ {
+ control_mode.resetSoft();
+ jointValues.clear();
+ reached = bool();
+ }
+
+ /**
+ * @brief writeType() - This method returns a new type from the class structure using a type writer implementation. This function is static.
+ * @return - the result of the writer implementation
+ */
+ template<class T>
+ static T writeType(armarx::aron::type::WriterInterface<T>& aron_w, armarx::aron::type::Maybe aron_maybeType = armarx::aron::type::Maybe::eNone, const armarx::aron::Path& aron_p = armarx::aron::Path())
+ {
+ std::map<std::string, T> aron_objectMembers;
+ auto aron_objectExtends = std::nullopt;
+ // members of armarx::NaturalIKResult
+ auto aron_variant_control_mode = armarx::NaturalIKControlMode::writeType(aron_w, armarx::aron::type::Maybe::eNone, armarx::aron::Path(aron_p, {"control_mode"})); // of control_mode
+ aron_objectMembers.emplace("control_mode", aron_variant_control_mode);
+ auto aron_variant_jointValues_dot_accepted_type = aron_w.writeFloat(armarx::aron::type::Maybe::eNone, armarx::aron::Path(aron_p, {"jointValues", "::accepted_type"})); // of float
+ auto aron_variant_jointValues = aron_w.writeList(aron_variant_jointValues_dot_accepted_type, armarx::aron::type::Maybe::eNone, armarx::aron::Path(aron_p, {"jointValues"})); // of jointValues
+ aron_objectMembers.emplace("jointValues", aron_variant_jointValues);
+ auto aron_variant_reached = aron_w.writeBool(armarx::aron::type::Maybe::eNone, armarx::aron::Path(aron_p, {"reached"})); // of bool
+ aron_objectMembers.emplace("reached", aron_variant_reached);
+ return aron_w.writeObject("armarx::NaturalIKResult", aron_objectMembers, aron_objectExtends, aron_maybeType, aron_p); // of top level object armarx::NaturalIKResult
+ }
+
+ /**
+ * @brief write() - This method returns a new type from the member data types using a data writer implementation.
+ * @param w - The writer implementation
+ * @return - the result of the writer implementation
+ */
+ template<class T>
+ T write(armarx::aron::data::WriterInterface<T>& aron_w, const armarx::aron::Path& aron_p = armarx::aron::Path()) const
+ {
+ std::map<std::string, T> aron_objectMembers;
+ std::optional<T> aron_objectExtends;
+ // members of armarx::NaturalIKResult
+ auto aron_variant_control_mode = aron_w.writeNull();
+ aron_variant_control_mode = control_mode.write(aron_w, armarx::aron::Path(aron_p, {"control_mode"})); // of control_mode
+ aron_objectMembers.emplace("control_mode", aron_variant_control_mode);
+ auto aron_variant_jointValues = aron_w.writeNull();
+ std::vector<T> aron_variant_jointValues_listElements;
+ for(unsigned int aron_jointValues_it = 0; aron_jointValues_it < jointValues.size(); ++aron_jointValues_it)
+ {
+ auto aron_variant_jointValues_dot_at_lbrR_aron_jointValues_it_rbrR_ = aron_w.writeNull();
+ aron_variant_jointValues_dot_at_lbrR_aron_jointValues_it_rbrR_ = aron_w.writePrimitive(jointValues.at(aron_jointValues_it), armarx::aron::Path(aron_p, {"jointValues", std::to_string(aron_jointValues_it)})); // of jointValues.at(aron_jointValues_it)
+ aron_variant_jointValues_listElements.push_back(aron_variant_jointValues_dot_at_lbrR_aron_jointValues_it_rbrR_);
+ }
+ aron_variant_jointValues = aron_w.writeList(aron_variant_jointValues_listElements, armarx::aron::Path(aron_p, {"jointValues"})); // of jointValues
+ aron_objectMembers.emplace("jointValues", aron_variant_jointValues);
+ auto aron_variant_reached = aron_w.writeNull();
+ aron_variant_reached = aron_w.writePrimitive(reached, armarx::aron::Path(aron_p, {"reached"})); // of reached
+ aron_objectMembers.emplace("reached", aron_variant_reached);
+ return aron_w.writeDict(aron_objectMembers, aron_objectExtends, aron_p); // of top level object armarx::NaturalIKResult
+ }
+
+ /**
+ * @brief read() - This method sets the struct members to new values given in a data reader implementation.
+ * @param r - The reader implementation
+ * @return - nothing
+ */
+ template<class T>
+ void read(armarx::aron::data::ReaderInterface<T>& aron_r, T& input)
+ {
+ using TNonConst = typename std::remove_const<T>::type;
+ {
+ const TNonConst* _suppressUnusedWarning;
+ (void) _suppressUnusedWarning;
+ }
+ this->resetSoft();
+ if (aron_r.readNull(input))
+ {
+ throw armarx::aron::error::AronException(__PRETTY_FUNCTION__, "The input to the read method must not be null.");
+ }
+ std::map<std::string, TNonConst> aron_objectMembers;
+ aron_r.readDict(input, aron_objectMembers); // of top level object armarx::NaturalIKResult
+ control_mode.read<T>(aron_r, aron_objectMembers.at("control_mode"));
+ std::vector<TNonConst> aron_jointValues_listElements;
+ aron_r.readList(aron_objectMembers.at("jointValues"), aron_jointValues_listElements);
+ for (const auto& aron_jointValues_listValue : aron_jointValues_listElements)
+ {
+ float aron_jointValues_listTmp;
+ aron_r.readPrimitive(aron_jointValues_listValue, aron_jointValues_listTmp); // of aron_jointValues_listTmp
+ jointValues.push_back(aron_jointValues_listTmp);
+ }
+ aron_r.readPrimitive(aron_objectMembers.at("reached"), reached); // of reached
+ }
+
+ /**
+ * @brief toAron() - This method returns a new data from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
+ armarx::aron::data::DictPtr toAron() const
+ {
+ armarx::aron::data::writer::VariantWriter writer;
+ return armarx::aron::data::Dict::DynamicCastAndCheck(this->write(writer));
+ }
+
+ /**
+ * @brief FromAron() - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
+ static This FromAron(const armarx::aron::data::DictPtr& input)
+ {
+ This t;
+ t.fromAron(input);
+ return t;
+ }
+
+ /**
+ * @brief fromAron - This method sets the struct members to new values given in a reader implementation.
+ * @return - nothing
+ */
+ void fromAron(const armarx::aron::data::DictPtr& input)
+ {
+ armarx::aron::data::reader::VariantReader reader;
+ this->read<armarx::aron::data::reader::VariantReader::InputType>(reader, (input));
+ }
+
+ /**
+ * @brief toAronType() - This method returns a new type from the member data types using a writer implementation.
+ * @return - the result of the writer implementation
+ */
+ static armarx::aron::type::ObjectPtr toAronType()
+ {
+ armarx::aron::type::writer::VariantWriter writer;
+ return armarx::aron::type::Object::DynamicCastAndCheck(writeType(writer));
+ }
+
+ }; // class NaturalIKResult
+} // 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.
+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.
+
+Futher, 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
+
+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.
+
+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 the header, declare functions following this form:
+
+```cpp
+#pragma once
+
+#include <SimoxUtility/shapes/OrientedBox.h>
+#include <RobotAPI/libraries/aron/common/aron/OrientedBox.aron.generated.h>
+
+
+namespace simox
+{
+ void fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo);
+ void toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo);
+}
+```
+
+Note:
+
+- Both functions take the DTO first and the BO second.
+- `fromAron()` takes a `const` DTO and a `non-const` BO.
+- `toAron()` takes a `non-const` DTO and a `const` BO.
+- All arguments are passed as reference (especially the `non-const` arguments).
+- The functions are defined in the BO's namespace (`simox` in this example, `armarx::mydata` in the running example).
+
+
+* In the cpp file, define the functions like this:
+
+```cpp
+#include "aron_conversions.h"
+
+// Include this and use armarx::aron::to/fromAron() for std::vector, std::map.
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+// Include other aron_conversions.h files for other ARON types
+
+
+void simox::fromAron(const arondto::OrientedBox& dto, OrientedBoxf& bo)
+{
+ bo = OrientedBoxf(dto.center, dto.orientation, dto.extents);
+}
+
+void simox::toAron(arondto::OrientedBox& dto, const OrientedBoxf& bo)
+{
+ dto.center = bo.center();
+ dto.orientation = bo.rotation();
+ dto.extents = bo.dimensions();
+}
+```
+
+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.
+
+# 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
diff --git a/docs/aron/conversion/README.md b/docs/aron/conversion/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..a2233c26e99620a5331c2a1ea2448a7c2189656d
--- /dev/null
+++ b/docs/aron/conversion/README.md
@@ -0,0 +1,35 @@
+# 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
new file mode 100644
index 0000000000000000000000000000000000000000..1a9c9987d50c3784a87807595353b68362419293
--- /dev/null
+++ b/docs/aron/introduction/README.md
@@ -0,0 +1,213 @@
+[[_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
new file mode 100644
index 0000000000000000000000000000000000000000..c72e3a92e31b44b4bccbe2cfcd73be2aecb0c1b4
--- /dev/null
+++ b/docs/aron/visitors/README.md
@@ -0,0 +1,101 @@
+[[_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/etc/doxygen/pages/HowTos.dox b/etc/doxygen/pages/HowTos.dox
index aed242e5dd4a966612bb69f31a30d8efc73e292d..73d1f6aa03ce74996e4fb74e9476b829dffb5f12 100644
--- a/etc/doxygen/pages/HowTos.dox
+++ b/etc/doxygen/pages/HowTos.dox
@@ -85,7 +85,7 @@ With the corresponding configuration in ./config/Armar3Config.cfg:
\section RobotAPI-HowTos-RobotViewer How to inspect the robot's structure
-Robots are usually defined in the Simox XML (https://gitlab.com/Simox/simox/wikis/FileFormat) or in the URDF format. To inspect the kinematic structure, visualizations and physical properties, you can use the <i>RobotViewer</i> tool which is part of the Simox library. In particlular you can visualize all coordinate frames that are present in the robot defintion.
+Robots are usually defined in the Simox XML (https://git.h2t.iar.kit.edu/sw/simox/simox/-/wikis/FileFormat) or in the URDF format. To inspect the kinematic structure, visualizations and physical properties, you can use the <i>RobotViewer</i> tool which is part of the Simox library. In particlular you can visualize all coordinate frames that are present in the robot defintion.
Start it with the following command:
\code
RobotViewer --robot path/to/robot.xml
@@ -96,7 +96,7 @@ Start it with the following command:
\section RobotAPI-HowTos-RemoteRobot-Access How to access a RobotStateComponent
The RobotStateComponent provides several methods for accessing the current configuration of the robot and for getting a snapshot of the current state which is compatible with
-models of the Simox/VirtualRobot framework. With these models the whole functionality of Simox (https://gitlab.com/Simox/simox) can be used, e.g. IK solving, collision detection or motion and grasp planning.
+models of the Simox/VirtualRobot framework. With these models the whole functionality of Simox (https://git.h2t.iar.kit.edu/sw/simox/simox) can be used, e.g. IK solving, collision detection or motion and grasp planning.
See also \ref RobotAPI-RemoteRobot-Overview.
\par Creating a synchronized model (RemoteRobot)
diff --git a/etc/doxygen/pages/Tutorials.dox b/etc/doxygen/pages/Tutorials.dox
index ca71a9b1d96933319cdbda29d65d4d0e6b1d5e78..ff5d3d89165100062c7016e19466717f3decd6be 100644
--- a/etc/doxygen/pages/Tutorials.dox
+++ b/etc/doxygen/pages/Tutorials.dox
@@ -8,6 +8,6 @@ Tutorials related to RobotAPI:
\li \ref Component-ArVizExample (Example component for how to use ArViz)
-\li <a href="https://gitlab.com/ArmarX/meta/Academy/-/tree/master/tutorials/100_beginner/301_memory_server_and_client_tutorial_cpp">Memory Server and Client Tutorial</a>
+\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/armarpose.dox b/etc/doxygen/pages/armarpose.dox
index 4dcacff7b984c98f7b2070a6db9dbcbc260309a4..66d53a3a8b52b3c67aac0dba6150573969e18b4f 100644
--- a/etc/doxygen/pages/armarpose.dox
+++ b/etc/doxygen/pages/armarpose.dox
@@ -23,7 +23,8 @@ Use this variable if you specify global poses and an empty Agent-string. Empty f
\section FramedPose-FramedPositionCreation Creation of new FramedPositions
To create a new FramedPosition (FramedOrientation, FramedVector and FramedPose work analogously) one needs
to know the position, the coordinate frame name and the agent name.
-The coordinate frame is usually the name of the RobotNode, e.g. the tcp of the robot. All nodes of a robot can be inspected with the <a href="https://gitlab.com/Simox/simox/wikis/Examples">Simox tool *RobotViewer*</a>.
+The coordinate frame is usually the name of the RobotNode, e.g. the tcp of the robot.
+All nodes of a robot can be inspected with the <a href="https://git.h2t.iar.kit.edu/sw/simox/simox/-/wikis/Examples">Simox tool *RobotViewer*</a>.
The agent name can be retrieved via the \ref armarx::RobotStateComponent like this:
\code
diff --git a/etc/raw_socket/.gitignore b/etc/raw_socket/.gitignore
deleted file mode 100644
index cdf73671f14517e6101d00d84aa2b8e43729ee4c..0000000000000000000000000000000000000000
--- a/etc/raw_socket/.gitignore
+++ /dev/null
@@ -1,2 +0,0 @@
-raw_socket
-
diff --git a/etc/raw_socket/Makefile b/etc/raw_socket/Makefile
deleted file mode 100644
index af261855d35ddad9b644cd0cf62f9aa963eb15ff..0000000000000000000000000000000000000000
--- a/etc/raw_socket/Makefile
+++ /dev/null
@@ -1,18 +0,0 @@
-# raw_socket
-
-PREFIX = /usr/local
-BDIR = $(DESTDIR)/$(PREFIX)
-
-.PHONY: all clean install
-
-all: raw_socket
-
-raw_socket: raw_socket.c
- $(CC) -o $@ raw_socket.c
-
-install: raw_socket
- install -D raw_socket $(BDIR)/bin/raw_socket
- setcap cap_net_raw,cap_ipc_lock,cap_sys_nice+ep $(BDIR)/bin/raw_socket
-
-clean:
- rm -f raw_socket
diff --git a/etc/raw_socket/raw_socket.c b/etc/raw_socket/raw_socket.c
deleted file mode 100644
index 32322d9bea97949fc4b18b26e07790f59db58e99..0000000000000000000000000000000000000000
--- a/etc/raw_socket/raw_socket.c
+++ /dev/null
@@ -1,115 +0,0 @@
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <sysexits.h>
-#include <unistd.h>
-
-#include <arpa/inet.h>
-#include <net/ethernet.h>
-#include <net/if.h>
-#include <linux/if_packet.h>
-#include <sys/ioctl.h>
-#include <sys/socket.h>
-#include <sys/un.h>
-
-#define ETH_P_ECAT 0x88A4
-
-extern char **environ;
-
-int main(int argc, char *argv[])
-{
- fprintf(stderr, "raw_socket was build: %s\n", __TIMESTAMP__);
- int i, ret, ifindex, psock;
- struct ifreq ifr;
- struct sockaddr_ll sll;
- struct timeval timeout;
- int argssize = argc + 1;
- char *args[argssize];
- char sock[5];
-
- if (argc < 3) {
- fprintf(stderr, "usage: %s [ifname] [binary] <args>\n", argv[0]);
- return EX_USAGE;
- }
-
- /* we use RAW packet socket, with packet type ETH_P_ECAT */
- psock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ECAT));
- if (psock < 0) {
- perror("socket");
- fprintf(stderr, "raw_socket error: socket SOCK_RAW\n");
- return EX_OSERR;
- }
-
- timeout.tv_sec = 0;
- timeout.tv_usec = 1;
- fprintf(stderr, "raw_socket: setsockopt SO_RCVTIMEO\n");
- ret = setsockopt(psock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
- if (ret != 0) {
- perror("setsockopt");
- fprintf(stderr, "raw_socket error: setsockopt SO_RCVTIMEO\n");
- return EX_OSERR;
- }
-
- fprintf(stderr, "raw_socket: setsockopt SO_SNDTIMEO\n");
- ret = setsockopt(psock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout));
- if (ret != 0) {
- perror("setsockopt");
- fprintf(stderr, "raw_socket error: setsockopt SO_SNDTIMEO\n");
- return EX_OSERR;
- }
-
- i = 1;
- fprintf(stderr, "raw_socket: setsockopt SO_DONTROUTE\n");
- ret = setsockopt(psock, SOL_SOCKET, SO_DONTROUTE, &i, sizeof(i));
- if (ret != 0) {
- perror("setsockopt");
- fprintf(stderr, "raw_socket error: setsockopt SO_DONTROUTE\n");
- return EX_OSERR;
- }
- /* connect socket to NIC by name */
- strcpy(ifr.ifr_name, argv[1]);
- ret = ioctl(psock, SIOCGIFINDEX, &ifr);
- ifindex = ifr.ifr_ifindex;
- strcpy(ifr.ifr_name, argv[1]);
- ifr.ifr_flags = 0;
- /* reset flags of NIC interface */
- ret = ioctl(psock, SIOCGIFFLAGS, &ifr);
- /* set flags of NIC interface, here promiscuous and broadcast */
- ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST;
- ret = ioctl(psock, SIOCGIFFLAGS, &ifr);
- /* bind socket to protocol, in this case RAW EtherCAT */
- sll.sll_family = AF_PACKET;
- sll.sll_ifindex = ifindex;
- sll.sll_protocol = htons(ETH_P_ECAT);
- ret = bind(psock, (struct sockaddr *)&sll, sizeof(sll));
- /* setup ethernet headers in tx buffers so we don't have to repeat it */
-
- fprintf(stderr, "raw_socket: setenv ARMARX_LD_LIBRARY_PATH\n");
- ret = setenv("LD_LIBRARY_PATH", getenv("ARMARX_LD_LIBRARY_PATH"), 1);
- if (ret != 0) {
- perror("setenv");
- fprintf(stderr, "raw_socket error: setenv ARMARX_LD_LIBRARY_PATH\n");
- return EX_OSERR;
- }
-
- i=0;
- fprintf(stderr, "ARGUMENT %d/%d: %s\n", i+1, argssize, strrchr(argv[2], '/'));
- args[i++] = strrchr(argv[2], '/');
-
- snprintf(sock, 5, "%d", psock);
-
- fprintf(stderr, "ARGUMENT %d/%d: %s\n", i+1, argssize, sock);
- args[i++] = sock;
- for (i = 2; argv[i] != NULL; i++) {
- fprintf(stderr, "ARGUMENT %d/%d: %s\n", i+1, argssize, argv[i]);
- args[i] = argv[i];
- }
- fprintf(stderr, "ARGUMENT %d/%d: NULL\n", i+1, argssize);
- args[i] = NULL;
- fprintf(stderr, "executing %s\n", argv[2]);
- execve(argv[2], args, environ);
- fprintf(stderr, "executing %s...done!\n", argv[2]);
- perror("execv");
-
- return EX_OSERR;
-}
diff --git a/scenarios/ArMemExample/config/ExampleMemory.cfg b/scenarios/ArMemExample/config/ExampleMemory.cfg
index 7989108bc28741f5c638fe53de5608ecaf8c7612..251382f2af086afccf73924421ea31a2aad01017 100644
--- a/scenarios/ArMemExample/config/ExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemory.cfg
@@ -143,21 +143,12 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemory.mem.MemoryName = Example
-# ArmarX.ExampleMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
+# ArmarX.ExampleMemory.mem.ltm.configuration:
# Attributes:
-# - Default: 10
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemory.mem.ltm..buffer.storeFreq = 10
-
-
-# ArmarX.ExampleMemory.mem.ltm.depthImageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.depthImageExtractor.Enabled = true
+# ArmarX.ExampleMemory.mem.ltm.configuration = {}
# ArmarX.ExampleMemory.mem.ltm.enabled:
@@ -169,100 +160,6 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemory.mem.ltm.enabled = false
-# ArmarX.ExampleMemory.mem.ltm.exrConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.exrConverter.Enabled = true
-
-
-# ArmarX.ExampleMemory.mem.ltm.imageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.imageExtractor.Enabled = true
-
-
-# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.Enabled = false
-
-
-# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-
-
-# ArmarX.ExampleMemory.mem.ltm.pngConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.pngConverter.Enabled = true
-
-
-# ArmarX.ExampleMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.Enabled = false
-
-
-# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
-
-
-# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.Enabled = false
-
-
-# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
-
-
-# ArmarX.ExampleMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ExampleMemory.mem.ltm.storagepath = Default value not mapped.
-
-
# ArmarX.ExampleMemory.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:
diff --git a/scenarios/ArMemIndex/ArMemIndex.scx b/scenarios/ArMemIndex/ArMemIndex.scx
new file mode 100644
index 0000000000000000000000000000000000000000..357d9d9fb0ebefa824edfc1f2f0003b70c0b0da8
--- /dev/null
+++ b/scenarios/ArMemIndex/ArMemIndex.scx
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="ArMemIndex" creation="2022-11-10.11:51:30" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="IndexMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ObjectInstanceToIndex" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/ArMemIndex/config/IndexMemory.cfg b/scenarios/ArMemIndex/config/IndexMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4669516acbe1d273ae850b7c11c985aa996174dc
--- /dev/null
+++ b/scenarios/ArMemIndex/config/IndexMemory.cfg
@@ -0,0 +1,247 @@
+# ==================================================================
+# IndexMemory properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.IndexMemory.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.IndexMemory.EnableProfiling = false
+
+
+# ArmarX.IndexMemory.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.IndexMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.IndexMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IndexMemory.ObjectName = ""
+
+
+# ArmarX.IndexMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IndexMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.IndexMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Index
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IndexMemory.mem.MemoryName = Index
+
+
+# ArmarX.IndexMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IndexMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.IndexMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.IndexMemory.mem.ltm.enabled = false
+
+
+# ArmarX.IndexMemory.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.IndexMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.IndexMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IndexMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.IndexMemory.p.maxHistorySize: The maximum size of entity histories.
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.IndexMemory.p.maxHistorySize = 1024
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArMemIndex/config/ObjectInstanceToIndex.cfg b/scenarios/ArMemIndex/config/ObjectInstanceToIndex.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..d3326ab2ef95e6bcee49a0325c9393f41392aeee
--- /dev/null
+++ b/scenarios/ArMemIndex/config/ObjectInstanceToIndex.cfg
@@ -0,0 +1,230 @@
+# ==================================================================
+# ObjectInstanceToIndex properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.ObjectInstanceToIndex.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.ObjectInstanceToIndex.EnableProfiling = false
+
+
+# ArmarX.ObjectInstanceToIndex.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.ObjectInstanceToIndex.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectInstanceToIndex.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectInstanceToIndex.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.ObjectInstanceToIndex.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectInstanceToIndex.ObjectName = ""
+
+
+# ArmarX.ObjectInstanceToIndex.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectInstanceToIndex.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.ObjectInstanceToIndex.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.ObjectInstanceToIndex.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ObjectInstanceToIndex.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectInstanceToIndex.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ObjectInstanceToIndex.p.object.maxFrequency:
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectInstanceToIndex.p.object.maxFrequency = 10
+
+
+# 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/SickLaserUnit/config/global.cfg b/scenarios/ArMemIndex/config/global.cfg
similarity index 75%
rename from scenarios/SickLaserUnit/config/global.cfg
rename to scenarios/ArMemIndex/config/global.cfg
index ede51536f872c9c144104e26b4754f6a0ca7b076..54a06a7b182fd44c1729bbe21869caf14b07a974 100644
--- a/scenarios/SickLaserUnit/config/global.cfg
+++ b/scenarios/ArMemIndex/config/global.cfg
@@ -1,4 +1,4 @@
# ==================================================================
-# Global Config from Scenario SickLaserUnit
+# Global Config from Scenario ArMemIndex
# ==================================================================
diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
index ce2498f12be6b0ce2cd02bfab3ebdc4140a797ca..70e2058fb47f4e30049a02831a4dcd08291144d5 100644
--- a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
@@ -93,6 +93,22 @@ ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.write.ProviderName
# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.class:
+# Attributes:
+# - Default: mobile-dishwasher
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.class = mobile-dishwasher
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset:
+# Attributes:
+# - Default: Kitchen
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset = Kitchen
+
+
# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
# - Default: DebugObserver
diff --git a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index 9afe99824d427e88708b43ddea4810c20a9a54ec..e7d8b1893f25e727867713fce421bab5ca60ae9c 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -150,6 +150,22 @@
# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
+# ArmarX.ObjectMemory.mem..marker.Name: Marker Memory Name
+# Attributes:
+# - Default: Marker
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem..marker.Name = Marker
+
+
+# ArmarX.ObjectMemory.mem..marker.maxHistorySize: Maximum marker memory history size
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem..marker.maxHistorySize = -1
+
+
# ArmarX.ObjectMemory.mem.MemoryName: Name of this memory server.
# Attributes:
# - Default: Object
@@ -375,9 +391,12 @@ ArmarX.ObjectMemory.mem.inst.robots.FallbackName = CameraOnTripod
# ArmarX.ObjectMemory.mem.inst.scene.11_Directory = scenes
-# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
-# You can also specify paths relative to 'Package/scenes/'.
-# You can also specify a ; separated list of scenes.
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene(s) to load on startup.
+# Specify multiple scenes in a ; separated list.
+# Each entry must be one of the following:
+# (1) A scene file in 'Package/scenes/' (with or without '.json' extension), e.g. 'MyScene', 'MyScene.json'
+# (2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' extension), e.g. 'path/to/MyScene', 'path/to/MyScene.json'
+# (3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'
# Attributes:
# - Default: ""
# - Case sensitivity: yes
@@ -496,13 +515,39 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003
# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
-# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.show: Show arrows linearly predicting object positions.
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha: Alpha of linear prediction ghosts.
+# Attributes:
+# - Default: 0.699999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha = 0.699999988
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow: Show arrows from current object poses to the linearly predicted ones.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame: Show frames at linearly predicted object poses.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost: Show ghosts at linearly predicted object poses.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.show = false
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost = false
# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset: The offset (in seconds) to the current time to make predictions for.
diff --git a/scenarios/ArMemObjectMemory/config/RobotStateMemory.cfg b/scenarios/ArMemObjectMemory/config/RobotStateMemory.cfg
index 56578126f5f55af02b52434b880195abfa39465d..6628b865775889f4fb61656c27a3bb98d59b3d2f 100644
--- a/scenarios/ArMemObjectMemory/config/RobotStateMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/RobotStateMemory.cfg
@@ -274,6 +274,15 @@
# ArmarX.RobotStateMemory.mem.visu.enabled = true
+# ArmarX.RobotStateMemory.mem.visu.famesEnabled: Enable or disable visualization of frames.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.visu.famesEnabled = false
+
+
# ArmarX.RobotStateMemory.mem.visu.frequenzyHz: Frequency of visualization.
# Attributes:
# - Default: 25
@@ -300,6 +309,14 @@
# ArmarX.RobotStateMemory.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.RobotStateMemory.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.prediction.TimeWindow = 2
+
+
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
# - Default: 0
diff --git a/scenarios/ArMemObjectMemory/config/SimpleVirtualRobot.cfg b/scenarios/ArMemObjectMemory/config/SimpleVirtualRobot.cfg
index 55fd820ebb611bf06941cf5c250e8621b825ce1d..12d7610381208f1735c56021f1614810335aa8da 100644
--- a/scenarios/ArMemObjectMemory/config/SimpleVirtualRobot.cfg
+++ b/scenarios/ArMemObjectMemory/config/SimpleVirtualRobot.cfg
@@ -170,6 +170,14 @@
# ArmarX.SimpleVirtualRobot.p.oneShot = true
+# ArmarX.SimpleVirtualRobot.p.robot.jointValues: Specify a certain joint configuration.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SimpleVirtualRobot.p.robot.jointValues = ""
+
+
# ArmarX.SimpleVirtualRobot.p.robot.name: Optional override for the robot name. If not set, the default name from the robot model is used.
# Attributes:
# - Default: ""
diff --git a/scenarios/ArVizExample/config/ArVizInteractExample.cfg b/scenarios/ArVizExample/config/ArVizInteractExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..0892ab764555b0fba8759653513371035dc9b45d
--- /dev/null
+++ b/scenarios/ArVizExample/config/ArVizInteractExample.cfg
@@ -0,0 +1,204 @@
+# ==================================================================
+# ArVizInteractExample 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.ArVizInteractExample.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizInteractExample.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.ArVizInteractExample.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizInteractExample.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ArVizInteractExample.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.ArVizInteractExample.EnableProfiling = false
+
+
+# ArmarX.ArVizInteractExample.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.ArVizInteractExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArVizInteractExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizInteractExample.ObjectName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/AronComponentConfigExample/AronComponentConfigExample.scx b/scenarios/AronComponentConfigExample/AronComponentConfigExample.scx
new file mode 100644
index 0000000000000000000000000000000000000000..250a99e0a034d4addcef5a6039bf3b4f61e9bd24
--- /dev/null
+++ b/scenarios/AronComponentConfigExample/AronComponentConfigExample.scx
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="AronComponentConfigExample" creation="2022-11-02.11:11:59 AM" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="AronComponentConfigExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg b/scenarios/AronComponentConfigExample/config/AronComponentConfigExample.cfg
similarity index 63%
rename from scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
rename to scenarios/AronComponentConfigExample/config/AronComponentConfigExample.cfg
index b988f3b13d77676d1b3a85206cf37013b3ea36a4..dcf2233099e0acef6a620567979cc71e1da2cc29 100644
--- a/scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
+++ b/scenarios/AronComponentConfigExample/config/AronComponentConfigExample.cfg
@@ -1,5 +1,5 @@
# ==================================================================
-# SickLaserUnit properties
+# AronComponentConfigExample 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.
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
@@ -26,263 +26,277 @@
# ArmarX.CachePath = mongo/.cache
-# ArmarX.Config: Comma-separated list of configuration files
+# ArmarX.ComponentConfigTest.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: ""
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Config = ""
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ComponentConfigTest.EnableProfiling = false
-# ArmarX.DataPath: Semicolon-separated search list for data files
+# ArmarX.ComponentConfigTest.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: ""
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DataPath = ""
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ComponentConfigTest.MinimumLoggingLevel = Undefined
-# 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'.
+# ArmarX.ComponentConfigTest.ObjectName: Name of IceGrid well-known object
# Attributes:
-# - Default: Default value not mapped.
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DefaultPackages = Default value not mapped.
+# ArmarX.ComponentConfigTest.ObjectName = ""
-# 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.
+# ArmarX.ComponentConfigTest.RemoteGuiName: Name of the remote gui provider
# Attributes:
-# - Default: ./config/dependencies.cfg
+# - Default: RemoteGuiProvider
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+# ArmarX.ComponentConfigTest.RemoteGuiName = RemoteGuiProvider
-# ArmarX.DisableLogging: Turn logging off in whole application
+# ArmarX.ComponentConfigTest.boolMember:
# Attributes:
-# - Default: false
+# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.DisableLogging = false
+# ArmarX.ComponentConfigTest.boolMember = true
-# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# ArmarX.ComponentConfigTest.floatMember:
# Attributes:
-# - Default: false
+# - Default: 100
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.EnableProfiling = false
+# ArmarX.ComponentConfigTest.floatMember = 100
-# 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;...
+# ArmarX.ComponentConfigTest.intMember:
# Attributes:
-# - Default: ""
+# - Default: 1000
# - Case sensitivity: yes
# - Required: no
-# ArmarX.LoadLibraries = ""
+# ArmarX.ComponentConfigTest.intMember = 1000
-# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# ArmarX.ComponentConfigTest.stringMember:
# Attributes:
-# - Default: ""
+# - Default: initial
# - Case sensitivity: yes
# - Required: no
-# ArmarX.LoggingGroup = ""
+# ArmarX.ComponentConfigTest.stringMember = initial
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# ArmarX.ComponentConfigTest.subMember.boolMember:
# Attributes:
-# - Default: true
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
+# ArmarX.ComponentConfigTest.subMember.boolMember = false
-# 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)
+# ArmarX.ComponentConfigTest.subMember.doubleMember:
# Attributes:
-# - Default: 3000
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# ArmarX.ComponentConfigTest.subMember.doubleMember = 0
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# ArmarX.ComponentConfigTest.subMember.floatMember:
# Attributes:
# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+# ArmarX.ComponentConfigTest.subMember.floatMember = 0
-# ArmarX.SickLaserUnit.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ComponentConfigTest.subMember.intMember:
# Attributes:
-# - Default: false
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.EnableProfiling = false
+# ArmarX.ComponentConfigTest.subMember.intMember = 0
-# ArmarX.SickLaserUnit.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.ComponentConfigTest.subMember.stringMember:
# Attributes:
-# - Default: Undefined
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.SickLaserUnit.MinimumLoggingLevel = Undefined
+# ArmarX.ComponentConfigTest.subMember.stringMember = ""
-# ArmarX.SickLaserUnit.ObjectName: Name of IceGrid well-known object
+# ArmarX.ComponentConfigTest.subMember.subsubMember.boolMember:
# Attributes:
-# - Default: ""
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ComponentConfigTest.subMember.subsubMember.boolMember = false
+
+
+# ArmarX.ComponentConfigTest.subMember.subsubMember.enumMember:
+# Attributes:
+# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.ObjectName = ""
+# - Possible values: {Bar, Baz, Foo, Qux}
+# ArmarX.ComponentConfigTest.subMember.subsubMember.enumMember = 1
-# ArmarX.SickLaserUnit.TopicName: Name of the laserscanner topic to report to.
+# ArmarX.ComponentConfigTest.subMember.subsubMember.floatMember:
# Attributes:
-# - Default: LaserScans
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.TopicName = LaserScans
+# ArmarX.ComponentConfigTest.subMember.subsubMember.floatMember = 0
-# ArmarX.SickLaserUnit.angleOffset: No Description
+# ArmarX.ComponentConfigTest.subMember.subsubMember.intDictMember:
# Attributes:
-# - Default: 0.0
-# - Case sensitivity: no
+# - Default: int1:1,int2:2
+# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.angleOffset = 0.0
+ArmarX.ComponentConfigTest.subMember.subsubMember.intDictMember = int1:1,int2:2,int3:3,int5:4
-# ArmarX.SickLaserUnit.devices: List of Devices in format frame1,ip1,port1;frame2,ip2,port2
+# ArmarX.ComponentConfigTest.subMember.subsubMember.intListMember:
# Attributes:
-# - Default: LaserScannerFront,192.168.8.133,2112
+# - Default: 1, 2, 3, 4, 5, 6
# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.devices = LaserScannerFront,192.168.8.133,2112;LaserScannerBack,192.168.8.133,2112
+# ArmarX.ComponentConfigTest.subMember.subsubMember.intListMember = 1, 2, 3, 4, 5, 6
-# ArmarX.SickLaserUnit.heartbeat.TopicName: Name of the topic the DebugObserver listens on
+# ArmarX.ComponentConfigTest.subMember.subsubMember.intMember:
# Attributes:
-# - Default: DebugObserver
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.heartbeat.TopicName = DebugObserver
+# ArmarX.ComponentConfigTest.subMember.subsubMember.intMember = 0
-# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeErrorMS: TODO: maximumCycleTimeErrorMS
+# ArmarX.ComponentConfigTest.subMember.subsubMember.stringDictMember:
# Attributes:
+# - Default: string1:blub,string2:duh
# - Case sensitivity: yes
-# - Required: yes
-# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeErrorMS = ::_NOT_SET_::
+# - Required: no
+# ArmarX.ComponentConfigTest.subMember.subsubMember.stringDictMember = string1:blub,string2:duh
-# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeWarningMS: TODO: maximumCycleTimeWarningMS
+# ArmarX.ComponentConfigTest.subMember.subsubMember.stringListMember:
# Attributes:
+# - Default: a, b, c, d, e
# - Case sensitivity: yes
-# - Required: yes
-# ArmarX.SickLaserUnit.heartbeat.maximumCycleTimeWarningMS = ::_NOT_SET_::
+# - Required: no
+# ArmarX.ComponentConfigTest.subMember.subsubMember.stringListMember = a, b, c, d, e
-# ArmarX.SickLaserUnit.heartbeatErrorMS: maximum cycle time before heartbeat Error
+# ArmarX.ComponentConfigTest.subMember.subsubMember.stringMember:
# Attributes:
-# - Default: 800
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.heartbeatErrorMS = 800
+# ArmarX.ComponentConfigTest.subMember.subsubMember.stringMember = ""
-# ArmarX.SickLaserUnit.heartbeatWarnMS: maximum cycle time before heartbeat Warning
+# ArmarX.Config: Comma-separated list of configuration files
# Attributes:
-# - Default: 500
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.heartbeatWarnMS = 500
+# ArmarX.Config = ""
-# ArmarX.SickLaserUnit.hostname: No Description
+# ArmarX.DataPath: Semicolon-separated search list for data files
# Attributes:
-# - Default: 192.168.8.133
-# - Case sensitivity: no
+# - Default: ""
+# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.hostname = 192.168.8.133
+# ArmarX.DataPath = ""
-# ArmarX.SickLaserUnit.laserScannerTopicName: No Description
+# 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: "SickTopic"
-# - Case sensitivity: no
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.laserScannerTopicName = "SickTopic"
+# ArmarX.DefaultPackages = Default value not mapped.
-# ArmarX.SickLaserUnit.port: No Description
+# 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: 2112
-# - Case sensitivity: no
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.port = 2112
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
-# ArmarX.SickLaserUnit.protocol: No Description
+# ArmarX.DisableLogging: Turn logging off in whole application
# Attributes:
-# - Default: ASCII
-# - Case sensitivity: no
+# - Default: false
+# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.protocol = ASCII
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
-# ArmarX.SickLaserUnit.rangeMax: maximum Range of the Scanner
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
# Attributes:
-# - Default: 10
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.rangeMax = 10
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
-# ArmarX.SickLaserUnit.rangeMin: minimum Range of the Scanner
+# 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: 0
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.rangeMin = 0
+# ArmarX.LoadLibraries = ""
-# ArmarX.SickLaserUnit.scannerType: Name of the LaserScanner
+# 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: sick_tim_5xx
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
+# ArmarX.LoggingGroup = ""
-# ArmarX.SickLaserUnit.timeIncrement: No Description
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
-# - Default: 0.1
-# - Case sensitivity: no
+# - Default: true
+# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.timeIncrement = 0.1
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
-# ArmarX.SickLaserUnit.timelimit: timelimit for communication
+# 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: 5
+# - Default: 3000
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.timelimit = 5
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.SickLaserUnit.updatePeriod: No Description
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
-# - Default: 1
-# - Case sensitivity: no
+# - Default: 0
+# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.updatePeriod = 1
+# ArmarX.SecondsStartupDelay = 0
# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
diff --git a/scenarios/AronComponentConfigExample/config/RemoteGuiProviderApp.cfg b/scenarios/AronComponentConfigExample/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4b6abea40d72afd7d313ee47a9b191f3b26de30d
--- /dev/null
+++ b/scenarios/AronComponentConfigExample/config/RemoteGuiProviderApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# RemoteGuiProviderApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteGuiProvider.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RemoteGuiProvider.EnableProfiling = false
+
+
+# ArmarX.RemoteGuiProvider.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.RemoteGuiProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RemoteGuiProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.ObjectName = ""
+
+
+# ArmarX.RemoteGuiProvider.TopicName: Name of the topic on which updates to the remote state are reported.
+# Attributes:
+# - Default: RemoteGuiTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.TopicName = RemoteGuiTopic
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/AronComponentConfigExample/config/global.cfg b/scenarios/AronComponentConfigExample/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..44dde159548b3d54011a075a37f6ac0d7fc41f14
--- /dev/null
+++ b/scenarios/AronComponentConfigExample/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario AronComponentConfigExample
+# ==================================================================
+
diff --git a/scenarios/ArticulatedObjectWriterExample/ArticulatedObjectWriterExample.scx b/scenarios/ArticulatedObjectWriterExample/ArticulatedObjectWriterExample.scx
new file mode 100644
index 0000000000000000000000000000000000000000..9e4ef2810eae6db0c1a475e9b220fae7e18418b6
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/ArticulatedObjectWriterExample.scx
@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="ArticulatedObjectWriterExample" creation="2022-10-10.18:54:39" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ArticulatedObjectLocalizerExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotStateMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/ArticulatedObjectWriterExample/config/ArVizStorage.cfg b/scenarios/ArticulatedObjectWriterExample/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..302ac28c37dd28de3e68fb4fe4c2174faa4ec3bf
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/config/ArVizStorage.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# ArVizStorage properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.ArVizStorage.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArVizStorage.EnableProfiling = false
+
+
+# ArmarX.ArVizStorage.HistoryPath: Destination path where the history is serialized to
+# Attributes:
+# - Default: RobotAPI/ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.HistoryPath = RobotAPI/ArVizStorage
+
+
+# ArmarX.ArVizStorage.MaxHistorySize: How many layer updates are saved in the history until they are compressed
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.MaxHistorySize = 1000
+
+
+# ArmarX.ArVizStorage.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ArVizStorage.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArVizStorage.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.ObjectName = ""
+
+
+# ArmarX.ArVizStorage.TopicName: Layer updates are sent over this topic.
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.TopicName = ArVizTopic
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/SickLaserUnit/config/SickLaserUnit.cfg b/scenarios/ArticulatedObjectWriterExample/config/ArticulatedObjectLocalizerExample.cfg
similarity index 69%
rename from scenarios/SickLaserUnit/config/SickLaserUnit.cfg
rename to scenarios/ArticulatedObjectWriterExample/config/ArticulatedObjectLocalizerExample.cfg
index 1a54fe66a787a3e000d81f934a6ec0b011e1e0f5..e6c221120c7a8f33693caf9dc5fa229875dabb4a 100644
--- a/scenarios/SickLaserUnit/config/SickLaserUnit.cfg
+++ b/scenarios/ArticulatedObjectWriterExample/config/ArticulatedObjectLocalizerExample.cfg
@@ -1,5 +1,5 @@
# ==================================================================
-# SickLaserUnit properties
+# ArticulatedObjectLocalizerExample 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.
@@ -18,234 +18,210 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.ArticulatedObjectLocalizerExample.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: mongo/.cache
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.CachePath = mongo/.cache
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArticulatedObjectLocalizerExample.EnableProfiling = false
-# ArmarX.Config: Comma-separated list of configuration files
+# ArmarX.ArticulatedObjectLocalizerExample.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: ""
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# ArmarX.Config = ""
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ArticulatedObjectLocalizerExample.MinimumLoggingLevel = Undefined
-# ArmarX.DataPath: Semicolon-separated search list for data files
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DataPath = ""
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName = ""
-# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
# Attributes:
-# - Default: Default value not mapped.
+# - Default: Class
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DefaultPackages = Default value not mapped.
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = Class
-# 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.
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName:
# Attributes:
-# - Default: ./config/dependencies.cfg
+# - Default: Object
# - Case sensitivity: yes
# - Required: no
-# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName = Object
-# ArmarX.DisableLogging: Turn logging off in whole application
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.read.ProviderName:
# Attributes:
-# - Default: false
+# - Default: PriorKnowledgeData
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.DisableLogging = false
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.read.ProviderName = PriorKnowledgeData
-# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.write.ProviderName: Name of this provider
# Attributes:
-# - Default: false
# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.EnableProfiling = false
+# - Required: yes
+ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.write.ProviderName = ExampleProvider
-# 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;...
+# ArmarX.ArticulatedObjectLocalizerExample.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: ""
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.LoadLibraries = ""
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled = true
-# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: ""
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.LoggingGroup = ""
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.class:
# Attributes:
-# - Default: true
+# - Default: mobile-dishwasher
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
+ArmarX.ArticulatedObjectLocalizerExample.p.obj.class = cabinet
-# 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)
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset:
# Attributes:
-# - Default: 3000
+# - Default: Kitchen
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
+ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset = RBO-articulated-object-dataset
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
# Attributes:
-# - Default: 0
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver = DebugObserver
-# ArmarX.SickLaserUnit.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency: Memory update frequency (write).
# Attributes:
-# - Default: false
+# - Default: 25
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.EnableProfiling = false
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency = 25
-# ArmarX.SickLaserUnit.MinimumLoggingLevel: Local logging level only for this component
+# 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: Undefined
+# - Default: mongo/.cache
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.SickLaserUnit.MinimumLoggingLevel = Undefined
+# ArmarX.CachePath = mongo/.cache
-# ArmarX.SickLaserUnit.ObjectName: Name of IceGrid well-known object
+# ArmarX.Config: Comma-separated list of configuration files
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.ObjectName = ""
+# ArmarX.Config = ""
-# ArmarX.SickLaserUnit.deviceNumber: number of the LaserScanner Device
+# ArmarX.DataPath: Semicolon-separated search list for data files
# Attributes:
-# - Default: 0
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.deviceNumber = 0
+# ArmarX.DataPath = ""
-# ArmarX.SickLaserUnit.emulateSensor: overwrite the default Settings and don't connect to Scanner
+# 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: false
+# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.emulateSensor = false
+# ArmarX.DefaultPackages = Default value not mapped.
-# ArmarX.SickLaserUnit.hostname: Hostname of the LaserScanner
+# 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: 192.168.8.133
+# - Default: ./config/dependencies.cfg
# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.hostname = 192.168.8.133
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
-# ArmarX.SickLaserUnit.newIpAddress: New IP address for the LaserScanner
+# ArmarX.DisableLogging: Turn logging off in whole application
# Attributes:
-# - Default: ""
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.newIpAddress = ""
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
-# ArmarX.SickLaserUnit.port: port to use on the LaserScanner
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
# Attributes:
-# - Default: 2112
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.port = 2112
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
-# ArmarX.SickLaserUnit.protocol: Either use ASCII or Binary protocol
+# 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: Binary
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {ASCII, Binary}
-ArmarX.SickLaserUnit.protocol = ASCII
-
-
-# ArmarX.SickLaserUnit.rangeMax: maximum Range of the Scanner
-# Attributes:
-# - Case sensitivity: yes
-# - Required: yes
-ArmarX.SickLaserUnit.rangeMax = 1.00
-
-
-# ArmarX.SickLaserUnit.rangeMin: minimum Range of the Scanner
-# Attributes:
-# - Case sensitivity: yes
-# - Required: yes
-ArmarX.SickLaserUnit.rangeMin = 0.01
-
-
-# ArmarX.SickLaserUnit.scannerType: Name of the LaserScanner
-# Attributes:
-# - Case sensitivity: yes
-# - Required: yes
-ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
+# ArmarX.LoadLibraries = ""
-# ArmarX.SickLaserUnit.sopasProtocolType: Automatically set to true if the Scanner does not support ASCII communication
+# 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: false
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.sopasProtocolType = false
+# ArmarX.LoggingGroup = ""
-# ArmarX.SickLaserUnit.subscribeDatagram: subscribe to Datagram in communication or not
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
-# - Default: false
+# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SickLaserUnit.subscribeDatagram = false
+# ArmarX.RedirectStdout = true
-# ArmarX.SickLaserUnit.timeIncrement: timeIncrement??
+# 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: 0
+# - Default: 3000
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SickLaserUnit.timeIncrement = 0
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.SickLaserUnit.timelimit: timelimit for communication
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
-# - Default: 5
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-ArmarX.SickLaserUnit.timelimit = 5
+# ArmarX.SecondsStartupDelay = 0
# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
diff --git a/scenarios/ArticulatedObjectWriterExample/config/MemoryNameSystem.cfg b/scenarios/ArticulatedObjectWriterExample/config/MemoryNameSystem.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b8bc70a66ca7f32a628886ad1bf13e373f9750d3
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/config/MemoryNameSystem.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# MemoryNameSystem properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.MemoryNameSystem.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.MemoryNameSystem.EnableProfiling = false
+
+
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.MemoryNameSystem.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.MemoryNameSystem.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.ObjectName = ""
+
+
+# ArmarX.MemoryNameSystem.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.MemoryNameSystem.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArticulatedObjectWriterExample/config/ObjectMemory.cfg b/scenarios/ArticulatedObjectWriterExample/config/ObjectMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..95961b39febd5772f9f3ee485552631f407b73cc
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/config/ObjectMemory.cfg
@@ -0,0 +1,701 @@
+# ==================================================================
+# ObjectMemory properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.ObjectMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.ObjectMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ObjectMemory.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.EnableProfiling = false
+
+
+# ArmarX.ObjectMemory.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.ObjectMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ObjectMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.ObjectName = ""
+
+
+# ArmarX.ObjectMemory.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName: Name of the kinematic unit observer.
+# Attributes:
+# - Default: KinematicUnitObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
+
+
+# ArmarX.ObjectMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.MemoryName = Object
+
+
+# ArmarX.ObjectMemory.mem.attachments.CoreSegmentName: Name of the object instance core segment.
+# Attributes:
+# - Default: Attachments
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.attachments.CoreSegmentName = Attachments
+
+
+# ArmarX.ObjectMemory.mem.attachments.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.attachments.MaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName: Object class entity of the floor.
+# Attributes:
+# - Default: Building/floor-20x20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.EntityName = Building/floor-20x20
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Height: Height (z) of the floor plane.
+# Set slightly below 0 to avoid z-fighting when drawing planes on the ground.
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.Height = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName: Layer to draw the floor on.
+# Attributes:
+# - Default: Floor
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.Floor.LayerName = Floor
+
+
+# ArmarX.ObjectMemory.mem.cls.Floor.Show: Whether to show the floor.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.Floor.Show = true
+
+
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.cls.LoadFromObjectsPackage = true
+
+
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage: Name of the objects package to load from.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = PriorKnowledgeData
+
+
+# ArmarX.ObjectMemory.mem.cls.seg.CoreMaxHistorySize: Maximal size of the Class entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.ObjectMemory.mem.cls.seg.CoreSegmentName: Name of the Class core segment.
+# Attributes:
+# - Default: Class
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.cls.seg.CoreSegmentName = Class
+
+
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
+# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.DiscardSnapshotsWhileAttached = true
+
+
+# ArmarX.ObjectMemory.mem.inst.calibration.offset: Offset for the node to be calibrated.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.offset = 0
+
+
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName: Name of robot whose note can be calibrated.
+# If not given, the 'fallbackName' is used.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.robotName = ""
+
+
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode: Robot node which can be calibrated.
+# Attributes:
+# - Default: Neck_2_Pitch
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.calibration.robotNode = Neck_2_Pitch
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds: Duration after latest localization before decay starts.
+# Attributes:
+# - Default: 5
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.delaySeconds = 5
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds: How long to reach minimal confidence.
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.durationSeconds = 20
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.enabled: If true, object poses decay over time when not localized anymore.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.decay.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence: Confidence when decay starts.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.maxConfidence = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence: Confidence after decay duration.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.minConfidence = 0
+
+
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence: Remove objects whose confidence is lower than this value.
+# Attributes:
+# - Default: 0.100000001
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.decay.removeObjectsBelowConfidence = 0.100000001
+
+
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity: If true, check whether the head is moving and discard updates in the meantime.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.head.checkHeadVelocity = true
+
+
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS: For how long new updates are ignored after moving the head.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.head.discardIntervalAfterMoveMS = 100
+
+
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity: If a head joint's velocity is higher, the head is considered moving.
+# Attributes:
+# - Default: 0.0500000007
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity = 0.0500000007
+
+
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName: Robot name to use as fallback if the robot name is not specified in a provided object pose.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.robots.FallbackName = ""
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.10_Package: ArmarX package containing the scene snapshots.
+# Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.10_Package = PriorKnowledgeData
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.11_Directory: Directory in Package/data/Package/ containing the scene snapshots.
+# Attributes:
+# - Default: scenes
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.11_Directory = scenes
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad: Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
+# You can also specify paths relative to 'Package/scenes/'.
+# You can also specify a ; separated list of scenes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = ""
+
+
+# ArmarX.ObjectMemory.mem.inst.seg.CoreMaxHistorySize: Maximal size of the Instance entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.ObjectMemory.mem.inst.seg.CoreSegmentName: Name of the Instance core segment.
+# Attributes:
+# - Default: Instance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.seg.CoreSegmentName = Instance
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.alpha = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence: If true, use the pose confidence as alpha (if < 1.0).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.alphaByConfidence = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.enabled: Enable or disable visualization of objects.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.enabled = true
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz: Frequency of visualization.
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.frequenzyHz = 25
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position: Enable showing pose gaussians (orientation part).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.position = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced: Displace center orientation (co)variance circle arrows along their rotation axis.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionDisplaced = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale: Scaling of pose gaussians (orientation part).
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.gaussians.positionScale = 100
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame: If true, show global poses. If false, show poses in robot frame.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.inGlobalFrame = true
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames: Enable showing object frames.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.objectFrames = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale: Scaling of object frames.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.objectFramesScale = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs: Enable showing oriented bounding boxes.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha: Alpha of linear prediction ghosts.
+# Attributes:
+# - Default: 0.699999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha = 0.699999988
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow: Show arrows from current object poses to the linearly predicted ones.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame: Show frames at linearly predicted object poses.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost: Show ghosts at linearly predicted object poses.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset: The offset (in seconds) to the current time to make predictions for.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset = 1
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeWindow: The time window (in seconds) into the past to perform the regression on.
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeWindow = 2
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels: Prefer articulated object models if available.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
+
+
+# ArmarX.ObjectMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.ObjectMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.ltm.enabled = false
+
+
+# ArmarX.ObjectMemory.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ObjectMemory.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.prediction.TimeWindow = 2
+
+
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
+# Attributes:
+# - Default: ObjectPoseTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic = ObjectPoseTopic
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArticulatedObjectWriterExample/config/RemoteGuiProviderApp.cfg b/scenarios/ArticulatedObjectWriterExample/config/RemoteGuiProviderApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..4b6abea40d72afd7d313ee47a9b191f3b26de30d
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/config/RemoteGuiProviderApp.cfg
@@ -0,0 +1,196 @@
+# ==================================================================
+# RemoteGuiProviderApp properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteGuiProvider.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RemoteGuiProvider.EnableProfiling = false
+
+
+# ArmarX.RemoteGuiProvider.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.RemoteGuiProvider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RemoteGuiProvider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.ObjectName = ""
+
+
+# ArmarX.RemoteGuiProvider.TopicName: Name of the topic on which updates to the remote state are reported.
+# Attributes:
+# - Default: RemoteGuiTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteGuiProvider.TopicName = RemoteGuiTopic
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArticulatedObjectWriterExample/config/RobotStateMemory.cfg b/scenarios/ArticulatedObjectWriterExample/config/RobotStateMemory.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5839620e5e3c5df7e080f39405a021bdbcd4a0ea
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/config/RobotStateMemory.cfg
@@ -0,0 +1,361 @@
+# ==================================================================
+# RobotStateMemory properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.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.RobotStateMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.RobotStateMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.RobotStateMemory.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.RobotStateMemory.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.RobotStateMemory.EnableProfiling = false
+
+
+# ArmarX.RobotStateMemory.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.RobotStateMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotStateMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.ObjectName = ""
+
+
+# ArmarX.RobotStateMemory.RobotUnit.SensorValuePrefix: Prefix of all sensor values.
+# Attributes:
+# - Default: sens.*
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.RobotUnit.SensorValuePrefix = sens.*
+
+
+# ArmarX.RobotStateMemory.RobotUnit.UpdateFrequency: The frequency to store values in Hz. All other values get discarded. Minimum is 1, max is 100.000000.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.RobotUnit.UpdateFrequency = 50
+
+
+# ArmarX.RobotStateMemory.RobotUnitName: Name of the RobotUnit
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.RobotUnitName = ""
+
+
+# ArmarX.RobotStateMemory.WaitForRobotUnit: Add the robot unit as dependency to the component. This memory requires a running RobotUnit, therefore we should add it as explicit dependency.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.WaitForRobotUnit = false
+
+
+# ArmarX.RobotStateMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.MemoryName = RobotState
+
+
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreMaxHistorySize: Maximal size of the Description entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreSegmentName: Name of the Description core segment.
+# Attributes:
+# - Default: Description
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.desc.seg.CoreSegmentName = Description
+
+
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreMaxHistorySize: Maximal size of the Localization entity histories (-1 for infinite).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreMaxHistorySize = 1024
+
+
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreSegmentName: Name of the Localization core segment.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.loc.seg.CoreSegmentName = Localization
+
+
+# ArmarX.RobotStateMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.RobotStateMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.ltm.enabled = false
+
+
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreMaxHistorySize: Maximal size of the Proprioception entity histories (-1 for infinite).
+# Attributes:
+# - Default: 1024
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreMaxHistorySize = 1024
+
+
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreSegmentName: Name of the Proprioception core segment.
+# Attributes:
+# - Default: Proprioception
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.prop.seg.CoreSegmentName = Proprioception
+
+
+# ArmarX.RobotStateMemory.mem.visu.enabled: Enable or disable visualization of objects.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.visu.enabled = true
+
+
+# ArmarX.RobotStateMemory.mem.visu.frequenzyHz: Frequency of visualization.
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mem.visu.frequenzyHz = 25
+
+
+# ArmarX.RobotStateMemory.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.RobotStateMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.RobotStateMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.RobotStateMemory.prediction.TimeWindow: Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStateMemory.prediction.TimeWindow = 2
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/ArticulatedObjectWriterExample/config/global.cfg b/scenarios/ArticulatedObjectWriterExample/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..fdfd38bb91a262ada278ef24d00788bd824755ed
--- /dev/null
+++ b/scenarios/ArticulatedObjectWriterExample/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario ArticulatedObjectWriterExample
+# ==================================================================
+
diff --git a/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg b/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg
index 64f5c48c04cd7febdaa8c31cf99d2d8a8a1bb69a..c502c781ec39ec0600a1cb99a0e74729a32410bb 100644
--- a/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg
+++ b/scenarios/LaserScannerTest/config/HokuyoLaserUnitApp.cfg
@@ -2,235 +2,13 @@
# HokuyoLaserUnitApp 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.
+# ArmarX.HokuyoLaserUnit.AngleOffset:
# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.AdditionalPackages = Default value not mapped.
-
-
-# ArmarX.ApplicationName: Application name
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ApplicationName = ""
-
-
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
-# Attributes:
-# - Default: mongo/.cache
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.CachePath = mongo/.cache
-
-
-# ArmarX.Config: Comma-separated list of configuration files
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.Config = ""
-
-
-# ArmarX.DataPath: Semicolon-separated search list for data files
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.DataPath = ""
-
-
-# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.DefaultPackages = Default value not mapped.
-
-
-# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
-# Attributes:
-# - Default: ./config/dependencies.cfg
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.DependenciesConfig = ./config/dependencies.cfg
-
-
-# ArmarX.DisableLogging: Turn logging off in whole application
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.DisableLogging = false
-
-
-# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.EnableProfiling = false
-
-
-# ArmarX.HokuyoLaserUnit.AngleOffset: Offset is applied the raw angles before reporting them
-# Attributes:
-# - Default: -2.3561945
-# - Case sensitivity: yes
-# - Required: no
ArmarX.HokuyoLaserUnit.AngleOffset = 0
-# ArmarX.HokuyoLaserUnit.DebugObserverName: Name of the topic the DebugObserver listens on
-# Attributes:
-# - Default: DebugObserver
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.HokuyoLaserUnit.DebugObserverName = DebugObserver
-
-
-# ArmarX.HokuyoLaserUnit.Devices: List of devices in form of 'IP1,port1,frame1;IP2,port2,frame2;...'
+# ArmarX.HokuyoLaserUnit.Devices:
# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
ArmarX.HokuyoLaserUnit.Devices = "192.168.0.11,10940,Laser Scanner A"
-# ArmarX.HokuyoLaserUnit.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.HokuyoLaserUnit.EnableProfiling = false
-
-
-# ArmarX.HokuyoLaserUnit.LaserScannerTopicName: Name of the laser scan topic.
-# Attributes:
-# - Default: LaserScans
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.HokuyoLaserUnit.LaserScannerTopicName = LaserScans
-
-
-# ArmarX.HokuyoLaserUnit.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.HokuyoLaserUnit.MinimumLoggingLevel = Undefined
-
-
-# ArmarX.HokuyoLaserUnit.ObjectName: Name of IceGrid well-known object
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.HokuyoLaserUnit.ObjectName = ""
-
-
-# ArmarX.HokuyoLaserUnit.RobotHealthTopicName: Name of the RobotHealth topic
-# Attributes:
-# - Default: RobotHealthTopic
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.HokuyoLaserUnit.RobotHealthTopicName = RobotHealthTopic
-
-
-# ArmarX.HokuyoLaserUnit.UpdatePeriod: Update period for laser scans
-# Attributes:
-# - Default: 25
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.HokuyoLaserUnit.UpdatePeriod = 25
-
-
-# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.LoadLibraries = ""
-
-
-# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.LoggingGroup = ""
-
-
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
-
-
-# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
-# Attributes:
-# - Default: 3000
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
-
-
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
-# Attributes:
-# - Default: 0
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SecondsStartupDelay = 0
-
-
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
-
-
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
-# Attributes:
-# - Default: 1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ThreadPoolSize = 1
-
-
-# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.TopicSuffix = ""
-
-
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
-
-
-# ArmarX.Verbosity: Global logging level for whole application
-# Attributes:
-# - Default: Info
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
-
-
diff --git a/scenarios/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg b/scenarios/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg
index 18521b1447699a031af7d198c2de4138876681b0..04e3f5637379f9dfdaec5521d008164bbeb804d9 100644
--- a/scenarios/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg
+++ b/scenarios/LaserScannerTest/config/LaserScannerUnitObserverApp.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/SickLaserUnit/SickLaserUnit.scx b/scenarios/SickLaserUnit/SickLaserUnit.scx
deleted file mode 100644
index 398cd914600aeaf8faf8147dc2feb83b989d70e0..0000000000000000000000000000000000000000
--- a/scenarios/SickLaserUnit/SickLaserUnit.scx
+++ /dev/null
@@ -1,5 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<scenario name="SickLaserUnit" creation="2021-06-25.04:20:13 PM" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
- <application name="SickLaserUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
-</scenario>
-
diff --git a/scenarios/SickLaserUnitTest/SickLaserUnitTest.scx b/scenarios/SickLaserUnitTest/SickLaserUnitTest.scx
deleted file mode 100644
index 8fdb28bdf069b4ec5ffd5d91cffb7b38d19d5a1f..0000000000000000000000000000000000000000
--- a/scenarios/SickLaserUnitTest/SickLaserUnitTest.scx
+++ /dev/null
@@ -1,5 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<scenario name="SickLaserUnitTest" creation="2021-06-17.04:54:46 PM" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
- <application name="SickLaserUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
-</scenario>
-
diff --git a/scenarios/SkillProviderTest/config/SkillsMemory.cfg b/scenarios/SkillProviderTest/config/SkillsMemory.cfg
index 711047a4029175550111268cf857a613c2dd48b2..8b2bf04855a21e5a8ca955da89ba3a309460241e 100644
--- a/scenarios/SkillProviderTest/config/SkillsMemory.cfg
+++ b/scenarios/SkillProviderTest/config/SkillsMemory.cfg
@@ -167,21 +167,12 @@
# ArmarX.SkillMemory.mem.MemoryName = Skill
-# ArmarX.SkillMemory.mem.ltm..buffer.storeFreq: Frequency to store the buffer to the LTM in Hz.
+# ArmarX.SkillMemory.mem.ltm.configuration:
# Attributes:
-# - Default: 10
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SkillMemory.mem.ltm..buffer.storeFreq = 10
-
-
-# ArmarX.SkillMemory.mem.ltm.depthImageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.depthImageExtractor.Enabled = true
+# ArmarX.SkillMemory.mem.ltm.configuration = {}
# ArmarX.SkillMemory.mem.ltm.enabled:
@@ -193,100 +184,6 @@
# ArmarX.SkillMemory.mem.ltm.enabled = false
-# ArmarX.SkillMemory.mem.ltm.exrConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.exrConverter.Enabled = true
-
-
-# ArmarX.SkillMemory.mem.ltm.imageExtractor.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.imageExtractor.Enabled = true
-
-
-# ArmarX.SkillMemory.mem.ltm.memFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.memFreqFilter.Enabled = false
-
-
-# ArmarX.SkillMemory.mem.ltm.memFreqFilter.WaitingTime: Waiting time in MS after each LTM update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.memFreqFilter.WaitingTime = -1
-
-
-# ArmarX.SkillMemory.mem.ltm.pngConverter.Enabled:
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.pngConverter.Enabled = true
-
-
-# ArmarX.SkillMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.SkillMemory.mem.ltm.snapEqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.snapEqFilter.Enabled = false
-
-
-# ArmarX.SkillMemory.mem.ltm.snapEqFilter.MaxWaitingTime: Max Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.snapEqFilter.MaxWaitingTime = -1
-
-
-# ArmarX.SkillMemory.mem.ltm.snapFreqFilter.Enabled:
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.SkillMemory.mem.ltm.snapFreqFilter.Enabled = false
-
-
-# ArmarX.SkillMemory.mem.ltm.snapFreqFilter.WaitingTime: Waiting time in MS after each Entity update.
-# Attributes:
-# - Default: -1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.snapFreqFilter.WaitingTime = -1
-
-
-# ArmarX.SkillMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SkillMemory.mem.ltm.storagepath = Default value not mapped.
-
-
# ArmarX.SkillMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
diff --git a/scenarios/edit_object_memory/config/ObjectMemoryEditor.cfg b/scenarios/edit_object_memory/config/ObjectMemoryEditor.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..827455bc124e50f821cb0552776f6a0335f5a5ee
--- /dev/null
+++ b/scenarios/edit_object_memory/config/ObjectMemoryEditor.cfg
@@ -0,0 +1,244 @@
+# ==================================================================
+# ObjectMemoryEditor properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.InteractiveMemoryEditor.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.InteractiveMemoryEditor.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.InteractiveMemoryEditor.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.InteractiveMemoryEditor.Editor.ConfidenceThreshold: Only objects with a confidence greater than this value are shown.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.Editor.ConfidenceThreshold = 0
+
+
+# ArmarX.InteractiveMemoryEditor.Editor.ObjectScaling: Scaling factor that is applied to all intractable objects.
+# Attributes:
+# - Default: 1.00999999
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.Editor.ObjectScaling = 1.00999999
+
+
+# ArmarX.InteractiveMemoryEditor.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.InteractiveMemoryEditor.EnableProfiling = false
+
+
+# ArmarX.InteractiveMemoryEditor.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.InteractiveMemoryEditor.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.InteractiveMemoryEditor.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.InteractiveMemoryEditor.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.ObjectName = ""
+
+
+# ArmarX.InteractiveMemoryEditor.ObjectPoseTopicName: Name of the object pose topic.
+# Attributes:
+# - Default: ObjectPoseTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.InteractiveMemoryEditor.ObjectPoseTopicName = ObjectPoseTopic
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/SickLaserUnitTest/config/global.cfg b/scenarios/edit_object_memory/config/global.cfg
similarity index 73%
rename from scenarios/SickLaserUnitTest/config/global.cfg
rename to scenarios/edit_object_memory/config/global.cfg
index 7772a85fc1250ba7cdb93d22b93c17d92759d93f..f49a7112c97e55b4b0956b7811c87f912ed9886a 100644
--- a/scenarios/SickLaserUnitTest/config/global.cfg
+++ b/scenarios/edit_object_memory/config/global.cfg
@@ -1,4 +1,4 @@
# ==================================================================
-# Global Config from Scenario SickLaserUnitTest
+# Global Config from Scenario edit_object_memory
# ==================================================================
diff --git a/scenarios/edit_object_memory/edit_object_memory.scx b/scenarios/edit_object_memory/edit_object_memory.scx
new file mode 100644
index 0000000000000000000000000000000000000000..3a4e04db004a526197b0571500276fca63c646a6
--- /dev/null
+++ b/scenarios/edit_object_memory/edit_object_memory.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="edit_object_memory" creation="2022-10-06.15:51:45" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ObjectMemoryEditor" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/scenarios/robot_state_prediction_example/config/RobotStatePredictionClientExample.cfg b/scenarios/robot_state_prediction_example/config/RobotStatePredictionClientExample.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..cafd45782108fea5f823933938463751733671b6
--- /dev/null
+++ b/scenarios/robot_state_prediction_example/config/RobotStatePredictionClientExample.cfg
@@ -0,0 +1,238 @@
+# ==================================================================
+# RobotStatePredictionClientExample properties
+# ==================================================================
+
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.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.RobotStatePredictionClientExample.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.RobotStatePredictionClientExample.EnableProfiling = false
+
+
+# ArmarX.RobotStatePredictionClientExample.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.RobotStatePredictionClientExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotStatePredictionClientExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.ObjectName = ""
+
+
+# ArmarX.RobotStatePredictionClientExample.RemoteGuiName: Name of the remote gui provider
+# Attributes:
+# - Default: RemoteGuiProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.RemoteGuiName = RemoteGuiProvider
+
+
+# ArmarX.RobotStatePredictionClientExample.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.RobotStatePredictionClientExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.RobotStatePredictionClientExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.RobotStatePredictionClientExample.p.predictAheadSeconds: How far into the future to predict [s].
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.p.predictAheadSeconds = 1
+
+
+# ArmarX.RobotStatePredictionClientExample.p.robotName: Name of the robot.
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.p.robotName = Armar6
+
+
+# ArmarX.RobotStatePredictionClientExample.p.updateFrequencyHz: Frequency of predictions [Hz].
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RobotStatePredictionClientExample.p.updateFrequencyHz = 10
+
+
+# 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/robot_state_prediction_example/config/global.cfg b/scenarios/robot_state_prediction_example/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..06e504e09e3cd61958e2dc2c5d617882978b41d7
--- /dev/null
+++ b/scenarios/robot_state_prediction_example/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario robot_state_prediction_example
+# ==================================================================
+
diff --git a/scenarios/robot_state_prediction_example/robot_state_prediction_example.scx b/scenarios/robot_state_prediction_example/robot_state_prediction_example.scx
new file mode 100644
index 0000000000000000000000000000000000000000..2d5de4f038dc12a381b8bc22ae3113ff589b2707
--- /dev/null
+++ b/scenarios/robot_state_prediction_example/robot_state_prediction_example.scx
@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="robot_state_prediction_example" creation="2022-08-15.11:20:29" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="RobotStatePredictionClientExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
index 8630c8a9f76cce5bb9b6d61005bed907786e8b3c..3f4ff4644d9bd11068fd86c55820bbce98d19ebb 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
@@ -599,8 +599,9 @@ void armarx::ArVizStorage::stopRecording(const Ice::Current&)
recordingMetaData.firstTimestampInMicroSeconds = -1;
}
-armarx::viz::data::RecordingSeq armarx::ArVizStorage::getAllRecordings(const Ice::Current&)
+armarx::viz::data::RecordingsInfo armarx::ArVizStorage::getAllRecordings(const Ice::Current&)
{
+ viz::data::RecordingsInfo recordingsInfo;
viz::data::RecordingSeq result;
for (std::filesystem::directory_entry const& entry : std::filesystem::directory_iterator(historyPath))
@@ -619,8 +620,10 @@ armarx::viz::data::RecordingSeq armarx::ArVizStorage::getAllRecordings(const Ice
}
}
+ recordingsInfo.recordings = result;
+ recordingsInfo.recordingsPath = historyPath;
- return result;
+ return recordingsInfo;
}
armarx::viz::data::RecordingBatch armarx::ArVizStorage::getRecordingBatch(std::string const& recordingID, Ice::Long batchIndex, const Ice::Current&)
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.h b/source/RobotAPI/components/ArViz/ArVizStorage.h
index c484b6d21c28f8ba25ebf7c8f1ff1e40f816c8d7..27bb6b1fcf130e23844c497a8a2a85cc1b392943 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.h
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.h
@@ -92,7 +92,7 @@ namespace armarx
const Ice::Current&) override;
std::string startRecording(std::string const& prefix, const Ice::Current&) override;
void stopRecording(const Ice::Current&) override;
- viz::data::RecordingSeq getAllRecordings(const Ice::Current&) override;
+ viz::data::RecordingsInfo getAllRecordings(const Ice::Current&) override;
viz::data::RecordingBatch getRecordingBatch(const std::string&, Ice::Long, const Ice::Current&) override;
private:
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index f479f4ef5456fc7a88d0f3a0fa87d2e77e9aab66..d97fea3b03d55ddb82a379628197a5f04ffc3d74 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -107,9 +107,9 @@ namespace armarx::viz
float angle = std::acos(naturalDir.dot(dir));
if (cross.squaredNorm() < 1.0e-12f)
{
- // Directions are almost colinear ==> Do no rotation
+ // Directions are almost colinear ==> Angle is either 0 or 180 deg
cross = Eigen::Vector3f::UnitX();
- angle = 0.0f;
+ // Keep angle
}
Eigen::Vector3f axis = cross.normalized();
Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 825dc08f01013c42091bb0c98790727509ce5820..11379504f65b8bdbb702227602df5dcd5fdfa533 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -340,7 +340,7 @@ namespace armarx::viz
/**
* @brief Set the file so it could be found using `armarx::ObjectFinder` (also on remote machine).
* @param objectID The object ID, see <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
- * @param objectsPackage The objects package ("ArmarXObjects" by default)
+ * @param objectsPackage The objects package ("PriorKnowledgeData" by default)
* @see <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
*/
Object& fileByObjectFinder(const armarx::ObjectID& objectID,
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index 2c93faf7fa4a464909bd6dea214c74022571474f..e52609fa085feac2dc4ef8fc93f68127aef8b040 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -275,13 +275,18 @@ namespace armarx
void fillExampleLayer(viz::Layer& layer, double timeInSeconds)
{
+ for (int i = 0; i < 6; ++i)
{
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +300.0f;
+ Eigen::Vector3f pos = Eigen::Vector3f(-200.0, 200.0, 300);
+ pos.x() += -300 * i;
+
+ Eigen::Vector3f normal = Eigen::Vector3f::Zero();
+ normal(i / 2) = (i % 2 == 0 ? 1.0 : -1.0);
- viz::ArrowCircle circle = viz::ArrowCircle("circle")
+ viz::ArrowCircle circle = viz::ArrowCircle("circle " + std::to_string(i))
.position(pos)
.radius(100.0f)
+ .normal(normal)
.width(10.0f)
.color(viz::Color::fromRGBA(255, 0, 255));
diff --git a/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1b2b3df5038078ad16a14af3cb977767129e096d
--- /dev/null
+++ b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp
@@ -0,0 +1,119 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 06.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h>
+#include <RobotAPI/libraries/aron_component_config/RemoteGui.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include "AronComponentConfigExample.h"
+
+namespace armarx
+{
+
+ void
+ AronComponentConfigExample::onInitComponent()
+ {
+
+ }
+
+ void
+ AronComponentConfigExample::onConnectComponent()
+ {
+ remote_gui_plugin_->createOrUpdateTab(aron_component_config_plugin_->buildRemoteGui("Config"),
+ [this](armarx::RemoteGui::TabProxy& prx)
+ {
+ prx.receiveUpdates();
+ std::lock_guard lock(write_mutex);
+ if (aron_component_config_plugin_->updateRemoteGui(prx))
+ {
+ remote_gui_plugin_->createOrUpdateTab("",
+ aron_component_config_plugin_
+ ->buildRemoteGui(
+ "Config"));
+ }
+ prx.sendUpdates();
+ });
+ periodicTask = new SimplePeriodicTask<>([&, this]
+ {
+ std::lock_guard lock(write_mutex);
+ ARMARX_TRACE;
+ auto& config =
+ aron_component_config_plugin_->config_.getWriteBuffer();
+// ARMARX_INFO << VAROUT(config.orientation) << VAROUT(config.pose) << VAROUT(config.position);
+ config.intMember--;
+ aron_component_config_plugin_->config_.commitWrite();
+ }, 1000);
+ periodicTask->start();
+ }
+
+ void
+ AronComponentConfigExample::onDisconnectComponent()
+ {
+ }
+
+ void
+ AronComponentConfigExample::onExitComponent()
+ {
+ }
+
+ armarx::PropertyDefinitionsPtr
+ AronComponentConfigExample::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new AronComponentConfigExamplePropertyDefinitions(getConfigIdentifier());
+ auto& config = aron_component_config_plugin_->config_.getWriteBuffer();
+ config.boolMember = true;
+ config.floatMember = 100.0f;
+ config.intMember = 1000;
+ config.stringMember = "initial";
+ config.longMember = 0;
+ config.subMember
+ .subsubMember
+ .intListMember
+ .insert(config.subMember.subsubMember.intListMember.end(), {1, 2, 3, 4, 5, 6});
+ config.subMember
+ .subsubMember
+ .stringListMember
+ .insert(config.subMember.subsubMember.stringListMember.end(), {"a", "b", "c", "d", "e"});
+ config.subMember.subsubMember.intDictMember.emplace("int1", 1);
+ config.subMember.subsubMember.intDictMember.emplace("int2", 2);
+ config.subMember.subsubMember.stringDictMember.emplace("string1", "blub");
+ config.subMember.subsubMember.stringDictMember.emplace("string2", "duh");
+ aron_component_config_plugin_->config_.commitWrite();
+ return defs;
+ }
+
+ std::string
+ AronComponentConfigExample::getDefaultName() const
+ {
+ return "ComponentConfigTest";
+ }
+
+ AronComponentConfigExample::AronComponentConfigExample()
+ {
+ addPlugin(remote_gui_plugin_, "");
+ addPlugin(aron_component_config_plugin_, "");
+ }
+
+ AronComponentConfigExamplePropertyDefinitions::AronComponentConfigExamplePropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(std::move(prefix))
+ {
+ }
+}
\ No newline at end of file
diff --git a/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.h b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.h
new file mode 100644
index 0000000000000000000000000000000000000000..e466b0a6ae20979766da06f839d31f45b76c3574
--- /dev/null
+++ b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.h
@@ -0,0 +1,69 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 06.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.aron.generated.h>
+#include <RobotAPI/libraries/aron_component_config/ComponentPlugin.h>
+
+namespace armarx
+{
+
+ class AronComponentConfigExamplePropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ AronComponentConfigExamplePropertyDefinitions(std::string prefix);
+ };
+
+ class AronComponentConfigExample :
+ virtual public armarx::Component
+ {
+ public:
+ std::string getDefaultName() const override;
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ AronComponentConfigExample();
+ private:
+ SimplePeriodicTask<>::pointer_type periodicTask;
+ std::experimental::observer_ptr<plugins::RemoteGuiComponentPlugin> remote_gui_plugin_{nullptr};
+ std::experimental::observer_ptr<plugins::AronComponentConfigPlugin<armarx::component_config::aron::TestConfig>>
+ aron_component_config_plugin_{nullptr};
+ std::mutex write_mutex;
+
+ };
+}
\ No newline at end of file
diff --git a/source/RobotAPI/components/AronComponentConfigExample/CMakeLists.txt b/source/RobotAPI/components/AronComponentConfigExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c219bee7094345ac6eb814b9568f711d37431f59
--- /dev/null
+++ b/source/RobotAPI/components/AronComponentConfigExample/CMakeLists.txt
@@ -0,0 +1,26 @@
+armarx_component_set_name("AronComponentConfigExample")
+
+set(COMPONENT_LIBS
+ ArmarXCore ArmarXCoreInterfaces
+ RobotAPICore RobotAPIInterfaces RobotAPIComponentPlugins # for ArViz and other plugins
+ aron_component_config
+ )
+
+set(SOURCES
+ AronComponentConfigExample.cpp
+ )
+set(HEADERS
+ AronComponentConfigExample.h
+ )
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ ${ARMARX_COMPONENT_NAME}
+ ARON_FILES
+ aron/ComponentConfig.xml
+)
+
+#generate the application
+armarx_generate_and_add_component_executable()
\ No newline at end of file
diff --git a/source/RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.xml b/source/RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.xml
new file mode 100644
index 0000000000000000000000000000000000000000..aabd27602d4f4ef4f0313dd00af36ccfa9c2a9dd
--- /dev/null
+++ b/source/RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.xml
@@ -0,0 +1,105 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <GenerateTypes>
+
+ <IntEnum name='armarx::component_config::aron::EnumTest'>
+ <EnumValue key="Foo" value="0"/>
+ <EnumValue key="Bar" value="1"/>
+ <EnumValue key="Baz" value="2"/>
+ <EnumValue key="Qux" value="3"/>
+ </IntEnum>
+
+ <Object name='armarx::component_config::aron::SubSubConfig'>
+ <ObjectChild key='stringMember'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='intMember'>
+ <int/>
+ </ObjectChild>
+ <ObjectChild key='floatMember'>
+ <float/>
+ </ObjectChild>
+ <ObjectChild key='longMember'>
+ <long/>
+ </ObjectChild>
+ <ObjectChild key='boolMember'>
+ <bool/>
+ </ObjectChild>
+ <ObjectChild key='enumMember'>
+ <armarx::component_config::aron::EnumTest/>
+ </ObjectChild>
+ <ObjectChild key='stringListMember'>
+ <List>
+ <string/>
+ </List>
+ </ObjectChild>
+ <ObjectChild key='intListMember'>
+ <List>
+ <int/>
+ </List>
+ </ObjectChild>
+ <ObjectChild key='stringDictMember'>
+ <Dict>
+ <string/>
+ </Dict>
+ </ObjectChild>
+ <ObjectChild key='intDictMember'>
+ <Dict>
+ <int/>
+ </Dict>
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::component_config::aron::SubConfig'>
+ <ObjectChild key='stringMember'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='intMember'>
+ <int/>
+ </ObjectChild>
+ <ObjectChild key='floatMember'>
+ <float/>
+ </ObjectChild>
+ <ObjectChild key='doubleMember'>
+ <double/>
+ </ObjectChild>
+ <ObjectChild key='boolMember'>
+ <bool/>
+ </ObjectChild>
+ <ObjectChild key='subsubMember'>
+ <armarx::component_config::aron::SubSubConfig />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::component_config::aron::TestConfig'>
+ <ObjectChild key='stringMember'>
+ <string/>
+ </ObjectChild>
+ <ObjectChild key='intMember'>
+ <int/>
+ </ObjectChild>
+ <ObjectChild key='floatMember'>
+ <float/>
+ </ObjectChild>
+ <ObjectChild key='longMember'>
+ <long/>
+ </ObjectChild>
+ <ObjectChild key='boolMember'>
+ <bool/>
+ </ObjectChild>
+ <ObjectChild key='subMember'>
+ <armarx::component_config::aron::SubConfig />
+ </ObjectChild>
+ <ObjectChild key='position'>
+ <framedposition />
+ </ObjectChild>
+ <ObjectChild key='pose'>
+ <framedpose />
+ </ObjectChild>
+ <ObjectChild key='orientation'>
+ <framedorientation />
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 8e172a97d95ed3ce515743a877c8c5bb3ed168d1..77943d887c6833b87a730b1ce45b926273795ad2 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -32,29 +32,25 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/CycleUtil.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::articulated_object
{
- ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
- articulatedObjectWriter(new ::armarx::armem::articulated_object::ArticulatedObjectWriter(memoryNameSystem())),
- articulatedObjectReader(new ::armarx::armem::articulated_object::ArticulatedObjectReader(memoryNameSystem()))
+ ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample()
{
+ addPlugin(articulatedObjectReaderPlugin);
+ addPlugin(articulatedObjectWriterPlugin);
}
- armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ ArticulatedObjectLocalizerExample::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs =
new ComponentPropertyDefinitions(getConfigIdentifier());
@@ -63,60 +59,74 @@ namespace armarx::articulated_object
defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
- // Reader will override some properties of writer.
- articulatedObjectWriter->registerPropertyDefinitions(defs);
- articulatedObjectReader->registerPropertyDefinitions(defs);
+ defs->optional(p.obj.dataset, "p.obj.dataset", "");
+ defs->optional(p.obj.className, "p.obj.class", "");
+
return defs;
}
- std::string ArticulatedObjectLocalizerExample::getDefaultName() const
+ std::string
+ ArticulatedObjectLocalizerExample::getDefaultName() const
{
return "ArticulatedObjectLocalizerExample";
}
- void ArticulatedObjectLocalizerExample::onInitComponent()
+ void
+ ArticulatedObjectLocalizerExample::onInitComponent()
{
+ auto& articulatedObjectReader = articulatedObjectReaderPlugin->get();
+ auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
+
// Reader overwrote property registered property of articulatedObjectWriter.
- articulatedObjectWriter->setProviderName(articulatedObjectReader->getProviderName());
+ articulatedObjectWriter.setProviderName(articulatedObjectReader.getProviderName());
}
- void ArticulatedObjectLocalizerExample::onConnectComponent()
+ void
+ ArticulatedObjectLocalizerExample::onConnectComponent()
{
- articulatedObjectWriter->connect();
- articulatedObjectReader->connect();
-
ARMARX_IMPORTANT << "Running example.";
start = armem::Time::Now();
task = new PeriodicTask<ArticulatedObjectLocalizerExample>(
- this, &ArticulatedObjectLocalizerExample::run,
+ this,
+ &ArticulatedObjectLocalizerExample::run,
static_cast<int>(1000.f / p.updateFrequency));
task->start();
}
- void ArticulatedObjectLocalizerExample::onDisconnectComponent()
+ void
+ ArticulatedObjectLocalizerExample::onDisconnectComponent()
{
task->stop();
}
- void ArticulatedObjectLocalizerExample::onExitComponent()
+ void
+ ArticulatedObjectLocalizerExample::onExitComponent()
{
}
- VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
+ VirtualRobot::RobotPtr
+ ArticulatedObjectLocalizerExample::createArticulatedObject()
{
- const std::string dishwasherName = "Kitchen/mobile-dishwasher";
+ auto& articulatedObjectReader = articulatedObjectReaderPlugin->get();
+
+ const std::string dishwasherName = p.obj.dataset + "/" + p.obj.className;
- const auto descriptions = articulatedObjectReader->queryDescriptions(armem::Time::Now());
+ const auto descriptions = articulatedObjectReader.queryDescriptions(armem::Time::Now());
ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+ for(const auto& description: descriptions)
+ {
+ ARMARX_INFO << "- " << description.name;
+ }
+
const auto it = std::find_if(
- descriptions.begin(),
- descriptions.end(),
- [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
- { return desc.name == dishwasherName; });
+ descriptions.begin(),
+ descriptions.end(),
+ [&](const armem::articulated_object::ArticulatedObjectDescription& desc) -> bool
+ { return desc.name == dishwasherName; });
if (it == descriptions.end())
{
@@ -133,21 +143,21 @@ namespace armarx::articulated_object
return nullptr;
}
- obj->setName("MobileDishwasher0");
- obj->setType(it->name);
+ obj->setName("0"); // aka instance name
+ obj->setType(it->name); // aka dataset/class name
return obj;
}
-
- void ArticulatedObjectLocalizerExample::run()
+ void
+ ArticulatedObjectLocalizerExample::run()
{
- if (dishwasher == nullptr)
+ if (articulatedObject == nullptr)
{
- dishwasher = createDishwasher();
+ articulatedObject = createArticulatedObject();
- if (dishwasher == nullptr) // still
+ if (articulatedObject == nullptr) // still
{
return;
}
@@ -156,21 +166,24 @@ namespace armarx::articulated_object
ARMARX_DEBUG << "Reporting articulated objects";
const armem::Time now = armem::Time::Now();
- const float t = float((now - start).toSecondsDouble());
+ 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 std::map<std::string, float> jointValues
+ auto jointValues = articulatedObject->getJointValues();
+
+ for (auto& [name, jointValue] : jointValues)
{
- {"dishwasher_door_joint", M_PIf32 / 2 * k},
- {"drawer_joint", 350 * k}
- };
+ const auto node = articulatedObject->getRobotNode(name);
+ jointValue = node->unscaleJointValue(k, 0, 1);
+ }
- dishwasher->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
- dishwasher->setJointValues(jointValues);
+ articulatedObject->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
+ articulatedObject->setJointValues(jointValues);
- articulatedObjectWriter->storeArticulatedObject(dishwasher, now);
+ auto& articulatedObjectWriter = articulatedObjectWriterPlugin->get();
+ articulatedObjectWriter.storeArticulatedObject(articulatedObject, now);
}
} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index d401eed5e741d3d797dfaa8a47f347f4616a53db..384068048dd1ab8d1457065d15873de08ff7b82b 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -3,17 +3,19 @@
#include <VirtualRobot/VirtualRobot.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h>
@@ -53,8 +55,8 @@ namespace armarx::articulated_object
void run();
private:
- VirtualRobot::RobotPtr createDishwasher();
- std::shared_ptr<VirtualRobot::Robot> dishwasher;
+ VirtualRobot::RobotPtr createArticulatedObject();
+ std::shared_ptr<VirtualRobot::Robot> articulatedObject;
/// Reference timestamp for object movement
armem::Time start;
@@ -63,13 +65,23 @@ namespace armarx::articulated_object
armarx::DebugObserverInterfacePrx debugObserver;
- std::unique_ptr<::armarx::armem::articulated_object::ArticulatedObjectWriter> articulatedObjectWriter;
- std::unique_ptr<::armarx::armem::articulated_object::ArticulatedObjectReader>
- articulatedObjectReader;
+ armem::client::plugins::ReaderWriterPlugin<
+ ::armarx::armem::articulated_object::ArticulatedObjectWriter>*
+ articulatedObjectWriterPlugin = nullptr;
+
+ armem::client::plugins::ReaderWriterPlugin<
+ ::armarx::armem::articulated_object::ArticulatedObjectReader>*
+ articulatedObjectReaderPlugin = nullptr;
struct Properties
{
float updateFrequency{25.F};
+
+ struct
+ {
+ std::string dataset = "Kitchen";
+ std::string className = "mobile-dishwasher";
+ } obj;
} p;
};
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 7debb7e6f4aa280c4e28cc1a40bbc59af0bf25c3..11908aeda6ade0ff4b46f63d0590c88d007e51c9 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -3,6 +3,7 @@ add_subdirectory(units)
add_subdirectory(armem)
add_subdirectory(skills)
+add_subdirectory(AronComponentConfigExample)
add_subdirectory(ArticulatedObjectLocalizerExample)
add_subdirectory(ArViz)
@@ -32,3 +33,5 @@ add_subdirectory(RobotToArViz)
add_subdirectory(StatechartExecutorExample)
add_subdirectory(TopicTimingTest)
add_subdirectory(ViewSelection)
+
+add_subdirectory(ObjectMemoryEditor)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 1987708750187cc7a7e212b6c2c105157707b6ff..5cffdc6d755be9696f23479a64b4077380228046 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -543,15 +543,7 @@ namespace armarx
}
SoSeparator* newS = new SoSeparator;
- Eigen::Matrix4f lp1 = Eigen::Matrix4f::Identity();
- lp1(0, 3) = d.p1.x();
- lp1(1, 3) = d.p1.y();
- lp1(2, 3) = d.p1.z();
- Eigen::Matrix4f lp2 = Eigen::Matrix4f::Identity();
- lp2(0, 3) = d.p2.x();
- lp2(1, 3) = d.p2.y();
- lp2(2, 3) = d.p2.z();
- newS->addChild(CoinVisualizationFactory::createCoinLine(lp1, lp2, d.scale, d.color.r, d.color.g, d.color.b));
+ newS->addChild(CoinVisualizationFactory::createCoinLine(d.p1, d.p2, d.scale, d.color));
layer.addedLineVisualizations[d.name] = newS;
layer.mainNode->addChild(newS);
ARMARX_DEBUG << "drawLine2" << flush;
@@ -872,10 +864,7 @@ namespace armarx
auto node = CoinVisualizationFactory().createCircleArrow(d.radius,
d.width,
d.circleCompletion,
- d.color.r,
- d.color.g,
- d.color.b,
- d.color.transparency,
+ d.color,
16, 30);
SoNode* circle = dynamic_cast<CoinVisualizationNode&>(*node).getCoinVisualization();
circle->setName(SELECTION_NAME(d.layerName, d.name));
@@ -2946,5 +2935,3 @@ namespace armarx
removeCircleVisu(DEBUG_LAYER_NAME, circleName, c);
}
}
-
-
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
index 9b0bacd5092012472d3f975e1c6d40357ba6594c..da5945c44c7e289081df35385ea7e75184d69c90 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
@@ -31,6 +31,7 @@
#include <RobotAPI/interface/units/HandUnitInterface.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
namespace armarx
{
@@ -129,4 +130,3 @@ namespace armarx
};
}
-
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/CMakeLists.txt b/source/RobotAPI/components/ObjectMemoryEditor/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3c9023550e1d93a2d4bd1b8a0a61b8a5fdc7bd5d
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/CMakeLists.txt
@@ -0,0 +1,29 @@
+armarx_component_set_name("ObjectMemoryEditor")
+
+
+set(COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCoreComponentPlugins
+ # RobotAPI
+ RobotAPI::ArmarXObjects
+ RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+)
+
+set(SOURCES
+ ObjectMemoryEditor.cpp
+ InteractionObserver.cpp
+ Editor.cpp
+)
+set(HEADERS
+ ObjectMemoryEditor.h
+ InteractionObserver.h
+ Editor.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+#generate the application
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/Editor.cpp b/source/RobotAPI/components/ObjectMemoryEditor/Editor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..74bdd96793868911322b960af2973a0576809e93
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/Editor.cpp
@@ -0,0 +1,547 @@
+#include "Editor.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include <utility>
+
+namespace armarx
+{
+ Editor::Editor(viz::Client& client,
+ Properties properties,
+ std::function<void(objpose::ProvidedObjectPoseSeq &)> pushToMemory,
+ std::function<objpose::ObjectPoseSeq(void)> pullFromMemory)
+ : properties(std::move(properties))
+ , client(client)
+ , pushToMemory(std::move(pushToMemory))
+ , pullFromMemory(std::move(pullFromMemory))
+ , isCommitRequired(false)
+ , isUpdateRequired(true)
+ , isMemoryVizRequired(true)
+ , isMetaVizRequired(true)
+ {
+ placeholderOptions.emplace_back("Remove");
+ placeholderOptions.emplace_back(lineString);
+
+ for (auto const& availableObject : this->properties.availableObjects)
+ {
+ placeholderOptions.push_back(armarx::fromIce(availableObject).str());
+ }
+ }
+
+ void Editor::step()
+ {
+ viz::StagedCommit stage = client.stage();
+
+ if (isUpdateRequired)
+ {
+ storedPoses = update();
+
+ isUpdateRequired = false;
+ }
+
+ if (isResetRequired)
+ {
+ reset();
+
+ isResetRequired = false;
+ }
+
+ if (isMemoryVizRequired)
+ {
+ visualizeMemory();
+ stage.add(memoryLayer);
+
+ isMemoryVizRequired = false;
+ }
+
+ if (isMetaVizRequired)
+ {
+ visualizeMeta();
+ stage.add(metaLayer);
+
+ isMetaVizRequired = false;
+ }
+
+ if (isCommitRequired)
+ {
+ commit();
+
+ isCommitRequired = false;
+ }
+
+ observer.requestInteractions(stage);
+
+ viz::CommitResult result = client.commit(stage);
+
+ observer.process(result.interactions());
+ }
+
+ objpose::ObjectPoseSeq
+ Editor::update()
+ {
+ objpose::ObjectPoseSeq newRequestedPoses = pullFromMemory();
+
+ isMemoryVizRequired = true;
+
+ return newRequestedPoses;
+ }
+
+ void Editor::reset()
+ {
+ changes.clear();
+
+ isMemoryVizRequired = true;
+ }
+
+ void Editor::commit()
+ {
+ changes.moveNewObjectsTo(storedPoses);
+
+ objpose::ProvidedObjectPoseSeq providingPoses;
+ objpose::ObjectPoseSeq remainingPoses;
+
+ remainingPoses.reserve(storedPoses.size());
+ for (objpose::ObjectPose & current : storedPoses)
+ {
+ bool isChanged = changes.applyTo(current);
+
+ if (isChanged)
+ {
+ providingPoses.push_back(current.toProvidedObjectPoseGlobal());
+ objpose::ProvidedObjectPose& providing = providingPoses.back();
+
+ providing.providerName = properties.providerName;
+ providing.timestamp = DateTime::Now();
+ }
+
+ if (current.confidence > properties.confidenceThreshold)
+ {
+ remainingPoses.push_back(current);
+ }
+ }
+
+ pushToMemory(providingPoses);
+
+ changes.clear();
+ storedPoses = remainingPoses;
+
+ isMemoryVizRequired = true;
+ }
+
+ void Editor::visualizeMemory()
+ {
+ observer.clearObservedLayer(memoryLayer);
+
+ for (objpose::ObjectPose & objectPose: storedPoses)
+ {
+ if (objectPose.confidence > properties.confidenceThreshold)
+ {
+ visualizeObject(objectPose);
+ }
+ }
+
+ changes.visualizeNewObjects();
+ }
+
+ void Editor::visualizeObject(objpose::ObjectPose& objectPose)
+ {
+ VisualizationDescription description = changes.buildVisualizationDescription(objectPose);
+
+ viz::InteractionDescription interaction = viz::interaction().selection();
+
+ if (description.allowTransforming)
+ {
+ interaction.transform().hideDuringTransform();
+ }
+
+ if (not description.options.empty())
+ {
+ interaction.contextMenu(description.options);
+ }
+
+ viz::Object object =
+ viz::Object(objectPose.objectID.str())
+ .pose(description.transform * objectPose.objectPoseGlobal)
+ .scale(properties.objectScaling)
+ .fileByObjectFinder(objectPose.objectID)
+ .enable(interaction);
+
+ if (description.alpha.has_value())
+ {
+ object.alpha(description.alpha.value());
+ }
+
+ if (description.color.has_value())
+ {
+ object.overrideColor(description.color.value());
+ }
+
+ observer.addObserved(memoryLayer, object)
+ .onContextMenu(description.cloneIndex, [this, &objectPose]
+ {
+ changes.cloneObject(objectPose);
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(description.deleteIndex, [this, &objectPose]
+ {
+ changes.deleteObject(objectPose);
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(description.resetIndex, [this, &objectPose]
+ {
+ changes.resetObject(objectPose);
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(description.prototypeIndex, [this, &objectPose]
+ {
+ const float defaultExtents = 100;
+ simox::OrientedBoxf box(objectPose.objectPoseGlobal, Eigen::Vector3f(defaultExtents, defaultExtents, defaultExtents));
+
+ box = objectPose.oobbGlobal().value_or(box);
+ box = box.transformed(changes.getTransform(objectPose));
+
+ placeholders.addPlaceholder(box);
+ isMetaVizRequired = true;
+ })
+ .onContextMenu(description.commitIndex, [this]
+ {
+ isCommitRequired = true;
+ })
+ .onContextMenu(description.updateIndex, [this]
+ {
+ isUpdateRequired = true;
+ })
+ .onContextMenu(description.resetAllIndex, [this]
+ {
+ isResetRequired = true;
+ })
+ .onTransformEnd([this, &objectPose](const Eigen::Matrix4f& transform)
+ {
+ changes.moveObject(objectPose, transform);
+ isMemoryVizRequired = true;
+ });
+ }
+
+ void Editor::visualizeMeta()
+ {
+ observer.clearObservedLayer(metaLayer);
+
+ placeholders.visualizePlaceholders();
+ }
+
+ void Editor::visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id)
+ {
+ viz::InteractionDescription interaction = viz::interaction()
+ .selection().transform().hideDuringTransform().contextMenu(placeholderOptions);
+
+ viz::Box box = viz::Box("placeholder_" + std::to_string(id))
+ .set(placeholder.box.transformed(placeholder.transform))
+ .color(simox::Color::yellow(255, 128))
+ .enable(interaction);
+
+ auto& observation = observer.addObserved(metaLayer, box)
+ .onContextMenu(0, [this, id]()
+ {
+ placeholders.removePlaceholder(id);
+ isMetaVizRequired = true;
+ })
+ .onTransformEnd([this, id](Eigen::Matrix4f const& transform)
+ {
+ placeholders.movePlaceholder(id, transform);
+ isMetaVizRequired = true;
+ });
+
+ for (size_t index = 2; index < placeholderOptions.size(); index++)
+ {
+ std::string const& object = placeholderOptions[index];
+
+ observation.onContextMenu(index, [this, id, &object]
+ {
+ placeholders.specifyObject(id, object, changes);
+
+ isMetaVizRequired = true;
+ isMemoryVizRequired = true;
+ });
+ }
+ }
+
+ void ChangeState::clear()
+ {
+ changed.clear();
+ newPoses.clear();
+ }
+
+ void ChangeState::moveNewObjectsTo(objpose::ObjectPoseSeq& seq)
+ {
+ seq.insert(seq.begin(), newPoses.begin(), newPoses.end());
+ newPoses.clear();
+ }
+
+ bool ChangeState::applyTo(objpose::ObjectPose& pose)
+ {
+ auto iterator = changed.find(pose.objectID.str());
+ bool isChanged = iterator != changed.end();
+
+ if (isChanged)
+ {
+ auto& [name, change] = *iterator;
+
+ if (change.kind == DELETE)
+ {
+ pose.confidence = 0;
+ }
+
+ pose.objectPoseGlobal = change.transform * pose.objectPoseGlobal;
+ }
+
+ return isChanged;
+ }
+
+ void ChangeState::visualizeNewObjects()
+ {
+ for (objpose::ObjectPose & objectPose: newPoses)
+ {
+ editor->visualizeObject(objectPose);
+ }
+ }
+
+ VisualizationDescription ChangeState::buildVisualizationDescription(objpose::ObjectPose& object)
+ {
+ VisualizationDescription description;
+
+ auto iterator = changed.find(object.objectID.str());
+ bool isChanged = iterator != changed.end();
+
+ ChangeKind kind = MOVE;
+ if (isChanged)
+ {
+ auto& [name, change] = *iterator;
+ kind = change.kind;
+ }
+
+ description.allowTransforming = kind != DELETE;
+
+ size_t currentIndex = 0;
+
+ if (kind == MOVE)
+ {
+ description.options.emplace_back("Clone");
+ description.cloneIndex = currentIndex++;
+
+ description.options.emplace_back("Delete");
+ description.deleteIndex = currentIndex++;
+ }
+
+ if (isChanged)
+ {
+ auto& [name, change] = *iterator;
+
+ description.options.emplace_back("Reset");
+ description.resetIndex = currentIndex++;
+
+ description.transform = change.transform;
+
+ const float alpha = 0.5;
+
+ switch (kind)
+ {
+ case MOVE:
+ description.alpha = alpha;
+ break;
+
+ case CREATE:
+ description.color = simox::Color(0.0F, 1.0F, 0.0F, alpha);
+ break;
+
+ case DELETE:
+ description.color = simox::Color(1.0F, 0.0F, 0.0F, alpha);
+ break;
+ }
+ }
+
+ description.options.emplace_back("Create Placeholder");
+ description.prototypeIndex = currentIndex++;
+
+ description.options.emplace_back(Editor::lineString);
+ currentIndex++;
+
+ description.options.emplace_back("Commit All Changes");
+ description.commitIndex = currentIndex++;
+
+ description.options.emplace_back("Update Unchanged");
+ description.updateIndex = currentIndex++;
+
+ description.options.emplace_back("Reset All");
+ description.resetAllIndex = currentIndex; // ++
+
+ return description;
+ }
+
+ void ChangeState::cloneObject(objpose::ObjectPose const& object)
+ {
+ std::string suffix = std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
+ std::chrono::system_clock::now().time_since_epoch()).count());
+
+ objpose::ObjectPose& newPose = newPoses.emplace_back(object);
+ newPose.objectID = object.objectID.withInstanceName(object.objectID.instanceName() + suffix);
+
+ const float minOffset = 100;
+ float offset = minOffset;
+ if (object.localOOBB.has_value())
+ {
+ Eigen::Vector3f size = object.localOOBB.value().corner_max() - object.localOOBB.value().corner_min();
+ float objectOffset = size.maxCoeff() / 2;
+
+ offset = std::max(minOffset, objectOffset);
+ }
+
+ Change& clonedChange = changed[newPose.objectID.str()];
+ clonedChange.kind = CREATE;
+ // Heuristic: Don't shift in Z direction to ease creation in a horizontal plane.
+ clonedChange.transform = Eigen::Affine3f(Eigen::Translation3f(offset, offset, 0)).matrix();
+ clonedChange.iterator = std::prev(newPoses.end());
+
+ auto iterator = changed.find(object.objectID.str());
+ if (iterator != changed.end())
+ {
+ auto& [name, originalChange] = *iterator;
+ clonedChange.transform *= originalChange.transform;
+ }
+ }
+
+ void ChangeState::createObject(const std::string &objectID, const Eigen::Matrix4f &pose)
+ {
+ std::string suffix = std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
+ std::chrono::system_clock::now().time_since_epoch()).count());
+
+ objpose::ObjectPose& newPose = newPoses.emplace_back();
+
+ armarx::ObjectID id(objectID);
+ newPose.objectID = id.withInstanceName(id.instanceName() + suffix);
+
+ newPose.providerName = editor->properties.providerName;
+ newPose.objectType = objpose::ObjectType::KnownObject;
+ newPose.isStatic = true;
+
+ newPose.objectPoseGlobal = pose;
+ newPose.confidence = 1;
+ newPose.timestamp = DateTime::Now();
+
+ std::optional<armarx::ObjectInfo> info = editor->objectFinder.findObject(id);
+ if (info.has_value())
+ {
+ newPose.localOOBB = info->loadOOBB();
+
+ if (newPose.localOOBB.has_value())
+ {
+ newPose.objectPoseGlobal = pose * newPose.localOOBB->transformation_centered().inverse();
+ }
+ }
+
+ Change& createdChange = changed[newPose.objectID.str()];
+ createdChange.kind = CREATE;
+ createdChange.transform = Eigen::Matrix4f::Identity();
+ createdChange.iterator = std::prev(newPoses.end());
+ }
+
+ void ChangeState::deleteObject(const objpose::ObjectPose& object)
+ {
+ changed[object.objectID.str()].kind = DELETE;
+ }
+
+ void ChangeState::resetObject(const objpose::ObjectPose& object)
+ {
+ auto iterator = changed.find(object.objectID.str());
+ if (iterator != changed.end())
+ {
+ auto& [name, change] = *iterator;
+
+ if (change.kind == CREATE)
+ {
+ newPoses.erase(change.iterator);
+ }
+
+ changed.erase(iterator);
+ }
+ }
+
+ void ChangeState::moveObject(const objpose::ObjectPose& object, const Eigen::Matrix4f& transform)
+ {
+ Change& change = changed[object.objectID.str()];
+ change.transform = transform * change.transform;
+ }
+
+ Eigen::Matrix4f ChangeState::getTransform(const objpose::ObjectPose &object)
+ {
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+
+ auto iterator = changed.find(object.objectID.str());
+ if (iterator != changed.end())
+ {
+ auto& [name, change] = *iterator;
+ transform = change.transform;
+ }
+
+ return transform;
+ }
+
+ void PlaceholderState::addPlaceholder(simox::OrientedBoxf box)
+ {
+ size_t id = getID();
+
+ auto& entry = placeholders[id];
+ entry.placeholder = { .box = std::move(box), .transform = Eigen::Matrix4f::Identity() };
+ entry.isActive = true;
+ }
+
+ void PlaceholderState::visualizePlaceholders()
+ {
+ for (size_t id = 0; id < placeholders.size(); id++)
+ {
+ auto& entry = placeholders[id];
+
+ if (entry.isActive)
+ {
+ editor->visualizePlaceholder(entry.placeholder, id);
+ }
+ }
+ }
+
+ void PlaceholderState::movePlaceholder(size_t id, Eigen::Matrix4f const& transform)
+ {
+ auto& placeholder = placeholders[id].placeholder;
+ placeholder.transform = transform * placeholder.transform;
+ }
+
+ void PlaceholderState::removePlaceholder(size_t id)
+ {
+ placeholders[id].isActive = false;
+
+ unusedIDs.push(id);
+ }
+
+ size_t PlaceholderState::getID()
+ {
+ if (unusedIDs.empty())
+ {
+ size_t id = placeholders.size();
+ placeholders.push_back({Placeholder(), false});
+ return id;
+ }
+
+ size_t id = unusedIDs.top();
+ unusedIDs.pop();
+ return id;
+ }
+
+ void PlaceholderState::specifyObject(size_t id, std::string const& objectID, ChangeState& changeState)
+ {
+ auto& placeholder = placeholders[id].placeholder;
+
+ Eigen::Matrix4f pose = placeholder.box.transformed(placeholder.transform).transformation_centered();
+ changeState.createObject(objectID, pose);
+
+ removePlaceholder(id);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/Editor.h b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
new file mode 100644
index 0000000000000000000000000000000000000000..f1a6ebc2a168de7938e552b44f1d2144a915dd35
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
@@ -0,0 +1,181 @@
+#pragma once
+
+#include <list>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h>
+
+namespace armarx
+{
+
+ class Editor;
+
+
+ struct VisualizationDescription
+ {
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ bool allowTransforming = true;
+
+ std::vector<std::string> options {};
+ std::optional<float> alpha {};
+ std::optional<simox::Color> color {};
+
+ // Context menu indices.
+ size_t cloneIndex = std::numeric_limits<size_t>::max();
+ size_t deleteIndex = std::numeric_limits<size_t>::max();
+ size_t resetIndex = std::numeric_limits<size_t>::max();
+ size_t prototypeIndex = std::numeric_limits<size_t>::max();
+ size_t commitIndex = std::numeric_limits<size_t>::max();
+ size_t updateIndex = std::numeric_limits<size_t>::max();
+ size_t resetAllIndex = std::numeric_limits<size_t>::max();
+ };
+
+
+
+ class ChangeState
+ {
+ public:
+ explicit ChangeState(Editor* editor) : editor(editor) {} ;
+
+ void clear();
+ void moveNewObjectsTo(objpose::ObjectPoseSeq& seq);
+ bool applyTo(objpose::ObjectPose& pose);
+ void visualizeNewObjects();
+ VisualizationDescription buildVisualizationDescription(objpose::ObjectPose& object);
+ Eigen::Matrix4f getTransform(objpose::ObjectPose const& object);
+
+ void cloneObject(objpose::ObjectPose const& object);
+ void createObject(std::string const& objectID, Eigen::Matrix4f const& pose);
+ void deleteObject(objpose::ObjectPose const& object);
+ void resetObject(objpose::ObjectPose const& object);
+ void moveObject(objpose::ObjectPose const& object, Eigen::Matrix4f const& transform);
+
+ private:
+ enum ChangeKind
+ {
+ MOVE,
+ CREATE,
+ DELETE
+ };
+
+ using ObjectPoseList = std::list<objpose::ObjectPose>;
+
+ struct Change
+ {
+ ChangeKind kind = MOVE;
+ Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
+ ObjectPoseList::iterator iterator {};
+ };
+
+ Editor* editor;
+
+ ObjectPoseList newPoses;
+ std::map<std::string, Change> changed;
+ };
+
+
+
+ class PlaceholderState
+ {
+ public:
+ explicit PlaceholderState(Editor* editor) : editor(editor) {} ;
+
+ struct Placeholder
+ {
+ simox::OrientedBoxf box;
+ Eigen::Matrix4f transform;
+ };
+
+ void addPlaceholder(simox::OrientedBoxf box);
+ void visualizePlaceholders();
+ void movePlaceholder(size_t id, Eigen::Matrix4f const& transform);
+ void removePlaceholder(size_t id);
+ void specifyObject(size_t id, std::string const& objectID, ChangeState& changeState);
+
+ private:
+ size_t getID();
+
+ Editor* editor;
+
+ std::priority_queue<size_t, std::vector<size_t>, std::greater<>> unusedIDs;
+ struct PlaceholderEntry
+ {
+ Placeholder placeholder;
+ bool isActive;
+ };
+ std::vector<PlaceholderEntry> placeholders;
+ };
+
+
+
+ class Editor
+ {
+ public:
+ struct Properties
+ {
+ std::string providerName;
+
+ float const& objectScaling;
+ float const& confidenceThreshold;
+
+ std::vector<data::ObjectID> const& availableObjects;
+ };
+
+
+ public:
+
+ explicit Editor(viz::Client& client,
+ Properties properties,
+ std::function<void(objpose::ProvidedObjectPoseSeq&)> pushToMemory,
+ std::function<objpose::ObjectPoseSeq(void)> pullFromMemory);
+ void step();
+
+ private:
+
+ void visualizeMemory();
+ void visualizeMeta();
+
+ void commit();
+ objpose::ObjectPoseSeq update();
+ void reset();
+
+ void visualizeObject(objpose::ObjectPose &objectPose);
+ void visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id);
+
+ private:
+ static constexpr const char* memoryLayerName = "Memory";
+ static constexpr const char* metaLayerName = "Meta";
+ static constexpr const char* lineString = "---------------------------";
+
+ std::vector<std::string> placeholderOptions;
+
+ Properties properties;
+ ObjectFinder objectFinder;
+
+ viz::Client& client;
+ const std::function<void(objpose::ProvidedObjectPoseSeq&)> pushToMemory;
+ const std::function<objpose::ObjectPoseSeq(void)> pullFromMemory;
+
+ viz::Layer memoryLayer {client.layer(memoryLayerName)};
+ viz::Layer metaLayer {client.layer(metaLayerName)};
+
+ InteractionObserver observer;
+
+ objpose::ObjectPoseSeq storedPoses;
+
+ ChangeState changes{this};
+ PlaceholderState placeholders{this};
+
+ bool isCommitRequired;
+ bool isUpdateRequired;
+ bool isResetRequired;
+ bool isMemoryVizRequired;
+ bool isMetaVizRequired;
+
+ friend ChangeState;
+ friend PlaceholderState;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.cpp b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c226e57a72b071eaf93c45ea52e5b1906789a54c
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.cpp
@@ -0,0 +1,74 @@
+#include "InteractionObserver.h"
+
+#include <utility>
+
+armarx::InteractionObserver::Observation &
+armarx::InteractionObserver::Observation::onContextMenu(size_t index, std::function<void()> action)
+{
+ contextMenuActions[index] = std::move(action);
+ return *this;
+}
+
+armarx::InteractionObserver::Observation &
+armarx::InteractionObserver::Observation::onTransformEnd(std::function<void(Eigen::Matrix4f const&)> action)
+{
+ transformEndAction = std::make_optional(std::move(action));
+ return *this;
+}
+
+void armarx::InteractionObserver::Observation::process(viz::InteractionFeedback const& interaction)
+{
+ if (interaction.type() == viz::InteractionFeedbackType::ContextMenuChosen)
+ {
+ auto iterator = contextMenuActions.find(interaction.chosenContextMenuEntry());
+
+ if (iterator != contextMenuActions.end())
+ {
+ auto [index, action] = *iterator;
+ action();
+ }
+ }
+
+ if (interaction.type() == viz::InteractionFeedbackType::Transform && interaction.isTransformEnd())
+ {
+ if (transformEndAction.has_value())
+ {
+ transformEndAction.value()(interaction.transformation());
+ }
+ }
+}
+
+void armarx::InteractionObserver::clearObservedLayer(armarx::viz::Layer & layer)
+{
+ layer.clear();
+ observedLayers.erase(layer.data_.name);
+}
+
+void armarx::InteractionObserver::requestInteractions(armarx::viz::StagedCommit& stage)
+{
+ for (auto& [name, observedLayer] : observedLayers)
+ {
+ stage.requestInteraction(observedLayer.layer);
+ }
+}
+
+void armarx::InteractionObserver::process(viz::InteractionFeedbackRange const& interactions)
+{
+ for (viz::InteractionFeedback const& interaction: interactions)
+ {
+ auto layerIterator = observedLayers.find(interaction.layer());
+
+ if (layerIterator != observedLayers.end())
+ {
+ auto& [name, layer] = *layerIterator;
+
+ auto observationIterator = layer.observations.find(interaction.element());
+
+ if (observationIterator != layer.observations.end())
+ {
+ auto& [element, observation] = *observationIterator;
+ observation.process(interaction);
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..b26ed2aca18754f22f5eead0ad345981b5ec0ae4
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h
@@ -0,0 +1,55 @@
+#pragma once
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+namespace armarx
+{
+ class InteractionObserver
+ {
+ public:
+ class Observation
+ {
+ public:
+ Observation& onContextMenu(size_t index, std::function<void()> action);
+ Observation& onTransformEnd(std::function<void(Eigen::Matrix4f const&)> action);
+
+ void process(viz::InteractionFeedback const& interaction);
+
+ private:
+ std::map<size_t, std::function<void()>> contextMenuActions;
+ std::optional<std::function<void(Eigen::Matrix4f const&)>> transformEndAction;
+ };
+
+ template <typename ElementT>
+ Observation& addObserved(viz::Layer & layer, ElementT const& element)
+ {
+ layer.add(element);
+
+ auto iterator = observedLayers.find(layer.data_.name);
+
+ if (iterator == observedLayers.end())
+ {
+ std::tie(iterator, std::ignore) = observedLayers.emplace(layer.data_.name, layer);
+ }
+
+ auto& [name, observedLayer] = *iterator;
+ return observedLayer.observations[element.data_->id];
+ }
+
+ void clearObservedLayer(viz::Layer & layer);
+
+ void requestInteractions(viz::StagedCommit & stage);
+ void process(viz::InteractionFeedbackRange const& interactions);
+
+ private:
+ struct ObservedLayer
+ {
+ explicit ObservedLayer(viz::Layer & layer) : layer(layer) {}
+
+ std::reference_wrapper<viz::Layer> layer;
+ std::map<std::string, Observation> observations;
+ };
+
+ std::map<std::string, ObservedLayer> observedLayers;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.cpp b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e723f5e467d7d6588084ee5366753d64ca112a03
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.cpp
@@ -0,0 +1,115 @@
+#include "ObjectMemoryEditor.h"
+
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
+
+#include "Editor.h"
+
+namespace armarx
+{
+ armarx::PropertyDefinitionsPtr ObjectMemoryEditor::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ defs->optional(this->objectScaling, "Editor.ObjectScaling",
+ "Scaling factor that is applied to all intractable objects.");
+
+ defs->optional(this->confidenceThreshold, "Editor.ConfidenceThreshold",
+ "Only objects with a confidence greater than this value are shown.");
+
+ return defs;
+ }
+
+ std::string ObjectMemoryEditor::getDefaultName() const
+ {
+ return "InteractiveMemoryEditor";
+ }
+
+ void ObjectMemoryEditor::onInitComponent()
+ {
+ {
+ providerInfo.objectType = objpose::ObjectType::KnownObject;
+
+ for (const auto& dataset : objectFinder.getDatasets())
+ {
+ std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset(dataset);
+
+ for (const auto& obj: objects)
+ {
+ providerInfo.supportedObjects.push_back(armarx::toIce(obj.id()));
+ }
+ }
+ }
+ }
+
+ void ObjectMemoryEditor::onConnectComponent()
+ {
+ setDebugObserverBatchModeEnabled(true);
+
+ objectVizTask = new SimpleRunningTask<>([this]()
+ {
+ this->run();
+ });
+ objectVizTask->start();
+ }
+
+ void ObjectMemoryEditor::onDisconnectComponent()
+ {
+ }
+
+ void ObjectMemoryEditor::onExitComponent()
+ {
+ }
+
+
+ void ObjectMemoryEditor::run()
+ {
+ objpose::ObjectPoseClient client = getClient();
+
+ Editor::Properties properties =
+ {
+ .providerName = getName(),
+ .objectScaling = objectScaling,
+ .confidenceThreshold = confidenceThreshold,
+ .availableObjects = providerInfo.supportedObjects,
+ };
+
+ Editor editor(arviz, properties,
+ [this](objpose::ProvidedObjectPoseSeq &poses)
+ {
+ objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(poses));
+ },
+ [client]() -> objpose::ObjectPoseSeq
+ {
+ return client.fetchObjectPoses();
+ });
+
+ Metronome metronome(Frequency::Hertz(20));
+ while (objectVizTask and not objectVizTask->isStopped())
+ {
+ editor.step();
+
+ metronome.waitForNextTick();
+ }
+ }
+
+ objpose::ProviderInfo ObjectMemoryEditor::getProviderInfo(const Ice::Current & /*unused*/)
+ {
+ return providerInfo;
+ }
+
+ objpose::provider::RequestObjectsOutput ObjectMemoryEditor::requestObjects(
+ const objpose::provider::RequestObjectsInput &input, const Ice::Current & /*unused*/)
+ {
+ objpose::provider::RequestObjectsOutput output;
+
+ for (const auto &id: input.objectIDs)
+ {
+ output.results[id].success = false;
+ }
+
+ return output;
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.h b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.h
new file mode 100644
index 0000000000000000000000000000000000000000..1cc90b97906890dced6bf29bd25a3c26fa164f3f
--- /dev/null
+++ b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
+#include "InteractionObserver.h"
+
+namespace armarx
+{
+ class ObjectMemoryEditor :
+ virtual public armarx::Component,
+ virtual public armarx::DebugObserverComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::ObjectPoseClientPluginUser,
+ virtual public armarx::ObjectPoseProviderPluginUser
+ {
+ public:
+ std::string getDefaultName() const override;
+
+ protected:
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+ public:
+ objpose::ProviderInfo getProviderInfo(const Ice::Current& = Ice::emptyCurrent) override;
+ objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
+
+ private:
+ void run();
+
+ armarx::ObjectFinder objectFinder;
+ objpose::ProviderInfo providerInfo;
+
+ float objectScaling = 1.01F;
+ float confidenceThreshold = 0.0F;
+
+ armarx::SimpleRunningTask<>::pointer_type objectVizTask;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 4d4d2399bb3ee5059d9788fb60f61cf23f78c2b0..a17005e6d573ff98accd7b40e9c45376ed8e85c4 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -38,8 +38,6 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/interface/units/PlatformUnitInterface.h>
-
using namespace Eigen;
using namespace Ice;
@@ -72,7 +70,6 @@ namespace armarx
defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history").setMin(0);
defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
- defineOptionalProperty<std::string>("PlatformTopicName", "PlatformState", "Topic where platform state is published.");
defineOptionalProperty<std::string>("GlobalRobotPoseLocalizationTopicName", "GlobalRobotPoseLocalization", "Topic where the global robot pose can be reported.");
}
@@ -149,7 +146,6 @@ namespace armarx
usingTopic(topicPrefix + robotNodeSetName + "State");
usingTopic(topicPrefix + getProperty<std::string>("GlobalRobotPoseLocalizationTopicName").getValue());
- usingTopic(topicPrefix + getProperty<std::string>("PlatformTopicName").getValue());
try
{
@@ -692,39 +688,4 @@ namespace armarx
return Timestamped<FramedPosePtr> {time, new FramedPose(globalPose, armarx::GlobalFrame, "")};
}
-
- // legacy
- void RobotStateComponent::reportPlatformPose(const PlatformPose& currentPose, const Current&)
- {
- const float z = 0;
- const Eigen::Vector3f position(currentPose.x, currentPose.y, z);
- const Eigen::Matrix3f orientation =
- Eigen::AngleAxisf(currentPose.rotationAroundZ, Eigen::Vector3f::UnitZ()).toRotationMatrix();
- const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation);
-
- IceUtil::Time time = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
- // ARMARX_IMPORTANT << VAROUT(currentPose.timestampInMicroSeconds);
-
- TransformStamped stamped;
- stamped.header.frame = armarx::GlobalFrame;
- stamped.header.agent = _synchronized->getName();
- stamped.header.timestampInMicroSeconds = time.toMicroSeconds();
- stamped.header.parentFrame = "";
- stamped.transform = globalPose;
-
- this->reportGlobalRobotPose(stamped);
-
- /*
- * old:
- insertPose(time, globalPose);
-
- if (_sharedRobotServant)
- {
- _sharedRobotServant->setTimestamp(time);
- }
- */
- }
-
-
-
}
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 685567a476bd7ecfd8d97981fa2cc205d53c43bd..d786dd0d67efc4b5e8fe07e02bef4f4b370588da 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -59,7 +59,7 @@ namespace armarx
/**
* \defgroup Component-RobotStateComponent RobotStateComponent
* \ingroup RobotAPI-Components
- * \brief Maintains a robot representation based on VirtualRobot (see [Simox](http://gitlab.com/Simox/simox)).
+ * \brief Maintains a robot representation based on VirtualRobot (see [Simox](https://git.h2t.iar.kit.edu/sw/simox/simox)).
*
* The robot can be loaded from a Simox robot XML file.
* Upon request, an Ice proxy to a shared instance of this internal robot
@@ -128,20 +128,6 @@ namespace armarx
void setRobotStateObserver(RobotStateObserverPtr observer);
-
- // PlatformUnitListener interface
- // TODO: Remove this interface and use GlobalRobotPoseLocalizationListener only.
- /// Stores the platform pose in the pose history.
- void reportPlatformPose(const PlatformPose& currentPose, const Ice::Current& = Ice::emptyCurrent) override;
- /// Does nothing.
- void reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& = Ice::emptyCurrent) override {}
- /// Does nothing.
- void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& = Ice::emptyCurrent) override {}
- /// Does nothing.
- void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current& = Ice::emptyCurrent) override {}
-
-
-
protected:
// Component interface.
@@ -239,4 +225,3 @@ namespace armarx
};
}
-
diff --git a/source/RobotAPI/components/RobotState/test/CMakeLists.txt b/source/RobotAPI/components/RobotState/test/CMakeLists.txt
index 2dfe68fa517eaf200803eaf8b997fcf738e76110..bdedba35eaee4df324cf3b09f65fd3ea1cf70a68 100644
--- a/source/RobotAPI/components/RobotState/test/CMakeLists.txt
+++ b/source/RobotAPI/components/RobotState/test/CMakeLists.txt
@@ -1,4 +1,3 @@
-set(LIBS ${LIBS} RobotAPICore RobotAPIRobotStateComponent)
+set(LIBS ${LIBS} RobotAPICore RobotAPIRobotStateComponent ArmarXCoreTest)
armarx_add_test(RobotStateTest RobotStateTest.cpp "${LIBS}")
-
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
index e59bbc11194b5c279e6f1278d9cbf60f57c2b8b1..1311e68e132cdbafc805351064d643494f48f87d 100644
--- a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
@@ -24,7 +24,7 @@
#include <filesystem>
-#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/Metronome.h>
namespace armarx
@@ -39,7 +39,7 @@ namespace armarx
armarx::PropertyDefinitionsPtr defs(new RobotToArVizPropertyDefinitions(getConfigIdentifier()));
// defs->optional(updateFrequency, "updateFrequency", "Target number of updates per second.");
- defs->defineOptionalProperty("updateFrequency", updateFrequency, "Target number of updates per second.");
+ defs->defineOptionalProperty("updateFrequency", updateFrequencyHz, "Target number of updates per second.");
defs->optional(gui.useCollisionModel, "UseCollisionModel", "Use the collision model for visualization");
defs->optional(gui.showRobotNodeFrames, "ShowRobotNodeFrames",
@@ -57,7 +57,7 @@ namespace armarx
void RobotToArViz::onInitComponent()
{
- getProperty(updateFrequency, "updateFrequency");
+ getProperty(updateFrequencyHz, "updateFrequency");
}
@@ -83,10 +83,10 @@ namespace armarx
createRemoteGuiTab();
RemoteGui_startRunningTask();
- task = new SimplePeriodicTask<>([this]()
+ task = new SimpleRunningTask<>([this]()
{
- this->updateRobot();
- }, int(1000 / updateFrequency));
+ this->updateRobotRun();
+ });
task->start();
}
@@ -135,7 +135,19 @@ namespace armarx
}
- void RobotToArViz::updateRobot()
+ void RobotToArViz::updateRobotRun()
+ {
+ Metronome metronome(Frequency::Hertz(updateFrequencyHz));
+ while (task and not task->isStopped())
+ {
+ updateRobotOnce();
+
+ metronome.waitForNextTick();
+ }
+ }
+
+
+ void RobotToArViz::updateRobotOnce()
{
RobotState::synchronizeLocalClone(robotName);
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.h b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
index e666ba1b12e13560be2f261b2954d490cdb80a01..5eea92090fd3d4826f87b99cd9676c8f6200ba8c 100644
--- a/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
@@ -96,14 +96,15 @@ namespace armarx
private:
- void updateRobot();
+ void updateRobotRun();
+ void updateRobotOnce();
static bool trySetFilePathFromDataDir(viz::Robot& robotViz, const std::string& absolutePath);
private:
- float updateFrequency = 100;
- SimplePeriodicTask<>::pointer_type task;
+ float updateFrequencyHz = 100;
+ SimpleRunningTask<>::pointer_type task;
std::string robotName = "robot";
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
index b82eefe899e35b8e4ef63b538221f6ec0adb9159..ae751bb1ac1bfb0ba8e51a4fc9fc11b28c0e3cf2 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -21,6 +21,7 @@
*/
#include "LegacyRobotStateMemoryAdapter.h"
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
@@ -116,7 +117,7 @@ namespace armarx::armem
transform.header.agent = "Armar3";
transform.header.parentFrame = OdometryFrame;
transform.header.frame = armarx::armem::robot_state::constants::robotRootNodeName;
- transform.header.timestamp = armem::Time::microSeconds(_timestampUpdateFirstModifiedInUs);
+ transform.header.timestamp = armem::Time(armem::Duration::MicroSeconds(_timestampUpdateFirstModifiedInUs));
Eigen::Isometry3f tf = Eigen::Isometry3f::Identity();
tf.translation().x() = (update.platformPose.x);
@@ -211,17 +212,36 @@ namespace armarx::armem
}
- void LegacyRobotStateMemoryAdapter::reportPlatformPose(const PlatformPose& p, const Ice::Current &)
+ // void LegacyRobotStateMemoryAdapter::reportPlatformPose(const PlatformPose& p, const Ice::Current &)
+ // {
+ // ARMARX_DEBUG << "Got an update for platform pose";
+ // std::lock_guard l(updateMutex);
+ // update.platformPose = p;
+ // updateTimestamps(p.timestampInMicroSeconds);
+ // }
+
+ void LegacyRobotStateMemoryAdapter::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&)
{
+
ARMARX_DEBUG << "Got an update for platform pose";
- std::lock_guard l(updateMutex);
- update.platformPose = p;
- updateTimestamps(p.timestampInMicroSeconds);
- }
- void LegacyRobotStateMemoryAdapter::reportNewTargetPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &)
- {
+ const Eigen::Isometry3f global_T_robot(transformStamped.transform);
+ const float yaw = simox::math::mat3f_to_rpy(global_T_robot.linear()).z();
+
+ armarx::PlatformPose p;
+ p.x = global_T_robot.translation().x();
+ p.y = global_T_robot.translation().y();
+ p.rotationAroundZ = yaw;
+ p.timestampInMicroSeconds = transformStamped.header.timestampInMicroSeconds;
+
+ {
+ std::lock_guard l(updateMutex);
+ update.platformPose = p;
+ updateTimestamps(p.timestampInMicroSeconds);
+ }
}
+
+
void LegacyRobotStateMemoryAdapter::reportPlatformVelocity(Ice::Float f1, Ice::Float f2, Ice::Float f3, const Ice::Current &)
{
ARMARX_DEBUG << "Got an update for platform vels";
@@ -240,6 +260,8 @@ namespace armarx::armem
update.platformOdometryPose = {f1, f2, f3};
updateTimestamps(now);
}
+
+
void LegacyRobotStateMemoryAdapter::commitArmar3RobotDescription()
{
@@ -248,7 +270,7 @@ namespace armarx::armem
desc.name = "Armar3";
desc.timestamp = armem::Time::Now();
desc.xml.package = "RobotAPI";
- desc.xml.path = "RobotAPI/robots/Armar3/ArmarIII.xml";
+ desc.xml.path = "robots/Armar3/ArmarIII.xml";
armem::Commit c;
auto& entityUpdate = c.add();
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
index 9680d8c27a1e98e0e70c74308e5235f18bb4b1fc..71ab81f1a3582fafeed767ae7dcfc00f20cf0cad 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
@@ -59,11 +59,12 @@ namespace armarx::armem
void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override;
- void reportPlatformPose(const PlatformPose &, const Ice::Current &) override;
- void reportNewTargetPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
+ // void reportPlatformPose(const PlatformPose &, const Ice::Current &) override;
void reportPlatformVelocity(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
+ void reportGlobalRobotPose(const ::armarx::TransformStamped&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
+
protected:
/// @see PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index 7f3b065d376bd23e7c99348bb3d94e9f6ddd3abf..9e9e2da67d9f2b3c1ed686fcf71f15c0c7e6ad8c 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -1,5 +1,7 @@
add_subdirectory(ExampleMemoryClient)
add_subdirectory(GraspProviderExample)
-add_subdirectory(VirtualRobotWriterExample)
-add_subdirectory(VirtualRobotReaderExampleClient)
+add_subdirectory(ObjectInstanceToIndex)
+add_subdirectory(RobotStatePredictionClientExample)
add_subdirectory(SimpleVirtualRobot)
+add_subdirectory(VirtualRobotReaderExampleClient)
+add_subdirectory(VirtualRobotWriterExample)
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/CMakeLists.txt b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9d3c74998e5573364da4b2639587f5596e4df5df
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/CMakeLists.txt
@@ -0,0 +1,27 @@
+armarx_component_set_name(ObjectInstanceToIndex)
+
+set(COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCore
+ ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+ # ArmarXCore
+ RobotAPI::armem_index
+ RobotAPI::armem_objects
+ RobotAPI::armem_robot_state
+ aroncommon
+)
+
+set(SOURCES
+ ObjectInstanceToIndex.cpp
+ impl/ObjectInstanceToIndex.cpp
+)
+
+set(HEADERS
+ ObjectInstanceToIndex.h
+ impl/ObjectInstanceToIndex.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fedf604b04009ee1b1b3ef18e36df93f7a57b6a4
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
@@ -0,0 +1,238 @@
+/*
+ * 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::ObjectInstanceToIndex
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ObjectInstanceToIndex.h"
+
+#include <Eigen/Geometry>
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
+#include <RobotAPI/libraries/armem_index/memory_ids.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+
+
+namespace armarx
+{
+
+ armarx::PropertyDefinitionsPtr
+ ObjectInstanceToIndex::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ defs->optional(properties.object.maxFrequencyHz, "p.object.maxFrequency");
+
+ return defs;
+ }
+
+
+ ObjectInstanceToIndex::ObjectInstanceToIndex()
+ {
+ addPlugin(objectClientPlugin);
+ }
+
+ std::string
+ ObjectInstanceToIndex::getDefaultName() const
+ {
+ return "ObjectInstanceToIndex";
+ }
+
+
+ void
+ ObjectInstanceToIndex::onInitComponent()
+ {
+ using This = ObjectInstanceToIndex;
+ // This should not be necessary but seems to be. ToDo: Look into this.
+ memoryNameSystem().setComponent(this);
+
+ memoryNameSystem().subscribe(armem::robot_state::memoryID, this, &This::processRobotState);
+ memoryNameSystem().subscribe(
+ armem::objects::instaceSegmentID, this, &This::processObjectInstance);
+ }
+
+
+ void
+ ObjectInstanceToIndex::onConnectComponent()
+ {
+ try
+ {
+ indexSpatialMemoryWriter = memoryNameSystem().useWriter(armem::index::spatialSegmentID);
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ }
+
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+ }
+
+
+ void
+ ObjectInstanceToIndex::onDisconnectComponent()
+ {
+ }
+
+
+ void
+ ObjectInstanceToIndex::onExitComponent()
+ {
+ }
+
+
+ void
+ ObjectInstanceToIndex::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ VBoxLayout root = {VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void
+ ObjectInstanceToIndex::RemoteGui_update()
+ {
+ }
+
+
+ void
+ ObjectInstanceToIndex::processRobotState(const armem::MemoryID& id,
+ const std::vector<armem::MemoryID>& snapshotIDs)
+ {
+#if 0
+ if (not robotMemoryReader)
+ {
+ try
+ {
+ robotMemoryReader = memoryNameSystem().getReader(objectInstanceSegmentID);
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_INFO << e.what();
+ return;
+ }
+ }
+ ARMARX_CHECK(robotMemoryReader);
+
+ // Get only the latest snapshot per entity.
+ std::map<armem::MemoryID, const armem::MemoryID*> entityToSnapshot;
+ for (const armem::MemoryID& snapshotID : snapshotIDs)
+ {
+ const armem::MemoryID entityID = snapshotID.getEntityID();
+ if (auto it = entityToSnapshot.find(entityID); it != entityToSnapshot.end())
+ {
+ if (it->second->timestamp < snapshotID.timestamp)
+ {
+ entityToSnapshot[entityID] = &snapshotID;
+ }
+ }
+ else
+ {
+ entityToSnapshot[entityID] = &snapshotID;
+ }
+ }
+
+ // Query them.
+ std::vector<armem::MemoryID> queryIDs;
+ for (const auto& [_, snapshotID] : entityToSnapshot)
+ {
+ queryIDs.push_back(*snapshotID);
+ }
+
+ armem::client::QueryResult result = robotMemoryReader.queryMemoryIDs(queryIDs);
+ if (result.success)
+ {
+ // Prepare the commit.
+
+ armem::MemoryID provSegID = indexSpatialSegmentID.withProviderSegmentName(getName());
+
+ armem::Commit commit;
+
+ result.memory.forEachInstance(
+ [&commit, &provSegID](const armem::wm::EntityInstance& instance)
+ {
+ const armem::arondto::ObjectInstance data = instance.dataAs<armem::arondto::ObjectInstance>();
+
+ armem::index::arondto::Spatial spatial;
+ armem::toAron(spatial.id, instance.id());
+ spatial.oobbGlobal;
+ spatial.aabbGlobal;
+
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = provSegID.withEntityName(instance.id().str());
+ update.timeCreated = instance.id().timestamp;
+ update.instancesData = { spatial.toAron() };
+
+ return true;
+ });
+ }
+ else
+ {
+ ARMARX_INFO << result.errorMessage;
+ }
+#endif
+ }
+
+
+ void
+ ObjectInstanceToIndex::processObjectInstance(const armem::MemoryID& id,
+ const std::vector<armem::MemoryID>& snapshotIDs)
+ {
+ std::scoped_lock lock(objectMutex);
+ if (not object.has_value())
+ {
+ object = armem::objects::ObjectInstanceToIndex{
+ .objectPoseClient = objectClientPlugin->createClient(),
+ .indexSpatialMemoryWriter = indexSpatialMemoryWriter,
+ .indexSpatialProviderSegmentID =
+ armem::index::spatialSegmentID.withProviderSegmentName(getName()),
+ .indexNamedProviderSegmentID =
+ armem::index::namedSegmentID.withProviderSegmentName(getName()),
+ .params = armem::objects::ObjectInstanceToIndex::Parameters{
+ .maxFrequency = armarx::Frequency::Hertz(properties.object.maxFrequencyHz)
+ },
+ .state = {}
+ };
+ }
+ ARMARX_CHECK(object.has_value());
+
+ object->fetchAndCommitObjectInstances(snapshotIDs);
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h
new file mode 100644
index 0000000000000000000000000000000000000000..c5e6486e7ded27538c715f1ba48531a1fd552b1f
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h
@@ -0,0 +1,115 @@
+/*
+ * 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::ObjectInstanceToIndex
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/time/Frequency.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
+
+#include "impl/ObjectInstanceToIndex.h"
+
+
+namespace armarx
+{
+
+ /**
+ * @defgroup Component-ObjectInstanceToIndex ObjectInstanceToIndex
+ * @ingroup RobotAPI-Components
+ *
+ * Links object instances in the "Index/Spatial" and "Index/Named" memory segments.
+ *
+ * @class ObjectInstanceToIndex
+ * @ingroup Component-ObjectInstanceToIndex
+ * @brief Brief description of class ObjectInstanceToIndex.
+ *
+ * Links object instances in the "Index/Spatial" and "Index/Named" memory segments.
+ */
+ // Class is currently called like component for compatibility with legacy CMake.
+ class ObjectInstanceToIndex :
+ virtual public armarx::Component,
+ virtual public armarx::armem::ListeningClientPluginUser,
+ virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ {
+ public:
+ ObjectInstanceToIndex();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ protected:
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+ void onExitComponent() override;
+
+
+ // LightweightRemoteGuiComponentPluginUser interface
+ public:
+ void createRemoteGuiTab();
+ void RemoteGui_update() override;
+
+
+ private:
+ void processRobotState(const armem::MemoryID& id,
+ const std::vector<armem::MemoryID>& snapshotIDs);
+ void processObjectInstance(const armem::MemoryID& id,
+ const std::vector<armem::MemoryID>& snapshotIDs);
+
+
+ private:
+ struct Properties
+ {
+ struct Object
+ {
+ float maxFrequencyHz = 10;
+ };
+ Object object;
+ };
+ Properties properties;
+
+ armem::client::Writer indexSpatialMemoryWriter;
+
+
+ armarx::plugins::ObjectPoseClientPlugin* objectClientPlugin = nullptr;
+
+ std::mutex objectMutex;
+ std::optional<armem::objects::ObjectInstanceToIndex> object;
+
+ armem::client::Reader robotMemoryReader;
+
+ // armarx::RunningTask<ObjectInstanceToIndex>::pointer_type task;
+
+ struct RemoteGuiTab : RemoteGui::Client::Tab
+ {
+ };
+ RemoteGuiTab tab;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4498d01cec8f60ba81e864438671dc254b2bf5e9
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp
@@ -0,0 +1,177 @@
+/*
+ * 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::RobotAndObjectToIndex
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "ObjectInstanceToIndex.h"
+
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem_index/aron/Named.aron.generated.h>
+#include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
+#include <RobotAPI/libraries/armem_objects/utils.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+
+namespace armarx::armem::objects
+{
+
+ void
+ ObjectInstanceToIndex::fetchAndCommitObjectInstances(
+ const std::vector<armem::MemoryID>& updatedObjectInstanceSnapshotIDs)
+ {
+ ARMARX_CHECK(objectPoseClient.isConnected());
+
+ // Fetch the latest poses.
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient.fetchObjectPoses();
+
+ const auto filtered = filterObjectPoses(objectPoses, updatedObjectInstanceSnapshotIDs);
+
+
+ // Prepare the commit.
+
+ armem::Commit commit;
+
+ for (const objpose::ObjectPose* objectPosePtr : filtered)
+ {
+ const objpose::ObjectPose& objectPose = *objectPosePtr;
+
+ armem::MemoryID objectInstanceID =
+ armem::objects::reconstructObjectInstanceID(objectPose);
+
+ // Spatial
+
+ std::optional<simox::OrientedBoxf> oobb = objectPose.oobbGlobal();
+ if (oobb.has_value())
+ {
+ simox::AxisAlignedBoundingBox aabb =
+ simox::AxisAlignedBoundingBox::from_points(oobb->corners());
+
+ armem::index::arondto::Spatial spatial;
+ armem::toAron(spatial.id, objectInstanceID);
+ aron::toAron(spatial.oobbGlobal, oobb);
+ toAron(spatial.aabbGlobal, aabb);
+
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = indexSpatialProviderSegmentID.withEntityName(
+ objectInstanceID.getEntityID().str());
+ update.timeCreated = objectPose.timestamp;
+ update.instancesData = {spatial.toAron()};
+ }
+
+ // Named
+
+ // Load object class information.
+ std::optional<ObjectInfo> info =
+ objectPoseClient.getObjectFinder().findObject(objectPose.objectID);
+ if (info.has_value())
+ {
+ std::optional<std::vector<std::string>> recognized, spoken;
+ recognized = info->loadRecognizedNames();
+ spoken = info->loadSpokenNames();
+
+ armem::index::arondto::Named named;
+ armem::toAron(named.id, objectInstanceID);
+
+ if (recognized.has_value())
+ {
+ named.names.recognized = recognized.value();
+ }
+ else
+ {
+ named.names.recognized = {info->className()};
+ }
+
+ if (spoken.has_value())
+ {
+ named.names.spoken = spoken.value();
+ }
+ else
+ {
+ named.names.spoken = {info->className()};
+ }
+
+ armem::EntityUpdate& update = commit.add();
+ update.entityID = indexNamedProviderSegmentID.withEntityName(
+ objectInstanceID.getEntityID().str());
+ update.timeCreated = objectPose.timestamp;
+ update.instancesData = {named.toAron()};
+ }
+ }
+
+ // Commit.
+ indexSpatialMemoryWriter.commit(commit);
+ }
+
+
+ std::vector<const objpose::ObjectPose*>
+ ObjectInstanceToIndex::filterObjectPoses(const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<MemoryID>& updatedSnapshotIDs)
+ {
+ // Returns true to keep the item, false to skip it.
+ auto filter = [this, &updatedSnapshotIDs](const objpose::ObjectPose& objectPose)
+ {
+ auto it = state.latestUpdateDateTimes.find(objectPose.objectID);
+ if (it == state.latestUpdateDateTimes.end())
+ {
+ // Never encountered that before, commit it.
+ return true;
+ }
+
+ const armarx::DateTime& latestTime = it->second;
+ armarx::DateTime nextDueTime = latestTime + params.maxFrequency.toCycleDuration();
+ if (objectPose.timestamp < nextDueTime)
+ {
+ // Skip.
+ return false;
+ }
+
+ armem::MemoryID objectInstanceID =
+ armem::objects::reconstructObjectInstanceID(objectPose);
+ bool found = false;
+ for (const MemoryID& updatedSnapshotID : updatedSnapshotIDs)
+ {
+ if (armem::contains(updatedSnapshotID, objectInstanceID))
+ {
+ found = true;
+ break;
+ }
+ }
+ return found;
+ };
+
+
+ std::vector<const objpose::ObjectPose*> filtered;
+ for (const objpose::ObjectPose& objectPose : objectPoses)
+ {
+ if (filter(objectPose))
+ {
+ filtered.push_back(&objectPose);
+ state.latestUpdateDateTimes[objectPose.objectID] = objectPose.timestamp;
+ }
+ }
+
+ return filtered;
+ }
+
+} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd8f29399757375351a9c855e77cd1e009a00f6f
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h
@@ -0,0 +1,69 @@
+/*
+ * 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::RobotAndObjectToIndex
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <ArmarXCore/core/time/Frequency.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::armem::objects
+{
+
+ class ObjectInstanceToIndex
+ {
+ public:
+ void fetchAndCommitObjectInstances(
+ const std::vector<armem::MemoryID>& updatedObjectInstanceSnapshotIDs);
+
+ private:
+ std::vector<const objpose::ObjectPose*>
+ filterObjectPoses(const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<armem::MemoryID>& updatedObjectInstanceSnapshotIDs);
+
+ public:
+ objpose::ObjectPoseClient objectPoseClient;
+ armem::client::Writer indexSpatialMemoryWriter;
+
+ armem::MemoryID indexSpatialProviderSegmentID;
+ armem::MemoryID indexNamedProviderSegmentID;
+
+
+ struct Parameters
+ {
+ armarx::Frequency maxFrequency = armarx::Frequency::HertzDouble(60);
+ };
+ Parameters params;
+
+ struct State
+ {
+ std::map<armarx::ObjectID, armarx::DateTime> latestUpdateDateTimes;
+ };
+ State state;
+ };
+
+} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..91c7bdc1834110f3c0be76f7d6eefb6f001338b9
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/CMakeLists.txt
@@ -0,0 +1,40 @@
+armarx_component_set_name(RobotStatePredictionClientExample)
+
+
+set(COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCore ArmarXCoreInterfaces
+ # ArmarxGui
+ ArmarXGuiComponentPlugins
+ # RobotAPI
+ RobotAPICore
+ RobotAPIComponentPlugins
+ armem
+ armem_robot_state
+)
+
+set(SOURCES
+ # ToDo: Move to library.
+ RobotStatePredictionClient.cpp
+
+ Component.cpp
+ Impl.cpp
+)
+
+set(HEADERS
+ # ToDo: Move to library.
+ RobotStatePredictionClient.h
+ # ToDo: Move to simox.
+ simox_alg.hpp
+
+ Component.h
+ Impl.h
+
+ RobotStatePredictionClientExample.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+# Generate the application.
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f13e111eec8e51f3fa67e5d04f03d3df1fd0b64
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.cpp
@@ -0,0 +1,97 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "RobotStatePredictionClientExample.h"
+#include "Impl.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::robot_state_prediction_client_example
+{
+
+ Component::Component() :
+ pimpl(std::make_unique<Impl>(memoryNameSystem()))
+ {
+ }
+
+
+ RobotStatePredictionClientExample::~RobotStatePredictionClientExample() = default;
+
+
+ std::string Component::getDefaultName() const
+ {
+ return "RobotStatePredictionClientExample";
+ }
+
+
+ armarx::PropertyDefinitionsPtr Component::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ ARMARX_CHECK_NOT_NULL(pimpl);
+ pimpl->defineProperties(defs, "p.");
+
+ return defs;
+ }
+
+
+ void Component::onInitComponent()
+ {
+ }
+
+
+ void Component::onConnectComponent()
+ {
+ pimpl->connect(memoryNameSystem(), arviz);
+ pimpl->start();
+
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+
+
+ void Component::onDisconnectComponent()
+ {
+ pimpl->stop();
+ }
+
+
+ void Component::onExitComponent()
+ {
+ }
+
+
+ void Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ VBoxLayout root = {VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+ void Component::RemoteGui_update()
+ {
+ }
+
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h
new file mode 100644
index 0000000000000000000000000000000000000000..198257f6b033cfc0433ca5b0caad18e47ee77e45
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h
@@ -0,0 +1,100 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <memory>
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+
+// For some reason, the generated main requires Impl to be complete ...
+#include "Impl.h"
+
+
+namespace armarx::robot_state_prediction_client_example
+{
+ class Impl;
+
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ *
+ * An example for an ArMem Memory Client.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Connects to the example memory, and commits and queries example data.
+ */
+ class Component :
+ virtual public armarx::Component
+ , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
+ , virtual public armarx::armem::ClientPluginUser
+ {
+ public:
+ using Impl = robot_state_prediction_client_example::Impl;
+
+ Component();
+ virtual ~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;
+
+
+ private:
+
+ std::unique_ptr<Impl> pimpl = nullptr;
+
+ struct RemoteGuiTab : RemoteGui::Client::Tab
+ {
+ };
+ RemoteGuiTab tab;
+
+ };
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c433669630c9631e471666fa3b3cd02acd25c2f9
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.cpp
@@ -0,0 +1,261 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "Impl.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/Metronome.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/query.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+
+
+namespace simox::alg
+{
+ template <class... Args>
+ std::vector<Args...>
+ concatenate(const std::vector<Args...>& lhs, const std::vector<Args...>& rhs)
+ {
+ std::vector<Args...> conc = lhs;
+ std::copy(rhs.begin(), rhs.end(), std::back_inserter(conc));
+ return conc;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::map<KeyT, ValueT>
+ map_from_key_value_pairs(const std::vector<KeyT>& lhs, const std::vector<ValueT>& rhs)
+ {
+ const size_t size = std::min(lhs.size(), rhs.size());
+
+ std::map<KeyT, ValueT> map;
+ for (size_t i = 0; i < size; ++i)
+ {
+ map.emplace(lhs[i], rhs[i]);
+ }
+ return map;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::vector<ValueT>
+ multi_at(const std::map<KeyT, ValueT>& map,
+ const std::vector<KeyT>& keys,
+ bool skipMissing = false)
+ {
+ std::vector<ValueT> values;
+ values.reserve(keys.size());
+
+ for (const KeyT& key : keys)
+ {
+ if (skipMissing)
+ {
+ if (auto it = map.find(key); it != map.end())
+ {
+ values.push_back(it->second);
+ }
+ }
+ else
+ {
+ // Throw an exception if missing.
+ values.push_back(map.at(key));
+ }
+ }
+
+ return values;
+ }
+
+ template <class... Args>
+ std::vector<Args...>
+ slice(const std::vector<Args...>& vector,
+ size_t start = 0,
+ std::optional<size_t> end = std::nullopt)
+ {
+ std::vector<Args...> result;
+ auto beginIt = vector.begin() + start;
+ auto endIt = end ? vector.begin() + *end : vector.end();
+ std::copy(beginIt, endIt, std::back_inserter(result));
+ return result;
+ }
+
+} // namespace simox::alg
+
+namespace armarx::robot_state_prediction_client_example
+{
+
+ Impl::Impl(armem::client::MemoryNameSystem& memoryNameSystem)
+ {
+ client.remote.robotReader.emplace(memoryNameSystem);
+ }
+
+
+ Impl::~Impl() = default;
+
+
+ void
+ Impl::defineProperties(IceUtil::Handle<PropertyDefinitionContainer>& defs,
+ const std::string& prefix)
+ {
+ defs->optional(properties.robotName, prefix + "robotName", "Name of the robot.");
+
+ defs->optional(properties.predictAheadSeconds,
+ prefix + "predictAheadSeconds",
+ "How far into the future to predict [s].");
+
+ client.remote.robotReader->registerPropertyDefinitions(defs);
+ }
+
+
+ void
+ Impl::connect(armem::client::MemoryNameSystem& mns, viz::Client arviz)
+ {
+ try
+ {
+ client.remote.reader =
+ mns.useReader(armem::MemoryID(armem::robot_state::constants::memoryName));
+ ARMARX_IMPORTANT << "got reader";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_WARNING << e.what();
+ }
+
+ client.remote.robotReader->connect();
+
+ this->remote.arviz = arviz;
+ }
+
+
+ void
+ Impl::start()
+ {
+ task = new armarx::SimpleRunningTask<>([this]() { this->run(); });
+ task->start();
+ }
+
+
+ void
+ Impl::stop()
+ {
+ task->stop();
+ }
+
+
+ void
+ Impl::run()
+ {
+ Metronome metronome(Frequency::Hertz(properties.updateFrequencyHz));
+
+ while (task and not task->isStopped())
+ {
+ metronome.waitForNextTick();
+
+ runOnce();
+ }
+ }
+
+
+ void
+ Impl::runOnce()
+ {
+ ARMARX_CHECK(client.remote.reader);
+
+ const DateTime now = Clock::Now();
+ const DateTime predictedTime =
+ now + Duration::SecondsDouble(properties.predictAheadSeconds);
+
+ if (not robotViz)
+ {
+ auto desc = client.remote.robotReader->queryDescription(properties.robotName, now);
+ if (desc.has_value())
+ {
+ PackagePath& pp = desc->xml;
+ robotViz = viz::Robot(properties.robotName)
+ .file(pp.serialize().package, pp.serialize().path)
+ .overrideColor(simox::Color::cyan(255, 64));
+ }
+ else
+ {
+ ARMARX_INFO << "Failed to get robot description.";
+ }
+ }
+ if (not robotViz)
+ {
+ return;
+ }
+
+ // Which entities to predict?
+
+ const std::vector<armem::MemoryID> locEntityIDs = this->localizationEntityIDs.has_value()
+ ? this->localizationEntityIDs.value()
+ : client.queryLocalizationEntityIDs();
+ const std::vector<armem::MemoryID> propEntityIDs =
+ this->propioceptionEntityIDs.has_value() ? this->propioceptionEntityIDs.value()
+ : client.queryProprioceptionEntityIDs();
+
+ // Predict.
+
+ auto prediction = client.predictWholeBody(
+ locEntityIDs, propEntityIDs, predictedTime, properties.robotName, "Linear");
+
+ // Gather results.
+
+ if (prediction.globalPose.has_value())
+ {
+ // ARMARX_INFO << "Predicted global pose: \n" << globalPose->matrix();
+ robotViz->pose(prediction.globalPose->matrix());
+
+ if (not localizationEntityIDs)
+ {
+ // Store entity IDs for successful lookup.
+ this->localizationEntityIDs = locEntityIDs;
+ }
+ }
+ if (prediction.jointPositions.has_value())
+ {
+ robotViz->joints(prediction.jointPositions.value());
+
+ if (not propioceptionEntityIDs)
+ {
+ this->propioceptionEntityIDs = propEntityIDs;
+ }
+ }
+
+ // Visualize.
+ {
+ viz::Layer layer = remote.arviz.layer(properties.robotName + " Prediction");
+ layer.add(robotViz.value());
+ remote.arviz.commit(layer);
+ }
+ }
+
+
+} // namespace armarx::robot_state_prediction_client_example
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
new file mode 100644
index 0000000000000000000000000000000000000000..272f9e2e62d6ac16b49a2674a7952d96e8618975
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Impl.h
@@ -0,0 +1,87 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <vector>
+
+#include <ArmarXCore/util/tasks.h>
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include "RobotStatePredictionClient.h"
+
+
+namespace armarx::robot_state_prediction_client_example
+{
+
+ class Impl
+ {
+ public:
+
+ Impl(armem::client::MemoryNameSystem& memoryNameSystem);
+ ~Impl();
+
+
+ void defineProperties(IceUtil::Handle<armarx::PropertyDefinitionContainer>& defs, const std::string& prefix);
+ void connect(armem::client::MemoryNameSystem& mns, viz::Client arviz);
+
+ void start();
+ void stop();
+
+ void run();
+ void runOnce();
+
+
+ public:
+
+ struct Properties
+ {
+ float updateFrequencyHz = 10;
+
+ std::string robotName = "Armar6";
+ float predictAheadSeconds = 1.0;
+ };
+ Properties properties;
+
+ struct Remote
+ {
+ viz::Client arviz;
+ };
+ Remote remote;
+
+ armarx::SimpleRunningTask<>::pointer_type task;
+
+
+ armem::robot_state::RobotStatePredictionClient client;
+
+ std::optional<std::vector<armem::MemoryID>> localizationEntityIDs;
+ std::optional<std::vector<armem::MemoryID>> propioceptionEntityIDs;
+ std::optional<viz::Robot> robotViz;
+
+ };
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d98d0f2717747a91f7a56c3e0c547cbb0088fbd
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
@@ -0,0 +1,280 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotStatePredictionClient.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/query.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include "simox_alg.hpp"
+
+
+namespace armarx::armem::robot_state
+{
+
+ RobotStatePredictionClient::RobotStatePredictionClient()
+ {
+ }
+
+
+ std::vector<armem::MemoryID>
+ RobotStatePredictionClient::queryLocalizationEntityIDs()
+ {
+ return _queryEntityIDs(armem::robot_state::constants::localizationCoreSegment);
+ }
+
+
+ std::vector<MemoryID>
+ RobotStatePredictionClient::queryProprioceptionEntityIDs()
+ {
+ return _queryEntityIDs(armem::robot_state::constants::proprioceptionCoreSegment);
+ }
+
+
+ std::vector<MemoryID>
+ RobotStatePredictionClient::_queryEntityIDs(const std::string& coreSegmentName)
+ {
+ armem::client::QueryBuilder qb;
+ {
+ namespace qf = armarx::armem::client::query_fns;
+ qb.coreSegments(qf::withName(coreSegmentName))
+ .providerSegments(qf::all())
+ .entities(qf::all())
+ .snapshots(qf::latest());
+ }
+ armem::client::QueryResult result = remote.reader.query(qb);
+ if (result.success)
+ {
+ return armem::getEntityIDs(result.memory);
+ }
+ else
+ {
+ // ToDo: Use exceptions to escalate error.
+ ARMARX_VERBOSE << "Query failed: " << result.errorMessage;
+ return {};
+ }
+ }
+
+
+ std::vector<armem::PredictionRequest>
+ RobotStatePredictionClient::makePredictionRequests(
+ const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID)
+ {
+ std::vector<armem::PredictionRequest> requests;
+ requests.reserve(entityIDs.size());
+ for (const armem::MemoryID& entityID : entityIDs)
+ {
+ armem::PredictionRequest& request = requests.emplace_back();
+ request.snapshotID = entityID.withTimestamp(predictedTime);
+ request.predictionSettings.predictionEngineID = engineID;
+ }
+ ARMARX_CHECK_EQUAL(requests.size(), entityIDs.size());
+ return requests;
+ }
+
+
+ std::vector<PredictionResult>
+ RobotStatePredictionClient::predict(const std::vector<MemoryID>& entityIDs,
+ Time predictedTime,
+ const std::string& engineID)
+ {
+ const std::vector<PredictionRequest> requests =
+ makePredictionRequests(entityIDs, predictedTime, engineID);
+ return predict(requests);
+ }
+
+
+ std::vector<PredictionResult>
+ RobotStatePredictionClient::predict(const std::vector<PredictionRequest>& requests)
+ {
+ const std::vector<PredictionResult> results = remote.reader.predict(requests);
+ ARMARX_CHECK_EQUAL(results.size(), requests.size());
+ return results;
+ }
+
+
+ std::optional<Eigen::Affine3f>
+ RobotStatePredictionClient::lookupGlobalPose(
+ const std::vector<armem::PredictionResult>& localizationPredictionResults,
+ const std::vector<armem::MemoryID>& localizationEntityIDs,
+ armem::Time predictedTime)
+ {
+ ARMARX_CHECK_EQUAL(localizationPredictionResults.size(), localizationEntityIDs.size());
+ std::stringstream errorMessages;
+
+ namespace loc = armem::common::robot_state::localization;
+
+ std::optional<armem::wm::CoreSegment> coreSegment;
+
+ loc::TransformQuery query;
+ query.header.parentFrame = armarx::GlobalFrame;
+ query.header.frame = armem::robot_state::constants::robotRootNodeName;
+ query.header.agent = "";
+ query.header.timestamp = predictedTime;
+
+ for (size_t i = 0; i < localizationPredictionResults.size(); ++i)
+ {
+ const armem::PredictionResult& result = localizationPredictionResults[i];
+ const armem::MemoryID& entityID = localizationEntityIDs.at(i);
+
+ if (result.success)
+ {
+ if (query.header.agent.empty())
+ {
+ const arondto::Transform tf = arondto::Transform::FromAron(result.prediction);
+ query.header.agent = tf.header.agent;
+ }
+ if (not coreSegment)
+ {
+ coreSegment.emplace(entityID.getCoreSegmentID());
+ }
+ {
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+ update.timeCreated = predictedTime;
+ update.instancesData = {result.prediction};
+ coreSegment->update(update);
+ }
+ }
+ else
+ {
+ errorMessages << "Prediction of '" << entityID << "'"
+ << " failed: " << result.errorMessage << "\n";
+ }
+ }
+
+ std::optional<Eigen::Affine3f> result;
+ if (coreSegment.has_value())
+ {
+ loc::TransformResult tf =
+ loc::TransformHelper::lookupTransform(coreSegment.value(), query);
+
+ if (tf)
+ {
+ result = tf.transform.transform;
+ }
+ else
+ {
+ errorMessages << "\nFailed to lookup transform: " << tf.errorMessage << "\n";
+ }
+ }
+
+ if (not errorMessages.str().empty())
+ {
+ // ToDo: Introduce an exception here?
+ ARMARX_VERBOSE << errorMessages.str();
+ }
+ return result;
+ }
+
+ std::optional<std::map<std::string, float>>
+ RobotStatePredictionClient::lookupJointPositions(
+ const std::vector<PredictionResult>& proprioceptionPredictionResults,
+ const std::string& robotName)
+ {
+ auto it = std::find_if(proprioceptionPredictionResults.begin(),
+ proprioceptionPredictionResults.end(),
+ [&robotName](const PredictionResult& result)
+ { return result.snapshotID.entityName == robotName; });
+ if (it != proprioceptionPredictionResults.end())
+ {
+ auto result = *it;
+ auto prop = armem::arondto::Proprioception::FromAron(result.prediction);
+ return prop.joints.position;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ std::optional<Eigen::Affine3f>
+ RobotStatePredictionClient::predictGlobalPose(const std::vector<MemoryID>& entityIDs,
+ Time predictedTime,
+ const std::string& engineID)
+ {
+ const std::vector<PredictionResult> results = predict(entityIDs, predictedTime, engineID);
+ std::optional<Eigen::Affine3f> pose = lookupGlobalPose(results, entityIDs, predictedTime);
+ return pose;
+ }
+
+
+ std::optional<std::map<std::string, float>>
+ RobotStatePredictionClient::predictJointPositions(const std::vector<MemoryID>& entityIDs,
+ Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID)
+ {
+ const std::vector<PredictionResult> results = predict(entityIDs, predictedTime, engineID);
+ return lookupJointPositions(results, robotName);
+ }
+
+
+ RobotStatePredictionClient::WholeBodyPrediction
+ RobotStatePredictionClient::predictWholeBody(const std::vector<armem::MemoryID>& locEntityIDs,
+ const std::vector<armem::MemoryID>& propEntityIDs,
+ Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID)
+ {
+ RobotStatePredictionClient::WholeBodyPrediction result;
+
+ // Predict.
+ const std::vector<armem::MemoryID> entityIDs =
+ simox::alg::concatenate(locEntityIDs, propEntityIDs);
+
+ if (entityIDs.empty())
+ {
+ return result;
+ }
+
+ auto _results = predict(entityIDs, predictedTime, "Linear");
+
+ // Gather results.
+ std::vector<armem::PredictionResult> locResults, propResults;
+ locResults = simox::alg::slice(_results, 0, locEntityIDs.size());
+ propResults = simox::alg::slice(_results, locEntityIDs.size());
+ ARMARX_CHECK_EQUAL(locResults.size(), locEntityIDs.size());
+ ARMARX_CHECK_EQUAL(propResults.size(), propEntityIDs.size());
+
+
+ result.globalPose = lookupGlobalPose(locResults, locEntityIDs, predictedTime);
+ result.jointPositions = lookupJointPositions(propResults, robotName);
+
+ return result;
+ }
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h
new file mode 100644
index 0000000000000000000000000000000000000000..6ef31275e1efbf7ea9f5fd43ce2fb467d54b2bd7
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h
@@ -0,0 +1,106 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+#include <optional>
+#include <vector>
+
+#include <Eigen/Geometry>
+
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
+
+namespace armarx::armem::robot_state
+{
+
+ class RobotStatePredictionClient
+ {
+ public:
+ RobotStatePredictionClient();
+
+
+ struct WholeBodyPrediction
+ {
+ std::optional<Eigen::Affine3f> globalPose;
+ std::optional<std::map<std::string, float>> jointPositions;
+ };
+ WholeBodyPrediction
+ predictWholeBody(const std::vector<armem::MemoryID>& localizationEntityIDs,
+ const std::vector<armem::MemoryID>& proprioceptionEntityIDs,
+ armem::Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID = "Linear");
+
+ std::optional<Eigen::Affine3f>
+ predictGlobalPose(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID = "Linear");
+
+ std::optional<std::map<std::string, float>>
+ predictJointPositions(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& robotName,
+ const std::string& engineID = "Linear");
+
+
+ std::vector<armem::PredictionResult> predict(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID = "Linear");
+ std::vector<armem::PredictionResult>
+ predict(const std::vector<armem::PredictionRequest>& requests);
+
+
+ std::vector<armem::MemoryID> queryLocalizationEntityIDs();
+ std::vector<armem::MemoryID> queryProprioceptionEntityIDs();
+
+ std::vector<armem::PredictionRequest>
+ makePredictionRequests(const std::vector<armem::MemoryID>& entityIDs,
+ armem::Time predictedTime,
+ const std::string& engineID = "Linear");
+
+ std::optional<Eigen::Affine3f>
+ lookupGlobalPose(const std::vector<armem::PredictionResult>& localizationPredictionResults,
+ const std::vector<armem::MemoryID>& localizationEntityIDs,
+ armem::Time predictedTime);
+
+ std::optional<std::map<std::string, float>> lookupJointPositions(
+ const std::vector<armem::PredictionResult>& proprioceptionPredictionResults,
+ const std::string& robotName);
+
+
+ private:
+ std::vector<armem::MemoryID> _queryEntityIDs(const std::string& coreSegmentName);
+
+ public:
+ struct Remote
+ {
+ armem::client::Reader reader;
+ std::optional<armem::robot_state::VirtualRobotReader> robotReader;
+ };
+ Remote remote;
+ };
+
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h
new file mode 100644
index 0000000000000000000000000000000000000000..3023fef630da40fc660dc828bd66c14c5060982d
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h
@@ -0,0 +1,31 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "Component.h"
+
+
+namespace armarx
+{
+ using RobotStatePredictionClientExample = armarx::robot_state_prediction_client_example::Component;
+}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4a56a6ca012eb3b3180a757005f9a5d5633b1f82
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp
@@ -0,0 +1,96 @@
+/*
+ * 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::RobotStatePredictionClientExample
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <map>
+#include <vector>
+
+namespace simox::alg
+{
+ template <class... Args>
+ std::vector<Args...>
+ concatenate(const std::vector<Args...>& lhs, const std::vector<Args...>& rhs)
+ {
+ std::vector<Args...> conc = lhs;
+ std::copy(rhs.begin(), rhs.end(), std::back_inserter(conc));
+ return conc;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::map<KeyT, ValueT>
+ map_from_key_value_pairs(const std::vector<KeyT>& lhs, const std::vector<ValueT>& rhs)
+ {
+ const size_t size = std::min(lhs.size(), rhs.size());
+
+ std::map<KeyT, ValueT> map;
+ for (size_t i = 0; i < size; ++i)
+ {
+ map.emplace(lhs[i], rhs[i]);
+ }
+ return map;
+ }
+
+
+ template <class KeyT, class ValueT>
+ std::vector<ValueT>
+ multi_at(const std::map<KeyT, ValueT>& map,
+ const std::vector<KeyT>& keys,
+ bool skipMissing = false)
+ {
+ std::vector<ValueT> values;
+ values.reserve(keys.size());
+
+ for (const KeyT& key : keys)
+ {
+ if (skipMissing)
+ {
+ if (auto it = map.find(key); it != map.end())
+ {
+ values.push_back(it->second);
+ }
+ }
+ else
+ {
+ // Throw an exception if missing.
+ values.push_back(map.at(key));
+ }
+ }
+
+ return values;
+ }
+
+ template <class... Args>
+ std::vector<Args...>
+ slice(const std::vector<Args...>& vector,
+ size_t start = 0,
+ std::optional<size_t> end = std::nullopt)
+ {
+ std::vector<Args...> result;
+ auto beginIt = vector.begin() + start;
+ auto endIt = end ? vector.begin() + *end : vector.end();
+ std::copy(beginIt, endIt, std::back_inserter(result));
+ return result;
+ }
+
+} // namespace simox::alg
diff --git a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
index 7045df683d1fb8d5c7fafa83a049fc7f1e9d2ea1..df79259c85fb27ef40299fabf3af03e978c7f2e5 100644
--- a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
+++ b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
@@ -21,6 +21,7 @@
#include "SimpleVirtualRobot.h"
+#include <SimoxUtility/json/json.hpp>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
@@ -53,6 +54,12 @@ namespace armarx::simple_virtual_robot
defs->optional(properties.robot.package, "p.robot.package", "Package of the Simox robot XML.");
defs->optional(properties.robot.path, "p.robot.path", "Local path of the Simox robot XML.");
+ defs->optional(properties.robot.jointValues, "p.robot.jointValues", "Specify a certain joint configuration.");
+
+ defs->optional(properties.robot.globalPositionX, "p.robot.globalPositionX", "");
+ defs->optional(properties.robot.globalPositionY, "p.robot.globalPositionY", "");
+ defs->optional(properties.robot.globalPositionYaw, "p.robot.globalPositionYaw", "");
+
return defs;
}
@@ -106,6 +113,25 @@ namespace armarx::simple_virtual_robot
VirtualRobot::RobotPtr robot =
VirtualRobot::RobotIO::loadRobot(path.toSystemPath(), VirtualRobot::RobotIO::eStructure);
+ ARMARX_CHECK_NOT_NULL(robot) << "Failed to load robot `" << path << "`.";
+
+ if(not p.jointValues.empty())
+ {
+ ARMARX_DEBUG << "Parsing: " << p.jointValues;
+
+ const nlohmann::json j = nlohmann::json::parse(p.jointValues);
+ ARMARX_DEBUG << "JSON parsed as: " << j;
+
+ std::map<std::string, float> jointValues;
+ nlohmann::from_json(j, jointValues);
+
+ ARMARX_VERBOSE << "The following joint values are given by the user: " << jointValues;
+ robot->setJointValues(jointValues);
+ }
+
+ const Eigen::Isometry3f global_T_robot = Eigen::Translation3f{properties.robot.globalPositionX, properties.robot.globalPositionY, 0} * Eigen::AngleAxisf{properties.robot.globalPositionYaw, Eigen::Vector3f::UnitZ()};
+ robot->setGlobalPose(global_T_robot.matrix());
+
if (not p.name.empty())
{
robot->setName(p.name);
diff --git a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
index d1680923b5109098622b3030c8569f1633540e0f..d960e0ff3955b8159c63ed9c6e1eff465e26a3a2 100644
--- a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
+++ b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
@@ -60,6 +60,12 @@ namespace armarx::simple_virtual_robot
std::string name;
std::string package;
std::string path;
+
+ std::string jointValues; // json-style map<std::string, float>
+
+ float globalPositionX = 0;
+ float globalPositionY = 0;
+ float globalPositionYaw = 0;
};
Robot robot;
};
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
index 6aab4a4ff38ce842d3c00f4d57bceec3cd5f5672..f3d1b870cecb1100c521bf9017120f0b25a738fa 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -2,36 +2,15 @@
#include "VirtualRobotReaderExampleClient.h"
-
-#include <memory>
-
-#include <Eigen/Geometry>
-
-#include <IceUtil/Time.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+
#include <RobotAPI/libraries/armem/core/Time.h>
namespace armarx::robot_state
{
- VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() :
- virtualRobotReader(this->memoryNameSystem()) {}
+ VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() = default;
armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
{
@@ -40,63 +19,95 @@ namespace armarx::robot_state
defs->topic(debugObserver);
- defs->optional(p.robotName, "robotName");
- defs->optional(p.updateFrequency, "updateFrequency");
-
- virtualRobotReader.registerPropertyDefinitions(defs);
+ defs->optional(properties.robotName, "p.robotName",
+ "The name of the robot to use.");
+ defs->optional(properties.updateFrequency, "p.updateFrequency [Hz]",
+ "The frequency of the running loop.");
return defs;
}
+
std::string VirtualRobotReaderExampleClient::getDefaultName() const
{
return "VirtualRobotReaderExampleClient";
}
- void VirtualRobotReaderExampleClient::onInitComponent() {}
- void VirtualRobotReaderExampleClient::onConnectComponent()
+ void VirtualRobotReaderExampleClient::onInitComponent()
{
- virtualRobotReader.connect();
+ }
+
+ void VirtualRobotReaderExampleClient::onConnectComponent()
+ {
ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
- task = new PeriodicTask<VirtualRobotReaderExampleClient>(this, &VirtualRobotReaderExampleClient::run, 1000 / p.updateFrequency);
+ task = new SimpleRunningTask<>([this]()
+ {
+ this->run();
+ });
task->start();
}
+
void VirtualRobotReaderExampleClient::onDisconnectComponent()
{
task->stop();
}
- void VirtualRobotReaderExampleClient::onExitComponent() {}
- void VirtualRobotReaderExampleClient::run()
+ void VirtualRobotReaderExampleClient::onExitComponent()
{
- const armem::Time now = armem::Time::Now();
+ }
- // initialize if needed
- if (virtualRobot == nullptr)
- {
- TIMING_START(getRobot);
- virtualRobot = virtualRobotReader.getRobot(p.robotName, now);
+ void VirtualRobotReaderExampleClient::run()
+ {
+ Metronome metronome(Frequency::Hertz(properties.updateFrequency));
+ while (task and not task->isStopped())
+ {
+ const armem::Time now = armem::Time::Now();
- if (virtualRobot == nullptr)
+ // Initialize the robot if needed.
+ if (robot == nullptr)
{
- ARMARX_WARNING << deactivateSpam(1) << "Could not create virtual robot.";
- return;
+ // The TIMING_* macros are optional, you do not need them.
+ TIMING_START(getRobot);
+
+ robot = virtualRobotReaderPlugin->get().getRobot(properties.robotName, now);
+ if (robot)
+ {
+ // Only print timing once the robot is loadable & loaded.
+ TIMING_END_STREAM(getRobot, ARMARX_INFO);
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam(10) << "Could not create virtual robot.";
+ }
}
- // only print timing once the robot is loadable & loaded
- TIMING_END_STREAM(getRobot, ARMARX_INFO);
- }
+ if (robot)
+ {
+ ARMARX_INFO << deactivateSpam(60) << "Synchronizing robot.";
- ARMARX_INFO << deactivateSpam(10) << "Synchronizing robot";
+ TIMING_START(synchronizeRobot);
+ ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, now));
+ TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
- TIMING_START(synchronizeRobot);
- ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*virtualRobot, now));
- TIMING_END_STREAM(synchronizeRobot, ARMARX_INFO);
+
+ // Do something with the robot (your code follows here, there are just a examples) ...
+
+ Eigen::Matrix4f globalPose = robot->getGlobalPose();
+ (void) globalPose;
+
+ std::vector<std::string> nodeNames = robot->getRobotNodeNames();
+ (void) nodeNames;
+
+ // End.
+ }
+
+ metronome.waitForNextTick();
+ }
}
} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
index 322362fe4ea476c0701bfe137ab4cc1eaea08ec5..328f847e8a36655d24ba68ee09b13fd189cb0d02 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
@@ -22,21 +22,15 @@
#pragma once
-
-// ArmarX
-#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <ArmarXCore/util/tasks.h>
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-// RobotAPI
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
+
namespace armarx::robot_state
{
@@ -53,6 +47,7 @@ namespace armarx::robot_state
*/
class VirtualRobotReaderExampleClient :
virtual public armarx::Component,
+ // Use the memory client plugin.
virtual public armarx::armem::client::ComponentPluginUser
{
public:
@@ -70,6 +65,9 @@ namespace armarx::robot_state
void onDisconnectComponent() override;
void onExitComponent() override;
+
+ private:
+
void run();
@@ -79,17 +77,24 @@ namespace armarx::robot_state
{
std::string robotName{"Armar6"};
float updateFrequency{10.F};
- } p;
+ };
+ Properties properties;
- armarx::PeriodicTask<VirtualRobotReaderExampleClient>::pointer_type task;
+ // The running task which is started in onConnectComponent().
+ armarx::SimpleRunningTask<>::pointer_type task;
- armarx::DebugObserverInterfacePrx debugObserver;
+ // The reader used to get the robot state.
+ template <typename T>
+ using ReaderWriterPlugin = armem::client::plugins::ReaderWriterPlugin<T>;
- // std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+ ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
+ nullptr;
- armem::robot_state::VirtualRobotReader virtualRobotReader;
+ // The Simox robot model.
+ VirtualRobot::RobotPtr robot = nullptr;
- std::shared_ptr<VirtualRobot::Robot> virtualRobot{nullptr};
+ // For publishing timing information.
+ armarx::DebugObserverInterfacePrx debugObserver;
};
diff --git a/source/RobotAPI/components/armem/server/CMakeLists.txt b/source/RobotAPI/components/armem/server/CMakeLists.txt
index 7ddc79776f7e56d2c71596f4e3c0c5656f18875f..e652a1057af76e89e465615ea152beaab0829f3d 100644
--- a/source/RobotAPI/components/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/server/CMakeLists.txt
@@ -1,10 +1,12 @@
add_subdirectory(ExampleMemory)
add_subdirectory(GeneralPurposeMemory)
+add_subdirectory(GraspMemory)
+add_subdirectory(IndexMemory)
+add_subdirectory(MotionMemory)
add_subdirectory(ObjectMemory)
add_subdirectory(ReasoningMemory)
add_subdirectory(RobotStateMemory)
add_subdirectory(SkillsMemory)
-add_subdirectory(GraspMemory)
+add_subdirectory(LaserScansMemory)
#add_subdirectory(SubjectMemory)
-add_subdirectory(MotionMemory)
add_subdirectory(SystemStateMemory)
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index e2059d968293dce364030082be4455186400ce0b..77135658d4a93d47e0b4474002b589cf569de67f 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -73,7 +73,10 @@ namespace armarx
workingMemory().addCoreSegment("LinkedData", armem::example::LinkedData::ToAronType());
// We support the "Latest" prediction engine for the entire memory.
- workingMemory().addPredictionEngine({"Latest"});
+ workingMemory().addPredictor(
+ armem::PredictionEngine{.engineID = "Latest"},
+ [this](const armem::PredictionRequest& request)
+ { return this->predictLatest(request); });
// For illustration purposes, we add more segments (without types).
bool trim = true;
@@ -216,66 +219,44 @@ namespace armarx
}
// PREDICTING
-
- armem::prediction::data::PredictionResultSeq
- ExampleMemory::predict(const armem::prediction::data::PredictionRequestSeq& requests)
- {
- armem::prediction::data::PredictionResultSeq result;
- for (const auto& request : requests)
- {
- result.push_back(predictSingle(request));
- }
- return result;
- }
-
- armem::prediction::data::PredictionResult
- ExampleMemory::predictSingle(const armem::prediction::data::PredictionRequest& request)
+ armem::PredictionResult
+ ExampleMemory::predictLatest(const armem::PredictionRequest& request)
{
armem::PredictionResult result;
-
- std::string engine = request.settings.predictionEngineID;
- if (engine.empty() || engine == "Latest")
+ auto memID = request.snapshotID;
+ result.snapshotID = memID;
+
+ armem::client::QueryBuilder builder;
+ builder.latestEntitySnapshot(memID);
+ auto queryResult =
+ query(armarx::toIce<armem::query::data::Input>(builder.buildQueryInput()));
+ if (queryResult.success)
{
- auto boID = fromIce<armem::MemoryID>(request.snapshotID);
- armem::client::QueryBuilder builder;
- builder.latestEntitySnapshot(boID);
- auto queryResult =
- query(armarx::toIce<armem::query::data::Input>(builder.buildQueryInput()));
- if (queryResult.success)
+ auto readMemory = fromIce<armem::wm::Memory>(queryResult.memory);
+ auto* latest = readMemory.findLatestSnapshot(memID);
+ if (latest != nullptr)
{
- auto readMemory = fromIce<armem::wm::Memory>(queryResult.memory);
- auto* latest = readMemory.findLatestSnapshot(boID);
- if (latest != nullptr)
- {
- auto instance = boID.hasInstanceIndex()
- ? latest->getInstance(boID)
- : latest->getInstance(latest->getInstanceIndices().at(0));
- result.success = true;
- result.snapshotID = boID;
- result.prediction = instance.data();
- }
- else
- {
- result.success = false;
- result.errorMessage =
- "Could not find entity referenced by MemoryID '" + boID.str() + "'.";
- }
+ auto instance = memID.hasInstanceIndex()
+ ? latest->getInstance(memID)
+ : latest->getInstance(latest->getInstanceIndices().at(0));
+ result.success = true;
+ result.prediction = instance.data();
}
else
{
result.success = false;
result.errorMessage =
- "Could not find entity referenced by MemoryID '" + boID.str() + "'.";
+ "Could not find entity referenced by MemoryID '" + memID.str() + "'.";
}
}
else
{
result.success = false;
result.errorMessage =
- "This memory does not support the prediction engine '" + engine + "'.";
+ "Could not find entity referenced by MemoryID '" + memID.str() + "'.";
}
- return result.toIce();
+ return result;
}
// REMOTE GUI
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
index 76e437bcb6f6db0e31c023eed4386831267c5fc2..b41f1262755f8ddad59eb9dc3c03309766183116 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
@@ -28,6 +28,7 @@
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
@@ -71,11 +72,6 @@ namespace armarx
armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& input) override;
armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& input) override;
- // PredictingMemoryInterface interface
- public:
- armem::prediction::data::PredictionResultSeq
- predict(const armem::prediction::data::PredictionRequestSeq& requests) override;
-
protected:
@@ -89,8 +85,7 @@ namespace armarx
private:
- armem::prediction::data::PredictionResult
- predictSingle(const armem::prediction::data::PredictionRequest& request);
+ armem::PredictionResult predictLatest(const armem::PredictionRequest& request);
armarx::DebugObserverInterfacePrx debugObserver;
diff --git a/source/RobotAPI/components/armem/server/IndexMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/IndexMemory/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..81badbec270e5237060db403832a6f1ccff6c557
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/IndexMemory/CMakeLists.txt
@@ -0,0 +1,22 @@
+armarx_component_set_name(IndexMemory)
+
+set(COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXGuiComponentPlugins
+
+ # RobotAPI
+ armem_index_server
+)
+
+set(SOURCES
+ IndexMemory.cpp
+)
+set(HEADERS
+ IndexMemory.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+# Generate the application
+armarx_generate_and_add_component_executable()
diff --git a/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.cpp b/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dffd3d2cb5ae44c0f95830ef780c0494253d8830
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.cpp
@@ -0,0 +1,115 @@
+/*
+ * 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::IndexMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "IndexMemory.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem_index/aron/Named.aron.generated.h>
+#include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
+#include <RobotAPI/libraries/armem_index/memory_ids.h>
+
+
+namespace armarx
+{
+
+ armarx::PropertyDefinitionsPtr
+ IndexMemory::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ setMemoryName(armem::index::memoryID.memoryName);
+
+ defs->optional(properties.maxHistorySize, "p.maxHistorySize", "The maximum size of entity histories.")
+ .setMin(1);
+
+ return defs;
+ }
+
+
+ std::string
+ IndexMemory::getDefaultName() const
+ {
+ return "IndexMemory";
+ }
+
+
+ void
+ IndexMemory::onInitComponent()
+ {
+ size_t maxHistorySize = static_cast<size_t>(std::max(1, properties.maxHistorySize));
+ workingMemory()
+ .addCoreSegment(armem::index::namedSegmentID.coreSegmentName,
+ armem::index::arondto::Named::ToAronType())
+ .setMaxHistorySize(maxHistorySize);
+
+ workingMemory()
+ .addCoreSegment(armem::index::spatialSegmentID.coreSegmentName,
+ armem::index::arondto::Spatial::ToAronType())
+ .setMaxHistorySize(maxHistorySize);
+ }
+
+
+ void
+ IndexMemory::onConnectComponent()
+ {
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+ }
+
+
+ void
+ IndexMemory::onDisconnectComponent()
+ {
+ }
+
+
+ void
+ IndexMemory::onExitComponent()
+ {
+ }
+
+
+ void
+ IndexMemory::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ {
+ }
+
+ VBoxLayout root = {VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void
+ IndexMemory::RemoteGui_update()
+ {
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.h b/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.h
new file mode 100644
index 0000000000000000000000000000000000000000..b272fcafc11e7d398b95175a26d1fcbf5fdf75d6
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.h
@@ -0,0 +1,83 @@
+/*
+ * 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::IndexMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+
+
+namespace armarx
+{
+ /**
+ * @defgroup Component-IndexMemory IndexMemory
+ * @ingroup RobotAPI-Components
+ * A description of the component IndexMemory.
+ *
+ * @class IndexMemory
+ * @ingroup Component-IndexMemory
+ * @brief Brief description of class IndexMemory.
+ *
+ * Detailed description of class IndexMemory.
+ */
+ class IndexMemory :
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser,
+ virtual public LightweightRemoteGuiComponentPluginUser
+ {
+ public:
+ 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;
+
+
+ private:
+ struct Properties
+ {
+ int maxHistorySize = 1024;
+ };
+ Properties properties;
+
+
+ struct RemoteGuiTab : RemoteGui::Client::Tab
+ {
+ };
+ RemoteGuiTab tab;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt b/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a94f44f2160ec2ee4ab67618a8552ffdbdf8ed28
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/LaserScansMemory/CMakeLists.txt
@@ -0,0 +1,29 @@
+armarx_component_set_name("LaserScansMemory")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
+ ArmarXCoreComponentPlugins
+ ArmarXGuiComponentPlugins
+ RobotAPICore RobotAPIInterfaces armem_server
+ RobotAPIComponentPlugins # for ArViz and other plugins
+ armem_robot_state
+ armem_robot
+
+ armem_laser_scans
+
+ ${IVT_LIBRARIES}
+)
+
+set(SOURCES
+ LaserScansMemory.cpp
+)
+set(HEADERS
+ LaserScansMemory.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#generate the application
+armarx_generate_and_add_component_executable(COMPONENT_NAMESPACE armarx::armem::server::laser_scans)
diff --git a/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.cpp b/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63d5bdbd644a75960a9c3824d870faf69f1eb476
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.cpp
@@ -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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "LaserScansMemory.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "RobotAPI/libraries/armem/server/MemoryToIceAdapter.h"
+#include "RobotAPI/libraries/armem_laser_scans/constants.h"
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+
+
+namespace armarx::armem::server::laser_scans
+{
+ LaserScansMemory::LaserScansMemory()
+ {
+ addPlugin(virtualRobotReaderPlugin);
+ addPlugin(debugObserver);
+ }
+
+ armarx::PropertyDefinitionsPtr
+ LaserScansMemory::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ workingMemory().name() = "LaserScans";
+
+ {
+ const std::string prefix = "";
+ commonVisu.defineProperties(defs, prefix + "visu.");
+ }
+
+ return defs;
+ }
+
+
+ std::string
+ LaserScansMemory::getDefaultName() const
+ {
+ return "LaserScansMemory";
+ }
+
+
+ void
+ LaserScansMemory::onInitComponent()
+ {
+ const auto& coreSegment = workingMemory().addCoreSegment(
+ "LaserScans", armarx::armem::laser_scans::arondto::LaserScanStamped::ToAronType());
+
+ commonVisu.init(&coreSegment, &virtualRobotReaderPlugin->get());
+ }
+
+
+ void
+ LaserScansMemory::onConnectComponent()
+ {
+ commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
+ }
+
+
+ void
+ LaserScansMemory::onDisconnectComponent()
+ {
+ }
+
+
+ void
+ LaserScansMemory::onExitComponent()
+ {
+ }
+
+
+ armem::data::AddSegmentsResult
+ LaserScansMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
+ {
+ // Allowing adding core segments.
+ armem::data::AddSegmentsResult result =
+ ReadWritePluginUser::addSegments(input, addCoreSegmentOnUsage);
+ return result;
+ }
+
+} // namespace armarx::armem::server::laser_scans
diff --git a/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.h b/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b1275985b57f5d3bf8ad6bcfd939c186f7462ba
--- /dev/null
+++ b/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.h
@@ -0,0 +1,89 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_laser_scans/server/Visu.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+
+namespace armarx::armem::server::laser_scans
+{
+ /**
+ * @defgroup Component-GeneralPurposeMemory GeneralPurposeMemory
+ * @ingroup RobotAPI-Components
+ * A description of the component GeneralPurposeMemory.
+ *SpecializedCoreSegment
+ * @class GeneralPurposeMemory
+ * @ingroup Component-GeneralPurposeMemory
+ * @brief Brief description of class GeneralPurposeMemory.
+ *
+ * Detailed description of class GeneralPurposeMemory.
+ */
+ class LaserScansMemory :
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser,
+ virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+ LaserScansMemory();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input,
+ const Ice::Current&) override;
+
+ protected:
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+ private:
+ bool addCoreSegmentOnUsage = true;
+
+ armem::client::plugins::ReaderWriterPlugin<armarx::armem::robot_state::VirtualRobotReader>*
+ virtualRobotReaderPlugin = nullptr;
+
+ armarx::plugins::DebugObserverComponentPlugin* debugObserver = nullptr;
+
+
+ Visu commonVisu;
+ };
+} // namespace armarx::armem::server::laser_scans
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index fd3e2cdd938ca611c53ed13cf783744f399a8080..6fe11f3196b211677110b31dd697a3eb4f055d5d 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -32,20 +32,19 @@
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
namespace armarx::armem::server::obj
{
- const std::string ObjectMemory::defaultMemoryName = "Object";
-
-
armarx::PropertyDefinitionsPtr ObjectMemory::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
const std::string prefix = "mem.";
- workingMemory().name() = defaultMemoryName;
+ workingMemory().name() = objects::memoryID.memoryName;
classSegment.defineProperties(defs, prefix + "cls.");
instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
@@ -67,6 +66,9 @@ namespace armarx::armem::server::obj
"Duration of time window into the past to use for predictions"
" when requested via the PredictingMemoryInterface (in seconds).");
+ defs->optional(p.markerMemoryName, prefix + ".marker.Name", "Marker Memory Name");
+ defs->optional(p.maxMarkerHistorySize, prefix + ".marker.maxHistorySize", "Maximum marker memory history size");
+
return defs;
}
@@ -126,6 +128,13 @@ namespace armarx::armem::server::obj
{
attachmentSegment.init();
});
+
+ initSegmentWithCatch(p.markerMemoryName, [&]()
+ {
+ workingMemory()
+ .addCoreSegment(p.markerMemoryName, arondto::Marker::ToAronType())
+ .setMaxHistorySize(p.maxMarkerHistorySize);
+ });
}
@@ -288,9 +297,7 @@ namespace armarx::armem::server::obj
{
result.success = false;
result.errorMessage << "No predictions are supported for MemoryID "
- << boRequest.snapshotID
- << ". Have you given an instance index if requesting"
- << " an object pose prediction?";
+ << boRequest.snapshotID;
}
results.push_back(result.toIce());
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 4eb71acf3b9374eff330ae463acc208872a918f2..566d1fec8a6ea1fe39f12edabbaaaa76b08ea9e0 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -22,8 +22,8 @@
#pragma once
-#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
-#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
@@ -66,11 +66,6 @@ namespace armarx::armem::server::obj
, virtual public armarx::LightweightRemoteGuiComponentPluginUser
, virtual public armarx::ArVizComponentPluginUser
{
- public:
-
- static const std::string defaultMemoryName;
-
-
public:
ObjectMemory();
@@ -134,6 +129,12 @@ namespace armarx::armem::server::obj
};
std::unique_ptr<RemoteGuiTab> tab;
+ struct Properties {
+ int64_t maxMarkerHistorySize = -1;
+ std::string markerMemoryName = "Marker";
+ };
+ Properties p;
+
armem::client::plugins::ReaderWriterPlugin<robot_state::VirtualRobotReader>* virtualRobotReaderPlugin;
};
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index fc71209b76d3b0c356df7a9895e5af95ba43faaf..f77e01607b0036a2f9525b88dc919f0f1882608b 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -21,21 +21,21 @@
*/
#include "RobotStateMemory.h"
-#include "RobotAPI/libraries/armem/core/forward_declarations.h"
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
-
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <IceUtil/Time.h>
-
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/contains.h>
#include <SimoxUtility/algorithm/string.h>
@@ -47,6 +47,7 @@ namespace armarx::armem::server::robot_state
RobotStateMemory::RobotStateMemory() :
descriptionSegment(iceAdapter()),
proprioceptionSegment(iceAdapter()),
+ exteroceptionSegment(iceAdapter()),
localizationSegment(iceAdapter()),
commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
@@ -83,9 +84,10 @@ namespace armarx::armem::server::robot_state
const std::string prefix = "mem.";
- setMemoryName("RobotState");
+ setMemoryName(armem::robot_state::memoryID.memoryName);
descriptionSegment.defineProperties(defs, prefix + "desc.");
+ exteroceptionSegment.defineProperties(defs, prefix + "ext.");
proprioceptionSegment.defineProperties(defs, prefix + "prop.");
localizationSegment.defineProperties(defs, prefix + "loc.");
commonVisu.defineProperties(defs, prefix + "visu.");
@@ -104,6 +106,7 @@ namespace armarx::armem::server::robot_state
{
descriptionSegment.init();
proprioceptionSegment.init();
+ exteroceptionSegment.init();
localizationSegment.init();
commonVisu.init();
@@ -160,15 +163,12 @@ namespace armarx::armem::server::robot_state
robotUnit.reader.task = new SimpleRunningTask<>([this]()
{
- robotUnit.reader.run(
- robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex
- );
+ robotUnit.reader.run(robotUnit.pollFrequency, robotUnit.dataBuffer);
}, "Robot Unit Reader");
robotUnit.writer.task = new SimpleRunningTask<>([this]()
{
robotUnit.writer.run(
- robotUnit.pollFrequency, robotUnit.dataQueue, robotUnit.dataMutex,
- iceAdapter(), localizationSegment
+ robotUnit.pollFrequency, robotUnit.dataBuffer, iceAdapter(), localizationSegment
);
}, "Robot State Writer");
@@ -204,7 +204,6 @@ namespace armarx::armem::server::robot_state
return { new Pose(poseMap[robotName].matrix()) };
}
-
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index d402496819431eded99b506b69fbab48e6804086..70513a0cf56ff0cd6d82603cc3266fb405e5af3d 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -27,26 +27,25 @@
#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/core/RobotLocalization.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
-#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
-
-#include <RobotAPI/interface/core/RobotLocalization.h>
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h>
namespace armarx::plugins
{
class DebugObserverComponentPlugin;
class RobotUnitComponentPlugin;
-}
+} // namespace armarx::plugins
namespace armarx::armem::server::robot_state
{
@@ -68,7 +67,6 @@ namespace armarx::armem::server::robot_state
virtual public armarx::GlobalRobotPoseProvider
{
public:
-
RobotStateMemory();
virtual ~RobotStateMemory() override;
@@ -77,11 +75,12 @@ namespace armarx::armem::server::robot_state
// GlobalRobotPoseProvider interface
- armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
+ armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp,
+ const std::string& robotName,
+ const ::Ice::Current&) override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -91,13 +90,11 @@ namespace armarx::armem::server::robot_state
private:
-
void startRobotUnitStream();
void stopRobotUnitStream();
private:
-
armarx::plugins::DebugObserverComponentPlugin* debugObserver = nullptr;
@@ -109,6 +106,9 @@ namespace armarx::armem::server::robot_state
proprioception::Segment proprioceptionSegment;
armem::data::MemoryID robotUnitProviderID;
+ // - exteroception
+ exteroception::Segment exteroceptionSegment;
+
// - localization
localization::Segment localizationSegment;
@@ -129,12 +129,12 @@ namespace armarx::armem::server::robot_state
proprioception::RobotStateWriter writer;
// queue
- std::queue<proprioception::RobotUnitData> dataQueue;
- std::mutex dataMutex;
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
+ Queue dataBuffer;
- bool waitForRobotUnit = false;
+ bool waitForRobotUnit = false;
};
RobotUnit robotUnit;
};
-} // namespace armarx::armem::server::robot_state
+} // namespace armarx::armem::server::robot_state
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
index bc50155edc0d42a775c63ade67e35f38e34fd8c5..985ef8171792570e6dad35d7a0abb09d78492006 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.cpp
@@ -20,6 +20,8 @@ namespace armarx::skills::provider
default_params.some_float = 5;
default_params.some_int = 42;
default_params.some_text = "YOLO";
+ default_params.some_list_of_matrices.push_back(Eigen::Matrix3f::Zero());
+ //default_params.some_matrix = Eigen::Matrix3f::Zero();
return SkillDescription{
"HelloWorld",
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
index 553a93ad0898ddc40d112e097af9c95ca801da55..a2423c560b8548c265af0ae14602b8771fe46218 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
+++ b/source/RobotAPI/components/skills/SkillProviderExample/aron/HelloWorldAcceptedType.xml
@@ -1,6 +1,17 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<GenerateTypes>
+ <IntEnum name="HelloWorldInteEnum">
+ <EnumValue key="INT_ENUM_VALUE_0" value="0" />
+ <EnumValue key="INT_ENUM_VALUE_1" value="1" />
+ <EnumValue key="INT_ENUM_VALUE_2" value="2" />
+ <EnumValue key="INT_ENUM_VALUE_3" value="3" />
+ <EnumValue key="INT_ENUM_VALUE_4" value="4" />
+ <EnumValue key="INT_ENUM_VALUE_5" value="5" />
+ <EnumValue key="INT_ENUM_VALUE_6" value="6" />
+ <EnumValue key="INT_ENUM_VALUE_7" value="7" />
+ <EnumValue key="INT_ENUM_VALUE_8" value="8" />
+ </IntEnum>
<Object name='armarx::skills::Example::HelloWorldAcceptedType'>
<ObjectChild key='some_int'>
<Int />
@@ -11,6 +22,23 @@
<ObjectChild key='some_text'>
<String />
</ObjectChild>
+
+ <ObjectChild key='some_list_of_matrices'>
+ <List>
+ <Matrix rows="3" cols="3" type="float32" />
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='some_dict_of_matrices'>
+ <Dict>
+ <Matrix rows="3" cols="3" type="float32" />
+ </Dict>
+ </ObjectChild>
+
+ <ObjectChild key='some_matrix'>
+ <Matrix rows="3" cols="3" type="float32" />
+ </ObjectChild>
+
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/components/units/HandUnit.h b/source/RobotAPI/components/units/HandUnit.h
index 3816605266f1bc44dce40a752bfe94b14af8177b..0241caff84c93709619c0bcabae2736d443e5c1b 100644
--- a/source/RobotAPI/components/units/HandUnit.h
+++ b/source/RobotAPI/components/units/HandUnit.h
@@ -172,7 +172,7 @@ namespace armarx
// HandUnitInterface interface
public:
- std::string getHandName(const Ice::Current&) override;
+ std::string getHandName(const Ice::Current& = Ice::emptyCurrent) override;
void setJointForces(const NameValueMap& targetJointForces, const Ice::Current&) override;
void sendJointCommands(const NameCommandMap& targetJointCommands, const Ice::Current&) override;
};
diff --git a/source/RobotAPI/components/units/PlatformUnit.cpp b/source/RobotAPI/components/units/PlatformUnit.cpp
index 83da0bb8486a38a075e10c9c6fae5a273c254f51..a5414a34e838b6fd2268f7bfb26ddc527530624f 100644
--- a/source/RobotAPI/components/units/PlatformUnit.cpp
+++ b/source/RobotAPI/components/units/PlatformUnit.cpp
@@ -38,7 +38,7 @@ namespace armarx
def->topic(odometryPrx);
def->topic(globalPosePrx);
- // def->topic(listenerPrx, listenerChannelName, "");
+ def->topic(listenerPrx, "PlatformState");
def->component(robotStateComponent);
@@ -50,17 +50,12 @@ namespace armarx
{
std::string platformName = getProperty<std::string>("PlatformName").getValue();
- listenerChannelName = platformName + "State";
- offeringTopic(listenerChannelName);
-
this->onInitPlatformUnit();
}
void PlatformUnit::onConnectComponent()
{
- listenerPrx = getTopic<PlatformUnitListenerPrx>(listenerChannelName);
-
this->onStartPlatformUnit();
}
diff --git a/source/RobotAPI/components/units/PlatformUnit.h b/source/RobotAPI/components/units/PlatformUnit.h
index 1b0b9499a4f50e6ec0b64631aaf9f85c9fa5bca5..ee3c2a6999c0ba4f1f9560106f818826162c432f 100644
--- a/source/RobotAPI/components/units/PlatformUnit.h
+++ b/source/RobotAPI/components/units/PlatformUnit.h
@@ -117,7 +117,7 @@ namespace armarx
protected:
- std::string listenerChannelName;
+ // std::string listenerChannelName;
/**
* PlatformUnitListener proxy for publishing state updates
*/
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.cpp b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
index d0e0cd8e0162983b77708d5499b60c6e012d71ba..2db1b8dae17b84c6d0ef98fea1129cebd97f9e75 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
@@ -21,6 +21,8 @@
*/
#include "PlatformUnitObserver.h"
+#include <Eigen/src/Geometry/Transform.h>
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include "PlatformUnit.h"
@@ -54,13 +56,13 @@ namespace armarx
offerConditionCheck("smaller", new ConditionCheckSmaller());
usingTopic(platformNodeName + "State");
+ usingTopic("GlobalRobotPoseLocalization");
}
void PlatformUnitObserver::onConnectObserver()
{
// register all channels
offerChannel("platformPose", "Current Position of " + platformNodeName);
- offerChannel("targetPose", "Target Position of " + platformNodeName);
offerChannel("platformVelocity", "Current velocity of " + platformNodeName);
offerChannel("platformOdometryPose", "Current Odometry Position of " + platformNodeName);
@@ -69,10 +71,6 @@ namespace armarx
offerDataField("platformPose", "positionY", VariantType::Float, "Current Y position of " + platformNodeName + " in mm");
offerDataField("platformPose", "rotation", VariantType::Float, "Current Rotation of " + platformNodeName + " in radian");
- offerDataField("targetPose", "positionX", VariantType::Float, "Target X position of " + platformNodeName + " in mm");
- offerDataField("targetPose", "positionY", VariantType::Float, "Target Y position of " + platformNodeName + " in mm");
- offerDataField("targetPose", "rotation", VariantType::Float, "Target Rotation of " + platformNodeName + " in radian");
-
offerDataField("platformVelocity", "velocityX", VariantType::Float, "Current X velocity of " + platformNodeName + " in mm/s");
offerDataField("platformVelocity", "velocityY", VariantType::Float, "Current Y velocity of " + platformNodeName + " in mm/s");
offerDataField("platformVelocity", "velocityRotation", VariantType::Float, "Current Rotation velocity of " + platformNodeName + " in radian/s");
@@ -86,16 +84,16 @@ namespace armarx
}
- void PlatformUnitObserver::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
+ void PlatformUnitObserver::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&)
{
- nameValueMapToDataFields("platformPose", currentPose.x, currentPose.y, currentPose.rotationAroundZ);
- updateChannel("platformPose");
- }
+ const Eigen::Isometry3f global_T_robot(transformStamped.transform);
+
+ const float x = global_T_robot.translation().x();
+ const float y = global_T_robot.translation().y();
+ const float rotationAroundZ = simox::math::mat3f_to_rpy(global_T_robot.linear()).z();
- void PlatformUnitObserver::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
- {
- nameValueMapToDataFields("targetPose", newPlatformPositionX, newPlatformPositionY, newPlatformRotation);
- updateChannel("targetPose");
+ nameValueMapToDataFields("platformPose", x, y, rotationAroundZ);
+ updateChannel("platformPose");
}
void PlatformUnitObserver::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
@@ -128,4 +126,6 @@ namespace armarx
nameValueMapToDataFields("platformOdometryPose", x, y, angle);
updateChannel("platformOdometryPose");
}
+
+
}
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.h b/source/RobotAPI/components/units/PlatformUnitObserver.h
index 66b24989d267e30fb9a6f1ebe3e3f3bfa3a0dced..20cdfefcd7c380eccf2e1144b3d4206b239cf088 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.h
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.h
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/core/RobotLocalization.h>
#include <RobotAPI/interface/observers/PlatformUnitObserverInterface.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
@@ -78,11 +79,13 @@ namespace armarx
void onConnectObserver() override;
// slice interface implementation
- void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&) override;
+ // slice interface implementation
+ void reportGlobalRobotPose(const ::armarx::TransformStamped&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
+
+
std::string getDefaultName() const override
{
return "PlatformUnitObserver";
@@ -143,6 +146,5 @@ namespace armarx
namespace armarx::channels::PlatformUnitObserver
{
const PlatformUnitDatafieldCreator platformPose("platformPose");
- const PlatformUnitDatafieldCreator targetPose("targetPose");
const PlatformUnitDatafieldCreator platformVelocity("platformVelocity");
}
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 3c9cde6b67a1106976beee9032795262755a4be3..6abaa676025df57bbfbbde4f61ad147ac54e97eb 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -103,8 +103,6 @@ namespace armarx
globalPosePrx->reportGlobalRobotPose(currentPose);
- // legacy
- listenerPrx->reportPlatformPose(toPlatformPose(currentPose));
simulationTask->start();
}
@@ -229,8 +227,17 @@ namespace armarx
odometryPrx->reportOdometryPose(platformPose);
odometryPrx->reportOdometryVelocity(platformVelocity);
- // legacy
- listenerPrx->reportPlatformPose(toPlatformPose(platformPose));
+ {
+ TransformStamped currentPose;
+ currentPose.header.parentFrame = GlobalFrame;
+ currentPose.header.frame = robotRootFrame;
+ currentPose.header.agent = agentName;
+ currentPose.header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ currentPose.transform = currentPlatformPose();
+
+ globalPosePrx->reportGlobalRobotPose(currentPose);
+ }
+
// legacy
const auto& velo = platformVelocity.twist;
@@ -259,9 +266,6 @@ namespace armarx
TransformStamped targetPose;
targetPose.header = header;
targetPose.transform = VirtualRobot::MathTools::posrpy2eigen4f(targetPositionX, targetPositionY, 0, 0, 0, targetRotation);
-
- const auto tp = toPlatformPose(targetPose);
- listenerPrx->reportNewTargetPose(tp.x, tp.y, tp.rotationAroundZ);
}
void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 92f65d1941cd1c88dde3ace948d0c37a85d4ab60..0f45fe1f7bbd22ff33afc7859f595ecdc952ab60 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -389,6 +389,8 @@ namespace armarx::RobotUnitModule
DataStreamingEntry& streamingEntry = rtDataStreamingEntry[receiver];
getProperty(streamingEntry.rtStreamMaxClientErrors,
"RTLogging_StreamingDataMaxClientConnectionFailures");
+
+ getProperty(logAllEntries, "RTLogging_LogAllMessages");
ARMARX_INFO << "start data streaming to " << receiver->ice_getIdentity().name
<< ". Values: " << config.loggingNames;
@@ -575,14 +577,29 @@ namespace armarx::RobotUnitModule
<< '\n'
<< "Number of streams " << rtDataStreamingEntry.size();
}
- _module<ControlThreadDataBuffer>()
- .getControlThreadOutputBuffer()
- .foreachNewLoggingEntry(
- [&](const auto& data, auto i, auto num)
- {
- ARMARX_TRACE;
- doLogging(dlogdurs, now, data, i, num);
- });
+
+
+ if(logAllEntries)
+ {
+ _module<ControlThreadDataBuffer>()
+ .getControlThreadOutputBuffer()
+ .foreachNewLoggingEntry(
+ [&](const auto& data, auto i, auto num)
+ {
+ ARMARX_TRACE;
+ doLogging(dlogdurs, now, data, i, num);
+ });
+ }else { // only log newest entry
+ _module<ControlThreadDataBuffer>()
+ .getControlThreadOutputBuffer()
+ .forLatestLoggingEntry(
+ [&](const auto& data, auto i, auto num)
+ {
+ ARMARX_TRACE;
+ doLogging(dlogdurs, now, data, i, num);
+ });
+ }
+
}
ARMARX_DEBUG << ::deactivateSpam() << "the last " << backlog.size()
<< " iterations are stored";
@@ -863,7 +880,7 @@ namespace armarx::RobotUnitModule
rtLoggingBacklogRetentionTime =
IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
-
+
ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
}
@@ -993,9 +1010,9 @@ namespace armarx::RobotUnitModule
{
return;
}
- result.emplace_back(getResultElement());
+
+ auto& data = result.emplace_back(getResultElement());
- auto& data = result.back();
data.iterationId = e.iteration;
data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index 732b3a193d6da5831634a1c6caa0206e9c391140..bb6a625e3484fc843f4d27224a5801d416d91858 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -79,6 +79,9 @@ namespace armarx::RobotUnitModule
defineOptionalProperty<std::size_t>(
"RTLogging_StreamingDataMaxClientConnectionFailures", 10,
"If sending data to a client fails this often, the client is removed from the list");
+
+ defineOptionalProperty<bool>("RTLogging_LogAllMessages", true,
+ "If true, logs all messages from the control thread. If false, only the newest message will be logged");
}
};
@@ -301,6 +304,9 @@ namespace armarx::RobotUnitModule
/// @brief The time an entry shold remain in the backlog.
IceUtil::Time rtLoggingBacklogRetentionTime;
+ /// @brief
+ bool logAllEntries = true;
+
friend void WriteTo(const auto& dentr,
const Logging::DataStreamingEntry::OutVal& out,
const auto& val,
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 24f8ac9db2bd10eb6c9e8e7e2f36ac75931a69e0..93ea9a009c91a8c46b2c96ac0ac4917d0fb36850 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -61,7 +61,6 @@ namespace armarx
// PlatformUnit interface
void onInitPlatformUnit() override
{
- usingTopic("PlatformState");
}
void onStartPlatformUnit() override
{
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index 8e5aa6c3238a3149180217c31bfff947adc27bff..26a7dd83ddcfd736b85cc4466686c9ea93c02809 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -21,6 +21,7 @@
*/
#include "ControlThreadOutputBuffer.h"
#include <memory>
+#include <utility>
namespace armarx
{
@@ -124,6 +125,29 @@ namespace armarx
}
+
+ void ControlThreadOutputBuffer::forLatestLoggingEntry(ConsumerFunctor consumer)
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_EXPRESSION(isInitialized);
+ if (writePosition - onePastLoggingReadPosition >= numEntries)
+ {
+ ARMARX_ERROR << "There are " << writePosition - onePastLoggingReadPosition
+ << " unlogged entries, but only the last " << numEntries << " are saved! "
+ "There seems to be something wrong (e.g. the rt logging threw an exception, "
+ "the system load is too high or the logging takes to long). "
+ "The log position will be reset to the newest entry!";
+ resetLoggingPosition();
+ }
+
+ // skip all messages except the latest
+ onePastLoggingReadPosition = writePosition.load() - 1;
+
+ // use already existing impl.
+ foreachNewLoggingEntry(std::move(consumer));
+
+ }
+
std::size_t ControlThreadOutputBuffer::initialize(std::size_t numEntries,
const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
const KeyValueVector<std::string, SensorDevicePtr>& sensorDevices,
@@ -314,7 +338,7 @@ namespace armarx
<< " bytes for messages buffer, but the maximal buffer size is "
<< getMaximalBufferSize();
}
- bufferSize = std::max(requiredSpace, getMaximalBufferSize());
+ bufferSize = std::min(requiredSpace, getMaximalBufferSize());
buffer.resize(bufferSize, 0);
requiredAdditionalBufferSpace = 0;
messagesLost = 0;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index 40d70b0d55cc93a372a058f0a850aa344a426bcd..4cbdf1e4b7fcde010ea07db88f4d8585163cf2d9 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -228,6 +228,7 @@ namespace armarx
//logging read
void resetLoggingPosition() const;
void foreachNewLoggingEntry(ConsumerFunctor consumer);
+ void forLatestLoggingEntry(ConsumerFunctor consumer);
std::size_t getNumberOfBytes() const;
diff --git a/source/RobotAPI/drivers/CMakeLists.txt b/source/RobotAPI/drivers/CMakeLists.txt
index f8de74539d68dfc44a556fb478d97f75ccaf22d6..731765e4bbcbdcb597b34f5fb6de5a5b9ae7b0ed 100644
--- a/source/RobotAPI/drivers/CMakeLists.txt
+++ b/source/RobotAPI/drivers/CMakeLists.txt
@@ -7,4 +7,3 @@ add_subdirectory(GamepadUnit)
add_subdirectory(MetaWearIMU)
add_subdirectory(KITProstheticHandDriver)
add_subdirectory(KITProsthesisIceDriver)
-add_subdirectory(SickLaserUnit)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
deleted file mode 100644
index af02b7d0e859068bf508d27316ce719517c1de17..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+++ /dev/null
@@ -1,89 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-
-set(LIB_NAME SickLaserUnit)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-
-# If your component needs a special ice interface, define it here:
-# armarx_add_component_interface_lib(
-# SLICE_FILES
-# SickLaserUnit.ice
-# ICE_LIBS
-# # RobotAPI
-#)
-
-
-find_package(sick_scan_base)
-armarx_build_if(sick_scan_base_FOUND "sick_scan_base not available")
-
-find_package(Boost COMPONENTS system filesystem thread REQUIRED)
-armarx_build_if(Boost_FOUND "Boost not available")
-
-# Add the component
-armarx_add_component(
- COMPONENT_LIBS
- # ArmarXCore
- ArmarXCore
- ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
- # ArmarXGui
- ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
- # RobotAPI
- RobotAPICore
- ## RobotAPIInterfaces
- RobotAPIComponentPlugins # For ArViz and other plugins.
-
- # This project
- ## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
- # This component
- ## SickLaserUnitInterfaces # If you defined a component ice interface above.
-
- sick_scan_base::sick_scan_generic
- ${Boost_LIBRARIES}
-
- SOURCES
- SickLaserUnit.cpp
- SickScanAdapter.cpp
-
- HEADERS
- SickLaserUnit.h
- SickScanAdapter.h
-)
-
-
-# Add dependencies
-if(sick_scan_base_FOUND)
- target_include_directories(${LIB_NAME} SYSTEM PUBLIC ${sick_scan_base_INCLUDE_DIRS})
- target_compile_definitions(${LIB_NAME} PUBLIC "-Dlinux")
-endif()
-
-if(Boost_FOUND)
- target_include_directories(${LIB_NAME} PUBLIC ${Boost_INCLUDE_DIR})
-endif()
-
-# All target_include_directories must be guarded by if(Xyz_FOUND)
-# For multiple libraries write: if(X_FOUND AND Y_FOUND) ...
-#if(MyLib_FOUND)
-# target_include_directories(SickLaserUnit PUBLIC ${MyLib_INCLUDE_DIRS})
-#endif()
-
-
-# Add ARON files
-#armarx_enable_aron_file_generation_for_target(
-# TARGET_NAME
-# ${ARMARX_COMPONENT_NAME}
-# ARON_FILES
-# aron/ExampleData.xml
-#)
-
-
-# Add unit tests
-add_subdirectory(test)
-#add_subdirectory(thirdparty/sick_scan_base)
-
-# Generate the application
-armarx_generate_and_add_component_executable(
- # If your component is not defined in ::armarx, specify its namespace here:
- # COMPONENT_NAMESPACE "armarx::mynamespace"
-)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/README.md b/source/RobotAPI/drivers/SickLaserUnit/README.md
deleted file mode 100644
index decfdadc6dee52122631a84d7365f4d5a7e38129..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/README.md
+++ /dev/null
@@ -1,33 +0,0 @@
-# SickLaserUnit
-Armarx adapter to the SICK laser scanner driver
-
-Uses the sick_scan_base driver to communicate with the SICK_TiM571 Laser Scanners installed in Armar-DE and translates their timestamps and data to ArmarX types.
-After conversion, the scan data is published to the configured topic.
-
-
-## Installation
-
-1. Download and build the sick_scan_base project from https://github.com/SICKAG/sick_scan_base
- ```console
- git clone https://github.com/SICKAG/sick_scan_base.git
-
- cd sick_scan_base
- mkdir -p ./build
- cd ./build
- cmake .. && make
- ```
-
-2. The package uses the script RobotAPI/etc/cmake/Findsick_scan_base.cmake to link against the sick drivers.
- For this to work, the CMake variable sick_scan_base_DIR must be set to the path where the driver has been downloaded to.
-
-3. Build this package and configure its parameters to match the Laser Scanner setup.
-
-## Runnig
-
-To use the package, make sure your LaserScanners are connected to the PC and the configured IP addresses match the ones set by the SOPAS tool.
-
-You can use ping to check whether the scanners are connected.
-```console
-ping 192.168.8.133
-```
-
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
deleted file mode 100644
index 8fdadffeb177990068d890b26551b88d5712cc6f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ /dev/null
@@ -1,242 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::SickLaserUnit
- * @author Johann Mantel ( j-mantel at gmx dot net )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#include "SickLaserUnit.h"
-#include <exception>
-
-// Include headers you only need in function definitions in the .cpp.
-
-namespace armarx
-{
-
- void SickLaserScanDevice::run()
- {
- while (!task->isStopped())
- {
- switch (runState)
- {
- case RunState::scannerInit:
- if (initCnt < 5)
- {
- initCnt++;
- initScanner();
- }
- else
- {
- ARMARX_WARNING << "Maximum number of reinitializations reached - going to idle state";
- runState = RunState::scannerFinalize;
- }
- break;
- case RunState::scannerRun:
- if (result == sick_scan::ExitSuccess) // OK -> loop again
- {
- scanData.clear();
- result = scanner->loopOnce(scanData, scanTime, scanInfo, false);
- if (scanTopic)
- {
- TimestampVariantPtr scanT(new TimestampVariant(scanTime));
- scanTopic->reportSensorValues(scannerName, scannerName, scanData, scanT);
- //trigger heartbeat
- scannerHeartbeat->heartbeat(scannerName);
- }
- else
- {
- ARMARX_WARNING << "No scan topic available: IP: " << ip << ", Port: " << port;
- }
- }
- else
- {
- runState = RunState::scannerInit;
- }
- break;
- case RunState::scannerFinalize:
- ARMARX_WARNING << "Scanner offline - stopping task.";
- this->task->stop();
- break;
- default:
- ARMARX_WARNING << "Invalid run state in task loop";
- break;
- }
- }
- }
-
- void SickLaserScanDevice::initScanner()
- {
- ARMARX_INFO_S << "Start initialising scanner " << scannerName
- << " [Ip: " << this->ip << "] [Port: " << this->port << "]";
- // attempt to connect/reconnect
- if (this->scanner)
- {
- ARMARX_WARNING_S << "Scanner already initialized - reinit.";
- this->scanner.reset(); // disconnect scanner
- }
- this->scanner = std::make_unique<SickScanAdapter>(this->ip, this->port, this->timelimit, this->parser.get(), 'A');
- this->result = this->scanner->init();
-
- if (this->result == sick_scan::ExitSuccess)
- {
- //read the scanner parameters for initialization
- this->result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
- }
- if (this->result == sick_scan::ExitSuccess) // OK -> loop again
- {
- ARMARX_INFO_S << "Scanner successfully initialized.";
- this->runState = RunState::scannerRun; // after initialising switch to run state
- }
- else
- {
- this->runState = RunState::scannerInit; // If there was an error, try to restart scanner
- }
- }
-
-
- armarx::PropertyDefinitionsPtr SickLaserUnit::createPropertyDefinitions()
- {
- armarx::PropertyDefinitionsPtr def =
- new ComponentPropertyDefinitions(getConfigIdentifier());
- // Publish to a topic (passing the TopicListenerPrx).
- // def->topic(myTopicListener);
-
- // Use (and depend on) another component (passing the ComponentInterfacePrx).
- // def->component(myComponentProxy)
-
- def->topic(topic, properties.topicName, "TopicName", "Name of the laserscanner topic to report to.");
- //Scanner parameters
- def->optional(properties.devices, "devices", "List of Devices in format frame1,ip1,port1;frame2,ip2,port2");
- def->optional(properties.scannerType, "scannerType", "Name of the LaserScanner");
- def->optional(properties.timelimit, "timelimit", "timelimit for communication");
- def->optional(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
- def->optional(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
- def->optional(properties.heartbeatWarnMS, "heartbeatWarnMS", "maximum cycle time before heartbeat Warning");
- def->optional(properties.heartbeatErrorMS, "heartbeatErrorMS", "maximum cycle time before heartbeat Error");
- return def;
- }
-
- SickLaserUnit::SickLaserUnit()
- {
- addPlugin(heartbeat);
- ARMARX_CHECK_NOT_NULL(heartbeat);
- }
-
- void SickLaserUnit::onInitComponent()
- {
- ARMARX_INFO << "On init";
- // Topics and properties defined above are automagically registered.
- //offeringTopic(properties.topicName);
-
- // Keep debug observer data until calling `sendDebugObserverBatch()`.
- // (Requies the armarx::DebugObserverComponentPluginUser.)
- // setDebugObserverBatchModeEnabled(true);
-
- std::vector<std::string> splitDeviceStrings = Split(properties.devices, ";");
- scanDevices.clear();
- scanDevices.reserve(splitDeviceStrings.size());
- for (std::string const& deviceString : splitDeviceStrings)
- {
- std::vector<std::string> deviceInfo = Split(deviceString, ",");
- if (deviceInfo.size() != 3)
- {
- ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
- << " (split size: " << deviceInfo.size() << ", expected: 3)";
- continue;
- }
- SickLaserScanDevice& device = scanDevices.emplace_back();
- device.scannerName = deviceInfo[0];
- device.ip = deviceInfo[1];
- device.port = deviceInfo[2];
- device.timelimit = properties.timelimit;
- //scanInfo
- device.scanInfo.device = device.ip;
- device.scanInfo.frame = device.scannerName;
- //scanner Parameters
- try
- {
- device.parser = std::make_unique<sick_scan::SickGenericParser>(properties.scannerType);
- device.parser->set_range_min(properties.rangeMin);
- device.parser->set_range_max(properties.rangeMax);
- device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
- }
- catch (std::exception const& e)
- {
- ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
- return;
- }
- armarx::RobotHealthHeartbeatArgs heartbeatArgs =
- armarx::RobotHealthHeartbeatArgs(properties.heartbeatWarnMS, properties.heartbeatErrorMS, "No LaserScan data available");
- device.scannerHeartbeat = heartbeat;
- device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName, heartbeatArgs);
- }
- }
-
- void SickLaserUnit::onConnectComponent()
- {
- for (SickLaserScanDevice& device : scanDevices)
- {
- device.scanTopic = topic;
- //start the laser scanner
- if (device.task)
- {
- ARMARX_WARNING << "this should not happen.";
- device.task->stop();
- device.task = nullptr;
- }
- device.runState = RunState::scannerInit;
- device.task = new RunningTask<SickLaserScanDevice>(&device, &SickLaserScanDevice::run, "SickLaserScanUpdate_" + device.ip);
- device.task->start();
- }
- // Do things after connecting to topics and components.
-
- /* (Requies the armarx::DebugObserverComponentPluginUser.)
- // Use the debug observer to log data over time.
- // The data can be viewed in the ObserverView and the LivePlotter.
- // (Before starting any threads, we don't need to lock mutexes.)
- {
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- */
- }
-
- void SickLaserUnit::onDisconnectComponent()
- {
- ARMARX_INFO_S << "Disconnecting LaserScanner.";
- for (SickLaserScanDevice& device : scanDevices)
- {
- if (device.task)
- {
- device.task->stop();
- device.task = nullptr;
- }
- }
- }
-
- void SickLaserUnit::onExitComponent()
- {
- }
-
- std::string SickLaserUnit::getDefaultName() const
- {
- return "SickLaserUnit";
- }
-
-} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
deleted file mode 100644
index 8fa16e4bf6961f13e1cd8acdf64b9acea8e40332..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ /dev/null
@@ -1,143 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::SickLaserUnit
- * @author Johann Mantel ( j-mantel at gmx dot net )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-#pragma once
-
-// #include <mutex>
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
-
-#include <vector>
-
-#include "SickScanAdapter.h"
-#include <sick_scan/sick_scan_common.h>
-#include <sick_scan/sick_scan_common_tcp.h>
-#include <sick_scan/sick_generic_laser.h>
-
-namespace armarx
-{
-
- enum class ScanProtocol
- {
- ASCII,
- Binary
- };
-
- enum class RunState
- {
- scannerInit,
- scannerRun,
- scannerFinalize
- };
-
- struct SickLaserScanDevice
- {
- std::string scannerName;
- //communication parameters
- std::string ip;
- std::string port;
- int timelimit = 5;
- //data and task pointers
- IceUtil::Time scanTime;
- LaserScan scanData;
- LaserScannerInfo scanInfo;
- int initCnt = 0;
- int result = sick_scan::ExitError;
- RunState runState = RunState::scannerFinalize;
- RunningTask<SickLaserScanDevice>::pointer_type task;
- LaserScannerUnitListenerPrx scanTopic;
- plugins::HeartbeatComponentPlugin* scannerHeartbeat;
- //scanner pointers
- std::unique_ptr<sick_scan::SickGenericParser> parser;
- std::unique_ptr<SickScanAdapter> scanner;
-
- void initScanner();
- void run();
- };
- /**
- * @defgroup Component-SickLaserUnit SickLaserUnit
- * @ingroup RobotAPI-Components
- * A description of the component SickLaserUnit.
- *
- * @class SickLaserUnit
- * @ingroup Component-SickLaserUnit
- * @brief Brief description of class SickLaserUnit.
- *
- * Detailed description of class SickLaserUnit.
- */
- class SickLaserUnit :
- //virtual public armarx::LaserScannerUnitInterface,
- virtual public armarx::Component
- // , virtual public armarx::RobotHealthComponentUser
- // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
- {
- public:
- /// @see armarx::ManagedIceObject::getDefaultName()
- std::string getDefaultName() const override;
- SickLaserUnit();
-
- protected:
- /// @see PropertyUser::createPropertyDefinitions()
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
- /// @see armarx::ManagedIceObject::onInitComponent()
- void onInitComponent() override;
-
- /// @see armarx::ManagedIceObject::onConnectComponent()
- void onConnectComponent() override;
-
- /// @see armarx::ManagedIceObject::onDisconnectComponent()
- void onDisconnectComponent() override;
-
- /// @see armarx::ManagedIceObject::onExitComponent()
- void onExitComponent() override;
-
- private:
- // Private methods go here.
-
- private:
- // Private member variables go here.
-
- /// Properties shown in the Scenario GUI.
- struct Properties
- {
- std::string topicName = "LaserScans";
- //scanner parameters
- std::string devices = "LaserScannerFront,192.168.8.133,2112";
- std::string scannerType = "sick_tim_5xx";
- int timelimit = 5;
- double rangeMin = 0.0;
- double rangeMax = 10.0;
- int heartbeatWarnMS = 500;
- int heartbeatErrorMS = 800;
- };
- Properties properties;
- std::vector<SickLaserScanDevice> scanDevices;
- LaserScannerUnitListenerPrx topic;
- plugins::HeartbeatComponentPlugin* heartbeat = NULL;
-
- };
-} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
deleted file mode 100644
index 40250b2b1a7d0dc472806715fe6c4daf9b532f17..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ /dev/null
@@ -1,988 +0,0 @@
-/**
-* \file
-* \brief Laser Scanner communication (TCP Helper Class)
-* Copyright (C) 2013, Osnabrück University
-* Copyright (C) 2017, Ing.-Buero Dr. Michael Lehning, Hildesheim
-* Copyright (C) 2017, SICK AG, Waldkirch
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above copyright
-* notice, this list of conditions and the following disclaimer in the
-* documentation and/or other materials provided with the distribution.
-* * Neither the name of Osnabrück University nor the names of its
-* contributors may be used to endorse or promote products derived from
-* this software without specific prior written permission.
-* * Neither the name of SICK AG nor the names of its
-* contributors may be used to endorse or promote products derived from
-* this software without specific prior written permission
-* * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
-* contributors may be used to endorse or promote products derived from
-* this software without specific prior written permission
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Last modified: 12th Dec 2017
-*
-* Authors:
-* Michael Lehning <michael.lehning@lehning.de>
-* Jochen Sprickerhof <jochen@sprickerhof.de>
-* Martin Günther <mguenthe@uos.de>
-*
-* Based on the TiM communication example by SICK AG.
-*
-*/
-
-#ifdef _MSC_VER
-#pragma warning(disable: 4996)
-#pragma warning(disable: 4267)
-#pragma warning(disable: 4101) // C4101: "e" : Unreferenzierte lokale Variable
-#define _WIN32_WINNT 0x0501
-
-#endif
-
-#include "SickScanAdapter.h"
-#include <sick_scan/tcp/colaa.hpp>
-#include <sick_scan/tcp/colab.hpp>
-#include <sick_scan/sick_generic_radar.h>
-#include <sick_scan/helper/angle_compensator.h>
-#include <sick_scan/sick_scan_config_internal.h>
-#include <sick_scan/sick_generic_imu.h>
-
-#ifdef ROSSIMU
-#include <sick_scan/rosconsole_simu.hpp>
-#endif
-
-#include <sick_scan/binScanf.hpp>
-#include <sick_scan/RadarScan.h>
-#include <sick_scan/Encoder.h>
-
-#include <boost/asio.hpp>
-#include <boost/lambda/lambda.hpp>
-#include <algorithm>
-#include <iterator>
-#include <boost/lexical_cast.hpp>
-#include <vector>
-
-#ifndef rad2deg
-#define rad2deg(x) ((x) / M_PI * 180.0)
-#endif
-#define deg2rad_const (0.017453292519943295769236907684886f)
-
-
-//std::vector<unsigned char> exampleData(65536);
-//std::vector<unsigned char> receivedData(65536);
-//static long receivedDataLen = 0;
-
-//static int getDiagnosticErrorCode()
-//{
-//#ifdef _MSC_VER
-//#undef ERROR
-// return (2);
-//#else
-// return (diagnostic_msgs::DiagnosticStatus::ERROR);
-//#endif
-//}
-
-namespace armarx
-{
- SickScanAdapter::SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit,
- sick_scan::SickGenericParser* parser, char cola_dialect_id)
- :
- sick_scan::SickScanCommon(parser),
- socket_(io_service_),
- deadline_(io_service_),
- hostname_(hostname),
- port_(port),
- timelimit_(timelimit)
- {
- if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
- {
- this->setProtocolType(CoLa_A);
- }
-
- if ((cola_dialect_id == 'b') || (cola_dialect_id == 'B'))
- {
- this->setProtocolType(CoLa_B);
- }
- ARMARX_CHECK(this->getProtocolType() != CoLa_Unknown);
-
- m_numberOfBytesInReceiveBuffer = 0;
- m_alreadyReceivedBytes = 0;
- this->setReplyMode(0);
- // io_service_.setReadCallbackFunction(boost::bind(&SopasDevice::readCallbackFunction, this, _1, _2));
-
- // Set up the deadline actor to implement timeouts.
- // Based on blocking TCP example on:
- // http://www.boost.org/doc/libs/1_46_0/doc/html/boost_asio/example/timeouts/blocking_tcp_client.cpp
- deadline_.expires_at(boost::posix_time::pos_infin);
- checkDeadline();
- }
-
- SickScanAdapter::~SickScanAdapter()
- {
- //stopScanData();
- close_device();
- }
-
- using boost::asio::ip::tcp;
- using boost::lambda::var;
- using boost::lambda::_1;
-
-
- /*!
- \brief parsing datagram into ARMARX message
- \return error code
- */
- int SickScanAdapter::loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo)
- {
- unsigned char receiveBuffer[65536];
- int actual_length = 0;
- int packetsInLoop = 0;
-
- //always use ASCII
- int result = getDatagramIceTime(scanTime, receiveBuffer, &actual_length, &packetsInLoop);
- //ros::Duration dur = recvTimeStampPush - recvTimeStamp;
- if (result != 0)
- {
- ARMARX_ERROR_S << "Read Error when getting datagram: " << result;
- return sick_scan::ExitError; // return failure to exit node
- }
- if (actual_length <= 0)
- {
- return sick_scan::ExitSuccess;
- } // return success to continue looping
-
- //datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
- char* buffer_pos = (char*) receiveBuffer;
- char* dstart = NULL;
- char* dend = NULL;
- bool dataToProcess = true;
- std::vector<float> vang_vec;
- vang_vec.clear();
-
- while (dataToProcess)
- {
- size_t dlength;
- int success = -1;
- // Always Parsing Ascii-Encoding of datagram
- dstart = strchr(buffer_pos, 0x02);
- if (dstart != NULL)
- {
- dend = strchr(dstart + 1, 0x03);
- }
- if ((dstart != NULL) && (dend != NULL))
- {
- dataToProcess = true; // continue parsing
- dlength = dend - dstart;
- *dend = '\0';
- dstart++;
- }
- else
- {
- dataToProcess = false;
- break;
- }
- // HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
- // <frameid>_<sign>00500_DIST[1|2|3]
- success = parseDatagram(dstart, dlength, scanData, scanInfo, updateScannerInfo);
- if (success != sick_scan::ExitSuccess)
- {
- ARMARX_WARNING << "parseDatagram returned ErrorCode: " << success;
- }
- // Start Point
- if (dend != NULL)
- {
- buffer_pos = dend + 1;
- }
- } // end of while loop
- return sick_scan::ExitSuccess; // return success to continue looping
- }
-
- //Adapted from sick_generic_parser
- int SickScanAdapter::parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo)
- {
- int HEADER_FIELDS = 32;
- char* cur_field;
- size_t count = 0;
-
- // Reserve sufficient space
- std::vector<char*> fields;
- fields.reserve(datagram_length / 2);
- std::vector<char> datagram_copy_vec;
- datagram_copy_vec.resize(datagram_length + 1); // to avoid using malloc. destructor frees allocated mem.
- char* datagram_copy = &(datagram_copy_vec[0]);
- strncpy(datagram_copy, datagram, datagram_length); // datagram will be changed by strtok
- datagram_copy[datagram_length] = 0;
-
- cur_field = strtok(datagram, " ");
- while (cur_field != NULL)
- {
- fields.push_back(cur_field);
- cur_field = strtok(NULL, " ");
- }
- count = fields.size();
-
- // Validate header. Total number of tokens is highly unreliable as this may
- // change when you change the scanning range or the device name using SOPAS ET
- // tool. The header remains stable, however.
- if ((int) count < HEADER_FIELDS)
- {
- ARMARX_ERROR_S << "received less fields than minimum fields (actual: " << count
- << ", minimum: " << HEADER_FIELDS << "), ignoring scan\n"
- << "are you using the correct node? (124 --> sick_tim310_1130000m01, > 33 --> sick_tim551_2050001, 580 --> sick_tim310s01, 592 --> sick_tim310)";
- return sick_scan::ExitError;
- }
-
- if (strcmp(fields[15], "0"))
- {
- ARMARX_ERROR_S << "Field 15 of received data is not equal to 0. Unexpected data, ignoring scan";
- return sick_scan::ExitError;
- }
- if (strcmp(fields[20], "DIST1"))
- {
- ARMARX_ERROR_S << "Field 20 of received data is not equal to DIST1i. Unexpected data, ignoring scan";
- return sick_scan::ExitError;
- }
-
- // More in depth checks: check data length and RSSI availability
- // 25: Number of data (<= 10F)
- unsigned short int number_of_data = 0;
- sscanf(fields[25], "%hx", &number_of_data);
-
- if ((int) count < HEADER_FIELDS + number_of_data)
- {
- ARMARX_ERROR_S << "Less fields than expected for data points - Ignoring scan";
- return sick_scan::ExitError;
- }
-
- // Calculate offset of field that contains indicator of whether or not RSSI data is included
- size_t rssi_idx = 26 + number_of_data;
- bool rssi = false;
- unsigned short int number_of_rssi_data = 0;
- if (strcmp(fields[rssi_idx], "RSSI1") == 0)
- {
- rssi = true;
- }
- if (rssi)
- {
- sscanf(fields[rssi_idx + 5], "%hx", &number_of_rssi_data);
- // Number of RSSI data should be equal to number of data
- if (number_of_rssi_data != number_of_data)
- {
- ARMARX_ERROR_S << "Number of RSSI data is lower than number of range data";
- return sick_scan::ExitError;
- }
-
- // Check if the total length is still appropriate.
- // RSSI data size = number of RSSI readings + 6 fields describing the data
- if ((int) count < HEADER_FIELDS + number_of_data + number_of_rssi_data + 6)
- {
- ARMARX_ERROR_S << "Less fields than expected for %d data points (%zu). Ignoring scan";
- return sick_scan::ExitError;
- }
-
- if (strcmp(fields[rssi_idx], "RSSI1"))
- {
- ARMARX_ERROR_S << "A Field of received data is not equal to RSSI1 - Unexpected data, ignoring scan";
- }
- }
-
- //IceUtil::Time start_time = IceUtil::Time::now(); // will be adjusted in the end
-
- // <STX> (\x02)
- // 0: Type of command (SN)
- // 1: Command (LMDscandata)
- // 2: Firmware version number (1)
- // 3: Device number (1)
- // 4: Serial number (eg. B96518)
- // 5 + 6: Device Status (0 0 = ok, 0 1 = error)
- // 7: Telegram counter (eg. 99)
- // 8: Scan counter (eg. 9A)
- // 9: Time since startup (eg. 13C8E59)
- // 10: Time of transmission (eg. 13C9CBE)
- // 11 + 12: Input status (0 0)
- // 13 + 14: Output status (8 0)
- // 15: Reserved Byte A (0)
- // 16: Scanning Frequency (5DC)
- unsigned short scanning_freq = -1;
- sscanf(fields[16], "%hx", &scanning_freq);
- // msg.scan_time = 1.0 / (scanning_freq / 100.0);
-
- // 17: Measurement Frequency (36)
- unsigned short measurement_freq = -1;
- sscanf(fields[17], "%hx", &measurement_freq);
- // msg.time_increment = 1.0 / (measurement_freq * 100.0);
-
- // 18: Number of encoders (0)
- // 19: Number of 16 bit channels (1)
- // 20: Measured data contents (DIST1)
- // 21: Scaling factor (3F800000)
- // ignored for now (is always 1.0):
- // 22: Scaling offset (00000000) -- always 0
- // 23: Starting angle (FFF92230)
- if (updateScannerInfo)
- {
- scanInfo.device = hostname_;
- uint starting_angle = (uint) - 1;
- sscanf(fields[23], "%x", &starting_angle);
- scanInfo.minAngle = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
-
- // 24: Angular step width (2710)
- unsigned short angular_step_width = -1;
- sscanf(fields[24], "%hx", &angular_step_width);
- scanInfo.stepSize = (angular_step_width / 10000.0) / 180.0 * M_PI;
- scanInfo.maxAngle = scanInfo.minAngle + (number_of_data - 1) * scanInfo.stepSize;
- }
-
- // 25: Number of data (<= 10F)
- // This is already determined above in number_of_data
-
- int distNum = 0;
- int rssiNum = 0;
- int baseOffset = 20;
- // More in depth checks: check data length and RSSI availability
- // 25: Number of data (<= 10F)
- unsigned short int curr_number_of_data = 0;
- if (strstr(fields[baseOffset], "DIST") != fields[baseOffset]) // First initial check
- {
- ARMARX_ERROR_S << "Field 20 of received data does not start with DIST - Unexpected data, ignoring scan";
- return sick_scan::ExitError;
- }
-
- //Read either intensity or distance data, regarding what type is given in the datagram
- std::vector<float> distVal, intensityVal;
- for (int offset = 20; offset < (int) fields.size();)
- {
- bool distFnd = false;
- bool rssiFnd = false;
- if (strlen(fields[offset]) == 5)
- {
- if (strstr(fields[offset], "DIST") == fields[offset])
- {
- distNum++;
- distFnd = true;
- }
- else if (strstr(fields[offset], "RSSI") == fields[offset])
- {
- rssiNum++;
- rssiFnd = true;
- }
- }
- if (rssiFnd || distFnd)
- {
- offset += 5;
- if (offset >= (int) fields.size())
- {
- ARMARX_ERROR_S << "Missing RSSI or DIST data";
- return sick_scan::ExitError;
- }
- curr_number_of_data = 0;
- sscanf(fields[offset], "%hx", &curr_number_of_data);
- if (curr_number_of_data != number_of_data)
- {
- ARMARX_ERROR_S << "number of dist or rssi values mismatching.";
- return sick_scan::ExitError;
- }
- offset++;
- // Here is the first value
- for (int i = 0; i < curr_number_of_data; i++)
- {
- if (distFnd)
- {
- unsigned short iRange;
- sscanf(fields[offset + i], "%hx", &iRange);
- float range = iRange / 1000.0;
- distVal.push_back(range);
- }
- else
- {
- unsigned short iRSSI;
- sscanf(fields[offset + i], "%hx", &iRSSI);
- intensityVal.push_back((float) iRSSI);
- }
- }
- offset += number_of_data;
- }
- else
- {
- offset++;
- }
- }
-
- if (distVal.size() != intensityVal.size())
- {
- ARMARX_ERROR_S << "Number of distance measurements does not match number of intensity values - Skipping";
- return sick_scan::ExitError;
- }
- scanData.reserve(distVal.size());
- for (int i = 0; i < (int) distVal.size(); i++)
- {
- LaserScanStep step;
- step.angle = i * scanInfo.stepSize;
- step.distance = distVal[i];
- //step.intensity = intensityVal[i];
- scanData.push_back(step);
- }
-
- // 26 + n: RSSI data included
- // IF RSSI not included:
- // 26 + n + 1 .. 26 + n + 3 = unknown (but seems to be [0, 1, B] always)
- // 26 + n + 4 .. count - 4 = device label
- // count - 3 .. count - 1 = unknown (but seems to be 0 always)
- // <ETX> (\x03)
- return sick_scan::ExitSuccess;
- }
-
-
- void SickScanAdapter::disconnectFunction()
- {
- }
-
- void SickScanAdapter::disconnectFunctionS(void* obj)
- {
- if (obj != NULL)
- {
- ((SickScanAdapter*)(obj))->disconnectFunction();
- }
- }
-
- void SickScanAdapter::readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes)
- {
- ((SickScanAdapter*) obj)->readCallbackFunction(buffer, numOfBytes);
- }
-
-
- void SickScanAdapter::setReplyMode(int _mode)
- {
- m_replyMode = _mode;
- }
-
- int SickScanAdapter::getReplyMode()
- {
- return (m_replyMode);
- }
-
- //
- // Look for 23-frame (STX/ETX) in receive buffer.
- // Move frame to start of buffer
- //
- // Return: 0 : No (complete) frame found
- // >0 : Frame length
- //
- SopasEventMessage SickScanAdapter::findFrameInReceiveBuffer()
- {
- UINT32 frameLen = 0;
- UINT32 i;
-
- // Depends on protocol...
- if (getProtocolType() == CoLa_A)
- {
- //
- // COLA-A
- //
- // Must start with STX (0x02)
- if (m_receiveBuffer[0] != 0x02)
- {
- // Look for starting STX (0x02)
- for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
- {
- if (m_receiveBuffer[i] == 0x02)
- {
- break;
- }
- }
-
- // Found beginning of frame?
- if (i >= m_numberOfBytesInReceiveBuffer)
- {
- // No start found, everything can be discarded
- m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
- return SopasEventMessage(); // No frame found
- }
-
- // Move frame start to index 0
- UINT32 newLen = m_numberOfBytesInReceiveBuffer - i;
- memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), newLen);
- m_numberOfBytesInReceiveBuffer = newLen;
- }
-
- // Look for ending ETX (0x03)
- for (i = 1; i < m_numberOfBytesInReceiveBuffer; i++)
- {
- if (m_receiveBuffer[i] == 0x03)
- {
- break;
- }
- }
-
- // Found end?
- if (i >= m_numberOfBytesInReceiveBuffer)
- {
- // No end marker found, so it's not a complete frame (yet)
- return SopasEventMessage(); // No frame found
- }
-
- // Calculate frame length in byte
- frameLen = i + 1;
-
- return SopasEventMessage(m_receiveBuffer, CoLa_A, frameLen);
- }
- else if (getProtocolType() == CoLa_B)
- {
- UINT32 magicWord;
- UINT32 payloadlength;
-
- if (m_numberOfBytesInReceiveBuffer < 4)
- {
- return SopasEventMessage();
- }
- UINT16 pos = 0;
- magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
- if (magicWord != 0x02020202)
- {
- // Look for starting STX (0x02020202)
- for (i = 1; i <= m_numberOfBytesInReceiveBuffer - 4; i++)
- {
- pos = i; // this is needed, as the position value is updated by getIntegerFromBuffer
- magicWord = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
- if (magicWord == 0x02020202)
- {
- // found magic word
- break;
- }
- }
-
- // Found beginning of frame?
- if (i > m_numberOfBytesInReceiveBuffer - 4)
- {
- // No start found, everything can be discarded
- m_numberOfBytesInReceiveBuffer = 0; // Invalidate buffer
- return SopasEventMessage(); // No frame found
- }
- else
- {
- // Move frame start to index
- UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - i;
- memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[i]), bytesToMove); // payload+magic+length+s+checksum
- m_numberOfBytesInReceiveBuffer = bytesToMove;
- }
- }
-
- // Pruefe Laenge des Pufferinhalts
- if (m_numberOfBytesInReceiveBuffer < 9)
- {
- // Es sind nicht genug Daten fuer einen Frame
- printInfoMessage("SickScanCommonNw::findFrameInReceiveBuffer: Frame cannot be decoded yet, only " +
- ::toString(m_numberOfBytesInReceiveBuffer) + " bytes in the buffer.", m_beVerbose);
- return SopasEventMessage();
- }
-
- // Read length of payload
- pos = 4;
- payloadlength = colab::getIntegerFromBuffer<UINT32>(m_receiveBuffer, pos);
- printInfoMessage(
- "SickScanCommonNw::findFrameInReceiveBuffer: Decoded payload length is " + ::toString(payloadlength) +
- " bytes.", m_beVerbose);
-
- // Ist die Datenlaenge plausibel und wuede in den Puffer passen?
- if (payloadlength > (sizeof(m_receiveBuffer) - 9))
- {
- // magic word + length + checksum = 9
- printWarning(
- "SickScanCommonNw::findFrameInReceiveBuffer: Frame too big for receive buffer. Frame discarded with length:"
- + ::toString(payloadlength) + ".");
- m_numberOfBytesInReceiveBuffer = 0;
- return SopasEventMessage();
- }
- if ((payloadlength + 9) > m_numberOfBytesInReceiveBuffer)
- {
- // magic word + length + s + checksum = 10
- printInfoMessage(
- "SickScanCommonNw::findFrameInReceiveBuffer: Frame not complete yet. Waiting for the rest of it (" +
- ::toString(payloadlength + 9 - m_numberOfBytesInReceiveBuffer) + " bytes missing).", m_beVerbose);
- return SopasEventMessage(); // frame not complete
- }
-
- // Calculate the total frame length in bytes: Len = Frame (9 bytes) + Payload
- frameLen = payloadlength + 9;
-
- //
- // test checksum of payload
- //
- UINT8 temp = 0;
- UINT8 temp_xor = 0;
- UINT8 checkSum;
-
- // Read original checksum
- pos = frameLen - 1;
- checkSum = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
-
- // Erzeuge die Pruefsumme zum Vergleich
- for (UINT16 i = 8; i < (frameLen - 1); i++)
- {
- pos = i;
- temp = colab::getIntegerFromBuffer<UINT8>(m_receiveBuffer, pos);
- temp_xor = temp_xor ^ temp;
- }
-
- // Vergleiche die Pruefsummen
- if (temp_xor != checkSum)
- {
- printWarning("SickScanCommonNw::findFrameInReceiveBuffer: Wrong checksum, Frame discarded.");
- m_numberOfBytesInReceiveBuffer = 0;
- return SopasEventMessage();
- }
-
- return SopasEventMessage(m_receiveBuffer, CoLa_B, frameLen);
- }
-
- // Return empty frame
- return SopasEventMessage();
- }
-
-
- /**
- * Read callback. Diese Funktion wird aufgerufen, sobald Daten auf der Schnittstelle
- * hereingekommen sind.
- */
-
- void SickScanAdapter::processFrame(IceUtil::Time timeStamp, SopasEventMessage& frame)
- {
-
- if (getProtocolType() == CoLa_A)
- {
- printInfoMessage(
- "SickScanCommonNw::processFrame: Calling processFrame_CoLa_A() with " + ::toString(frame.size()) + " bytes.",
- m_beVerbose);
- // processFrame_CoLa_A(frame);
- }
- else if (getProtocolType() == CoLa_B)
- {
- printInfoMessage(
- "SickScanCommonNw::processFrame: Calling processFrame_CoLa_B() with " + ::toString(frame.size()) + " bytes.",
- m_beVerbose);
- // processFrame_CoLa_B(frame);
- }
-
- // Push frame to recvQueue
-
- DatagramWithTimeStamp dataGramWidthTimeStamp(timeStamp, std::vector<unsigned char>(frame.getRawData(),
- frame.getRawData() +
- frame.size()));
- // recvQueue.push(std::vector<unsigned char>(frame.getRawData(), frame.getRawData() + frame.size()));
- recvQueue.push(dataGramWidthTimeStamp);
- }
-
- void SickScanAdapter::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
- {
- IceUtil::Time rcvTimeStamp = IceUtil::Time::now(); // stamp received datagram
- bool beVerboseHere = false;
-
- ScopedLock lock(&m_receiveDataMutex); // Mutex for access to the input buffer
- UINT32 remainingSpace = sizeof(m_receiveBuffer) - m_numberOfBytesInReceiveBuffer;
- UINT32 bytesToBeTransferred = numOfBytes;
- if (remainingSpace < numOfBytes)
- {
- bytesToBeTransferred = remainingSpace;
- // printWarning("SickScanCommonNw::readCallbackFunction(): Input buffer space is to small, transferring only " +
- // ::toString(bytesToBeTransferred) + " of " + ::toString(numOfBytes) + " bytes.");
- }
- else
- {
- // printInfoMessage("SickScanCommonNw::readCallbackFunction(): Transferring " + ::toString(bytesToBeTransferred) +
- // " bytes from TCP to input buffer.", beVerboseHere);
- }
-
- if (bytesToBeTransferred > 0)
- {
- // Data can be transferred into our input buffer
- memcpy(&(m_receiveBuffer[m_numberOfBytesInReceiveBuffer]), buffer, bytesToBeTransferred);
- m_numberOfBytesInReceiveBuffer += bytesToBeTransferred;
-
- UINT32 size = 0;
-
- while (1)
- {
- // Now work on the input buffer until all received datasets are processed
- SopasEventMessage frame = findFrameInReceiveBuffer();
-
- size = frame.size();
- if (size == 0)
- {
- // Framesize = 0: There is no valid frame in the buffer. The buffer is either empty or the frame
- // is incomplete, so leave the loop
- printInfoMessage("SickScanCommonNw::readCallbackFunction(): No complete frame in input buffer, we are done.",
- beVerboseHere);
-
- // Leave the loop
- break;
- }
- else
- {
- // A frame was found in the buffer, so process it now.
- printInfoMessage(
- "SickScanCommonNw::readCallbackFunction(): Processing a frame of length " + ::toString(frame.size()) +
- " bytes.", beVerboseHere);
- processFrame(rcvTimeStamp, frame);
- UINT32 bytesToMove = m_numberOfBytesInReceiveBuffer - size;
- memmove(&(m_receiveBuffer[0]), &(m_receiveBuffer[size]), bytesToMove); // payload+magic+length+s+checksum
- m_numberOfBytesInReceiveBuffer = bytesToMove;
-
- }
- }
- }
- else
- {
- // There was input data from the TCP interface, but our input buffer was unable to hold a single byte.
- // Either we have not read data from our buffer for a long time, or something has gone wrong. To re-sync,
- // we clear the input buffer here.
- m_numberOfBytesInReceiveBuffer = 0;
- }
- }
-
-
- int SickScanAdapter::init_device()
- {
- int portInt;
- sscanf(port_.c_str(), "%d", &portInt);
- m_nw.init(hostname_, portInt, disconnectFunctionS, (void*) this);
- m_nw.setReadCallbackFunction(readCallbackFunctionS, (void*) this);
- m_nw.connect();
- return sick_scan::ExitSuccess;
- }
-
- int SickScanAdapter::close_device()
- {
- ARMARX_ERROR_S << "Disconnecting TCP-Connection.";
- m_nw.disconnect();
- return 0;
- }
-
-
- bool SickScanAdapter::stopScanData()
- {
- stop_scanner();
- return (true);
- }
-
- void SickScanAdapter::handleRead(boost::system::error_code error, size_t bytes_transfered)
- {
- ec_ = error;
- bytes_transfered_ += bytes_transfered;
- }
-
-
- void SickScanAdapter::checkDeadline()
- {
- if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now())
- {
- // The reason the function is called is that the deadline expired. Close
- // the socket to return all IO operations and reset the deadline
- socket_.close();
- deadline_.expires_at(boost::posix_time::pos_infin);
- }
-
- // Nothing bad happened, go back to sleep
- deadline_.async_wait(boost::bind(&SickScanAdapter::checkDeadline, this));
- }
-
-
- int SickScanAdapter::numberOfDatagramInInputFifo()
- {
- int numVal = this->recvQueue.getNumberOfEntriesInQueue();
- return (numVal);
- }
-
- int SickScanAdapter::readWithTimeout(size_t timeout_ms, char* buffer, int buffer_size, int* bytes_read,
- bool* exception_occured, bool isBinary)
- {
- // Set up the deadline to the proper timeout, error and delimiters
- deadline_.expires_from_now(boost::posix_time::milliseconds(timeout_ms));
-
- ec_ = boost::asio::error::would_block;
- bytes_transfered_ = 0;
-
- // Polling - should be changed to condition variable in the future
- int waitingTimeInMs = 1; // try to lookup for new incoming packages
- size_t i;
- for (i = 0; i < timeout_ms; i += waitingTimeInMs)
- {
- if (false == this->recvQueue.isQueueEmpty())
- {
- break;
- }
- boost::this_thread::sleep(boost::posix_time::milliseconds(waitingTimeInMs));
- }
- if (i >= timeout_ms)
- {
- ROS_ERROR("no answer received after %zu ms. Maybe sopas mode is wrong.\n", timeout_ms);
- return (sick_scan::ExitError);
- }
- DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
-
- *bytes_read = datagramWithTimeStamp.datagram.size();
- memcpy(buffer, &(datagramWithTimeStamp.datagram[0]), datagramWithTimeStamp.datagram.size());
- return (sick_scan::ExitSuccess);
- }
-
- /**
- * Send a SOPAS command to the device and print out the response to the console.
- */
- int SickScanAdapter::sendSOPASCommand(const char* request, std::vector<unsigned char>* reply, int cmdLen)
- {
- int sLen = 0;
- int preambelCnt = 0;
- bool cmdIsBinary = false;
-
- if (request != NULL)
- {
- sLen = cmdLen;
- preambelCnt = 0; // count 0x02 bytes to decide between ascii and binary command
- if (sLen >= 4)
- {
- for (int i = 0; i < 4; i++)
- {
- if (request[i] == 0x02)
- {
- preambelCnt++;
- }
- }
- }
-
- if (preambelCnt < 4)
- {
- cmdIsBinary = false;
- }
- else
- {
- cmdIsBinary = true;
- }
- int msgLen = 0;
- if (cmdIsBinary == false)
- {
- msgLen = strlen(request);
- }
- else
- {
- int dataLen = 0;
- for (int i = 4; i < 8; i++)
- {
- dataLen |= ((unsigned char) request[i] << (7 - i) * 8);
- }
- msgLen = 8 + dataLen + 1; // 8 Msg. Header + Packet +
- }
-#if 1
-
- bool debugBinCmd = false;
- if (debugBinCmd)
- {
- printf("=== START HEX DUMP ===\n");
- for (int i = 0; i < msgLen; i++)
- {
- unsigned char* ptr = (UINT8*) request;
- printf("%02x ", ptr[i]);
- }
- printf("\n=== END HEX DUMP ===\n");
- }
- m_nw.sendCommandBuffer((UINT8*) request, msgLen);
-#else
-
- /*
- * Write a SOPAS variable read request to the device.
- */
- try
- {
- boost::asio::write(socket_, boost::asio::buffer(request, msgLen));
- }
- catch (boost::system::system_error& e)
- {
- ROS_ERROR("write error for command: %s", request);
- diagnostics_.broadcast(getDiagnosticErrorCode(), "Write error for sendSOPASCommand.");
- return sick_scan::ExitError;
- }
-#endif
- }
-
- // Set timeout in 5 seconds
- const int BUF_SIZE = 65536;
- char buffer[BUF_SIZE];
- int bytes_read;
- // !!!
- if (readWithTimeout(getReadTimeOutInMs(), buffer, BUF_SIZE, &bytes_read, 0, cmdIsBinary) == sick_scan::ExitError)
- {
- ARMARX_INFO_S << "sendSOPASCommand: no full reply available for read after (ms): " << getReadTimeOutInMs();
- return sick_scan::ExitError;
- }
-
-
- if (reply)
- {
- reply->resize(bytes_read);
-
- std::copy(buffer, buffer + bytes_read, &(*reply)[0]);
- }
- return sick_scan::ExitSuccess;
- }
-
- int SickScanAdapter::get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
- bool isBinaryProtocol, int* numberOfRemainingFifoEntries)
- {
- return sick_scan::ExitError;
- }
-
- int SickScanAdapter::getDatagramIceTime(IceUtil::Time& recvTimeStamp, unsigned char* receiveBuffer,
- int* actual_length, int* numberOfRemainingFifoEntries)
- {
- if (NULL != numberOfRemainingFifoEntries)
- {
- *numberOfRemainingFifoEntries = 0;
- }
- this->setReplyMode(1);
- const int maxWaitInMs = getReadTimeOutInMs();
- std::vector<unsigned char> dataBuffer;
-#if 1 // prepared for reconnect
- bool retVal = this->recvQueue.waitForIncomingObject(maxWaitInMs);
- if (retVal == false)
- {
- ARMARX_ERROR_S << "Timeout during waiting for new datagram";
- return sick_scan::ExitError;
- }
- else
- {
- // Look into receiving queue for new Datagrams
- //
- //
- DatagramWithTimeStamp datagramWithTimeStamp = this->recvQueue.pop();
- if (NULL != numberOfRemainingFifoEntries)
- {
- *numberOfRemainingFifoEntries = this->recvQueue.getNumberOfEntriesInQueue();
- }
- recvTimeStamp = datagramWithTimeStamp.timeStamp;
- dataBuffer = datagramWithTimeStamp.datagram;
-
- }
-#endif
- // dataBuffer = this->recvQueue.pop();
- long size = dataBuffer.size();
- memcpy(receiveBuffer, &(dataBuffer[0]), size);
- *actual_length = size;
-
- return sick_scan::ExitSuccess;
- }
-
-}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
deleted file mode 100644
index 6fe671d13f2974a6fba85835dfb09cf35282858a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * Copyright (C) 2013, Freiburg University
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of Osnabrück University nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * Created on: 15.11.2013
- *
- * Authors:
- * Christian Dornhege <c.dornhege@googlemail.com>
- */
-
-#ifndef SICK_TIM3XX_COMMON_TCP_H
-#define SICK_TIM3XX_COMMON_TCP_H
-
-#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <boost/asio.hpp>
-
-#undef NOMINMAX // to get rid off warning C4005: "NOMINMAX": Makro-Neudefinition
-
-#include <sick_scan/sick_scan_common.h>
-#include <sick_scan/sick_generic_parser.h>
-#include <sick_scan/template_queue.h>
-
-namespace armarx
-{
- /* class prepared for optimized time stamping */
-
- class DatagramWithTimeStamp
- {
- public:
- DatagramWithTimeStamp(IceUtil::Time timeStamp_, std::vector<unsigned char> datagram_)
- {
- timeStamp = timeStamp_;
- datagram = datagram_;
- }
-
- // private:
- IceUtil::Time timeStamp;
- std::vector<unsigned char> datagram;
- };
-
-
- class SickScanAdapter : public sick_scan::SickScanCommon
- {
- public:
- static void disconnectFunctionS(void* obj);
-
- SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit, sick_scan::SickGenericParser* parser,
- char cola_dialect_id);
-
- virtual ~SickScanAdapter();
-
- int loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
-
- int parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
-
- static void readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes);
-
- void readCallbackFunction(UINT8* buffer, UINT32& numOfBytes);
-
- void setReplyMode(int _mode);
-
- int getReplyMode();
-
- bool stopScanData();
-
- int numberOfDatagramInInputFifo();
-
- SopasEventMessage findFrameInReceiveBuffer();
-
- void processFrame(IceUtil::Time timeStamp, SopasEventMessage& frame);
-
- // Queue<std::vector<unsigned char> > recvQueue;
- Queue<DatagramWithTimeStamp> recvQueue;
- UINT32 m_alreadyReceivedBytes;
- UINT32 m_lastPacketSize;
- UINT8 m_packetBuffer[480000];
- /**
- * Read callback. Diese Funktion wird aufgerufen, sobald Daten auf der Schnittstelle
- * hereingekommen sind.
- */
-
- protected:
- void disconnectFunction();
-
- void readCallbackFunctionOld(UINT8* buffer, UINT32& numOfBytes);
-
- virtual int init_device();
-
- virtual int close_device();
-
- /// Send a SOPAS command to the device and print out the response to the console.
- virtual int sendSOPASCommand(const char* request, std::vector<unsigned char>* reply, int cmdLen);
-
- /// Read a datagram from the device.
- /**
- * \param [out] recvTimeStamp timestamp of received datagram
- * \param [in] receiveBuffer data buffer to fill
- * \param [in] bufferSize max data size to write to buffer (result should be 0 terminated)
- * \param [out] actual_length the actual amount of data written
- * \param [in] isBinaryProtocol true=binary False=ASCII
- */
- virtual int get_datagram(ros::Time& recvTimeStamp, unsigned char* receiveBuffer, int bufferSize, int* actual_length,
- bool isBinaryProtocol, int* numberOfRemainingFifoEntries);
-
- int getDatagramIceTime(IceUtil::Time& recvTimeStamp, unsigned char* receiveBuffer, int* actual_length, int* numberOfRemainingFifoEntries);
-
- // Helpers for boost asio
- int readWithTimeout(size_t timeout_ms, char* buffer, int buffer_size, int* bytes_read = 0,
- bool* exception_occured = 0, bool isBinary = false);
-
- void handleRead(boost::system::error_code error, size_t bytes_transfered);
-
- void checkDeadline();
-
- private:
-
- // Response buffer
- UINT32 m_numberOfBytesInResponseBuffer; ///< Number of bytes in buffer
- UINT8 m_responseBuffer[1024]; ///< Receive buffer for everything except scan data and eval case data.
- Mutex m_receiveDataMutex; ///< Access mutex for buffer
-
- // Receive buffer
- UINT32 m_numberOfBytesInReceiveBuffer; ///< Number of bytes in buffer
- UINT8 m_receiveBuffer[480000]; ///< Low-Level receive buffer for all data
-
- bool m_beVerbose;
- bool m_emulSensor;
-
- boost::asio::io_service io_service_;
- boost::asio::ip::tcp::socket socket_;
- boost::asio::deadline_timer deadline_;
- boost::asio::streambuf input_buffer_;
- boost::system::error_code ec_;
- size_t bytes_transfered_;
-
- std::string hostname_;
- std::string port_;
- int timelimit_;
- int m_replyMode;
- };
-
-}
-#endif /* SICK_TIM3XX_COMMON_TCP_H */
-
diff --git a/source/RobotAPI/drivers/SickLaserUnit/test/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/test/CMakeLists.txt
deleted file mode 100644
index 0e95852596696c86a098f188f17370f808251b09..0000000000000000000000000000000000000000
--- a/source/RobotAPI/drivers/SickLaserUnit/test/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore SickLaserUnit)
-
-armarx_add_test(SickLaserUnitTest SickLaserUnitTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 1962d259c0d2329c9870f1e3d314fdea17a1f60d..945013a9905234d8eb807745bea34e04c8456b31 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -621,7 +621,7 @@ namespace armarx
void ArVizWidgetController::onRefreshRecordings()
{
- allRecordings = storage->getAllRecordings();
+ allRecordings = storage->getAllRecordings().recordings;
std::sort(allRecordings.begin(), allRecordings.end(),
[](viz::data::Recording const & lhs, viz::data::Recording const & rhs)
{
diff --git a/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp
index ac67b3a3b5ebfa719a6af1cca86032cd81238f25..c895e131e75cc63287f43dc323c600e825734550 100644
--- a/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp
@@ -89,7 +89,7 @@ namespace armarx
while (rec.size() < n)
{
rec.emplace_back(getRobotUnitComponentPlugin()
- .startDataSatreming(cfg));
+ .startDataStreaming(cfg));
ARMARX_INFO << rec.back()->getDataDescriptionString();
}
for (auto& r : rec)
diff --git a/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.h b/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.h
index 4b813cd58e53b9f7772a4598bad06b3c2bab5643..8c69592b4d3b18b4dca7877fe4a8de4264ba855e 100644
--- a/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.h
@@ -175,7 +175,7 @@ namespace armarx
Eigen::Matrix6f _conversion_matrix;
Eigen::Vector6f _offset;
- Eigen::Vector6f _channel_order;
+ Eigen::Vector6i _channel_order;
float _ticks_to_volt_factor;
RobotUnitDataStreamingReceiverPtr _streaming_handler;
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index 132f0b639df824fe91a3b6bd9f25427f50950d7d..a7c2374677fde81a140b3479a3dd626101d79525 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -26,6 +26,8 @@
#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitConfigDialog.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <Eigen/Geometry>
+
// Qt headers
#include <Qt>
#include <QtGlobal>
@@ -35,6 +37,8 @@
#include <QHBoxLayout>
//std
+#include <RobotAPI/interface/core/RobotLocalization.h>
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
#include <memory>
#include <cmath>
@@ -86,8 +90,8 @@ PlatformUnitWidget::PlatformUnitWidget() :
void PlatformUnitWidget::onInitComponent()
{
usingProxy(platformUnitProxyName);
- usingTopic(platformName + "State");
- ARMARX_INFO << "Listening on Topic: " << platformName + "State";
+ usingTopic("GlobalRobotPoseLocalization");
+ // ARMARX_INFO << "Listening on Topic: " << platformName + "State";
}
@@ -177,7 +181,6 @@ void PlatformUnitWidget::setNewPlatformPoseLabels(float x, float y, float alpha)
ui.labelCurrentPositionX->setText(QString::number(x));
ui.labelCurrentPositionY->setText(QString::number(y));
ui.labelCurrentRotation->setText(QString::number(alpha));
-
}
@@ -203,26 +206,16 @@ void PlatformUnitWidget::stopControlTimer()
}
-void PlatformUnitWidget::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
+void PlatformUnitWidget::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&)
{
- // moved to qt thread for thread safety
- QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, currentPose.x), Q_ARG(float, currentPose.y), Q_ARG(float, currentPose.rotationAroundZ));
- platformRotation = currentPose.rotationAroundZ;
-}
+ const Eigen::Isometry3f global_T_robot(transformStamped.transform);
+ const float x = global_T_robot.translation().x();
+ const float y = global_T_robot.translation().y();
+ const float yaw = simox::math::mat3f_to_rpy(global_T_robot.linear()).z();
-
-void PlatformUnitWidget::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
-{
// moved to qt thread for thread safety
- QMetaObject::invokeMethod(this, "setNewTargetPoseLabels",
- Q_ARG(float, newPlatformPositionX),
- Q_ARG(float, newPlatformPositionY),
- Q_ARG(float, newPlatformRotation));
-}
-
-void PlatformUnitWidget::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
-{
-
+ QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, x), Q_ARG(float, y), Q_ARG(float, yaw));
+ platformRotation = yaw;
}
@@ -396,9 +389,3 @@ void KeyboardPlatformHookWidget::keyReleaseEvent(QKeyEvent* event)
}
QWidget::keyReleaseEvent(event);
}
-
-
-void armarx::PlatformUnitWidget::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
-{
- // ignore for now
-}
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
index aeb8e8ce8f414bac77ec83e9e9237679b76873c8..7587750d212e51f88666dffcf236556370b432c3 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
@@ -30,6 +30,7 @@
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <RobotAPI/interface/core/RobotLocalization.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
/* Qt headers */
@@ -101,7 +102,7 @@ namespace armarx
*/
class PlatformUnitWidget :
public ArmarXComponentWidgetControllerTemplate<PlatformUnitWidget>,
- public PlatformUnitListener
+ public GlobalRobotPoseLocalizationListener
{
Q_OBJECT
public:
@@ -116,11 +117,8 @@ namespace armarx
void onExitComponent() override;
// slice interface implementation
- void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;
-
+ void reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current& = ::Ice::emptyCurrent) override;
+
// inherited of ArmarXWidget
static QString GetWidgetName()
{
@@ -228,4 +226,3 @@ namespace armarx
};
using PlatformUnitGuiPluginPtr = std::shared_ptr<PlatformUnitWidget>;
}
-
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
index 89a90ab482f9916ff11ad149faa1a3fc1b7d483c..b40d5a2fcf9c480a71f0576d049ad0a311813815 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/CMakeLists.txt
@@ -9,15 +9,22 @@ set(SOURCES
aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
+ aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp
aronTreeWidget/Data.cpp
+ aronTreeWidget/widgets/CustomWidget.cpp
+ aronTreeWidget/widgets/EditMatrixWidget.cpp
+ aronTreeWidget/widgets/IntEnumWidget.cpp
+ aronTreeWidget/ListDictHelper.cpp
+ aronTreeWidget/widgets/QuaternionWidget.cpp
aronTreeWidget/AronTreeWidgetItem.cpp
aronTreeWidget/AronTreeWidgetController.cpp
- aronTreeWidget/modal/AronTreeWidgetModal.cpp
- aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.cpp
- aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.cpp
- aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.cpp
aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp
aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
+ aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.cpp
+ aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.cpp
+ aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.cpp
+ aronTreeWidget/modal/AronTreeWidgetModal.cpp
+ ColorPalettes.cpp
SkillManagerMonitorWidgetController.cpp
)
@@ -26,25 +33,29 @@ set(HEADERS
aronTreeWidget/visitors/AronTreeWidgetConverter.h
aronTreeWidget/visitors/AronTreeWidgetSetter.h
aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
+ aronTreeWidget/visitors/AronTreeWidgetContextMenu.h
aronTreeWidget/Data.h
+ aronTreeWidget/widgets/NDArrayHelper.h
+ aronTreeWidget/widgets/EditMatrixWidget.h
+ aronTreeWidget/widgets/CustomWidget.h
+ aronTreeWidget/widgets/IntEnumWidget.h
+ aronTreeWidget/ListDictHelper.h
+ aronTreeWidget/widgets/QuaternionWidget.h
aronTreeWidget/AronTreeWidgetItem.h
aronTreeWidget/AronTreeWidgetController.h
aronTreeWidget/modal/AronTreeWidgetModal.h
- aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.h
- aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.h
- aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.h
aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h
aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.h
+ aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.h
+ aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.h
+ aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.h
+ ColorPalettes.h
SkillManagerMonitorWidgetController.h
)
set(GUI_UIS
SkillManagerMonitorWidget.ui
- aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModal.ui
- aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModal.ui
- aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModal.ui
aronTreeWidget/modal/text/AronTreeWidgetTextInputModal.ui
- aronTreeWidget/modal/dict/AronTreeWidgetDictInputModal.ui
)
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..05cd7a5cc08224561af8df64cf6be2347adb3e37
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.cpp
@@ -0,0 +1,32 @@
+#include "ColorPalettes.h"
+
+namespace armarx::gui_color_palette
+{
+ QPalette
+ getErrorPalette()
+ {
+ QPalette errorPalette;
+ errorPalette.setColor(QPalette::Base, Qt::red);
+ return errorPalette;
+ }
+ QPalette
+ getNormalPalette()
+ {
+
+ QPalette normalPalette;
+
+ normalPalette.setColor(QPalette::Base, Qt::white);
+ return normalPalette;
+ }
+
+ QPalette
+ getIndirectErrorPalette()
+ {
+ QPalette indirectErr;
+ static QColor orange = QColor::fromRgb(255, 165, 0);
+ // orange color
+ indirectErr.setColor(QPalette::Base, orange);
+ return indirectErr;
+ }
+
+} // namespace armarx::gui_color_palette
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.h
new file mode 100644
index 0000000000000000000000000000000000000000..ea2c247a9428559de4be7bdb8fc1497c14b1ea00
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.h
@@ -0,0 +1,11 @@
+#pragma once
+#include <QPalette>
+
+namespace armarx::gui_color_palette
+{
+ QPalette getNormalPalette();
+
+ QPalette getErrorPalette();
+
+ QPalette getIndirectErrorPalette();
+} // namespace armarx::gui_color_palette
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
index de1c749700be65384719385ede79e9fe3d0cc70f..1e80202efb18b2aca77e8ba45b87827e362ae233 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidget.ui
@@ -6,31 +6,37 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1015</width>
- <height>498</height>
+ <width>1060</width>
+ <height>657</height>
</rect>
</property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="windowTitle">
<string>SkillManagerMonitorWidget</string>
</property>
- <layout class="QHBoxLayout" name="horizontalLayout">
- <item>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="0">
<widget class="QSplitter" name="splitter_2">
- <property name="enabled">
- <bool>true</bool>
- </property>
- <property name="lineWidth">
- <number>5</number>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
</property>
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
- <property name="handleWidth">
- <number>25</number>
+ <property name="childrenCollapsible">
+ <bool>false</bool>
</property>
<widget class="QGroupBox" name="groupBoxActiveSkills">
<property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@@ -38,10 +44,7 @@
<property name="title">
<string>Active Skills</string>
</property>
- <layout class="QGridLayout" name="gridLayout_3">
- <property name="bottomMargin">
- <number>9</number>
- </property>
+ <layout class="QGridLayout" name="gridLayout_4">
<item row="0" column="0">
<widget class="QListWidget" name="listWidgetActiveSkills">
<property name="sizePolicy">
@@ -55,20 +58,33 @@
</layout>
</widget>
<widget class="QSplitter" name="splitter">
- <property name="lineWidth">
- <number>5</number>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
- <property name="handleWidth">
- <number>25</number>
- </property>
<widget class="QGroupBox" name="groupBoxSkills">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="title">
<string>Manager</string>
</property>
<layout class="QGridLayout" name="gridLayout">
+ <item row="2" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Update Frequency:</string>
+ </property>
+ </widget>
+ </item>
<item row="3" column="0" colspan="3">
<widget class="QTreeWidget" name="treeWidgetSkills">
<column>
@@ -91,22 +107,27 @@
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBoxUpdateFreq"/>
</item>
- <item row="2" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Update Frequency:</string>
- </property>
- </widget>
- </item>
</layout>
</widget>
<widget class="QGroupBox" name="groupBoxSkillDetails">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="title">
<string>Skill Details</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
<item row="1" column="0" colspan="4">
<widget class="QTreeWidget" name="treeWidgetSkillDetails">
+ <property name="contextMenuPolicy">
+ <enum>Qt::CustomContextMenu</enum>
+ </property>
+ <property name="editTriggers">
+ <set>QAbstractItemView::DoubleClicked|QAbstractItemView::EditKeyPressed</set>
+ </property>
<column>
<property name="text">
<string>Key</string>
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 27a954593ad99d252410ffec11d086689c1d9e07..edc9eebb6d619dd09de033ac2959b4858d263246 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -20,52 +20,57 @@
* GNU General Public License
*/
+#include "SkillManagerMonitorWidgetController.h"
+
#include <string>
-#include "SkillManagerMonitorWidgetController.h"
+#include <RobotAPI/libraries/skills/provider/Skill.h>
-#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
#include "aronTreeWidget/visitors/AronTreeWidgetConverter.h"
+#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
#include "aronTreeWidget/visitors/AronTreeWidgetModalCreator.h"
-#include <RobotAPI/libraries/skills/provider/Skill.h>
-
// modals
#include "aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h"
// debug
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
-#include <QDoubleSpinBox>
#include <QClipboard>
+#include <QDoubleSpinBox>
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
#include "aronTreeWidget/Data.h"
//config
namespace armarx
{
- QPointer<QDialog> SkillManagerMonitorWidgetController::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ SkillManagerMonitorWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
dialog = new SimpleConfigDialog(parent);
- dialog->addProxyFinder<skills::manager::dti::SkillManagerInterfacePrx>("SkillManager", "", "Skill*");
+ dialog->addProxyFinder<skills::manager::dti::SkillManagerInterfacePrx>(
+ "SkillManager", "", "Skill*");
}
return qobject_cast<SimpleConfigDialog*>(dialog);
}
- void SkillManagerMonitorWidgetController::configured()
+ void
+ SkillManagerMonitorWidgetController::configured()
{
observerName = dialog->getProxyName("SkillManager");
}
- void SkillManagerMonitorWidgetController::loadSettings(QSettings* settings)
+ void
+ SkillManagerMonitorWidgetController::loadSettings(QSettings* settings)
{
observerName = settings->value("SkillManager", "SkillManager").toString().toStdString();
}
- void SkillManagerMonitorWidgetController::saveSettings(QSettings* settings)
+ void
+ SkillManagerMonitorWidgetController::saveSettings(QSettings* settings)
{
settings->setValue("SkillManager", QString::fromStdString(observerName));
}
-}
+} // namespace armarx
// Others
namespace armarx
@@ -81,29 +86,40 @@ namespace armarx
widget.doubleSpinBoxUpdateFreq->setSuffix(" Hz");
refreshSkillsResultTimer = new QTimer(this);
- refreshSkillsResultTimer->setInterval(1000 / 5); // Keep this stable.
+ refreshSkillsResultTimer->setInterval(1000 / 5); // Keep this stable.
refreshSkillsResultTimer->start();
- connect(widget.doubleSpinBoxUpdateFreq, &QDoubleSpinBox::editingFinished,
- this, &SkillManagerMonitorWidgetController::updateTimerFrequency);
- connect(refreshSkillsResultTimer, &QTimer::timeout,
- this, &SkillManagerMonitorWidgetController::refreshSkills);
-
- connect(widget.pushButtonCopy, &QPushButton::clicked,
- this, &SkillManagerMonitorWidgetController::copyCurrentConfig);
- connect(widget.pushButtonPaste, &QPushButton::clicked,
- this, &SkillManagerMonitorWidgetController::pasteCurrentConfig);
-
- connect(widget.pushButtonExecuteSkill, &QPushButton::clicked,
- this, &SkillManagerMonitorWidgetController::executeSkill);
- connect(widget.pushButtonStopSkill, &QPushButton::clicked,
- this, &SkillManagerMonitorWidgetController::stopSkill);
-
- connect(widget.treeWidgetSkills, &QTreeWidget::currentItemChanged,
- this, &SkillManagerMonitorWidgetController::skillSelectionChanged);
-
- connect(widget.treeWidgetSkillDetails, &QTreeWidget::itemDoubleClicked,
- this, &SkillManagerMonitorWidgetController::onTreeWidgetItemDoubleClicked);
+ connect(widget.doubleSpinBoxUpdateFreq,
+ &QDoubleSpinBox::editingFinished,
+ this,
+ &SkillManagerMonitorWidgetController::updateTimerFrequency);
+ connect(refreshSkillsResultTimer,
+ &QTimer::timeout,
+ this,
+ &SkillManagerMonitorWidgetController::refreshSkills);
+
+ connect(widget.pushButtonCopy,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::copyCurrentConfig);
+ connect(widget.pushButtonPaste,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::pasteCurrentConfig);
+
+ connect(widget.pushButtonExecuteSkill,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::executeSkill);
+ connect(widget.pushButtonStopSkill,
+ &QPushButton::clicked,
+ this,
+ &SkillManagerMonitorWidgetController::stopSkill);
+
+ connect(widget.treeWidgetSkills,
+ &QTreeWidget::currentItemChanged,
+ this,
+ &SkillManagerMonitorWidgetController::skillSelectionChanged);
}
SkillManagerMonitorWidgetController::~SkillManagerMonitorWidgetController()
@@ -111,25 +127,38 @@ namespace armarx
}
- void SkillManagerMonitorWidgetController::onInitComponent()
+ void
+ SkillManagerMonitorWidgetController::reconnectToSkillManager()
+ {
+ if (connected)
+ {
+ getProxy(manager, observerName, 1000);
+ }
+ }
+
+ void
+ SkillManagerMonitorWidgetController::onInitComponent()
{
usingProxy(observerName);
}
- void SkillManagerMonitorWidgetController::onConnectComponent()
+ void
+ SkillManagerMonitorWidgetController::onConnectComponent()
{
- getProxy(manager, observerName);
widget.groupBoxSkills->setTitle(QString::fromStdString(observerName));
- widget.treeWidgetSkillDetails->setEditTriggers(QAbstractItemView::EditTrigger::NoEditTriggers);
+ widget.treeWidgetSkillDetails->setEditTriggers(
+ QAbstractItemView::EditTrigger::NoEditTriggers);
widget.treeWidgetSkillDetails->setColumnHidden(3, true);
connected = true;
}
- void SkillManagerMonitorWidgetController::onDisconnectComponent()
+ void
+ SkillManagerMonitorWidgetController::onDisconnectComponent()
{
connected = false;
+ manager = nullptr;
// reset all
skills.clear();
@@ -140,25 +169,30 @@ namespace armarx
selectedSkill.skillName = "";
}
- void SkillManagerMonitorWidgetController::updateTimerFrequency()
+ void
+ SkillManagerMonitorWidgetController::updateTimerFrequency()
{
int f = static_cast<int>(std::round(1000 / widget.doubleSpinBoxUpdateFreq->value()));
refreshSkillsResultTimer->setInterval(f);
}
- void SkillManagerMonitorWidgetController::refreshSkills()
+ void
+ SkillManagerMonitorWidgetController::refreshSkills()
{
- static std::map<skills::provider::dto::Execution::Status, std::string> ExecutionStatus2String = {
- {skills::provider::dto::Execution::Status::Aborted, "Aborted"},
- {skills::provider::dto::Execution::Status::Failed, "Failed"},
- {skills::provider::dto::Execution::Status::Idle, "Not yet started"},
- {skills::provider::dto::Execution::Status::Running, "Running"},
- {skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
- {skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}
- };
-
- if (!connected)
+ static std::map<skills::provider::dto::Execution::Status, std::string>
+ ExecutionStatus2String = {
+ {skills::provider::dto::Execution::Status::Aborted, "Aborted"},
+ {skills::provider::dto::Execution::Status::Failed, "Failed"},
+ {skills::provider::dto::Execution::Status::Idle, "Not yet started"},
+ {skills::provider::dto::Execution::Status::Running, "Running"},
+ {skills::provider::dto::Execution::Status::Scheduled, "Scheduled"},
+ {skills::provider::dto::Execution::Status::Succeeded, "Succeeded"}};
+
+ if (!manager)
+ {
+ reconnectToSkillManager();
return;
+ }
/* CHECK OWN SKILLS LIST */
// remove non-existing ones
@@ -196,7 +230,9 @@ namespace armarx
while (i < widget.treeWidgetSkills->topLevelItemCount())
{
QTreeWidgetItem* item = widget.treeWidgetSkills->topLevelItem(i);
- if (std::find(removedProviders.begin(), removedProviders.end(), item->text(0).toStdString()) != removedProviders.end())
+ if (std::find(removedProviders.begin(),
+ removedProviders.end(),
+ item->text(0).toStdString()) != removedProviders.end())
{
delete widget.treeWidgetSkills->takeTopLevelItem(i);
}
@@ -209,7 +245,8 @@ namespace armarx
// add new providers
for (const auto& [providerName, providerSkills] : skills)
{
- if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName); it != newProviders.end())
+ if (auto it = std::find(newProviders.begin(), newProviders.end(), providerName);
+ it != newProviders.end())
{
auto item = new QTreeWidgetItem(widget.treeWidgetSkills);
item->setText(0, QString::fromStdString(providerName));
@@ -225,7 +262,7 @@ namespace armarx
// update status and active skills window
std::map<skills::SkillID, std::string> activeSkillsAndPrefixes;
auto managerStatuses = manager->getSkillExecutionStatuses();
- for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
+ for (int i = 0; i < widget.treeWidgetSkills->topLevelItemCount(); ++i)
{
try
{
@@ -240,11 +277,15 @@ namespace armarx
skills::SkillID currentSkillId(providerName, skillItem->text(0).toStdString());
auto statusForSkill = allStatusesForProvider.at(currentSkillId.skillName);
- skillItem->setText(2, QString::fromStdString(ExecutionStatus2String.at(statusForSkill.header.status)));
+ skillItem->setText(2,
+ QString::fromStdString(ExecutionStatus2String.at(
+ statusForSkill.header.status)));
- if (not statusForSkill.header.executorName.empty()) // it means that the skill was called by someone
+ if (not statusForSkill.header.executorName
+ .empty()) // it means that the skill was called by someone
{
- activeSkillsAndPrefixes.insert({currentSkillId, statusForSkill.header.executorName});
+ activeSkillsAndPrefixes.insert(
+ {currentSkillId, statusForSkill.header.executorName});
}
}
}
@@ -261,7 +302,8 @@ namespace armarx
{
auto prefixedStr = id.toString(prefix);
bool longest = true;
- for (const auto& [id2, prefix2] : activeSkillsAndPrefixes) // check if there is a deeper skill currently executing
+ for (const auto& [id2, prefix2] :
+ activeSkillsAndPrefixes) // check if there is a deeper skill currently executing
{
auto prefixedStr2 = id.toString(prefix2);
if (prefixedStr == prefixedStr2)
@@ -278,12 +320,14 @@ namespace armarx
if (longest)
{
- widget.listWidgetActiveSkills->addItem(QString::fromStdString(id.toString() + ": " + id.toString(prefix)));
+ widget.listWidgetActiveSkills->addItem(
+ QString::fromStdString(id.toString() + ": " + id.toString(prefix)));
}
}
}
- void SkillManagerMonitorWidgetController::executeSkill()
+ void
+ SkillManagerMonitorWidgetController::executeSkill()
{
if (selectedSkill.providerName.empty() or selectedSkill.skillName.empty())
{
@@ -307,12 +351,14 @@ namespace armarx
exInfo.skillId = {selectedSkill.providerName, selectedSkill.skillName};
exInfo.params = aron::data::Dict::ToAronDictDTO(data);
- ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName << ". The data was: " << data;
+ ARMARX_IMPORTANT << "Executing skill from GUI: " << selectedSkill.providerName << "/"
+ << selectedSkill.skillName << ". The data was: " << data;
// Note that we execute the skill in a seperate thread so that the GUI thread does not freeze.
manager->begin_executeSkill(exInfo);
}
- void SkillManagerMonitorWidgetController::stopSkill()
+ void
+ SkillManagerMonitorWidgetController::stopSkill()
{
if (selectedSkill.providerName.empty() or selectedSkill.skillName.empty())
{
@@ -325,11 +371,14 @@ namespace armarx
return;
}
- ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.providerName << "/" << selectedSkill.skillName;
+ ARMARX_INFO << "Stopping skill from GUI: " << selectedSkill.providerName << "/"
+ << selectedSkill.skillName;
manager->abortSkill(selectedSkill.providerName, selectedSkill.skillName);
}
- void SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current, QTreeWidgetItem*)
+ void
+ SkillManagerMonitorWidgetController::skillSelectionChanged(QTreeWidgetItem* current,
+ QTreeWidgetItem*)
{
widget.groupBoxSkillDetails->setEnabled(false);
@@ -352,10 +401,12 @@ namespace armarx
newSelectedSkill.skillName = current->text(0).toStdString();
// setup groupBox
- widget.groupBoxSkillDetails->setTitle(QString::fromStdString(newSelectedSkill.providerName + "/" + newSelectedSkill.skillName));
+ widget.groupBoxSkillDetails->setTitle(QString::fromStdString(
+ newSelectedSkill.providerName + "/" + newSelectedSkill.skillName));
widget.groupBoxSkillDetails->setEnabled(true);
- if (newSelectedSkill.providerName == selectedSkill.providerName and newSelectedSkill.skillName == selectedSkill.skillName)
+ if (newSelectedSkill.providerName == selectedSkill.providerName and
+ newSelectedSkill.skillName == selectedSkill.skillName)
{
return;
}
@@ -370,37 +421,46 @@ namespace armarx
auto skillDesc = skills.at(selectedSkill.providerName).at(selectedSkill.skillName);
{
- auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
- {QString::fromStdString("Name"), QString::fromStdString(skillDesc.skillName)});
+ auto it = new QTreeWidgetItem(
+ widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Name"), QString::fromStdString(skillDesc.skillName)});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
{
- auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
- {QString::fromStdString("Robot"), QString::fromStdString(simox::alg::join(skillDesc.robots, ", "))});
+ auto it = new QTreeWidgetItem(
+ widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Robot"),
+ QString::fromStdString(simox::alg::join(skillDesc.robots, ", "))});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
{
auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
- {QString::fromStdString("Description"), QString::fromStdString(skillDesc.description)});
+ {QString::fromStdString("Description"),
+ QString::fromStdString(skillDesc.description)});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
{
- auto it = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
- {QString::fromStdString("Timeout"), QString::fromStdString(std::to_string(skillDesc.timeoutMs)) + " ms"});
+ auto it = new QTreeWidgetItem(
+ widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Timeout"),
+ QString::fromStdString(std::to_string(skillDesc.timeoutMs)) + " ms"});
widget.treeWidgetSkillDetails->addTopLevelItem(it);
}
- skillsArgumentsTreeWidgetItem = new QTreeWidgetItem(widget.treeWidgetSkillDetails, {QString::fromStdString("Arguments")});
+ skillsArgumentsTreeWidgetItem = new QTreeWidgetItem(widget.treeWidgetSkillDetails,
+ {QString::fromStdString("Arguments")});
auto aron_args = aron::type::Object::FromAronObjectDTO(skillDesc.acceptedType);
auto default_args = aron::data::Dict::FromAronDictDTO(skillDesc.defaultParams);
- aronTreeWidgetController = std::make_shared<AronTreeWidgetController>(widget.treeWidgetSkillDetails, skillsArgumentsTreeWidgetItem, aron_args, default_args);
+ aronTreeWidgetController = std::make_shared<AronTreeWidgetController>(
+ widget.treeWidgetSkillDetails, skillsArgumentsTreeWidgetItem, aron_args, default_args);
}
- aron::data::DictPtr SkillManagerMonitorWidgetController::getConfigAsAron() const
+ aron::data::DictPtr
+ SkillManagerMonitorWidgetController::getConfigAsAron() const
{
// create argument aron (if there is an accepted type set)
if (aronTreeWidgetController)
@@ -410,7 +470,8 @@ namespace armarx
return nullptr;
}
- void SkillManagerMonitorWidgetController::copyCurrentConfig()
+ void
+ SkillManagerMonitorWidgetController::copyCurrentConfig()
{
auto data = getConfigAsAron();
if (!data)
@@ -423,7 +484,8 @@ namespace armarx
clipboard->setText(QString::fromStdString(json.dump(2)));
}
- void SkillManagerMonitorWidgetController::pasteCurrentConfig()
+ void
+ SkillManagerMonitorWidgetController::pasteCurrentConfig()
{
QClipboard* clipboard = QApplication::clipboard();
std::string s = clipboard->text().toStdString();
@@ -438,34 +500,10 @@ namespace armarx
aronTreeWidgetController->setFromAron(data);
}
- void SkillManagerMonitorWidgetController::resetCurrentConfig()
+ void
+ SkillManagerMonitorWidgetController::resetCurrentConfig()
{
// TODO
}
- void SkillManagerMonitorWidgetController::onTreeWidgetItemDoubleClicked(QTreeWidgetItem* item, int column)
- {
- if (!item)
- {
- return;
- }
-
- if (column == 1)
- {
- if (item->flags() & Qt::ItemIsEditable) // we use the flag to indicate whether the item is editable or not
- {
- // we assume its aron item
- AronTreeWidgetItem* aItem = AronTreeWidgetItem::DynamicCastAndCheck(item);
- std::string name = aItem->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME).toStdString();
- std::string type = aItem->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE).toStdString();
-
- // why visitor?!?!?
- AronTreeWidgetModalCreatorVisitor v(name, aItem, widget.treeWidgetSkillDetails);
- aron::type::visit(v, aItem->aronType);
- auto modal = v.createdModal;
- modal->exec();
- }
- }
- }
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
index 64256f2d6530d766726df37ec5fc60c9429d3e70..d9a9379bdfdea0ee56c9c5f3e601ef68a5ce75b5 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.h
@@ -22,25 +22,24 @@
#pragma once
#include <stack>
-#include <vector>
#include <thread>
+#include <vector>
+
+#include <QTimer>
+
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <RobotAPI/interface/skills/SkillMemoryInterface.h>
-
#include <RobotAPI/gui-plugins/SkillManagerPlugin/ui_SkillManagerMonitorWidget.h>
-
-#include "aronTreeWidget/AronTreeWidgetController.h"
-
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <RobotAPI/interface/skills/SkillMemoryInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
-#include <QTimer>
+#include "aronTreeWidget/AronTreeWidgetController.h"
namespace armarx
{
@@ -71,6 +70,8 @@ namespace armarx
return "Skills.Manager";
}
+ void reconnectToSkillManager();
+
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
@@ -88,7 +89,6 @@ namespace armarx
void pasteCurrentConfig();
void resetCurrentConfig();
- void onTreeWidgetItemDoubleClicked(QTreeWidgetItem * item, int column);
private:
aron::data::DictPtr getConfigAsAron() const;
@@ -120,11 +120,13 @@ namespace armarx
AronTreeWidgetControllerPtr aronTreeWidgetController = nullptr;
// others
- std::atomic_bool connected = false;
QTimer* refreshSkillsResultTimer;
// skillExecutions
std::vector<std::thread> executions;
+
+ // connected flag
+ std::atomic_bool connected = false;
};
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/TODO b/source/RobotAPI/gui-plugins/SkillManagerPlugin/TODO
new file mode 100644
index 0000000000000000000000000000000000000000..d25aead078c039b5724f9a6f05abe669ed1caf50
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/TODO
@@ -0,0 +1,3 @@
+- keep set values around to not loose precision of values that have not been manipulated (in AronTreeWidgetItem, as well as Matrix and Quaternion)
+- get rid of any todos present
+
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
index 0ef63ff506ebb1f624f7380e61309f02c522b915..52110b494a2bfc38a361d8354eb18efccc760617 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
@@ -1,43 +1,62 @@
#include "AronTreeWidgetController.h"
+#include <RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h>
+
+#include "visitors/AronTreeWidgetContextMenu.h"
#include "visitors/AronTreeWidgetConverter.h"
#include "visitors/AronTreeWidgetSetter.h"
+
namespace armarx
{
- AronTreeWidgetController::AronTreeWidgetController(QTreeWidget* tree, QTreeWidgetItem* parent, const aron::type::ObjectPtr& type, const aron::data::DictPtr& data):
- parent(parent),
- tree(tree),
- type(type)
+ AronTreeWidgetController::AronTreeWidgetController(QTreeWidget* tree,
+ QTreeWidgetItem* parent,
+ const aron::type::ObjectPtr& type,
+ const aron::data::DictPtr& data) :
+ parent(parent), tree(tree), type(type)
{
- if (type) // if there is a type set, we create a tree widget from the typp
+ connect(tree,
+ SIGNAL(customContextMenuRequested(const QPoint&)),
+ this,
+ SLOT(ShowContextMenu(const QPoint&)));
+ connect(tree,
+ &QTreeWidget::itemDoubleClicked,
+ this,
+ &AronTreeWidgetController::onTreeWidgetItemDoubleClicked);
+
+ if (type) // if there is a type set, we create a tree widget from the type
{
- AronTreeWidgetCreatorVisitor v;
+ AronTreeWidgetCreatorVisitor v(parent);
+ v.setTopLevelWidget(tree);
aron::type::visit(v, type);
- if (v.createdQWidgetItem)
- {
- parent->addChild(v.createdQWidgetItem);
- }
-
if (data) // check if there is a default argument set. Prefill the GUI with it
{
setFromAron(data);
}
}
- else if(data) // there is no type but a default configuration. Prefill the GUI with the default arguments
+ // there is no type but a default configuration. Prefill the GUI with the default arguments
+ else if (data)
{
+ // TODO: There is no visitor for that (yet)...
// create type from data, ...
}
else
{
new QTreeWidgetItem(parent, {QString::fromStdString("No args")});
}
+
+ // connect change handling after args init
+ connect(tree,
+ &QTreeWidget::itemChanged,
+ this,
+ &AronTreeWidgetController::onTreeWidgetItemChanged);
}
- aron::data::DictPtr AronTreeWidgetController::convertToAron() const
+ aron::data::DictPtr
+ AronTreeWidgetController::convertToAron() const
{
- if (parent)
+ if (parent && type)
{
AronTreeWidgetConverterVisitor v(parent, 0);
aron::type::visit(v, type);
@@ -48,7 +67,8 @@ namespace armarx
return nullptr;
}
- void AronTreeWidgetController::setFromAron(const aron::data::DictPtr& data)
+ void
+ AronTreeWidgetController::setFromAron(const aron::data::DictPtr& data)
{
if (parent)
{
@@ -56,4 +76,87 @@ namespace armarx
aron::data::visit(v, data);
}
}
-}
+ void
+ AronTreeWidgetController::ShowContextMenu(const QPoint& pos)
+ {
+ tree->blockSignals(true);
+
+ auto idx = tree->indexAt(pos);
+ AronTreeWidgetItem* clickedItem = AronTreeWidgetItem::DynamicCast(tree->itemAt(pos));
+ if (clickedItem)
+ {
+ AronTreeWidgetContextMenuVisitor visitor(clickedItem, pos, tree, idx.row());
+
+ aron::type::visit(visitor, clickedItem->aronType);
+ visitor.showMenuAndExecute();
+ }
+
+ tree->blockSignals(false);
+ }
+ void
+ AronTreeWidgetController::onTreeWidgetItemDoubleClicked(QTreeWidgetItem* item, int column)
+ {
+ if (!item)
+ {
+ return;
+ }
+ tree->blockSignals(true);
+
+ auto* aronItem = AronTreeWidgetItem::DynamicCast(item);
+ if (column == 1 && aronItem)
+ {
+
+
+ std::string name =
+ aronItem->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME).toStdString();
+ // depending on aron type, create extra gui element.
+ AronTreeWidgetModalCreatorVisitor v(name, aronItem, tree);
+ aron::type::visit(v, aronItem->aronType);
+ auto modal = v.createdModal;
+
+ // if no modal is created, we instead use the edit field directly
+ if (modal)
+ {
+ modal->exec();
+ }
+ else if (aronItem->col1Editable)
+ {
+ item->treeWidget()->editItem(item, column);
+ }
+ }
+ else if (column == 0 && aronItem && aronItem->col0Editable)
+ {
+ item->treeWidget()->editItem(item, column);
+ }
+
+ tree->blockSignals(false);
+ }
+
+ void
+ AronTreeWidgetController::onTreeWidgetItemChanged(QTreeWidgetItem* item, int column)
+ {
+ tree->blockSignals(true);
+
+ auto* aronElem = AronTreeWidgetItem::DynamicCast(item);
+ if (aronElem)
+ {
+ aronElem->onUserChange(column);
+ }
+ // start conversion for entire tree -- this also sets the highlighting
+ if (parent->childCount() == 1)
+ {
+ auto* aronTreeRoot = AronTreeWidgetItem::DynamicCast(parent->child(0));
+ aronTreeRoot->resetError();
+ if (aronTreeRoot)
+ {
+ AronTreeWidgetConverterVisitor v(parent, 0);
+ aron::type::visit(v, type);
+ aronTreeRoot->setValueErrorState(v.hasDirectError(), v.onlyChildFailedConversion());
+ }
+ }
+ // else perhaps the GUI was stopped or died.
+
+ tree->blockSignals(false);
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h
index c70b0277b41684a7144319a3c9d07fcf7dd31668..c3c2c9c2c913fba11fdd4dfc362e98297c4338f4 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.h
@@ -1,24 +1,30 @@
#pragma once
#include <stack>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include "Data.h"
+#include <QTreeWidget>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <QTreeWidget>
-#include "visitors/AronTreeWidgetCreator.h"
#include "AronTreeWidgetItem.h"
+#include "Data.h"
+#include "visitors/AronTreeWidgetCreator.h"
namespace armarx
{
- class AronTreeWidgetController
+ // Main controller for any AronTreeWidget GUI. It attaches itself to the parent and needs the active widget.
+ // It's responsible to handle all signals for non-widget fields and click events.
+ class AronTreeWidgetController : public QObject
{
-
+ Q_OBJECT
public:
- AronTreeWidgetController(QTreeWidget* tree, QTreeWidgetItem* parent, const aron::type::ObjectPtr& type, const aron::data::DictPtr& data = nullptr);
+ AronTreeWidgetController(QTreeWidget* tree,
+ QTreeWidgetItem* parent,
+ const aron::type::ObjectPtr& type,
+ const aron::data::DictPtr& data = nullptr);
aron::data::DictPtr convertToAron() const;
void setFromAron(const aron::data::DictPtr&);
@@ -28,7 +34,19 @@ namespace armarx
QTreeWidget* tree;
aron::type::ObjectPtr type;
+
+ private slots:
+ // allows most primitive fields to be directly editable in the tree
+ // only String will open up a new widget
+ void onTreeWidgetItemDoubleClicked(QTreeWidgetItem* item, int column);
+ // check the new user input. Maybe undo if the field must not be edited.
+ // Also highlight if the input cannot be parsed. (Or the errors are now fixed)
+ void onTreeWidgetItemChanged(QTreeWidgetItem* item, int column);
+
+ public slots:
+ // hook for items to show a context menu (add / delete element)
+ void ShowContextMenu(const QPoint&);
};
using AronTreeWidgetControllerPtr = std::shared_ptr<AronTreeWidgetController>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
index e53161bb8b2ee4aee0c34ac2fc82c3214894f0cd..29d895f7b0f9666ebf42acd2bf3046ac90d7f1db 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
@@ -1,21 +1,24 @@
#include "AronTreeWidgetItem.h"
+#include <QAction>
+#include <QMenu>
+
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include "../ColorPalettes.h"
+#include "visitors/AronTreeWidgetConverter.h"
+#include "widgets/CustomWidget.h"
+
namespace armarx
{
- AronTreeWidgetItem* AronTreeWidgetItem::DynamicCast(QTreeWidgetItem* i)
+ AronTreeWidgetItem*
+ AronTreeWidgetItem::DynamicCast(QTreeWidgetItem* i)
{
return dynamic_cast<AronTreeWidgetItem*>(i);
}
- AronTreeWidgetItem* AronTreeWidgetItem::copy()
- {
- AronTreeWidgetItem* ret = new AronTreeWidgetItem(*this);
- return ret;
- }
-
- AronTreeWidgetItem* AronTreeWidgetItem::DynamicCastAndCheck(QTreeWidgetItem* i)
+ AronTreeWidgetItem*
+ AronTreeWidgetItem::DynamicCastAndCheck(QTreeWidgetItem* i)
{
if (!i)
{
@@ -25,4 +28,174 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(c);
return c;
}
-}
+
+ bool
+ AronTreeWidgetItem::isValueErrorneous()
+ {
+ return itemValueError;
+ }
+
+ void
+ AronTreeWidgetItem::setValueErrorState(bool isErrorSource, bool isTransitiveError)
+ {
+ itemValueError |= isErrorSource;
+ transitiveValueError |= isTransitiveError;
+
+
+ if (CustomWidget::DynamicCast(treeWidget()->itemWidget(this, 1)))
+ {
+ // The widgets handle errors themselves
+ return;
+ }
+ auto palette = gui_color_palette::getNormalPalette();
+ if (itemValueError)
+ {
+ palette = gui_color_palette::getErrorPalette();
+ }
+ else if (transitiveValueError)
+ {
+ palette = gui_color_palette::getIndirectErrorPalette();
+ }
+
+ QTreeWidgetItem::setBackground(1, QBrush(palette.color(QPalette::Base)));
+ }
+
+ void
+ AronTreeWidgetItem::setKeyErrorState(bool hasError)
+ {
+ ARMARX_CHECK(col0Editable); //only editable keys should call this function!
+ auto palette =
+ hasError ? gui_color_palette::getErrorPalette() : gui_color_palette::getNormalPalette();
+
+ keyValueError = hasError;
+
+ QTreeWidgetItem::setBackground(1, QBrush(palette.color(QPalette::Base)));
+ }
+
+ void
+ AronTreeWidgetItem::resetError()
+ {
+ keyValueError = false;
+ itemValueError = false;
+ transitiveValueError = false;
+ // also reset children
+ for (int i = 0; i < childCount(); ++i)
+ {
+ auto* arChild = AronTreeWidgetItem::DynamicCastAndCheck(QTreeWidgetItem::child(i));
+ arChild->resetError();
+ }
+ }
+
+
+ void
+ AronTreeWidgetItem::checkKeyValidityOfChildren()
+ {
+ ARMARX_CHECK(aronType->getDescriptor() == aron::type::Descriptor::DICT);
+ // return if check failed
+ if (aronType->getDescriptor() != aron::type::Descriptor::DICT)
+ {
+ return;
+ }
+ // iterate through children; memorize keys
+ std::map<QString, std::vector<int>> found_keys;
+ auto numChildren = childCount();
+ for (int i = 0; i < numChildren; ++i)
+ {
+ auto* casted = AronTreeWidgetItem::DynamicCastAndCheck(child(i));
+ if (!casted)
+ {
+ // soft error here, we already report it above. - Definetly programming error
+ continue;
+ }
+ auto& vec = found_keys[casted->text(0)];
+ vec.push_back(i);
+ }
+ // highlight keys that conflict
+ // memorize children that are not ok
+ std::set<int> errorneous_indices;
+ for (auto [key, vals] : found_keys)
+ {
+ if (vals.size() > 1)
+ {
+ for (int i : vals)
+ {
+ auto* casted = AronTreeWidgetItem::DynamicCastAndCheck(child(i));
+ if (!casted)
+ {
+ // soft error here, we already report it above. - Definetly programming error
+ continue;
+ }
+ casted->setKeyErrorState(true);
+ errorneous_indices.emplace(i);
+ }
+ }
+ }
+ // clear potential error state of other elements
+ for (int i = 0; i < numChildren; ++i)
+ {
+ if (errorneous_indices.find(i) != errorneous_indices.end())
+ {
+ continue;
+ }
+ auto* casted = AronTreeWidgetItem::DynamicCastAndCheck(child(i));
+ if (!casted)
+ {
+ // soft error here, we already report it above. - Definetly programming error
+ continue;
+ }
+ casted->setKeyErrorState(false);
+ }
+ }
+
+ void
+ AronTreeWidgetItem::onUserChange(int changedColumn)
+ {
+ QTreeWidgetItem* qParent = QTreeWidgetItem::parent();
+ ARMARX_CHECK(qParent);
+ AronTreeWidgetItem* aronParent = DynamicCast(qParent);
+ if (changedColumn == 0)
+ {
+ if (col0Editable)
+ {
+ // Topmost should always be an Object and col0 is not editable in the child then...
+ // -> aronParent should never be nullptr
+ ARMARX_CHECK(aronParent);
+ // check if the element is child of a dict. If so, validate uniqueness of keys
+ if (aronParent->aronType->getDescriptor() == aron::type::Descriptor::DICT)
+ {
+ aronParent->checkKeyValidityOfChildren();
+ }
+ }
+ else
+ {
+ // maybe while editing keys, we try to edit a key that should not change by tabbing.
+ // Catch that here and undo the edit
+ preventIllegalKeyChange();
+ }
+ }
+ }
+
+ void
+ AronTreeWidgetItem::preventIllegalKeyChange()
+ {
+ if (!col0Editable)
+ {
+ setText(0, unchangeableKey);
+ }
+ }
+
+ AronTreeWidgetItem::AronTreeWidgetItem(bool editKey,
+ bool editVal,
+ QString key,
+ aron::type::VariantPtr type) :
+ aronType(type), col0Editable(editKey), col1Editable(editVal)
+ {
+ this->setText(0, key);
+ // add hook to check for edited keys for children of dictionaries
+ if (!editKey)
+ {
+ unchangeableKey = std::move(key);
+ }
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h
index 3d6d0c2a329c5a4376fb24a406dadf706d6b898b..9106cb3192526f15355445deb0c6f1654921c17a 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.h
@@ -1,39 +1,65 @@
#pragma once
-#include <stack>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <QTreeWidget>
-#include "Data.h"
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
-#include <QTreeWidget>
+#include "Data.h"
namespace armarx
{
+ // Internal derived QtTreeWidgetItem class. Contains additional information to compare its data agains aron types.
+ // It contains 3 columns: Key ; Value ; Type
+ // The key can only be edited if the parent object is a dict
class AronTreeWidgetItem : public QObject, public QTreeWidgetItem
{
Q_OBJECT
public:
- AronTreeWidgetItem(const AronTreeWidgetItem& other) :
- QObject(),
- QTreeWidgetItem(other)
- {
- aronType = other.aronType;
- }
+ AronTreeWidgetItem(bool editKey, bool editVal, QString key, aron::type::VariantPtr type);
using QTreeWidgetItem::QTreeWidgetItem;
- AronTreeWidgetItem* copy(); // differs from clone!!!!
-
static AronTreeWidgetItem* DynamicCast(QTreeWidgetItem*);
-
static AronTreeWidgetItem* DynamicCastAndCheck(QTreeWidgetItem*);
aron::type::VariantPtr aronType;
+ // if editing the first column should be allowed
+ const bool col0Editable = false;
+ // if editing the second column should be allowed
+ const bool col1Editable = false;
+
+ bool isValueErrorneous();
+ // marks the gui in a specific color. Also stores if there is currently an error.
+ // the only way to reset the color back to normal is with resetError() (error is sticky)
+ void setValueErrorState(bool isErrorSource, bool isTransitiveError);
+ void setKeyErrorState(bool hasError);
+ // reset error storage that influences coloring for this and all children
+ void resetError();
+
+ // Checks if the children of a dict are unique
+ // should not be called on other types! (does nothing then)
+ void checkKeyValidityOfChildren();
+
+ // main logic on changes. Gets called from the onTreeWidgetItemChanged() slot in AronTreeWidgetController.
+ // (This class cannot directly consume this signal, at least I did not find a nice way...)
+ void onUserChange(int changedColumn);
+
+ private:
+ // because the editable keyword counts for all columns, it is possible to TAB into uneditable keys.
+ // We do not want those to change, so just change them back once they finished editing.
+ void preventIllegalKeyChange();
+ bool itemValueError = false;
+ bool transitiveValueError = false;
+ bool keyValueError = false;
+ // hacky storage of previous key value for items that usually should not be editable.
+ // Problem: Dict-keys are editable, one can tab from that to any other value field and edit it.
+ // Fix: Once the change is commited, the AronTreeWidgetController notices the change and checks if it was allowed.
+ QString unchangeableKey = "";
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9866e8bc9f8c35a00e6411331461511e4c031e9f
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.cpp
@@ -0,0 +1,28 @@
+#include "ListDictHelper.h"
+
+namespace armarx::misc
+{
+ QString
+ generateNumElementsText(int num)
+ {
+ QString numElemsText = "<";
+ if (num == 0)
+ {
+ numElemsText.append("no");
+ }
+ else
+ {
+ numElemsText.append(QString::number(num));
+ }
+ if (num > 1 || num == 0)
+ {
+ numElemsText.append(" elements>");
+ }
+ else
+ {
+ numElemsText.append(" element>");
+ }
+ return numElemsText;
+ }
+
+} // namespace armarx::misc
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..c5d991ad2861aef5c32fc82c033c9090070f13b5
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.h
@@ -0,0 +1,8 @@
+#include <QString>
+
+
+namespace armarx::misc
+{
+ // helper that generates a string on how many items are available
+ QString generateNumElementsText(int num);
+} // namespace armarx::misc
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
index f7d83d98b6cde491cad878822b69b50adb34d355..71a88e54fcf49d77ea5de6804ef06a11e90e6ae8 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
@@ -1,6 +1,7 @@
#include "AronTreeWidgetDictInputModalController.h"
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
#include "../../visitors/AronTreeWidgetCreator.h"
namespace armarx
@@ -29,7 +30,7 @@ namespace armarx
{
for (const auto& added : addedItems)
{
- item->addChild(added->copy());
+ //item->addChild(added->copy());
}
AronTreeWidgetModal::submit();
@@ -59,12 +60,14 @@ namespace armarx
auto d = aron::type::Dict::DynamicCastAndCheck(t);
auto ac = d->getAcceptedType();
- AronTreeWidgetCreatorVisitor v;
+ AronTreeWidgetCreatorVisitor v(nullptr);
+ v.setTopLevelWidget(widget.treeWidgetDict);
aron::type::visit(v, ac);
if (v.createdQWidgetItem)
{
- v.createdQWidgetItem->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME, s);
+ v.createdQWidgetItem->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME,
+ s);
addedItems.push_back(v.createdQWidgetItem);
widget.treeWidgetDict->addTopLevelItem(v.createdQWidgetItem);
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98e271cd81c8180af051e094169f63b58176b0e8
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp
@@ -0,0 +1,243 @@
+#include "AronTreeWidgetContextMenu.h"
+
+#include <QMenu>
+#include <QPoint>
+#include <QTreeWidget>
+#include <QTreeWidgetItem>
+
+#include "../AronTreeWidgetItem.h"
+#include "../ListDictHelper.h"
+#include "AronTreeWidgetCreator.h"
+
+namespace armarx
+{
+ AronTreeWidgetContextMenuVisitor::AronTreeWidgetContextMenuVisitor(
+ AronTreeWidgetItem* i,
+ const QPoint& pos,
+ QTreeWidget* contextMenuParent,
+ int x) :
+ parentItem(i), contextMenuParent(contextMenuParent), pos(pos), index(x)
+ {
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::addDeleteAction()
+ {
+ auto* castedParent = AronTreeWidgetItem::DynamicCast(parentItem->QTreeWidgetItem::parent());
+ if (!castedParent)
+ {
+ // must be top level element
+ return;
+ }
+ auto aronType = castedParent->aronType->getDescriptor();
+ if (aron::type::Descriptor::DICT == aronType || aron::type::Descriptor::LIST == aronType)
+ {
+ QMenu contextMenu("Context menu", contextMenuParent);
+ actions.emplace_back("remove element", contextMenuParent);
+ action_callbacks.push_back([this]() mutable { this->executeDelete(); });
+ }
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::executeDelete()
+ {
+ auto* containerPtr = parentItem->QTreeWidgetItem::parent();
+ containerPtr->removeChild(parentItem);
+ auto* castedContainer = AronTreeWidgetItem::DynamicCast(containerPtr);
+
+ // if the parent item is a List, we need to redo the numbers
+ if (castedContainer &&
+ castedContainer->aronType->getDescriptor() == aron::type::Descriptor::LIST)
+ {
+ // start renumbering from the removed child onwards
+ for (int i = index; i < castedContainer->childCount(); ++i)
+ {
+ std::string numberString = std::to_string(i);
+ castedContainer->child(i)->setText(0, numberString.c_str());
+ }
+ // This displays the number of children also when the list is collapsed
+ QString numElemsText = misc::generateNumElementsText(castedContainer->childCount());
+ containerPtr->setText(1, numElemsText);
+ // set italic
+ auto currFont = castedContainer->font(1);
+ currFont.setItalic(true);
+ castedContainer->setFont(1, currFont);
+ }
+ }
+
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::ObjectPtr&)
+ {
+ addDeleteAction();
+ }
+
+ // lol
+ void
+ armarx::AronTreeWidgetContextMenuVisitor::addAddAction()
+ {
+ actions.emplace_back("Add element", contextMenuParent);
+ action_callbacks.push_back([this]() mutable { this->executeAddElement(); });
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::executeAddElement()
+ {
+ AronTreeWidgetCreatorVisitor creator(parentItem);
+ aron::type::visit(creator, parentItem->aronType->getChildren()[0]);
+
+ if (!creator.createdQWidgetItem)
+ {
+ throw std::runtime_error("Creation of TreeElementChild failed unexpectedly");
+ }
+ // if it is a list, we update the number of children at the top
+ auto* castedContainer = AronTreeWidgetItem::DynamicCast(parentItem);
+
+ // if the parent item is a List, we need to redo the numbers
+ if (castedContainer &&
+ castedContainer->aronType->getDescriptor() == aron::type::Descriptor::LIST)
+ {
+ // This displays the number of children also when the list is collapsed
+ auto numElemsText = misc::generateNumElementsText(castedContainer->childCount());
+ castedContainer->setText(1, numElemsText);
+ // set italic
+ auto currFont = castedContainer->font(1);
+ currFont.setItalic(true);
+ castedContainer->setFont(1, currFont);
+ }
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::DictPtr&)
+ {
+ addAddAction();
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::PairPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::TuplePtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::ListPtr&)
+ {
+ addAddAction();
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::NDArrayPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::MatrixPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::QuaternionPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::ImagePtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::PointCloudPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::IntEnumPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::IntPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::LongPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::FloatPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::DoublePtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::BoolPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::StringPtr&)
+ {
+ addDeleteAction();
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::visitUnknown(Input&)
+ {
+ ARMARX_WARNING << "Tried to open Context menu on unknown aron type";
+ }
+
+ void
+ AronTreeWidgetContextMenuVisitor::showMenuAndExecute()
+ {
+ QMenu menu("Context Menu", contextMenuParent);
+ for (auto& el : actions)
+ {
+ menu.addAction(&el);
+ }
+ auto* chosenAction = menu.exec(contextMenuParent->mapToGlobal(pos));
+
+ if (!chosenAction)
+ {
+ return;
+ }
+
+ // not elegant, but is a small loop anyway
+ auto it = actions.begin();
+ size_t count = 0;
+ while (it != actions.end())
+ {
+ if (chosenAction == &*it)
+ {
+ action_callbacks[count]();
+ break;
+ }
+ ++it;
+ ++count;
+ }
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.h
new file mode 100644
index 0000000000000000000000000000000000000000..510f68292b91bf9934dcecd4300d698e64d516b3
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.h
@@ -0,0 +1,67 @@
+#pragma once
+
+
+#include <QAction>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
+
+class QTreeWidget;
+class QPoint;
+
+namespace armarx
+{
+
+ class AronTreeWidgetItem;
+
+ // Visitor on aron types. It creates a context menu dependent on the type of the AronTreeWidgetItem.
+ // Its only used for Lists and Dicts.
+ class AronTreeWidgetContextMenuVisitor : public armarx::aron::type::ConstVariantVisitor
+ {
+
+ AronTreeWidgetItem* parentItem;
+ QTreeWidget* contextMenuParent;
+ const QPoint& pos;
+ int index;
+
+ public:
+ AronTreeWidgetContextMenuVisitor() = delete;
+ AronTreeWidgetContextMenuVisitor(AronTreeWidgetItem* i,
+ const QPoint& pos,
+ QTreeWidget* contextMenuParent,
+ int x);
+
+ void visitAronVariant(const aron::type::ObjectPtr&) final;
+ void visitAronVariant(const aron::type::DictPtr&) final;
+ void visitAronVariant(const aron::type::PairPtr&) final;
+ void visitAronVariant(const aron::type::TuplePtr&) final;
+ void visitAronVariant(const aron::type::ListPtr&) final;
+ void visitAronVariant(const aron::type::NDArrayPtr&) final;
+ void visitAronVariant(const aron::type::MatrixPtr&) final;
+ void visitAronVariant(const aron::type::QuaternionPtr&) final;
+ void visitAronVariant(const aron::type::ImagePtr&) final;
+ void visitAronVariant(const aron::type::PointCloudPtr&) final;
+ void visitAronVariant(const aron::type::IntEnumPtr&) final;
+ void visitAronVariant(const aron::type::IntPtr&) final;
+ void visitAronVariant(const aron::type::LongPtr&) final;
+ void visitAronVariant(const aron::type::FloatPtr&) final;
+ void visitAronVariant(const aron::type::DoublePtr&) final;
+ void visitAronVariant(const aron::type::BoolPtr&) final;
+ void visitAronVariant(const aron::type::StringPtr&) final;
+ void visitUnknown(Input&) final;
+
+ void showMenuAndExecute();
+
+ private:
+ std::list<QAction> actions;
+ std::vector<std::function<void()>> action_callbacks;
+
+ // Creates a remove option if the element is a direct child of a list or dict
+ void addDeleteAction();
+ void executeDelete();
+
+ void addAddAction();
+ void executeAddElement();
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
index fbdc653abc5fa7cab715fc8abc7aa200de0243c1..c0a8088db7b5ad4e4e1db955b42e9316164038b3 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
@@ -26,16 +26,68 @@
#include "AronTreeWidgetConverter.h"
// armarx
+#include <SimoxUtility/algorithm/string.h>
+
#include <ArmarXCore/core/logging/Logging.h>
+#include "RobotAPI/libraries/aron/core/data/variant/All.h"
+
// qt
#include <QTreeWidgetItem>
+#include "../widgets/EditMatrixWidget.h"
+#include "../widgets/IntEnumWidget.h"
+#include "../widgets/QuaternionWidget.h"
+
+
namespace armarx
{
+ bool
+ AronTreeWidgetConverterVisitor::isConversionSuccessful()
+ {
+ return !isDirectError && !hasTransitiveError;
+ }
+
+ bool
+ AronTreeWidgetConverterVisitor::onlyChildFailedConversion()
+ {
+ return hasTransitiveError;
+ }
+
+ bool
+ AronTreeWidgetConverterVisitor::hasDirectError() const
+ {
+ return isDirectError;
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::handleErrors(AronTreeWidgetConverterVisitor childV,
+ bool ownFault)
+ {
+ ARMARX_TRACE;
+ isDirectError |= ownFault;
+ hasTransitiveError |= childV.isDirectError || childV.hasTransitiveError;
+
+ auto* aronItem = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
+ ARMARX_CHECK(aronItem);
+ aronItem->setValueErrorState(isDirectError, hasTransitiveError);
+ }
+
+ void
+ AronTreeWidgetConverterVisitor::handleErrors(bool ownFault)
+ {
+ ARMARX_TRACE;
+ isDirectError = ownFault;
+ auto* aronItem = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
+ ARMARX_CHECK(aronItem);
+ aronItem->setValueErrorState(isDirectError, false);
+ }
+
+
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ObjectPtr& i)
{
+ ARMARX_TRACE;
auto createdAronDict = std::make_shared<aron::data::Dict>(i->getPath());
createdAron = createdAronDict;
QTreeWidgetItem* el = parentItem->child(index);
@@ -43,16 +95,22 @@ namespace armarx
unsigned int x = 0;
for (const auto& [key, value] : i->getMemberTypes())
{
+ ARMARX_TRACE;
AronTreeWidgetConverterVisitor v(el, x++);
aron::type::visit(v, value);
- createdAronDict->addElement(key, v.createdAron);
+ handleErrors(v);
+ if (v.isConversionSuccessful())
+ {
+ createdAronDict->addElement(key, v.createdAron);
+ }
}
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::DictPtr& i)
{
+ ARMARX_TRACE;
auto createdAronDict = std::make_shared<aron::data::Dict>(i->getPath());
createdAron = createdAronDict;
QTreeWidgetItem* el = parentItem->child(index);
@@ -62,71 +120,206 @@ namespace armarx
auto it = el->child(x);
AronTreeWidgetConverterVisitor v(el, x);
aron::type::visit(v, i->getAcceptedType());
-
- if (v.createdAron)
+ auto key = it->text(0).toStdString();
+ // TODO: handle key errors more elegantly / separately, fine for now
+ handleErrors(v, createdAronDict->hasElement(key));
+ if (v.createdAron && v.isConversionSuccessful() && !createdAronDict->hasElement(key))
{
- createdAronDict->addElement(it->text(0).toStdString(), v.createdAron);
+ createdAronDict->addElement(key, v.createdAron);
}
}
}
void
- AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PairPtr& i)
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ListPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ auto createdAronList = std::make_shared<aron::data::List>(i->getPath());
+ createdAron = createdAronList;
+ auto* elem = parentItem->child(index);
+ auto childrenTypes = i->getChildren();
+ ARMARX_CHECK(childrenTypes.size() == 1);
+ for (int j = 0; j < elem->childCount(); ++j)
+ {
+ AronTreeWidgetConverterVisitor convVisitor(elem, j);
+ aron::type::visit(convVisitor, childrenTypes[0]);
+ handleErrors(convVisitor);
+
+ if (convVisitor.createdAron && convVisitor.isConversionSuccessful())
+ {
+ createdAronList->addElement(convVisitor.createdAron);
+ }
+ }
}
+
void
- AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::TuplePtr& i)
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PairPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ auto createdAronPair = std::make_shared<aron::data::List>(i->getPath());
+ createdAron = createdAronPair;
+ auto* elem = parentItem->child(index);
+
+ for (int j = 0; j < 2; ++j)
+ {
+ AronTreeWidgetConverterVisitor convVisitor(elem, j);
+ handleErrors(convVisitor);
+ if (convVisitor.createdAron && convVisitor.isConversionSuccessful())
+ {
+ createdAronPair->addElement(convVisitor.createdAron);
+ }
+ }
}
void
- AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ListPtr& i)
+ AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::TuplePtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ auto createdAronList = std::make_shared<aron::data::List>(i->getPath());
+ createdAron = createdAronList;
+ QTreeWidgetItem* el = parentItem->child(index);
+
+ for (int x = 0; x < el->childCount(); ++x)
+ {
+ auto* it = el->child(x);
+ AronTreeWidgetConverterVisitor v(it, x);
+ aron::type::visit(v, i->getAcceptedType(x));
+ handleErrors(v);
+
+ if (v.createdAron)
+ {
+ createdAronList->addElement(v.createdAron);
+ }
+ }
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ ARMARX_ERROR << "Currently do not support supplying raw NDArrays!";
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ auto createdMatrix = std::make_shared<aron::data::NDArray>(i->getPath());
+ int dataSize = 0;
+ switch (i->getElementType())
+ {
+ case armarx::aron::type::matrix::INT16:
+ dataSize = 2;
+ break;
+ case armarx::aron::type::matrix::INT32:
+ case armarx::aron::type::matrix::FLOAT32:
+ dataSize = 4;
+ break;
+ case armarx::aron::type::matrix::FLOAT64:
+ case armarx::aron::type::matrix::INT64:
+ dataSize = 8;
+ break;
+ };
+ createdMatrix->setShape({i->getRows(), i->getCols(), dataSize});
+ createdMatrix->setType(i->getFullName());
+ int totalByteSize = i->getRows() * i->getCols() * dataSize;
+ createdAron = createdMatrix;
+
+ auto* currElem = parentItem->child(index);
+ auto* rootWidget = currElem->treeWidget();
+ ARMARX_CHECK(rootWidget);
+ auto* widget = rootWidget->itemWidget(currElem, 1);
+ auto* matrixWidget = EditMatrixWidget::DynamicCastAndCheck(widget);
+
+ handleErrors(matrixWidget->hasParseErrors());
+ if (matrixWidget->hasParseErrors())
+ {
+ return;
+ }
+ // write to aron data
+ std::vector<unsigned char> elems;
+ elems.reserve(totalByteSize);
+ // CAUTION: Raw data has column based storage
+ for (size_t col = 0; col < (size_t)i->getCols(); ++col)
+ {
+ for (size_t row = 0; row < (size_t)i->getRows(); ++row)
+ {
+ // gets us directly the byte wise format
+ auto parsed = matrixWidget->parseElement(row, col);
+ // append vector to vector
+ elems.insert(elems.end(), parsed.begin(), parsed.end());
+ }
+ }
+ createdMatrix->setData(totalByteSize, elems.data());
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ auto createdQuat = std::make_shared<aron::data::NDArray>(i->getPath());
+ createdAron = createdQuat;
+ int dataSize = i->getElementType() == aron::type::quaternion::ElementType::FLOAT32 ? 4 : 8;
+ createdQuat->setShape({1, 4, dataSize});
+ createdQuat->setType(i->getFullName());
+ auto* currTreeElem = parentItem->child(index);
+ auto* itemWidget = currTreeElem->treeWidget()->itemWidget(currTreeElem, 1);
+ auto* quatWidget = QuaternionWidget::DynamicCastAndCheck(itemWidget);
+
+ // error handling
+ handleErrors(quatWidget->hasParseErrors());
+ if (quatWidget->hasParseErrors())
+ {
+ return;
+ }
+
+ // write to aron data
+ auto serialized = quatWidget->parseAllToNDArray();
+ if ((int)serialized.size() != dataSize * 4)
+ {
+ ARMARX_ERROR
+ << "serialized quaternions did not return byte sequence of correct length!";
+ }
+ createdQuat->setData(serialized.size(), serialized.data());
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ImagePtr& i)
{
+ ARMARX_TRACE;
// TODO
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
{
+ ARMARX_TRACE;
// TODO
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
{
- // TODO
+ ARMARX_TRACE;
+ QTreeWidgetItem* el = parentItem->child(index);
+ auto* genericWidget = el->treeWidget()->itemWidget(el, 1);
+ auto* intEnumWidget = IntEnumWidget::DynamicCastAndCheck(genericWidget);
+ if (!intEnumWidget)
+ {
+ // already reporting error; continue here
+ return;
+ }
+ bool success;
+ std::tie(success, createdAron) = intEnumWidget->parseToAron();
+
+ handleErrors(!success);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::IntPtr& i)
{
+ ARMARX_TRACE;
auto createdAronInt = std::make_shared<aron::data::Int>(i->getPath());
createdAron = createdAronInt;
QTreeWidgetItem* el = parentItem->child(index);
@@ -137,14 +330,25 @@ namespace armarx
createdAronInt->setValue(0);
return;
}
-
- int val = std::stoi(str);
- createdAronInt->setValue(val);
+ try
+ {
+ int val = simox::alg::to_<int>(str);
+ createdAronInt->setValue(val);
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ handleErrors();
+ ARMARX_VERBOSE << "Conversion from String to Int failed. Error:\"" << err.what()
+ << "\"";
+ return;
+ }
+ handleErrors(false);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::LongPtr& i)
{
+ ARMARX_TRACE;
auto createdAronLong = std::make_shared<aron::data::Long>(i->getPath());
createdAron = createdAronLong;
QTreeWidgetItem* el = parentItem->child(index);
@@ -152,15 +356,27 @@ namespace armarx
std::string str = el->text(1).toStdString();
if (str.empty())
{
+ //TODO: similar behaviour for rest?
str = el->text(3).toStdString();
}
-
- createdAronLong->fromString(str);
+ try
+ {
+ createdAronLong->setValue(simox::alg::to_<long>(str));
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ handleErrors();
+ ARMARX_VERBOSE << "Conversion from String to Long failed. Error:\"" << err.what()
+ << "\"";
+ return;
+ }
+ handleErrors(false);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::FloatPtr& i)
{
+ ARMARX_TRACE;
auto createdAronFloat = std::make_shared<aron::data::Float>(i->getPath());
createdAron = createdAronFloat;
QTreeWidgetItem* el = parentItem->child(index);
@@ -170,13 +386,24 @@ namespace armarx
{
str = el->text(3).toStdString();
}
-
- createdAronFloat->fromString(str);
+ try
+ {
+ createdAronFloat->setValue(simox::alg::to_<float>(str));
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ handleErrors();
+ ARMARX_VERBOSE << "Conversion from String to Float failed. Error:\"" << err.what()
+ << "\"";
+ return;
+ }
+ handleErrors(false);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::DoublePtr& i)
{
+ ARMARX_TRACE;
auto createdAronDouble = std::make_shared<aron::data::Double>(i->getPath());
createdAron = createdAronDouble;
QTreeWidgetItem* el = parentItem->child(index);
@@ -186,13 +413,24 @@ namespace armarx
{
str = el->text(3).toStdString();
}
-
- createdAronDouble->fromString(str);
+ try
+ {
+ createdAronDouble->setValue(simox::alg::to_<double>(str));
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ handleErrors();
+ ARMARX_VERBOSE << "Conversion from String to Double failed. Error:\"" << err.what()
+ << "\"";
+ return;
+ }
+ handleErrors(false);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::BoolPtr& i)
{
+ ARMARX_TRACE;
auto createdAronBool = std::make_shared<aron::data::Bool>(i->getPath());
createdAron = createdAronBool;
QTreeWidgetItem* el = parentItem->child(index);
@@ -202,24 +440,36 @@ namespace armarx
{
str = el->text(3).toStdString();
}
-
- createdAronBool->fromString(str);
+ try
+ {
+ createdAronBool->setValue(simox::alg::to_<bool>(str));
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ handleErrors();
+ ARMARX_VERBOSE << "Conversion from String to Bool failed. Error:\"" << err.what()
+ << "\"";
+ return;
+ }
+ handleErrors(false);
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::StringPtr& i)
{
+ ARMARX_TRACE;
auto createdAronString = std::make_shared<aron::data::String>(i->getPath());
createdAron = createdAronString;
QTreeWidgetItem* el = parentItem->child(index);
std::string str = el->text(1).toStdString();
- createdAronString->fromString(str);
+ createdAronString->setValue(str);
}
- void AronTreeWidgetConverterVisitor::visitUnknown(Input&)
+ void
+ AronTreeWidgetConverterVisitor::visitUnknown(Input&)
{
- ARMARX_WARNING_S << "Received an unknown type when trying to convert a skill argument type to an aron data object.";
+ ARMARX_WARNING_S << "Received an unknown type when trying to convert a skill argument type "
+ "to an aron data object.";
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
index eb056cdc2a65f14ef4d884124d6cb14ac2e9ed7e..c127ac5e49276e878221dc39a5db8a7b821ccec7 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
@@ -21,8 +21,8 @@
*/
#pragma once
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
// forward declarations of qt
@@ -32,8 +32,7 @@ class QTreeWidgetItem;
namespace armarx
{
// Conversion from TreeView to aron data
- class AronTreeWidgetConverterVisitor :
- public armarx::aron::type::ConstVariantVisitor
+ class AronTreeWidgetConverterVisitor : public armarx::aron::type::ConstVariantVisitor
{
public:
QTreeWidgetItem* parentItem;
@@ -41,10 +40,27 @@ namespace armarx
aron::data::VariantPtr createdAron = nullptr;
AronTreeWidgetConverterVisitor() = delete;
- AronTreeWidgetConverterVisitor(QTreeWidgetItem* i, int x) :
- parentItem(i), index(x)
- {}
+ AronTreeWidgetConverterVisitor(QTreeWidgetItem* i, int x) : parentItem(i), index(x)
+ {
+ }
+ // if the conversion was successful after calling visit()
+ bool isConversionSuccessful();
+ // returns true if this type itself was sucessfully parsed, but some contained object failed.
+ // also false if there is no error
+ bool onlyChildFailedConversion();
+
+ bool hasDirectError() const;
+
+ private:
+ bool isDirectError = false;
+ bool hasTransitiveError = false;
+ // adds all errors from other visitor to our own error collection -> collecting errors
+ // with ownFault, we also add this node to the collection
+ void handleErrors(AronTreeWidgetConverterVisitor childV, bool ownFault = false);
+ // we are the cause...
+ void handleErrors(bool ownFault = true);
+ public:
void visitAronVariant(const aron::type::ObjectPtr&) final;
void visitAronVariant(const aron::type::DictPtr&) final;
void visitAronVariant(const aron::type::PairPtr&) final;
@@ -64,6 +80,4 @@ namespace armarx
void visitAronVariant(const aron::type::StringPtr&) final;
void visitUnknown(Input&) final;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
index ee43334bb26862322610746269fee97a16d0cccd..84cb58585e3b4e7f3a058db66f3b5f3a40f54aea 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
@@ -22,28 +22,98 @@
#include <string>
+#include "../widgets/EditMatrixWidget.h"
+#include "../widgets/IntEnumWidget.h"
+#include "../widgets/QuaternionWidget.h"
+
// base class
#include "AronTreeWidgetCreator.h"
// data
+#include <QComboBox>
+
#include "../AronTreeWidgetItem.h"
#include "../Data.h"
+#include "../ListDictHelper.h"
+#include "AronTreeWidgetContextMenu.h"
namespace armarx
{
+
+ AronTreeWidgetCreatorVisitor::AronTreeWidgetCreatorVisitor(QTreeWidgetItem* parentInstance) :
+ parentOfCreatedObj(parentInstance)
+ {
+ // The parent of the root aron Tree Widget will not be aron type.
+ // there an explicit setTopLevelWidget() is required.
+ auto* aronParent = AronTreeWidgetItem::DynamicCast(parentInstance);
+ if (aronParent)
+ {
+ toplevelWidget = aronParent->treeWidget();
+ }
+ }
+
+ std::string
+ AronTreeWidgetCreatorVisitor::generateUniqueKeyFromSet(std::set<std::string>&& usedKeys)
+ {
+ size_t num = 0;
+ while (true)
+ {
+ std::string proposedName = this->defaultMapKeyName + std::to_string(num);
+ auto it = usedKeys.find(proposedName);
+ if (it == usedKeys.end())
+ {
+ break;
+ }
+ ++num;
+ }
+ return this->defaultMapKeyName + std::to_string(num);
+ }
+
void
- AronTreeWidgetCreatorVisitor::createSimpleTreeViewWidget(Input& i, const std::string& defaul)
+ AronTreeWidgetCreatorVisitor::insertNewTreeViewWidget(Input& i, const std::string& defaul)
{
ARMARX_CHECK_NOT_NULL(i);
auto key = i->getPath().getLastElement();
- createdQWidgetItem = new AronTreeWidgetItem();
- createdQWidgetItem->aronType = i;
- createdQWidgetItem->setText(0, QString::fromStdString(key));
+ bool isDictChild = false;
+ // edit key, to be a unique string, if the parent is a dict
+ auto* aronParent = AronTreeWidgetItem::DynamicCast(parentOfCreatedObj);
+ if (aronParent && aronParent->aronType->getDescriptor() == aron::type::Descriptor::DICT)
+ {
+ isDictChild = true;
+ std::set<std::string> usedKeys;
+ for (int i = 0; i < parentOfCreatedObj->childCount(); ++i)
+ {
+ auto* sibling = AronTreeWidgetItem::DynamicCast(parentOfCreatedObj->child(i));
+ if (sibling)
+ {
+ usedKeys.insert(sibling->text(0).toStdString());
+ }
+ }
+ key = generateUniqueKeyFromSet(std::move(usedKeys));
+ }
+ // if it's a list -> choose the right number
+ else if (aronParent &&
+ aronParent->aronType->getDescriptor() == aron::type::Descriptor::LIST)
+ {
+ key = std::to_string(parentOfCreatedObj->childCount());
+ }
+
+ createdQWidgetItem =
+ new AronTreeWidgetItem(isDictChild, editableValue, QString::fromStdString(key), i);
createdQWidgetItem->setText(1, QString::fromStdString(defaul));
createdQWidgetItem->setText(2, QString::fromStdString(i->getShortName()));
- createdQWidgetItem->setText(3, QString::fromStdString(aron_tree_widget::constantes::ITEM_EMPTY_MESSAGE) /*QString::fromStdString(i->getDefaultFromString())*/);
- createdQWidgetItem->setFlags(createdQWidgetItem->flags() | Qt::ItemIsEditable);
+ createdQWidgetItem->setText(
+ 3,
+ QString::fromStdString(
+ aron_tree_widget::constantes::
+ ITEM_EMPTY_MESSAGE) /*QString::fromStdString(i->getDefaultFromString())*/);
+
+ if (editableValue || isDictChild)
+ {
+ createdQWidgetItem->setFlags(createdQWidgetItem->flags() | Qt::ItemIsEditable);
+ }
+ parentOfCreatedObj->addChild(createdQWidgetItem);
}
void
@@ -57,73 +127,203 @@ namespace armarx
key = i->getPath().getLastElement();
}
- createdQWidgetItem = new AronTreeWidgetItem();
- createdQWidgetItem->setText(0, QString::fromStdString(key));
+ createdQWidgetItem =
+ new AronTreeWidgetItem(editableValue, false, QString::fromStdString(key), i);
+ parentOfCreatedObj->addChild(createdQWidgetItem);
for (const auto& [key, value] : i->getMemberTypes())
{
- AronTreeWidgetCreatorVisitor v;
+ AronTreeWidgetCreatorVisitor v(createdQWidgetItem);
aron::type::visit(v, value);
- if (v.createdQWidgetItem)
- {
- createdQWidgetItem->addChild(v.createdQWidgetItem);
- }
+ assert(v.createdQWidgetItem);
}
}
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::DictPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ ARMARX_CHECK_NOT_NULL(i);
+
+ insertNewTreeViewWidget(i, "");
+ // The DictType has only one member, its key-type. This also must be present
+ ARMARX_CHECK_EQUAL(i->getChildren().size(), 1);
+ }
void
- AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PairPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PairPtr& pair)
+ {
+ // create default, uneditable tree widget item.
+ insertNewTreeViewWidget(pair, "");
+ // attach two children
+ ARMARX_CHECK(pair->getChildren().size() == 2);
+ for (size_t i = 0; i < 2; ++i)
+ {
+ AronTreeWidgetCreatorVisitor v(createdQWidgetItem);
+ aron::type::visit(v, pair->getChildren()[i]);
+ if (v.createdQWidgetItem)
+ {
+ std::string descr = "p[" + std::to_string(i) + "]";
+ v.createdQWidgetItem->setText(0, descr.c_str());
+ }
+ }
+ }
void
- AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& tuple)
+ {
+ // CAUTION; UNTESTED
+ // create default, uneditable tree widget item.
+ insertNewTreeViewWidget(tuple, "");
+ // attach all children
+ for (size_t i = 0; i < tuple->getChildren().size(); ++i)
+ {
+ AronTreeWidgetCreatorVisitor v(createdQWidgetItem);
+ aron::type::visit(v, tuple->getChildren()[i]);
+ if (v.createdQWidgetItem)
+ {
+ std::string descr = "tup[" + std::to_string(i) + "]";
+ v.createdQWidgetItem->setText(0, descr.c_str());
+ }
+ }
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ListPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ insertNewTreeViewWidget(i, "");
+ auto txt = misc::generateNumElementsText(0);
+ createdQWidgetItem->setText(1, txt);
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ insertNewTreeViewWidget(i, "");
+ }
+
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ editableValue = false;
+ insertNewTreeViewWidget(i, "");
+ // special code to print the type of base type used
+ QString type = "";
+ switch (i->getElementType())
+ {
+ case armarx::aron::type::matrix::INT16:
+ type = "<int16>";
+ break;
+ case armarx::aron::type::matrix::INT32:
+ type = "<int32>";
+ break;
+ case armarx::aron::type::matrix::INT64:
+ type = "<int64>";
+ break;
+ case armarx::aron::type::matrix::FLOAT32:
+ type = "<float>";
+ break;
+ case armarx::aron::type::matrix::FLOAT64:
+ type = "<double>";
+ break;
+ }
+ type = createdQWidgetItem->text(2) + type;
+ createdQWidgetItem->setText(2, type);
+
+ // Widget fiddling source: https://blog.manash.io/quick-qt-6-how-to-add-qpushbutton-or-widgets-to-a-qtreewidget-as-qtreewidgetitem-2ae9f54c0e5f
+ // overlay custom widget in column 1
+ auto* toplevelWidget = createdQWidgetItem->treeWidget();
+ EditMatrixWidget* matWidget = new EditMatrixWidget(
+ i->getRows(), i->getCols(), i->getElementType(), createdQWidgetItem);
+ toplevelWidget->setItemWidget(createdQWidgetItem, 1, matWidget);
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ editableValue = false;
+ insertNewTreeViewWidget(i, "");
+ // special code to print the type of base type used
+ QString type = "";
+ switch (i->getElementType())
+ {
+ case armarx::aron::type::quaternion::FLOAT32:
+ type = "<float>";
+ break;
+ case armarx::aron::type::quaternion::FLOAT64:
+ type = "<double>";
+ break;
+ }
+ type = createdQWidgetItem->text(2) + type;
+ createdQWidgetItem->setText(2, type);
+
+ auto* toplevelWidget = createdQWidgetItem->treeWidget();
+ QuaternionWidget* quatWidget =
+ new QuaternionWidget(i->getElementType(), createdQWidgetItem);
+ toplevelWidget->setItemWidget(createdQWidgetItem, 1, quatWidget);
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::ImagePtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ insertNewTreeViewWidget(i, "");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ insertNewTreeViewWidget(i, "");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ editableValue = false;
+ ARMARX_CHECK_NOT_NULL(i);
+ ARMARX_CHECK_GREATER(i->getAcceptedValueNames().size(), 0);
+
+ insertNewTreeViewWidget(i, "");
+ IntEnumWidget* widget = new IntEnumWidget(i, createdQWidgetItem);
+ createdQWidgetItem->treeWidget()->setItemWidget(createdQWidgetItem, 1, widget);
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::IntPtr& i)
- { createSimpleTreeViewWidget(i, "0"); }
+ {
+ editableValue = true;
+ insertNewTreeViewWidget(i, "0");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::LongPtr& i)
- { createSimpleTreeViewWidget(i, "0"); }
+ {
+ editableValue = true;
+ insertNewTreeViewWidget(i, "0");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::FloatPtr& i)
- { createSimpleTreeViewWidget(i, "0.0"); }
+ {
+ editableValue = true;
+ insertNewTreeViewWidget(i, "0.0");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::DoublePtr& i)
- { createSimpleTreeViewWidget(i, "0.0"); }
+ {
+ editableValue = true;
+ insertNewTreeViewWidget(i, "0.0");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::BoolPtr& i)
- { createSimpleTreeViewWidget(i, "false"); }
+ {
+ editableValue = true;
+ insertNewTreeViewWidget(i, "false");
+ }
void
AronTreeWidgetCreatorVisitor::visitAronVariant(const aron::type::StringPtr& i)
- { createSimpleTreeViewWidget(i, ""); }
+ {
+ insertNewTreeViewWidget(i, "");
+ }
- void AronTreeWidgetCreatorVisitor::visitUnknown(Input&)
+ void
+ AronTreeWidgetCreatorVisitor::visitUnknown(Input&)
{
- ARMARX_WARNING_S << "Received an unknown type when trying to create a tree view widget for a skill argument type.";
+ ARMARX_WARNING_S << "Received an unknown type when trying to create a tree view widget for "
+ "a skill argument type.";
}
-}
+ void
+ AronTreeWidgetCreatorVisitor::setTopLevelWidget(QTreeWidget* widget)
+ {
+ toplevelWidget = widget;
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h
index 63dde7ef0be04d8d790c5f8434ae1b82faeb107a..b3d2ffb54cca6bf7fe8aac2395d733f3ba458817 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.h
@@ -21,8 +21,10 @@
*/
#pragma once
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <QTreeWidgetItem>
+
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
namespace armarx
@@ -30,15 +32,20 @@ namespace armarx
class AronTreeWidgetItem;
// Convert aron type to tree widgets
- class AronTreeWidgetCreatorVisitor :
- public armarx::aron::type::ConstVariantVisitor
+ class AronTreeWidgetCreatorVisitor : public armarx::aron::type::ConstVariantVisitor
{
public:
- AronTreeWidgetItem* createdQWidgetItem;
+ AronTreeWidgetItem* createdQWidgetItem = nullptr;
+
+ AronTreeWidgetCreatorVisitor() = delete;
- AronTreeWidgetCreatorVisitor() = default;
+ // Takes the parent tree element and attaches the newly crated object during visit() to it.
+ // This allows us to also use information of the parent object during creation (like if this child is a dict entry or part of a list)
+ // IMPORTANT: For the root element, manually set the topLevelWidget after this constructor
+ AronTreeWidgetCreatorVisitor(QTreeWidgetItem* parentInstance);
+
+ void insertNewTreeViewWidget(Input& i, const std::string&);
- void createSimpleTreeViewWidget(Input& i, const std::string&);
void visitAronVariant(const aron::type::ObjectPtr&) final;
void visitAronVariant(const aron::type::DictPtr& i) final;
@@ -58,7 +65,17 @@ namespace armarx
void visitAronVariant(const aron::type::BoolPtr& i) final;
void visitAronVariant(const aron::type::StringPtr& i) final;
void visitUnknown(Input&) final;
- };
-}
+ // setter for the widget attachment point. Only needs to be manually set for the root element.
+ void setTopLevelWidget(QTreeWidget* widget);
+ private:
+ void handleEditable();
+ std::string generateUniqueKeyFromSet(std::set<std::string>&& usedKeys);
+ QTreeWidgetItem* parentOfCreatedObj = nullptr;
+ QTreeWidget* toplevelWidget = nullptr;
+ const std::string defaultMapKeyName = "key_";
+ // controls, if values (column 1) can be edited directly
+ bool editableValue = false;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
index 7ff248bd58499028bcf43a158f83f67c1259b43c..fc58634bb9bc02834d4e72663e3c16ffe36d6de6 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
@@ -28,7 +28,7 @@
// modals
#include "../modal/text/AronTreeWidgetTextInputModalController.h"
-#include "../modal/dict/AronTreeWidgetDictInputModalController.h"
+//#include "../modal/dict/AronTreeWidgetDictInputModalController.h"
// qt
#include <QTreeWidget>
@@ -37,84 +37,6 @@
namespace armarx
{
- void AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::ObjectPtr& i)
- {
- // should not happen, right?
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::DictPtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetDictInputModalController>(label, item, parent);
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::PairPtr& i)
- {
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::TuplePtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::ListPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::NDArrayPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::ImagePtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::PointCloudPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
- { }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::IntPtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::LongPtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::FloatPtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::DoublePtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
- }
-
- void
- AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::BoolPtr& i)
- {
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
- }
-
void
AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::StringPtr& i)
{
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
index e22930a1080977d0feb6475c72a3b6c848d5d8e5..f4c8f0223a039c6b9be926d6bddf360bce32f629 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
@@ -29,15 +29,17 @@
namespace armarx
{
- // Convert aron type to tree widget
+ // Convert aron type to tree widget.
+ // (Widgets are only created for string types to enter longer texts.
+ // However, the visitor implementation allows modals for differnt types. Might be useful in the future..)
class AronTreeWidgetModalCreatorVisitor :
public armarx::aron::type::ConstVariantVisitor
{
public:
- std::string label;
- AronTreeWidgetItem* item;
- QTreeWidget* parent;
- AronTreeWidgetModalControllerPtr createdModal;
+ std::string label = "";
+ AronTreeWidgetItem* item = nullptr;
+ QTreeWidget* parent = nullptr;
+ AronTreeWidgetModalControllerPtr createdModal = nullptr;
AronTreeWidgetModalCreatorVisitor() = delete;
AronTreeWidgetModalCreatorVisitor(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
@@ -46,22 +48,6 @@ namespace armarx
parent(parent)
{}
- void visitAronVariant(const aron::type::ObjectPtr&) final;
- void visitAronVariant(const aron::type::DictPtr& i) final;
- void visitAronVariant(const aron::type::PairPtr& i) final;
- void visitAronVariant(const aron::type::TuplePtr& i) final;
- void visitAronVariant(const aron::type::ListPtr& i) final;
- void visitAronVariant(const aron::type::NDArrayPtr& i) final;
- void visitAronVariant(const aron::type::MatrixPtr& i) final;
- void visitAronVariant(const aron::type::QuaternionPtr& i) final;
- void visitAronVariant(const aron::type::ImagePtr& i) final;
- void visitAronVariant(const aron::type::PointCloudPtr& i) final;
- void visitAronVariant(const aron::type::IntEnumPtr& i) final;
- void visitAronVariant(const aron::type::IntPtr& i) final;
- void visitAronVariant(const aron::type::LongPtr& i) final;
- void visitAronVariant(const aron::type::FloatPtr& i) final;
- void visitAronVariant(const aron::type::DoublePtr& i) final;
- void visitAronVariant(const aron::type::BoolPtr& i) final;
void visitAronVariant(const aron::type::StringPtr& i) final;
void visitUnknown(Input&) final;
};
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
index 87d99fe162344661a0511f49563c5d601342fe11..cf719f8a2d1360d574fb8ebcac867378c3fd6f1b 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
@@ -20,34 +20,106 @@
* GNU General Public License
*/
+#include "AronTreeWidgetSetter.h"
+
#include <string>
-#include "AronTreeWidgetSetter.h"
+#include "../ListDictHelper.h"
+#include "../widgets/EditMatrixWidget.h"
+#include "../widgets/IntEnumWidget.h"
+#include "../widgets/QuaternionWidget.h"
+#include "AronTreeWidgetContextMenu.h"
+#include "AronTreeWidgetCreator.h"
+
+
+template <typename T>
+std::string
+usString(T number, size_t precision = 3)
+{
+ std::stringstream ss;
+ const char* locale = "C";
+ ss.imbue(std::locale(locale));
+
+ ss << std::fixed << std::setprecision(precision) << number;
+ return ss.str();
+}
+
//visitors
namespace armarx
{
- bool AronTreeWidgetSetterVisitor::checkTreeWidgetItemForSimilarName(const std::string& name) const
+ bool
+ AronTreeWidgetSetterVisitor::checkTreeWidgetItemForSimilarName(const std::string& name) const
{
QTreeWidgetItem* el = parentItem->child(index);
+
+ // do not check attribute name, if the element is part of a list or map
+ auto* castedThis = AronTreeWidgetItem::DynamicCast(el->parent());
+ if (castedThis)
+ {
+ auto descr = castedThis->aronType->getDescriptor();
+ if (descr == aron::type::Descriptor::LIST || descr == aron::type::Descriptor::DICT)
+ {
+ return true;
+ }
+ }
std::string n = el->text(0).toStdString();
if (name != n)
{
- ARMARX_WARNING_S << "Could not set a tree widget value for the element with key '" << name << "' because it is different from the expected name '" << n << "'.";
+ ARMARX_WARNING_S << "Could not set a tree widget value for the element with key '"
+ << name << "' because it is different from the expected name '" << n
+ << "'.";
return false;
}
return true;
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DictPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::adjustNumberOfChildren(AronTreeWidgetItem* parent,
+ size_t numChildren)
+ {
+ if (((size_t)parent->childCount()) < numChildren)
+ {
+ // The type to create must be the only child of the current aron type
+ ARMARX_CHECK_EQUAL(parent->aronType->childrenSize(), 1);
+ size_t childrenToAdd = numChildren - parent->childCount();
+ for (size_t j = 0; j < childrenToAdd; ++j)
+ {
+ AronTreeWidgetCreatorVisitor childCreator(parent);
+ aron::type::visit(childCreator, parent->aronType->getChildren()[0]);
+ ARMARX_CHECK_NOT_NULL(childCreator.createdQWidgetItem);
+ }
+ }
+ else if ((size_t)parent->childCount() > numChildren)
+ {
+ size_t numChilds = (size_t)parent->childCount() - numChildren;
+ // pop the last child
+ for (size_t j = 0; j < numChilds; ++j)
+ {
+ parent->removeChild(parent->child(parent->childCount() - 1));
+ }
+ }
+ }
+
+
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DictPtr& i)
{
- if (i->getPath().size() == 0 || checkTreeWidgetItemForSimilarName(i->getPath().getLastElement())) // either it is the root or it has a name
+ // either it is the root or it has a name
+ if (i->getPath().size() == 0 ||
+ checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
+ auto* aronTreeWidget = AronTreeWidgetItem::DynamicCastAndCheck(el);
+ // allocate enough child items
+ adjustNumberOfChildren(aronTreeWidget, i->childrenSize());
+ // write child values
unsigned int x = 0;
for (const auto& [key, value] : i->getElements())
{
+ el->child(x)->setText(0, {key.c_str()});
+
AronTreeWidgetSetterVisitor v(el, x++);
aron::data::visit(v, value);
}
@@ -55,72 +127,224 @@ namespace armarx
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::ListPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::ListPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
+ auto* aronTreeWidget = AronTreeWidgetItem::DynamicCastAndCheck(el);
+ adjustNumberOfChildren(aronTreeWidget, i->childrenSize());
unsigned int x = 0;
for (const auto& value : i->getElements())
{
- AronTreeWidgetSetterVisitor v(el, x++);
+ AronTreeWidgetSetterVisitor v(el, x);
aron::data::visit(v, value);
+ auto* currChild = el->child(x);
+ std::string listNum = usString(x);
+ currChild->setText(0, listNum.c_str());
+
+ ++x;
+ }
+ // This displays the number of children also when the list is collapsed
+ QString numElemsText = misc::generateNumElementsText(i->getElements().size());
+ aronTreeWidget->setText(1, numElemsText);
+ // set italic
+ auto currFont = aronTreeWidget->font(1);
+ currFont.setItalic(true);
+ aronTreeWidget->setFont(1, currFont);
+ }
+ }
+
+
+ void
+ visitMatrix(EditMatrixWidget* matrixWidget,
+ const std::shared_ptr<armarx::aron::type::Matrix>& matrixType,
+ const aron::data::NDArrayPtr& arr)
+ {
+ auto elemType = matrixType->getElementType();
+ auto* rawData = arr->getData();
+ // string can convert any item
+ auto toString = [elemType, rawData](size_t elementNr) -> std::string
+ {
+ switch (elemType)
+ {
+ case aron::type::matrix::ElementType::FLOAT32:
+ {
+ static_assert(sizeof(float) == 4);
+ float* interpreted = reinterpret_cast<float*>(rawData);
+ float laundered = std::launder(interpreted)[elementNr];
+ return usString<float>(laundered);
+ }
+ case aron::type::matrix::ElementType::FLOAT64:
+ {
+ static_assert(sizeof(double) == 8);
+ double* interpreted = reinterpret_cast<double*>(rawData);
+ float laundered = std::launder(interpreted)[elementNr];
+ return usString<double>(laundered);
+ }
+ case aron::type::matrix::ElementType::INT16:
+ {
+ int16_t* interpreted = reinterpret_cast<int16_t*>(rawData);
+ int16_t laudered = std::launder(interpreted)[elementNr];
+ return usString<int16_t>(laudered);
+ }
+ case aron::type::matrix::ElementType::INT32:
+ {
+ int32_t* interpreted = reinterpret_cast<int32_t*>(rawData);
+ int32_t laudered = std::launder(interpreted)[elementNr];
+ return usString<int32_t>(laudered);
+ }
+ case aron::type::matrix::ElementType::INT64:
+ {
+ int64_t* interpreted = reinterpret_cast<int64_t*>(rawData);
+ int64_t laudered = std::launder(interpreted)[elementNr];
+ return usString<int64_t>(laudered);
+ }
+ }
+ return "Error!";
+ };
+
+
+ for (size_t row = 0; (int)row < matrixType->getRows(); ++row)
+ {
+ for (size_t col = 0; (int)col < matrixType->getCols(); ++col)
+ {
+ matrixWidget->setText(row, col, toString(col * matrixType->getRows() + row));
+ }
+ }
+ }
+ void
+ visitQuaternion(QuaternionWidget* quatWidget,
+ std::shared_ptr<armarx::aron::type::Quaternion>& quatType,
+ const aron::data::NDArrayPtr& arr)
+ {
+ auto elemType = quatType->getElementType();
+ auto rawData = arr->getData();
+ auto shape = arr->getShape();
+ // string can convert any item
+ auto toString = [elemType, rawData](size_t elementNr) -> std::string
+ {
+ switch (elemType)
+ {
+ case aron::type::quaternion::ElementType::FLOAT32:
+ {
+ static_assert(sizeof(float) == 4);
+ float* interpreted = reinterpret_cast<float*>(rawData);
+ float laundered = std::launder(interpreted)[elementNr];
+ return usString<float>(laundered);
+ }
+ case aron::type::quaternion::ElementType::FLOAT64:
+ {
+ static_assert(sizeof(double) == 8);
+ double* interpreted = reinterpret_cast<double*>(rawData);
+
+ float laundered = std::launder(interpreted)[elementNr];
+ return usString<double>(laundered);
+ }
}
+ return "Error!";
+ };
+ for (size_t i = 0; i < 4; ++i)
+ {
+ quatWidget->setText((QuaternionWidget::QuaternionComponents)i, toString(i));
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::NDArrayPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::NDArrayPtr& arr)
{
+ // Matrices are handled as NDArray. Raw ndarrays cannot be created currently
+ auto* castedEl = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
+ ARMARX_CHECK(castedEl);
+
+ auto matrixCast = aron::type::Matrix::DynamicCast(castedEl->aronType);
+ auto quaternionCast = aron::type::Quaternion::DynamicCast(castedEl->aronType);
+
+ auto* rootWidget = castedEl->treeWidget();
+ ARMARX_CHECK(rootWidget);
+ auto* matrixWidget = EditMatrixWidget::DynamicCast(rootWidget->itemWidget(castedEl, 1));
+ auto* quaternionWidget = QuaternionWidget::DynamicCast(rootWidget->itemWidget(castedEl, 1));
+ if (matrixCast && matrixWidget)
+ {
+ visitMatrix(matrixWidget, matrixCast, arr);
+ }
+ else if (quaternionCast && quaternionWidget)
+ {
+ visitQuaternion(quaternionWidget, quaternionCast, arr);
+ }
+ else
+ {
+ ARMARX_ERROR
+ << "we do not support raw NDArrays. Ask Fabian Peller for more information.";
+ }
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::IntPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::IntPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
- el->setText(1, QString::fromStdString(std::to_string(i->getValue())));
+ auto* enumWidget = IntEnumWidget::DynamicCast(el->treeWidget()->itemWidget(el, 1));
+ auto newText = QString::fromStdString(usString<int>(i->getValue()));
+ if (enumWidget)
+ {
+ // Its an IntEnum! -> Ask the custom widget
+ enumWidget->setText(newText);
+ }
+ else
+ {
+ // Its just an int. -> do the QTreeWidgetItem call
+ el->setText(1, newText);
+ }
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::LongPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::LongPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
- el->setText(1, QString::fromStdString(std::to_string(i->getValue())));
+ el->setText(1, QString::fromStdString(usString<long>(i->getValue())));
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::FloatPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::FloatPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
- el->setText(1, QString::fromStdString(std::to_string(i->getValue())));
+ el->setText(1, QString::fromStdString(usString<float>(i->getValue())));
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DoublePtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DoublePtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
- el->setText(1, QString::fromStdString(std::to_string(i->getValue())));
+ el->setText(1, QString::fromStdString(usString<double>(i->getValue())));
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::BoolPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::BoolPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
QTreeWidgetItem* el = parentItem->child(index);
- el->setText(1, QString::fromStdString(std::to_string(i->getValue())));
+ el->setText(1, QString::fromStdString(usString<bool>(i->getValue())));
}
}
- void AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::StringPtr& i)
+ void
+ AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::StringPtr& i)
{
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
@@ -129,9 +353,10 @@ namespace armarx
}
}
- void AronTreeWidgetSetterVisitor::visitUnknown(Input&)
+ void
+ AronTreeWidgetSetterVisitor::visitUnknown(Input&)
{
- ARMARX_WARNING_S << "Received an unknown type when trying to set a skill argument type from an aron data object.";
+ ARMARX_WARNING_S << "Received an unknown type when trying to set a skill argument type "
+ "from an aron data object.";
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h
index 336be31e20d4ff44496a8a2f8424d7dd3c1419f1..5090c12e82d28498efe8e1e63015e659f1e1abce 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h
@@ -23,17 +23,18 @@
#include <stack>
-#include "../AronTreeWidgetItem.h"
-
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+
+#include "../AronTreeWidgetItem.h"
namespace armarx
{
- // Conversion from TreeView to aron data
- class AronTreeWidgetSetterVisitor :
- public armarx::aron::data::ConstVariantVisitor
+ // Conversion from aron data to Tree View data
+ // This should be used to programatically change data to be displayed in the GUI.
+ // This code will not be called, if changes are made on the GUI side.
+ class AronTreeWidgetSetterVisitor : public armarx::aron::data::ConstVariantVisitor
{
public:
QTreeWidgetItem* parentItem;
@@ -41,9 +42,9 @@ namespace armarx
AronTreeWidgetItem* qWidgetItem;
AronTreeWidgetSetterVisitor() = delete;
- AronTreeWidgetSetterVisitor(QTreeWidgetItem* i, int x) :
- parentItem(i), index(x)
- {}
+ AronTreeWidgetSetterVisitor(QTreeWidgetItem* i, int x) : parentItem(i), index(x)
+ {
+ }
virtual void visitAronVariant(const aron::data::DictPtr&) final;
virtual void visitAronVariant(const aron::data::ListPtr&) final;
@@ -58,7 +59,6 @@ namespace armarx
private:
bool checkTreeWidgetItemForSimilarName(const std::string& name) const;
+ void adjustNumberOfChildren(AronTreeWidgetItem* parent, size_t numChildren);
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..51836fde9dc7a71fa44740c415fbaf03c3df79e7
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.cpp
@@ -0,0 +1,42 @@
+#include "CustomWidget.h"
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+namespace armarx
+{
+ CustomWidget::CustomWidget(QTreeWidgetItem* overlayingItem) : overlayingItem(overlayingItem)
+ {
+ // connect to general QTreeWidgetItem callback.
+ // This also lets the conversion start for other objects. (Not just this widget)
+ ARMARX_CHECK(connect(this,
+ SIGNAL(elemChanged(QTreeWidgetItem*, int)),
+ overlayingItem->treeWidget(),
+ SIGNAL(itemChanged(QTreeWidgetItem*, int))));
+ }
+
+ CustomWidget*
+ CustomWidget::DynamicCast(QWidget* elem)
+ {
+ return dynamic_cast<CustomWidget*>(elem);
+ }
+
+ CustomWidget*
+ CustomWidget::DynamicCastAndCheck(QWidget* elem)
+ {
+ if (!elem)
+ {
+ return nullptr;
+ }
+ auto* casted = DynamicCast(elem);
+ ARMARX_CHECK_NOT_NULL(casted);
+ return casted;
+ }
+
+ void
+ CustomWidget::setSupressSignals(bool doSupress)
+ {
+ QObject::blockSignals(doSupress);
+ }
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..49477ffa148e6a5c38279015a356571e107dadb7
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/CustomWidget.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <QTreeWidgetItem>
+#include <QWidget>
+
+namespace armarx
+{
+
+ class CustomWidget : public QWidget
+ {
+ Q_OBJECT
+
+ public:
+ CustomWidget(QTreeWidgetItem* overlayingItem);
+ static CustomWidget* DynamicCast(QWidget*);
+ static CustomWidget* DynamicCastAndCheck(QWidget*);
+
+ virtual void setSupressSignals(bool doSupress);
+
+ protected:
+ QTreeWidgetItem* overlayingItem;
+
+ signals:
+ void elemChanged(QTreeWidgetItem* elem, int col);
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8e01710f03cf9fabe1f9f50b060bc058016099c8
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp
@@ -0,0 +1,250 @@
+#include "EditMatrixWidget.h"
+
+#include <QGridLayout>
+#include <QLabel>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "../../ColorPalettes.h"
+#include "../visitors/AronTreeWidgetCreator.h"
+#include "NDArrayHelper.h"
+
+namespace armarx
+{
+ EditMatrixWidget::EditMatrixWidget(long numRows,
+ long numCols,
+ aron::type::matrix::ElementType elemType,
+ QTreeWidgetItem* currentItem) :
+ CustomWidget(currentItem)
+ {
+ realRows = numRows;
+ realCols = numCols;
+ this->elemType = elemType;
+ // init surrounding layout
+ outerVerticalLayout = new QVBoxLayout();
+ innerHorizontalLayout = new QHBoxLayout();
+ outerVerticalLayout->addItem(innerHorizontalLayout);
+
+
+ QGridLayout* innerGrid = new QGridLayout();
+ const long createRows = std::min(numRows, MAX_ROWS_DISPLAY);
+ const long createCols = std::min(numCols, MAX_COLS_DISPLAY);
+
+ for (long i = 0; i < createRows * createCols; ++i)
+ {
+ auto* edit = new QLineEdit();
+ dispElements.push_back(edit);
+ int currRow = i / createCols;
+ int currCol = i % createCols;
+
+ innerGrid->addWidget(edit, currRow, currCol);
+ std::stringstream ss;
+ ss << "(" << currRow << ", " << currCol << ")";
+ std::string text = ss.str();
+
+ edit->setText(text.c_str());
+ }
+ innerHorizontalLayout->addItem(innerGrid);
+
+ // check, if we need to signal to the user, that not all elements are displayed.
+ if (numRows > MAX_ROWS_DISPLAY)
+ {
+ QLabel* fullLabel = new QLabel("...");
+ outerVerticalLayout->addWidget(fullLabel);
+ }
+ if (numCols > MAX_COLS_DISPLAY)
+ {
+ QLabel* fullLabel = new QLabel("...");
+ innerHorizontalLayout->addWidget(fullLabel);
+ }
+ setLayout(outerVerticalLayout);
+
+ // add hidden elements in vector
+ {
+ const auto hiddenCols = numCols - createCols;
+ // the 0th element holds all columns that are
+ // to the right of the displayed cols AND besides the displayed rows!
+ hiddenElems.push_back(std::vector<std::string>(hiddenCols * createRows, ""));
+
+ // add all rows that are hidden
+ for (long i = 0; i < numRows - createRows; ++i)
+ {
+ // add full cols; everything is hidden here
+ hiddenElems.push_back(std::vector<std::string>(numCols, ""));
+ }
+ }
+
+ for (size_t i = 0; i < dispElements.size(); ++i)
+ {
+ ARMARX_CHECK(connect(
+ dispElements[i], SIGNAL(editingFinished()), this, SLOT(matrixElementChanged())));
+ }
+ }
+
+ EditMatrixWidget*
+ EditMatrixWidget::DynamicCast(QWidget* elem)
+ {
+ return dynamic_cast<EditMatrixWidget*>(elem);
+ }
+
+ EditMatrixWidget*
+ EditMatrixWidget::DynamicCastAndCheck(QWidget* elem)
+ {
+ if (!elem)
+ {
+ return nullptr;
+ }
+ auto* casted = DynamicCast(elem);
+ ARMARX_CHECK_NOT_NULL(casted);
+ return casted;
+ }
+
+ void
+ EditMatrixWidget::setText(long row, long col, const std::string& str)
+ {
+ ARMARX_CHECK(row < realRows);
+ ARMARX_CHECK(col < realCols);
+ auto dispCols = getDisplayedCols();
+ auto dispRows = getDisplayedRows();
+ if (row < dispRows && col < dispCols)
+ {
+ dispElements[row * dispCols + col]->setText(str.c_str());
+ dispElements[row * dispCols + col]->setCursorPosition(0);
+ }
+ else if (row < dispRows)
+ {
+ long idx = row * (realCols - dispCols) + col - dispCols;
+ hiddenElems.at(0).at(idx) = str;
+ }
+ else
+ {
+ hiddenElems.at(row - dispRows + 1).at(col) = str;
+ }
+ }
+
+ std::string
+ EditMatrixWidget::getText(long row, long col)
+ {
+ ARMARX_CHECK(row < realRows);
+ ARMARX_CHECK(col < realCols);
+ const auto dispRows = getDisplayedRows();
+ const auto dispCols = getDisplayedCols();
+ if (row < dispRows && col < dispCols)
+ {
+ auto txt = dispElements.at(row * dispCols + col)->text();
+ return txt.toStdString();
+ }
+ else if (row < getDisplayedRows())
+ {
+ // the stuff besides the displayed rows
+ long idx = row * (realCols - dispCols) + col - dispCols;
+ return hiddenElems.at(0).at(idx);
+ }
+ else
+ {
+ // stuff beneath displayed rows
+ return hiddenElems.at(row - dispRows + 1).at(col);
+ }
+ }
+
+ void
+ EditMatrixWidget::highlightUnparsableEntries()
+ {
+ auto dispRows = getDisplayedRows();
+ auto dispCols = getDisplayedCols();
+ for (long row = 0; row < dispRows; ++row)
+ {
+ for (long col = 0; col < dispCols; ++col)
+ {
+ auto parsed = parseElement(row, col);
+ if (parsed.empty())
+ {
+ dispElements.at(row * dispCols + col)
+ ->setPalette(gui_color_palette::getErrorPalette());
+ }
+ else
+ {
+ dispElements.at(row * dispCols + col)
+ ->setPalette(gui_color_palette::getNormalPalette());
+ }
+ }
+ }
+ }
+
+ bool
+ EditMatrixWidget::hasParseErrors()
+ {
+ // also parse the hidden stuff!
+ for (long row = 0; row < realRows; ++row)
+ {
+ for (long col = 0; col < realCols; ++col)
+ {
+ auto parsed = parseElement(row, col);
+ if (parsed.empty())
+ {
+ if (row >= getDisplayedRows() || col >= getDisplayedCols())
+ {
+ ARMARX_ERROR
+ << "Programming error! Could not parse content in EditMatrixWidget "
+ "that was set programatically from inside the SkillManagerPlugin. "
+ "The error occured in row "
+ << row << " and col " << col << ".";
+ }
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
+ std::vector<unsigned char>
+ EditMatrixWidget::parseElement(long row, long col)
+ {
+ std::string str = getText(row, col);
+ try
+ {
+ switch (elemType)
+ {
+ case armarx::aron::type::matrix::INT16:
+ return NDArrayHelper::toByteVector<int16_t>(str);
+
+ case armarx::aron::type::matrix::INT32:
+ return NDArrayHelper::toByteVector<int32_t>(str);
+
+ case armarx::aron::type::matrix::INT64:
+ return NDArrayHelper::toByteVector<int64_t>(str);
+
+ case armarx::aron::type::matrix::FLOAT32:
+ return NDArrayHelper::toByteVector<float>(str);
+
+ case armarx::aron::type::matrix::FLOAT64:
+ return NDArrayHelper::toByteVector<double>(str);
+ }
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ return {};
+ }
+ return {};
+ }
+
+ long
+ EditMatrixWidget::getDisplayedRows() const
+ {
+ return std::min(realRows, MAX_ROWS_DISPLAY);
+ }
+ long
+ EditMatrixWidget::getDisplayedCols() const
+ {
+ return std::min(realCols, MAX_COLS_DISPLAY);
+ }
+ void
+ EditMatrixWidget::matrixElementChanged()
+ {
+ blockSignals(true);
+ highlightUnparsableEntries();
+ blockSignals(false);
+ emit elemChanged(overlayingItem, 1);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..1fdaaa9630a09ab4a73f83fc05fa065fd0663e4d
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h
@@ -0,0 +1,81 @@
+#pragma once
+#include <vector>
+
+#include <QHBoxLayout>
+#include <QLineEdit>
+#include <QObject>
+#include <QTreeWidgetItem>
+#include <QVBoxLayout>
+
+#include "RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h"
+
+#include "CustomWidget.h"
+
+namespace armarx
+{
+ // Custom Widget used to display Matrices
+ // MAX_ROWS_DISPLAY and MAX_COLS_DISPLAY are the maximal number of elements displayable
+ // everything else will not be displayed. However, there can still be made changes to this data with setText()
+ class EditMatrixWidget : public CustomWidget
+ {
+ Q_OBJECT
+ public:
+ EditMatrixWidget() = delete;
+ EditMatrixWidget(long numRows,
+ long numCols,
+ aron::type::matrix::ElementType elemType,
+ QTreeWidgetItem* currentWidget);
+
+ static EditMatrixWidget* DynamicCast(QWidget*);
+ static EditMatrixWidget* DynamicCastAndCheck(QWidget*);
+
+ // Sets the text on all visible rows and cols.
+ void setText(long row, long col, const std::string& str);
+ std::string getText(long row, long col);
+ void highlightUnparsableEntries();
+ bool hasParseErrors();
+ // returns an empty vector if parsing failed. Else, contains the individual bytes.
+ std::vector<unsigned char> parseElement(long row, long col);
+
+
+ private:
+ // Dimensions of the underlying Type to represent
+ long realRows = 1;
+ long realCols = 1;
+
+ // maximum of rows / cols to display
+ static constexpr long MAX_ROWS_DISPLAY = 5;
+ static constexpr long MAX_COLS_DISPLAY = 5;
+ long getDisplayedRows() const;
+ long getDisplayedCols() const;
+
+ // Want to add dots to show the user that not all is displayed. This needs some wrapping layouts
+ QVBoxLayout* outerVerticalLayout;
+ QHBoxLayout* innerHorizontalLayout;
+
+ // Using row major layout
+ std::vector<QLineEdit*> dispElements;
+ // rows<cols<text>>: stores get and set messages for hidden elements (if there is not enough space for all elements)
+ std::vector<std::vector<std::string>> hiddenElems;
+
+ template <typename T>
+ static std::vector<unsigned char> toByteVector(const std::string& str);
+
+ // TODO: It would be nice to have this code somewhere else, this way, at least the GUI-responses
+ // are restricted to this Widget (signal & slot stuff)
+ aron::type::matrix::ElementType elemType;
+
+ private slots:
+ void matrixElementChanged();
+ };
+
+ template <typename T>
+ std::vector<unsigned char>
+ EditMatrixWidget::toByteVector(const std::string& str)
+ {
+ auto val = simox::alg::to_<T>(str);
+ std::vector<unsigned char> res(sizeof(val), 0);
+ *reinterpret_cast<T*>(res.data()) = val;
+ return res;
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bfe1be2bf154768ae5ff49eff04d13caf492d133
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp
@@ -0,0 +1,139 @@
+#include "IntEnumWidget.h"
+
+#include <QHBoxLayout>
+
+#include "RobotAPI/libraries/aron/core/type/variant/All.h"
+
+#include "../../ColorPalettes.h"
+#include "../visitors/AronTreeWidgetConverter.h"
+
+namespace armarx
+{
+ IntEnumWidget::IntEnumWidget(const aron::type::IntEnumPtr& enumPtr,
+ QTreeWidgetItem* currentItem) :
+ CustomWidget(currentItem), element_type(enumPtr)
+ {
+ auto names = enumPtr->getAcceptedValueNames();
+ auto outerLayout = new QHBoxLayout();
+ outerLayout->setContentsMargins(0, 0, 0, 0);
+ innerWidget = new QComboBox();
+ outerLayout->addWidget(innerWidget);
+
+ for (const auto& n : names)
+ {
+ innerWidget->addItem(n.c_str());
+ }
+ innerWidget->setInsertPolicy(QComboBox::NoInsert);
+ innerWidget->setEditable(true);
+ innerWidget->setCurrentIndex(0);
+
+ setLayout(outerLayout);
+ ARMARX_CHECK(connect(innerWidget,
+ SIGNAL(currentTextChanged(const QString&)),
+ this,
+ SLOT(textChanged(const QString&))));
+ }
+
+ IntEnumWidget*
+ IntEnumWidget::DynamicCast(QWidget* widget)
+ {
+ return dynamic_cast<IntEnumWidget*>(widget);
+ }
+
+ IntEnumWidget*
+ IntEnumWidget::DynamicCastAndCheck(QWidget* elem)
+ {
+ if (!elem)
+ {
+ return nullptr;
+ }
+ auto* casted = DynamicCast(elem);
+ ARMARX_CHECK_NOT_NULL(casted);
+ return casted;
+ }
+
+ bool
+ IntEnumWidget::hasParseErrors()
+ {
+ return parseToAron().first;
+ }
+
+ void
+ IntEnumWidget::setText(const QString& text)
+ {
+ innerWidget->setEditText(text);
+ highlightUnparsableEntries();
+ }
+
+ QString
+ IntEnumWidget::getText()
+ {
+ return innerWidget->currentText();
+ }
+
+ std::pair<bool, aron::data::IntPtr>
+ IntEnumWidget::parseToAron()
+ {
+ auto crAron = std::make_shared<aron::data::Int>(element_type->getPath());
+
+ // first look for strings in value map
+ auto valueMap = element_type->getAcceptedValueMap();
+ std::string toParse = getText().toStdString();
+
+ auto searchRes = valueMap.find(toParse);
+ if (searchRes != valueMap.end())
+ {
+ crAron->setValue(searchRes->second);
+ return {true, crAron};
+ }
+ // maybe its the number directly
+ auto accVals = element_type->getAcceptedValues();
+ int res;
+ try
+ {
+ res = simox::alg::to_<int>(toParse);
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ ARMARX_VERBOSE << "Failed to parse IntEnum: Could not convert \'" << toParse
+ << "\' to an int. It also was not one of the accepted strings.";
+ return {false, nullptr};
+ }
+ if (std::find(accVals.begin(), accVals.end(), res) != accVals.end())
+ {
+
+ crAron->setValue(res);
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Parsed int " << res
+ << "but this int is not part of the accepted values.";
+ return {false, nullptr};
+ }
+ return {true, crAron};
+ }
+
+ void
+ IntEnumWidget::highlightUnparsableEntries()
+ {
+ auto parsed = parseToAron().first;
+ if (parsed)
+ {
+ innerWidget->setPalette(gui_color_palette::getNormalPalette());
+ }
+ else
+ {
+ innerWidget->setPalette(gui_color_palette::getErrorPalette());
+ }
+ }
+
+ void
+ IntEnumWidget::textChanged(const QString&)
+ {
+ setSupressSignals(true);
+ highlightUnparsableEntries();
+ setSupressSignals(false);
+ // fire change signal
+ emit CustomWidget::elemChanged(overlayingItem, 1);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..095ea480f00a6962bb0ebf407c4f43a8379a59e3
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.h
@@ -0,0 +1,42 @@
+#pragma once
+
+#include <QComboBox>
+
+#include "RobotAPI/libraries/aron/core/data/variant/primitive/Int.h"
+#include "RobotAPI/libraries/aron/core/type/variant/forward_declarations.h"
+
+#include "../AronTreeWidgetItem.h"
+#include "CustomWidget.h"
+
+namespace armarx
+{
+ // Custom wrapper around the QComboBox widget to represent IntEnums
+ // This class handles parsing once the user edited the field and is also responsible
+ // to parse the according aron data from text
+ class IntEnumWidget : public CustomWidget
+ {
+ Q_OBJECT
+ public:
+ IntEnumWidget(const aron::type::IntEnumPtr&, QTreeWidgetItem* currentItem);
+ // needs to be called after the constructor, because qt needs the meta object to be fully constructed at this point
+ void postCtorCall();
+
+ static IntEnumWidget* DynamicCast(QWidget*);
+ static IntEnumWidget* DynamicCastAndCheck(QWidget*);
+
+ bool hasParseErrors();
+ void setText(const QString& text);
+ QString getText();
+ std::pair<bool, aron::data::IntPtr> parseToAron();
+
+ private:
+ const aron::type::IntEnumPtr element_type;
+ QComboBox* innerWidget;
+
+ bool noParseErrors = true;
+
+ void highlightUnparsableEntries();
+ private slots:
+ void textChanged(const QString&);
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9599a2dbdfd456e1237542d34006f5d4c4c9bbf
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.cpp
@@ -0,0 +1 @@
+#include "NDArrayHelper.h"
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..ed91b874cc5f91ee24cf95eb6b46013da7b9d590
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/NDArrayHelper.h
@@ -0,0 +1,30 @@
+#pragma once
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string.h>
+
+class NDArrayHelper
+{
+public:
+ // Uses the simox utility to parse floatingpoint and fixed precision types
+ // it then writes the result into a byte vector
+ // an empty byte vector encodes a parsing error.
+ template <typename T>
+ std::vector<unsigned char> static toByteVector(const std::string& str)
+ {
+ try
+ {
+ auto val = simox::alg::to_<T>(str);
+ std::vector<unsigned char> res(sizeof(val), 0);
+ T* reinterpPtr = reinterpret_cast<T*>(res.data());
+ T* laundered = std::launder(reinterpPtr);
+ *laundered = val;
+ return res;
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ return {};
+ }
+ }
+};
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b1d6fa3b946a704b180109c2598b663e2ba03bad
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp
@@ -0,0 +1,160 @@
+#include "QuaternionWidget.h"
+
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QLineEdit>
+
+#include "../../ColorPalettes.h"
+#include "NDArrayHelper.h"
+namespace armarx
+{
+
+ QuaternionWidget*
+ QuaternionWidget::DynamicCast(QWidget* elem)
+ {
+ return dynamic_cast<QuaternionWidget*>(elem);
+ }
+
+ QuaternionWidget*
+ QuaternionWidget::DynamicCastAndCheck(QWidget* elem)
+ {
+ if (!elem)
+ {
+ return nullptr;
+ }
+ auto* casted = DynamicCast(elem);
+ ARMARX_CHECK_NOT_NULL(casted);
+ return casted;
+ }
+
+ void
+ QuaternionWidget::setText(QuaternionComponents col, const std::string& str)
+ {
+ size_t idx = (size_t)col;
+ xyzw_components.at(idx)->setText(str.c_str());
+ highlightUnparsableEntries();
+ }
+
+ bool
+ QuaternionWidget::hasParseErrors()
+ {
+ return parseErrors;
+ }
+
+ armarx::QuaternionWidget::QuaternionWidget(aron::type::quaternion::ElementType elType,
+ QTreeWidgetItem* currentItem) :
+ CustomWidget(currentItem), element_type(elType)
+ {
+ auto outerLayout = new QHBoxLayout();
+ auto labelCol1 = new QVBoxLayout();
+ auto labelCol2 = new QVBoxLayout();
+ auto xzEdit = new QVBoxLayout();
+ auto ywEdit = new QVBoxLayout();
+
+ outerLayout->addLayout(labelCol1);
+ outerLayout->addLayout(xzEdit);
+ outerLayout->addLayout(labelCol2);
+ outerLayout->addLayout(ywEdit);
+
+ xyzw_components.reserve(4);
+ for (size_t i = 0; i < 4; ++i)
+ {
+ xyzw_components.push_back(new QLineEdit());
+ }
+
+ // Tabbing order >> Viewing order. Thats why the indeces are a bit switched
+ labelCol1->addWidget(new QLabel("x"));
+ xzEdit->addWidget(xyzw_components.at(0));
+
+ labelCol2->addWidget(new QLabel("z"));
+ ywEdit->addWidget(xyzw_components.at(2));
+
+ labelCol1->addWidget(new QLabel("y"));
+ xzEdit->addWidget(xyzw_components.at(1));
+
+ labelCol2->addWidget(new QLabel("w"));
+ ywEdit->addWidget(xyzw_components.at(3));
+
+ for (const auto& el : xyzw_components)
+ {
+ ARMARX_CHECK(
+ connect(el, SIGNAL(editingFinished()), this, SLOT(quaternionElementChanged())));
+ }
+
+ setLayout(outerLayout);
+ highlightUnparsableEntries();
+ }
+
+ void
+ QuaternionWidget::highlightUnparsableEntries()
+ {
+ parseErrors = false;
+ for (size_t i = 0; i < 4; ++i)
+ {
+ bool ok = false;
+ switch (element_type)
+ {
+ case armarx::aron::type::quaternion::FLOAT32:
+ {
+ float flt;
+ std::tie(ok, flt) = parseQuatVals<float>((QuaternionComponents)i);
+ break;
+ }
+ case armarx::aron::type::quaternion::FLOAT64:
+ {
+ double dbl;
+ std::tie(ok, dbl) = parseQuatVals<double>((QuaternionComponents)i);
+ break;
+ }
+ }
+ if (ok)
+ {
+ xyzw_components.at(i)->setPalette(gui_color_palette::getNormalPalette());
+ }
+ else
+ {
+ xyzw_components.at(i)->setPalette(gui_color_palette::getErrorPalette());
+ parseErrors = true;
+ }
+ }
+ }
+
+ std::vector<unsigned char>
+ QuaternionWidget::parseAllToNDArray()
+ {
+ std::vector<unsigned char> res;
+ bool success = true;
+ if (element_type == aron::type::quaternion::FLOAT32)
+ {
+ res.reserve(16);
+ for (size_t i = 0; i < 4; ++i)
+ {
+ auto bytevec =
+ NDArrayHelper::toByteVector<float>(xyzw_components.at(i)->text().toStdString());
+ success &= !bytevec.empty();
+ res.insert(res.end(), bytevec.begin(), bytevec.end());
+ }
+ }
+ else
+ {
+ res.reserve(32);
+ for (size_t i = 0; i < 4; ++i)
+ {
+ auto bytevec = NDArrayHelper::toByteVector<double>(
+ xyzw_components.at(i)->text().toStdString());
+ success &= !bytevec.empty();
+ res.insert(res.end(), bytevec.begin(), bytevec.end());
+ }
+ }
+ return (success) ? res : std::vector<unsigned char>();
+ }
+
+ void
+ QuaternionWidget::quaternionElementChanged()
+ {
+ CustomWidget::setSupressSignals(true);
+ highlightUnparsableEntries();
+ CustomWidget::setSupressSignals(false);
+ emit CustomWidget::elemChanged(overlayingItem, 1);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..d55d2bba35e2712d1d161c83ed86b06b300bcf2d
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h
@@ -0,0 +1,68 @@
+#pragma once
+#include <vector>
+
+#include <QLineEdit>
+#include <QObject>
+#include <QVBoxLayout>
+
+#include "RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h"
+
+#include "CustomWidget.h"
+
+
+namespace armarx
+{
+ // Custom Widget which represents a Quaternion - This class does not normalize the inputs
+ // It can parse its data to NDArray and also handles user edit signals itself. (And also the highlighting if errors occur)
+ class QuaternionWidget : public CustomWidget
+ {
+ Q_OBJECT
+ public:
+ QuaternionWidget(aron::type::quaternion::ElementType type, QTreeWidgetItem* currentItem);
+
+ static QuaternionWidget* DynamicCast(QWidget*);
+ static QuaternionWidget* DynamicCastAndCheck(QWidget*);
+
+ enum struct QuaternionComponents
+ {
+ X = 0,
+ Y,
+ Z,
+ W
+ };
+ void setText(QuaternionComponents col, const std::string& str);
+ std::string getText(QuaternionComponents col);
+ bool hasParseErrors();
+ std::vector<unsigned char> parseAllToNDArray();
+
+ private:
+ std::vector<QLineEdit*> xyzw_components;
+ aron::type::quaternion::ElementType element_type;
+ bool parseErrors = false;
+
+ void highlightUnparsableEntries();
+ template <typename T>
+ std::pair<bool, T> parseQuatVals(QuaternionComponents comp);
+
+ private slots:
+ void quaternionElementChanged();
+ };
+
+ template <typename T>
+ std::pair<bool, T>
+ QuaternionWidget::parseQuatVals(QuaternionWidget::QuaternionComponents comp)
+ {
+ // size_t data_size = element_type == aron::type::quaternion::ElementType::FLOAT32 ? 4 : 8;
+ try
+ {
+ T val = simox::alg::to_<T>(xyzw_components.at((size_t)comp)->text().toStdString());
+ return {true, val};
+ }
+ catch (const simox::error::SimoxError& err)
+ {
+ return {false, NAN};
+ }
+ }
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/cache-v2 b/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/cache-v2
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/cmakeFiles-v1 b/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/cmakeFiles-v1
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/codemodel-v2 b/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/.cmake/api/v1/query/codemodel-v2
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/qtcsettings.cmake b/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/qtcsettings.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..fccf6a67eee120cbb1092f841fda5cca870a5a14
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/build/qtcsettings.cmake
@@ -0,0 +1,8 @@
+# This file is managed by Qt Creator, do not edit!
+
+set("CMAKE_BUILD_TYPE" "RelWithDebInfo" CACHE "STRING" "" FORCE)
+set("CMAKE_PROJECT_INCLUDE_BEFORE" "/common/homes/students/bodmer/opt/qtcreator/qtcreator/share/qtcreator/package-manager/auto-setup.cmake" CACHE "PATH" "" FORCE)
+set("QT_QMAKE_EXECUTABLE" "/usr/lib/qt5/bin/qmake" CACHE "STRING" "" FORCE)
+set("CMAKE_PREFIX_PATH" "/usr" CACHE "STRING" "" FORCE)
+set("CMAKE_C_COMPILER" "/usr/bin/gcc" CACHE "STRING" "" FORCE)
+set("CMAKE_CXX_COMPILER" "/usr/bin/g++" CACHE "STRING" "" FORCE)
\ No newline at end of file
diff --git a/source/RobotAPI/interface/ArViz/Component.ice b/source/RobotAPI/interface/ArViz/Component.ice
index 33fd7ca5728456079298f4ca34c5aee074d88a62..6ba528b4f87fe2dab27c6d9511612b3ae01055ed 100644
--- a/source/RobotAPI/interface/ArViz/Component.ice
+++ b/source/RobotAPI/interface/ArViz/Component.ice
@@ -106,6 +106,13 @@ struct RecordingBatch
sequence<Recording> RecordingSeq;
+struct RecordingsInfo
+{
+ data::RecordingSeq recordings;
+ // Equivalent to the property "HistoryPath" of ArVizStorage.
+ string recordingsPath;
+};
+
};
interface StorageInterface
@@ -121,7 +128,7 @@ interface StorageInterface
void stopRecording();
- data::RecordingSeq getAllRecordings();
+ data::RecordingsInfo getAllRecordings();
data::RecordingBatch getRecordingBatch(string recordingID, long batchIndex);
};
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 5d9d3c04b278f8032125f98cf82d63aaa8253155..9e290598fee8047f853044e54ea3775b62ab49f7 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -108,6 +108,7 @@ set(SLICE_FILES
aron.ice
aron/Aron.ice
+ aron/test/AronConversionTestInterface.ice
armem.ice
diff --git a/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice b/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
index ffd72e20c51675c207869ae3c8c66e52b84c77fa..baad0e798bc8a3762ec28ac4394b1a3556e3c576 100644
--- a/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
+++ b/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
@@ -2,6 +2,7 @@
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+#include <RobotAPI/interface/core/RobotLocalization.ice>
module armarx
{
@@ -9,7 +10,7 @@ module armarx
{
module robot_state
{
- interface LegacyRobotStateMemoryAdapterInterface extends armarx::KinematicUnitListener, armarx::PlatformUnitListener
+ interface LegacyRobotStateMemoryAdapterInterface extends armarx::KinematicUnitListener, armarx::PlatformUnitListener, armarx::GlobalRobotPoseLocalizationListener
{
}
}
diff --git a/source/RobotAPI/interface/aron/Aron.ice b/source/RobotAPI/interface/aron/Aron.ice
index c766750c2b8fbb33940fa09b0b0978ea73512ae7..fb412c46c2409c60ac236ba7579c64fe795dff28 100644
--- a/source/RobotAPI/interface/aron/Aron.ice
+++ b/source/RobotAPI/interface/aron/Aron.ice
@@ -170,6 +170,7 @@ module armarx
// useful for memory ice_conversions
sequence<Dict> AronDictSeq;
+ sequence<Dict> DictSeq;
};
};
};
diff --git a/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice b/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..924f40c6d8664123c822eb9f7eb338d8d50b52d5
--- /dev/null
+++ b/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice
@@ -0,0 +1,61 @@
+/*
+ * 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 SpeechX::aron_cpp_to_python_conv_test
+ * 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 <RobotAPI/interface/aron/Aron.ice>
+
+
+module armarx { module aron { module test
+{
+
+module dto
+{
+
+ struct TestAronConversionRequest
+ {
+ string aronClassName;
+ ::armarx::aron::data::dto::Dict probe;
+ };
+ struct TestAronConversionResponse
+ {
+ bool success;
+ string errorMessage;
+
+ ::armarx::aron::data::dto::Dict probe;
+ };
+
+};
+
+
+module dti
+{
+
+ interface AronConversionTestInterface
+ {
+ dto::TestAronConversionResponse testAronConversion(dto::TestAronConversionRequest req);
+ };
+
+};
+
+};};};
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index a80b9b22c09d331e30f726dfb751c45543319fc5..6b6ad3bd501a57603a8da4226cd92e7f59ba1796 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -24,13 +24,14 @@
#pragma once
#include <ArmarXCore/interface/events/SimulatorResetEvent.ice>
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
-#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
-#include <RobotAPI/interface/core/FramedPoseBase.ice>
-
#include <ArmarXCore/interface/observers/Timestamp.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/serialization/Eigen.ice>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+#include <RobotAPI/interface/core/RobotLocalization.ice>
+
#include <Ice/BuiltinSequences.ice>
module armarx
@@ -188,7 +189,6 @@ module armarx
*/
interface RobotStateComponentInterface extends
KinematicUnitListener,
- PlatformUnitListener,
GlobalRobotPoseLocalizationListener,
SimulatorResetEvent
{
diff --git a/source/RobotAPI/interface/observers/PlatformUnitObserverInterface.ice b/source/RobotAPI/interface/observers/PlatformUnitObserverInterface.ice
index e7eddd170418f8ed671695807e3817e0522bf1da..20709f1fedeb65bb3766d2008607ee750a74965a 100644
--- a/source/RobotAPI/interface/observers/PlatformUnitObserverInterface.ice
+++ b/source/RobotAPI/interface/observers/PlatformUnitObserverInterface.ice
@@ -26,10 +26,11 @@
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <RobotAPI/interface/core/RobotLocalization.ice>
+
module armarx
{
- interface PlatformUnitObserverInterface extends ObserverInterface, PlatformUnitListener
+ interface PlatformUnitObserverInterface extends ObserverInterface, PlatformUnitListener, GlobalRobotPoseLocalizationListener
{
};
};
-
diff --git a/source/RobotAPI/interface/units/PlatformUnitInterface.ice b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
index f7f62d13d0b490fd50284582096e0308fff90223..f83ad97cf10f83c8975a8c79f1b44deecd594d4e 100644
--- a/source/RobotAPI/interface/units/PlatformUnitInterface.ice
+++ b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
@@ -24,22 +24,22 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
+
#include <RobotAPI/interface/core/GeometryBase.ice>
#include <RobotAPI/interface/core/RobotLocalization.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
-{
- /**
+{
+ /**
* Implements an interface to an PlatformUnit.
**/
interface PlatformUnitInterface extends SensorActorUnitInterface
{
- /**
+ /**
* moveTo moves the platform to a global pose specified by:
* @param targetPlatformPositionX Global x-coordinate of target position.
* @param targetPlatformPositionY Global y-coordinate of target position.
@@ -47,14 +47,20 @@ module armarx
* @param postionalAccuracy Platform stops translating if distance to target position gets lower than this threshhold.
* @param orientationalAccuracy Platform stops rotating if distance from current to target orientation gets lower than this threshhold.
**/
- void moveTo(float targetPlatformPositionX, float targetPlatformPositionY, float targetPlatformRotation, float positionalAccuracy, float orientationalAccuracy);
+ void moveTo(float targetPlatformPositionX,
+ float targetPlatformPositionY,
+ float targetPlatformRotation,
+ float positionalAccuracy,
+ float orientationalAccuracy);
/**
* move moves the platform with given velocities.
* @param targetPlatformVelocityX x-coordinate of target velocity defined in platform root.
* @param targetPlatformVelocityY y-coordinate of target velocity defined in platform root.
* @param targetPlatformVelocityRotation Target orientational velocity defined in platform root.
**/
- void move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation);
+ void move(float targetPlatformVelocityX,
+ float targetPlatformVelocityY,
+ float targetPlatformVelocityRotation);
/**
* moveRelative moves to a pose defined in platform coordinates.
* @param targetPlatformOffsetX x-coordinate of target position defined in platform root.
@@ -63,7 +69,11 @@ module armarx
* @param postionalAccuracy Platform stops translating if distance to target position gets lower than this threshhold.
* @param orientationalAccuracy Platform stops rotating if distance from current to target orientation gets lower than this threshhold.
**/
- void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy);
+ void moveRelative(float targetPlatformOffsetX,
+ float targetPlatformOffsetY,
+ float targetPlatformOffsetRotation,
+ float positionalAccuracy,
+ float orientationalAccuracy);
/**
* setMaxVelocities allows to specify max velocities in translation and orientation.
* @param positionalVelocity Max translation velocity.
@@ -84,42 +94,28 @@ module armarx
float rotationAroundZ = 0.0f;
};
- /**
+ /**
* Implements an interface to an PlatformUnitListener.
**/
interface PlatformUnitListener
{
/**
- * reportPlatformPose reports current platform pose.
- * @param currentPose Current global platform pose.
- **/
- ["deprecate:reportPlatformPose() has been deprecated, use GlobalRobotPoseLocalizationListener::reportGlobalRobotPose() instead."]
- void reportPlatformPose(PlatformPose currentPose);
-
- /**
- * reportNewTargetPose reports target platform pose.
- * @param newPlatformPositionX Global x-coordinate of target platform position.
- * @param newPlatformPositionY Global y-coordinate of target platform position.
- * @param newPlatformRotation Target global orientation of platform.
- **/
- ["deprecate:reportNewTargetPose() has been deprecated, use ... instead."]
- void reportNewTargetPose(float newPlatformPositionX, float newPlatformPositionY, float newPlatformRotation);
- /**
- * reportPlatformVelocity reports current platform velocities.
- * @param currentPlatformVelocityX x-coordinate of current velocity defined in platform root.
- * @param currentPlatformVelocityY y-coordinate of current velocity defined in platform root.
- * @param currentPlatformVelocityRotation Current orientational velocity defined in platform root.
- **/
- // ["deprecate:reportPlatformVelocity() has been deprecated, use OdometryListener::reportOdometryVelocity() instead."]
- void reportPlatformVelocity(float currentPlatformVelocityX, float currentPlatformVelocityY, float currentPlatformVelocityRotation);
+ * reportPlatformVelocity reports current platform velocities.
+ * @param currentPlatformVelocityX x-coordinate of current velocity defined in platform root.
+ * @param currentPlatformVelocityY y-coordinate of current velocity defined in platform root.
+ * @param currentPlatformVelocityRotation Current orientational velocity defined in platform root.
+ **/
+ // ["deprecate:reportPlatformVelocity() has been deprecated, use OdometryListener::reportOdometryVelocity() instead."]
+ void reportPlatformVelocity(float currentPlatformVelocityX,
+ float currentPlatformVelocityY,
+ float currentPlatformVelocityRotation);
- ["deprecate:reportPlatformOdometryPose() has been deprecated, use OdometryListener::reportOdometryPose() instead."]
- void reportPlatformOdometryPose(float x, float y, float angle);
+ ["deprecate:reportPlatformOdometryPose() has been deprecated, use "
+ "OdometryListener::reportOdometryPose() instead."] void
+ reportPlatformOdometryPose(float x, float y, float angle);
};
interface PlatformSubUnitInterface extends PlatformUnitInterface
{
-
};
-
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index ec6658fa85afba280fef0613fbc60596da57ee8a..3e503f9a7f04ff1fdcdb4bb1988ee04f0b18666d 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -3,6 +3,9 @@ set(LIB_NAME ${PROJECT_NAME}ArmarXObjects)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(manif_FOUND "manif not available")
+
set(LIBS
# ArmarXGui
RemoteGui
@@ -10,6 +13,9 @@ set(LIBS
RobotAPI::Core
RobotAPIInterfaces
aroncommon
+
+ Eigen3::Eigen
+ ${manif_LIBRARIES}
)
set(LIB_FILES
@@ -64,6 +70,10 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+if(manif_FOUND)
+ target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
+endif()
+
add_library(${PROJECT_NAME}::ArmarXObjects ALIAS ${PROJECT_NAME}ArmarXObjects)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 4b61a29ecf41a7af19336ccdc81d30c7dc32189a..f5516353eecf459e4d2cde12db3737b396908972 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -104,6 +104,7 @@ namespace armarx
{
return findObject(id.dataset(), id.className());
}
+
std::optional<ObjectInfo> ObjectFinder::findObject(const objpose::ObjectPose& obj) const
{
return findObject(obj.objectID);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index ea3a7e9cb08f423716931086f309e6dabeedd1b9..708608528cd8d506d80f229a6c9c96896d18207a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -18,8 +18,7 @@ namespace armarx
/**
* @brief Used to find objects in the ArmarX objects repository [1] (formerly [2]).
*
- * @see [1] https://gitlab.com/ArmarX/PriorKnowledgeData
- * @see [2] https://gitlab.com/ArmarX/ArmarXObjects
+ * @see [1] https://git.h2t.iar.kit.edu/sw/armarx/prior-knowledge-data
*/
class ObjectFinder : Logging
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
index f2d76a501ecf8e1f53dcf9bf80a2e8a0f66d7536..8a2856b2516784d6d5c0413a5123af9665696d22 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
@@ -19,16 +19,7 @@ namespace armarx
{
if (nameOrID.find("/") != nameOrID.npos)
{
- const std::vector<std::string> split = simox::alg::split(nameOrID, "/", true);
- ARMARX_CHECK(split.size() == 2 || split.size() == 3)
- << "Expected ID of format 'Dataset/ClassName' or 'Dataset/ClassName/InstanceName'"
- << ", but got: '" << nameOrID << "' (too many '/').";
- _dataset = split[0];
- _className = split[1];
- if (split.size() == 3)
- {
- _instanceName = split[2];
- }
+ setFromString(nameOrID);
}
else
{
@@ -37,6 +28,30 @@ namespace armarx
}
}
+ ObjectID ObjectID::FromString(const std::string& idString)
+ {
+ ObjectID id;
+ id.setFromString(idString);
+ return id;
+ }
+
+ void ObjectID::setFromString(const std::string& idString)
+ {
+ const std::vector<std::string> split = simox::alg::split(idString, "/", true);
+ ARMARX_CHECK(split.size() == 2 || split.size() == 3)
+ << "Expected ID of format 'Dataset/ClassName' or 'Dataset/ClassName/InstanceName'"
+ << ", but got: '" << idString << "' "
+ << "(expected 2 or 3 '/'s, but found " << split.size() << ").";
+
+ _dataset = split[0];
+ _className = split[1];
+
+ if (split.size() == 3)
+ {
+ _instanceName = split[2];
+ }
+ }
+
std::string ObjectID::str() const
{
std::string _str = _dataset + "/" + _className;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
index 389b203a6ecc67f2f6cd074ce103c5b52687a2be..b152844b17f150b80c44f8ed3c849e263b2e8fd4 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
@@ -17,6 +17,9 @@ namespace armarx
/// Construct from either a class name ("myobject") or ID ("mydataset/myobject", "mydataset/myclass/myinstance").
ObjectID(const std::string& nameOrID);
+ /// Construct from a string produced by `str()`, e.g. ("mydataset/myobject", "mydataset/myclass/myinstance").
+ static ObjectID FromString(const std::string& idString);
+
inline std::string dataset() const
{
@@ -37,6 +40,8 @@ namespace armarx
/// Return "dataset/className" or "dataset/className/instanceName".
std::string str() const;
+ void setFromString(const std::string& idString);
+
/// Return just the class ID without an intance name.
ObjectID getClassID() const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index fea3677bd5aa3d0d2b762beb32e2c7ff8d6cc9ea..6d20564f660aee832698ca9d1eda297b56de9f3c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -59,12 +59,17 @@ namespace armarx
return _id.str();
}
- ObjectInfo::path ObjectInfo::objectDirectory() const
+ ObjectInfo::path ObjectInfo::objectDirectory(const bool fixDataPath) const
{
+ if(fixDataPath)
+ {
+ return _relObjectsPath / _id.dataset() / _id.className();
+ }
+
return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
}
- PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
+ PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix, const bool fixDataPath) const
{
std::string extension = _extension;
if (extension.at(0) != '.')
@@ -75,8 +80,8 @@ namespace armarx
PackageFileLocation loc;
loc.package = _packageName;
- loc.relativePath = objectDirectory() / filename;
- loc.absolutePath = _absPackageDataDir / loc.relativePath;
+ loc.relativePath = objectDirectory(fixDataPath) / filename;
+ loc.absolutePath = _absPackageDataDir / objectDirectory(false) / filename;
return loc;
}
@@ -86,14 +91,49 @@ namespace armarx
return file(".xml");
}
+ PackageFileLocation ObjectInfo::urdf() const
+ {
+ return file(".urdf");
+ }
+
+ PackageFileLocation ObjectInfo::sdf() const
+ {
+ return file(".sdf");
+ }
+
+ std::optional<PackageFileLocation> ObjectInfo::getModel() const
+ {
+ if (fs::is_regular_file(simoxXML().absolutePath))
+ {
+ return simoxXML();
+ }
+ else if (fs::is_regular_file(urdf().absolutePath))
+ {
+ return urdf();
+ }
+ else if (fs::is_regular_file(sdf().absolutePath))
+ {
+ return sdf();
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
PackageFileLocation ObjectInfo::articulatedSimoxXML() const
{
- return file(".xml", "_articulated");
+ return file(".xml", "_articulated", true);
}
PackageFileLocation ObjectInfo::articulatedUrdf() const
{
- return file(".urdf", "_articulated");
+ return file(".urdf", "_articulated", true);
+ }
+
+ PackageFileLocation ObjectInfo::articulatedSdf() const
+ {
+ return file(".sdf", "_articulated");
}
std::optional<PackageFileLocation> ObjectInfo::getArticulatedModel() const
@@ -106,6 +146,10 @@ namespace armarx
{
return articulatedUrdf();
}
+ else if (fs::is_regular_file(articulatedSdf().absolutePath))
+ {
+ return articulatedSdf();
+ }
else
{
return std::nullopt;
@@ -293,5 +337,3 @@ std::ostream& armarx::operator<<(std::ostream& os, const ObjectInfo& rhs)
{
return os << rhs.id();
}
-
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 70fd46b47fb7a8a3c992dfe5d6c3d4676915aed7..ccbd4eb1be32116d8fc7ced05d8148f027fefccf 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -76,14 +76,19 @@ namespace armarx
std::string idStr() const;
- PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
+ PackageFileLocation file(const std::string& extension, const std::string& suffix = "", bool fixDataPath = false) const;
PackageFileLocation simoxXML() const;
+ PackageFileLocation urdf() const;
+ PackageFileLocation sdf() const;
+ /// Return the Simox XML, URDF or SDF, if one exists.
+ std::optional<PackageFileLocation> getModel() const;
PackageFileLocation articulatedSimoxXML() const;
PackageFileLocation articulatedUrdf() const;
- /// Return the articulated Simox XML or URDF, if one exists.
+ PackageFileLocation articulatedSdf() const;
+ /// Return the articulated Simox XML, URDF or SDF, if one exists.
std::optional<PackageFileLocation> getArticulatedModel() const;
@@ -131,7 +136,7 @@ namespace armarx
private:
- path objectDirectory() const;
+ path objectDirectory(bool fixDataPath) const;
std::optional<std::vector<std::string>> loadNames(const std::string& jsonKey) const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 74f3233ee04475ba4cd8cca04cdc8b35ede5ba18..82dc134ee4b958981cf1ebe5d74047f49eb62a2a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -12,6 +12,7 @@
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include "ice_conversions.h"
@@ -171,6 +172,31 @@ namespace armarx::objpose
objpose::fromIce(provided.localOOBB, localOOBB);
}
+ // ToDo: We can provide ...Robot() and ...Original() versions as well.
+ objpose::ProvidedObjectPose ObjectPose::toProvidedObjectPoseGlobal() const
+ {
+ objpose::ProvidedObjectPose providedObjectPose;
+
+ providedObjectPose.objectID = this->objectID;
+ providedObjectPose.providerName = this->providerName;
+ providedObjectPose.objectType = this->objectType;
+
+ providedObjectPose.isStatic = this->isStatic;
+
+ providedObjectPose.objectPoseFrame = GlobalFrame;
+ providedObjectPose.objectPose = this->objectPoseGlobal;
+ providedObjectPose.objectPoseGaussian = this->objectPoseGlobalGaussian;
+
+ providedObjectPose.objectJointValues = this->objectJointValues;
+
+ providedObjectPose.confidence = this->confidence;
+ providedObjectPose.timestamp = this->timestamp;
+
+ providedObjectPose.localOOBB = this->localOOBB;
+
+ return providedObjectPose;
+ }
+
void ObjectPose::setObjectPoseRobot(
const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal)
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 106f52a842e1c3a5790068a30a96182326c24629..6ee27fa6e039a862217d48601ad8d2d02881a615 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -46,6 +46,7 @@ namespace armarx::objpose
void toIce(data::ObjectPose& ice) const;
void fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot = nullptr);
+ objpose::ProvidedObjectPose toProvidedObjectPoseGlobal() const;
void setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal = true);
void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot = true);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
index cf42e169e67f0f18aef427c65fd24a73160983bf..446b625e4e91359fadd8dd43b4dacdc460a9d513 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
@@ -83,7 +83,7 @@ void armarx::objects::to_json(simox::json::json& j, const SceneObject& rhs)
j["instanceName"] = rhs.instanceName;
j["collection"] = rhs.collection;
j["position"] = rhs.position;
- j["orientation"] = rhs.orientation;
+ j["orientation"] = rhs.orientation.normalized();
if (rhs.isStatic.has_value())
{
j["isStatic"] = rhs.isStatic.value();
@@ -106,6 +106,7 @@ void armarx::objects::from_json(const simox::json::json& j, SceneObject& rhs)
j.at("collection").get_to(rhs.collection);
j.at("position").get_to(rhs.position);
j.at("orientation").get_to(rhs.orientation);
+ rhs.orientation.normalize();
if (j.count("isStatic"))
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
index 62161ff5cafce827a99191e97ce7bfa6bb2abee4..6525be6b5414daa0d3052f3bc2d6effc7acf5c46 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
@@ -23,7 +23,7 @@ namespace armarx::plugins
void ObjectPoseClientPlugin::preOnConnectComponent()
{
- parent<ObjectPoseClientPluginUser>().objectPoseStorage = createObjectPoseStorage();
+ _objectPoseStorage = createObjectPoseStorage();
}
objpose::ObjectPoseStorageInterfacePrx ObjectPoseClientPlugin::createObjectPoseStorage()
@@ -31,6 +31,11 @@ namespace armarx::plugins
return parent<Component>().getProxyFromProperty<objpose::ObjectPoseStorageInterfacePrx>(makePropertyName(PROPERTY_NAME));
}
+ objpose::ObjectPoseClient ObjectPoseClientPlugin::createClient()
+ {
+ return objpose::ObjectPoseClient(createObjectPoseStorage(), getObjectFinder());
+ }
+
const ObjectFinder& ObjectPoseClientPlugin::setObjectFinderPath(const std::string& path)
{
_finder.setPath(path);
@@ -58,23 +63,18 @@ namespace armarx
objpose::ObjectPoseClient ObjectPoseClientPluginUser::getClient() const
{
- return objpose::ObjectPoseClient(objectPoseStorage, getObjectFinder());
+ return plugin->createClient();
}
objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPoses()
{
- if (!objectPoseStorage)
- {
- ARMARX_WARNING << "No object pose observer.";
- return {};
- }
- return objpose::fromIce(objectPoseStorage->getObjectPoses());
+ return getClient().fetchObjectPoses();
}
objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPosesByProvider(const std::string& providerName)
{
- return objpose::ObjectPoseClient(objectPoseStorage, getObjectFinder()).fetchObjectPosesFromProvider(providerName);
+ return getClient().fetchObjectPosesFromProvider(providerName);
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
index 00c53d4ead2c77fce8710312648d54ce59b29742..7f27d9768474ea7ff916d3b65fb03c7fff97aa56 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
@@ -16,8 +16,8 @@ namespace armarx::plugins
using ComponentPlugin::ComponentPlugin;
- void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
+ objpose::ObjectPoseClient createClient();
template<class...Ts>
std::optional<ObjectInfo> findObject(Ts&& ...ts) const
@@ -43,12 +43,15 @@ namespace armarx::plugins
private:
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
void preOnInitComponent() override;
void preOnConnectComponent() override;
static constexpr const char* PROPERTY_NAME = "ObjectMemoryName";
ObjectFinder _finder;
+ objpose::ObjectPoseStorageInterfacePrx _objectPoseStorage;
};
}
@@ -71,12 +74,8 @@ namespace armarx
ObjectPoseClientPluginUser();
- objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
- objpose::ObjectPoseStorageInterfacePrx objectPoseStorage;
-
-
objpose::ObjectPoseClient getClient() const;
-
+ objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
objpose::ObjectPoseSeq getObjectPoses();
objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
index fc9dfe5081f69215398cd5c6eefaf7c8c1fbd705..e74da09bfa4a0c26adf4f1579ead89947f282fba 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
@@ -22,6 +22,8 @@
#include "predictions.h"
+#include <manif/SE3.h>
+
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/regression/linear.h>
@@ -41,44 +43,45 @@ namespace armarx::objpose
const DateTime timeOrigin = DateTime::Now();
+ static const int tangentDims = 6;
+ using Vector6d = Eigen::Matrix<double, tangentDims, 1>;
+
std::vector<double> timestampsSec;
- std::vector<Eigen::Vector3d> positions;
+ std::vector<Vector6d> tangentPoses;
// ToDo: How to handle attached objects?
for (const auto& [timestamp, pose] : poses)
{
timestampsSec.push_back((timestamp - timeOrigin).toSecondsDouble());
- positions.emplace_back(simox::math::position(pose.objectPoseGlobal).cast<double>());
+ manif::SE3f se3(simox::math::position(pose.objectPoseGlobal),
+ Eigen::Quaternionf(simox::math::orientation(pose.objectPoseGlobal)));
+ tangentPoses.emplace_back(se3.cast<double>().log().coeffs());
}
- ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
+ ARMARX_CHECK_EQUAL(timestampsSec.size(), tangentPoses.size());
- Eigen::Vector3d prediction;
+ Eigen::Matrix4f prediction;
// Static objects don't move. Objects that haven't moved for a while probably won't either.
if (timestampsSec.size() <= 1 || latestPose.isStatic)
{
- prediction = simox::math::position(latestPose.objectPoseGlobal).cast<double>();
+ prediction = latestPose.objectPoseGlobal;
}
else
{
- using simox::math::LinearRegression3d;
+ using simox::math::LinearRegression;
const bool inputOffset = false;
- const LinearRegression3d model =
- LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
+ const LinearRegression<tangentDims> model =
+ LinearRegression<tangentDims>::Fit(timestampsSec, tangentPoses, inputOffset);
const auto predictionTime = armarx::fromIce<DateTime>(time);
- prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ Vector6d linearPred = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ prediction = manif::SE3Tangentd(linearPred).exp().transform().cast<float>();
}
// Used as a template for the returned pose prediction.
ObjectPose latestCopy = latestPose;
- // Reuse the rotation from the latest pose.
- // This is a pretty generous assumption, but the linear model doesn't cover rotations,
- // so it's our best guess.
- Eigen::Matrix4f latest = latestPose.objectPoseGlobal;
- simox::math::position(latest) = prediction.cast<float>();
- latestCopy.setObjectPoseGlobal(latest);
+ latestCopy.setObjectPoseGlobal(prediction);
result.success = true;
result.prediction = latestCopy.toIce();
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp
index d14a0d60406cd4074a6ab67d79885ad9041b5914..7063ef69b2490bdea0ba91d76a2a19da6c96e057 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp
@@ -36,21 +36,46 @@ namespace armarx
{
struct Fixture
{
- std::vector<armarx::ObjectID> dcs
+ std::vector<armarx::ObjectID> dcsStringConstructor
{
{ "Data/Class/0" },
{ "Data/Class/1" },
{ "Data/Class/2" }
};
- armarx::ObjectID dc0 { "Data/Class/0" };
+ std::vector<armarx::ObjectID> dcsFromString
+ {
+ ObjectID::FromString("Data/Class/0"),
+ ObjectID::FromString("Data/Class/1"),
+ ObjectID::FromString("Data/Class/2")
+ };
+ std::vector<std::vector<armarx::ObjectID>*> dcs {
+ &dcsStringConstructor,
+ &dcsFromString
+ };
+
+ armarx::ObjectID dc0StringConstructor { "Data/Class/0" };
+ armarx::ObjectID dc0FromString = ObjectID::FromString("Data/Class/0");
+ std::vector<armarx::ObjectID*> dc0s {
+ &dc0StringConstructor,
+ &dc0FromString,
+ };
- std::vector<armarx::ObjectID> ots
+ std::vector<armarx::ObjectID> otsStringConstructor
{
{ "Other/Type/0" },
{ "Other/Type/1" },
{ "Other/Type/2" }
};
- armarx::ObjectID ot0 { "Other/Type/0" };
+ std::vector<armarx::ObjectID> otsFromString
+ {
+ ObjectID::FromString("Other/Type/0"),
+ ObjectID::FromString("Other/Type/1"),
+ ObjectID::FromString("Other/Type/2")
+ };
+ std::vector<std::vector<armarx::ObjectID>*> ots {
+ &otsStringConstructor,
+ &otsFromString
+ };
};
}
@@ -58,29 +83,44 @@ BOOST_FIXTURE_TEST_SUITE(ObjectIDTests, armarx::Fixture)
BOOST_AUTO_TEST_CASE(test_construction_from_string)
{
- for (std::size_t i = 0; i < dcs.size(); ++i)
+ for (const auto* dc : dcs)
{
- BOOST_CHECK_EQUAL(dcs[i].dataset(), "Data");
- BOOST_CHECK_EQUAL(dcs[i].className(), "Class");
- BOOST_CHECK_EQUAL(dcs[i].instanceName(), std::to_string(i));
+ for (const auto* ot : ots)
+ {
+ for (std::size_t i = 0; i < dc->size(); ++i)
+ {
+ BOOST_CHECK_EQUAL((*dc)[i].dataset(), "Data");
+ BOOST_CHECK_EQUAL((*dc)[i].className(), "Class");
+ BOOST_CHECK_EQUAL((*dc)[i].instanceName(), std::to_string(i));
- BOOST_CHECK_EQUAL(ots[i].dataset(), "Other");
- BOOST_CHECK_EQUAL(ots[i].className(), "Type");
- BOOST_CHECK_EQUAL(ots[i].instanceName(), std::to_string(i));
+ BOOST_CHECK_EQUAL((*ot)[i].dataset(), "Other");
+ BOOST_CHECK_EQUAL((*ot)[i].className(), "Type");
+ BOOST_CHECK_EQUAL((*ot)[i].instanceName(), std::to_string(i));
+ }
+ }
}
}
BOOST_AUTO_TEST_CASE(test_equals_operator)
{
- for (std::size_t i = 0; i < dcs.size(); ++i)
+ for (const auto* dc : dcs)
{
- BOOST_TEST_CONTEXT("i=" << i)
+ for (const auto* ot : ots)
{
- BOOST_CHECK_EQUAL(dcs[i], dcs[i]);
- BOOST_CHECK_NE(dcs[i], ots[i]);
- if (i != 0)
+ for (const auto* dc0 : dc0s)
{
- BOOST_CHECK_NE(dcs[i], dc0);
+ for (std::size_t i = 0; i < (*dc).size(); ++i)
+ {
+ BOOST_TEST_CONTEXT("i=" << i)
+ {
+ BOOST_CHECK_EQUAL((*dc)[i], (*dc)[i]);
+ BOOST_CHECK_NE((*dc)[i], (*ot)[i]);
+ if (i != 0)
+ {
+ BOOST_CHECK_NE((*dc)[i], *dc0);
+ }
+ }
+ }
}
}
}
@@ -88,16 +128,19 @@ BOOST_AUTO_TEST_CASE(test_equals_operator)
BOOST_AUTO_TEST_CASE(test_less_than_operator)
{
- for (std::size_t i = 0; i < dcs.size(); ++i)
+ for (const auto* dc : dcs)
{
- for (std::size_t j = i; j < dcs.size(); ++j)
+ for (std::size_t i = 0; i < (*dc).size(); ++i)
{
- BOOST_CHECK_LE(dcs[i], dcs[j]);
- BOOST_CHECK_GE(dcs[j], dcs[i]);
- if (i != j)
+ for (std::size_t j = i; j < (*dc).size(); ++j)
{
- BOOST_CHECK_LT(dcs[i], dcs[j]);
- BOOST_CHECK_GT(dcs[j], dcs[i]);
+ BOOST_CHECK_LE((*dc)[i], (*dc)[j]);
+ BOOST_CHECK_GE((*dc)[j], (*dc)[i]);
+ if (i != j)
+ {
+ BOOST_CHECK_LT((*dc)[i], (*dc)[j]);
+ BOOST_CHECK_GT((*dc)[j], (*dc)[i]);
+ }
}
}
}
@@ -105,15 +148,21 @@ BOOST_AUTO_TEST_CASE(test_less_than_operator)
BOOST_AUTO_TEST_CASE(test_equalClass)
{
- for (std::size_t i = 0; i < dcs.size(); ++i)
+ for (const auto* dc : dcs)
{
- for (std::size_t j = 0; j < dcs.size(); ++j)
+ for (const auto* ot : ots)
{
- BOOST_CHECK(dcs[i].equalClass(dcs[j]));
- BOOST_CHECK(ots[i].equalClass(ots[j]));
-
- BOOST_CHECK(!dcs[i].equalClass(ots[j]));
- BOOST_CHECK(!ots[i].equalClass(dcs[j]));
+ for (std::size_t i = 0; i < (*dc).size(); ++i)
+ {
+ for (std::size_t j = 0; j < (*dc).size(); ++j)
+ {
+ BOOST_CHECK((*dc)[i].equalClass((*dc)[j]));
+ BOOST_CHECK((*ot)[i].equalClass((*ot)[j]));
+
+ BOOST_CHECK(!(*dc)[i].equalClass((*ot)[j]));
+ BOOST_CHECK(!(*ot)[i].equalClass((*dc)[j]));
+ }
+ }
}
}
}
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 099ad8b4b981aeed1cc9e274c25672a1014ee120..057440c94c97f8428c191c2d9c7a83edc95cbf51 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -16,10 +16,13 @@ add_subdirectory(natik)
add_subdirectory(ukfm)
add_subdirectory(aron)
+add_subdirectory(aron_component_config)
add_subdirectory(armem)
add_subdirectory(armem_grasping)
add_subdirectory(armem_gui)
+add_subdirectory(armem_index)
+add_subdirectory(armem_locations)
add_subdirectory(armem_motions)
add_subdirectory(armem_mps)
add_subdirectory(armem_objects)
@@ -28,10 +31,11 @@ add_subdirectory(armem_robot)
add_subdirectory(armem_robot_state)
add_subdirectory(armem_skills)
add_subdirectory(armem_system_state)
+add_subdirectory(armem_laser_scans)
add_subdirectory(armem_vision)
add_subdirectory(skills)
add_subdirectory(RobotUnitDataStreamingReceiver)
add_subdirectory(GraspingUtility)
-
+add_subdirectory(obstacle_avoidance)
diff --git a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
index 1ae8862c277d13836d015473953c605db92c9349..fdec590098ce75d6d9e4cc67f9fc8b945ddb43d2 100644
--- a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
+++ b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
@@ -19,6 +19,7 @@ armarx_add_library(
GraspCandidateWriter.cpp
GraspCandidateReader.cpp
GraspCandidateVisu.cpp
+ GraspTrajectory.cpp
HEADERS box_to_grasp_candidates.h
box_to_grasp_candidates.ipp
@@ -30,6 +31,7 @@ armarx_add_library(
GraspCandidateWriter.h
GraspCandidateReader.h
GraspCandidateVisu.h
+ GraspTrajectory.h
)
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4b170313f7d9d71d09b055ffd497b9b658af5ac
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -0,0 +1,485 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <VirtualRobot/math/Helpers.h>
+
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
+#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+
+#include "GraspTrajectory.h"
+
+namespace armarx
+{
+
+ void
+ GraspTrajectory::writeToFile(const std::string& filename)
+ {
+ RapidXmlWriter writer;
+ RapidXmlWriterNode root = writer.createRootNode("GraspTrajectory");
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ SimpleJsonLoggerEntry e;
+ e.Add("dt", keypoint->dt);
+ e.AddAsArr("Pose", keypoint->tcpTarget);
+ e.AddAsArr("HandValues", keypoint->handJointsTarget);
+ root.append_string_node("Keypoint", e.obj->toJsonString(0, "", true));
+ }
+ writer.saveToFile(filename, true);
+ }
+
+ GraspTrajectoryPtr
+ GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
+ {
+ RapidXmlReaderNode root = reader->getRoot();
+
+ GraspTrajectoryPtr traj;
+ for (const RapidXmlReaderNode& kpNode : root.nodes("Keypoint"))
+ {
+ StructuralJsonParser p(kpNode.value());
+ p.parse();
+ JPathNavigator nav(p.parsedJson);
+ float dt = nav.selectSingleNode("dt").asFloat();
+
+ Eigen::Matrix4f pose;
+ std::vector<JPathNavigator> rows = nav.select("Pose/*");
+ for (int i = 0; i < 4; i++)
+ {
+ std::vector<JPathNavigator> cells = rows.at(i).select("*");
+ for (int j = 0; j < 4; j++)
+ {
+ pose(i, j) = cells.at(j).asFloat();
+ }
+ }
+
+ Eigen::Vector3f handValues;
+ std::vector<JPathNavigator> cells = nav.select("HandValues/*");
+ for (int j = 0; j < 3; j++)
+ {
+ handValues(j) = cells.at(j).asFloat();
+ }
+
+ if (!traj)
+ {
+ traj = GraspTrajectoryPtr(new GraspTrajectory(pose, handValues));
+ }
+ else
+ {
+ traj->addKeypoint(pose, handValues, dt);
+ }
+ }
+ return traj;
+ }
+
+ GraspTrajectoryPtr
+ GraspTrajectory::ReadFromFile(const std::string& filename)
+ {
+ return ReadFromReader(RapidXmlReader::FromFile(filename));
+ }
+
+ GraspTrajectoryPtr
+ GraspTrajectory::ReadFromString(const std::string& xml)
+ {
+ return ReadFromReader(RapidXmlReader::FromXmlString(xml));
+ }
+
+ GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f &tcpStart,
+ const Eigen::Vector3f &handJointsStart) {
+ KeypointPtr keypoint(new Keypoint(tcpStart, handJointsStart));
+ keypointMap[0] = keypoints.size();
+ keypoints.push_back(keypoint);
+ }
+
+ void
+ GraspTrajectory::addKeypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget,
+ float dt) {
+ KeypointPtr prev = lastKeypoint();
+ KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+ keypoint->updateVelocities(prev, dt);
+ float t = getDuration() + dt;
+ keypointMap[t] = keypoints.size();
+ keypoints.push_back(keypoint);
+ }
+
+ size_t
+ GraspTrajectory::getKeypointCount() const {
+ return keypoints.size();
+ }
+
+ void
+ GraspTrajectory::insertKeypoint(size_t index, const Eigen::Matrix4f &tcpTarget,
+ const Eigen::Vector3f &handJointsTarget, float dt) {
+ if (index <= 0 || index > keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+ keypoint->updateVelocities(prev, dt);
+ if (index < keypoints.size())
+ {
+ KeypointPtr next = keypoints.at(index);
+ next->updateVelocities(keypoint, next->dt);
+ }
+ keypoints.insert(keypoints.begin() + index, keypoint);
+ updateKeypointMap();
+ }
+
+ void
+ GraspTrajectory::removeKeypoint(size_t index) {
+ if (index <= 0 || index >= keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ keypoints.erase(keypoints.begin() + index);
+ if (index < keypoints.size())
+ {
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr next = keypoints.at(index);
+ next->updateVelocities(prev, next->dt);
+ }
+ updateKeypointMap();
+ }
+
+ void
+ GraspTrajectory::replaceKeypoint(size_t index, const Eigen::Matrix4f &tcpTarget,
+ const Eigen::Vector3f &handJointsTarget, float dt) {
+ if (index <= 0 || index >= keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+ keypoint->updateVelocities(prev, dt);
+ keypoints.at(index) = keypoint;
+ updateKeypointMap();
+ }
+
+ void
+ GraspTrajectory::setKeypointDt(size_t index, float dt) {
+ if (index <= 0 || index >= keypoints.size())
+ {
+ throw LocalException("Index out of range" + std::to_string(index));
+ }
+ KeypointPtr prev = keypoints.at(index - 1);
+ KeypointPtr& keypoint = keypoints.at(index);
+ keypoint->updateVelocities(prev, dt);
+ updateKeypointMap();
+ }
+
+ GraspTrajectory::KeypointPtr &
+ GraspTrajectory::lastKeypoint() {
+ return keypoints.at(keypoints.size() - 1);
+ }
+
+ GraspTrajectory::KeypointPtr &
+ GraspTrajectory::getKeypoint(int i) {
+ return keypoints.at(i);
+ }
+
+ Eigen::Matrix4f
+ GraspTrajectory::getStartPose() {
+ return getKeypoint(0)->getTargetPose();
+ }
+
+ void
+ GraspTrajectory::getIndex(float t, int &i1, int &i2, float &f) {
+ if (t <= 0)
+ {
+ i1 = 0;
+ i2 = 0;
+ f = 0;
+ return;
+ }
+ std::map<float, size_t>::const_iterator it, prev;
+ it = keypointMap.upper_bound(t);
+ if (it == keypointMap.end())
+ {
+ i1 = keypoints.size() - 1;
+ i2 = keypoints.size() - 1;
+ f = 0;
+ return;
+ }
+ prev = std::prev(it);
+ i1 = prev->second;
+ i2 = it->second;
+ f = ::math::Helpers::ILerp(prev->first, it->first, t);
+ }
+
+ Eigen::Vector3f
+ GraspTrajectory::GetPosition(float t) {
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return ::math::Helpers::Lerp(getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
+ }
+
+ Eigen::Matrix3f
+ GraspTrajectory::GetOrientation(float t) {
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ Eigen::Vector3f oriVel = GetOrientationDerivative(t);
+ if (oriVel.squaredNorm() == 0)
+ {
+ return getKeypoint(i1)->getTargetOrientation();
+ }
+ Eigen::AngleAxisf aa(oriVel.norm(), oriVel.normalized());
+ aa.angle() = aa.angle() * f * getKeypoint(i2)->dt;
+ return aa.toRotationMatrix() * getKeypoint(i1)->getTargetOrientation();
+ }
+
+ Eigen::Matrix4f
+ GraspTrajectory::GetPose(float t) {
+ return ::math::Helpers::CreatePose(GetPosition(t), GetOrientation(t));
+ }
+
+ std::vector<Eigen::Matrix4f>
+ GraspTrajectory::getAllKeypointPoses() {
+ std::vector<Eigen::Matrix4f> res;
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ res.emplace_back(keypoint->getTargetPose());
+ }
+ return res;
+ }
+
+ std::vector<Eigen::Vector3f>
+ GraspTrajectory::getAllKeypointPositions() {
+ std::vector<Eigen::Vector3f> res;
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ res.emplace_back(keypoint->getTargetPosition());
+ }
+ return res;
+ }
+
+ std::vector<Eigen::Matrix3f>
+ GraspTrajectory::getAllKeypointOrientations() {
+ std::vector<Eigen::Matrix3f> res;
+ for (const KeypointPtr& keypoint : keypoints)
+ {
+ res.emplace_back(keypoint->getTargetOrientation());
+ }
+ return res;
+ }
+
+ Eigen::Vector3f
+ GraspTrajectory::GetHandValues(float t) {
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return ::math::Helpers::Lerp(getKeypoint(i1)->handJointsTarget, getKeypoint(i2)->handJointsTarget, f);
+ }
+
+ Eigen::Vector3f
+ GraspTrajectory::GetPositionDerivative(float t) {
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return getKeypoint(i2)->feedForwardPosVelocity;
+ }
+
+ Eigen::Vector3f
+ GraspTrajectory::GetOrientationDerivative(float t) {
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return getKeypoint(i2)->feedForwardOriVelocity;
+ }
+
+ Eigen::Matrix<float, 6, 1>
+ GraspTrajectory::GetTcpDerivative(float t) {
+ Eigen::Matrix<float, 6, 1> ffVel;
+ ffVel.block<3, 1>(0, 0) = GetPositionDerivative(t);
+ ffVel.block<3, 1>(3, 0) = GetOrientationDerivative(t);
+ return ffVel;
+ }
+
+ Eigen::Vector3f
+ GraspTrajectory::GetHandJointsDerivative(float t) {
+ int i1, i2;
+ float f;
+ getIndex(t, i1, i2, f);
+ return getKeypoint(i2)->feedForwardHandJointsVelocity;
+ }
+
+ float
+ GraspTrajectory::getDuration() const {
+ return keypointMap.rbegin()->first;
+ }
+
+ GraspTrajectory::Length
+ GraspTrajectory::calculateLength() const {
+ Length l;
+ for (size_t i = 1; i < keypoints.size(); i++)
+ {
+ KeypointPtr k0 = keypoints.at(i - 1);
+ KeypointPtr k1 = keypoints.at(i);
+
+ Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(k0->tcpTarget);
+ Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(k0->tcpTarget);
+
+ Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(k1->tcpTarget);
+ Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(k1->tcpTarget);
+
+ l.pos += (pos1 - pos0).norm();
+ l.ori += fabs(::math::Helpers::GetAngleAxisFromTo(ori0, ori1).angle());
+ }
+ return l;
+ }
+
+ GraspTrajectoryPtr
+ GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f &translation, const Eigen::Matrix3f &rotation) {
+ GraspTrajectoryPtr traj(new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(getKeypoint(0)->getTargetPose(), translation, rotation), getKeypoint(0)->handJointsTarget));
+ for (size_t i = 1; i < keypoints.size(); i++)
+ {
+ KeypointPtr& kp = keypoints.at(i);
+ traj->addKeypoint(::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation), kp->handJointsTarget, kp->dt);
+ }
+ return traj;
+ }
+
+ GraspTrajectoryPtr
+ GraspTrajectory::getTransformed(const Eigen::Matrix4f &transform) {
+ GraspTrajectoryPtr traj(new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
+ for (size_t i = 1; i < keypoints.size(); i++)
+ {
+ KeypointPtr& kp = keypoints.at(i);
+ traj->addKeypoint(transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt);
+ }
+ return traj;
+ }
+
+ GraspTrajectoryPtr
+ GraspTrajectory::getClone() {
+ return getTransformed(Eigen::Matrix4f::Identity());
+ }
+
+ GraspTrajectoryPtr
+ GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f &target, const Eigen::Vector3f &handForward) {
+ Eigen::Matrix4f startPose = getStartPose();
+ Eigen::Vector3f targetHandForward = ::math::Helpers::TransformDirection(target, handForward);
+ Eigen::Vector3f trajHandForward = ::math::Helpers::TransformDirection(startPose, handForward);
+ Eigen::Vector3f up(0, 0, 1);
+
+ float angle = ::math::Helpers::Angle(targetHandForward, trajHandForward, up);
+ Eigen::AngleAxisf aa(angle, up);
+
+ Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(-::math::Helpers::GetPosition(startPose), aa.toRotationMatrix(), ::math::Helpers::GetPosition(target));
+ return getTransformed(transform);
+ }
+
+ GraspTrajectoryPtr
+ GraspTrajectory::getTransformedToOtherHand() {
+ Eigen::Matrix3f flip_yz = Eigen::Matrix3f::Identity();
+ flip_yz(0, 0) *= -1.0;
+ Eigen::Matrix4f start_pose = getStartPose();
+ start_pose.block<3, 3>(0, 0) = flip_yz * start_pose.block<3, 3>(0, 0) * flip_yz;
+ GraspTrajectoryPtr output_trajectory(
+ new GraspTrajectory(start_pose, getKeypoint(0)->handJointsTarget));
+ for (size_t i = 1; i < getKeypointCount(); i++)
+ {
+ GraspTrajectory::KeypointPtr& kp = getKeypoint(i);
+ Eigen::Matrix4f target_pose = kp->getTargetPose();
+ target_pose.block<3, 3>(0, 0) = flip_yz * target_pose.block<3, 3>(0, 0) * flip_yz;
+ output_trajectory->addKeypoint(target_pose, kp->handJointsTarget, kp->dt);
+ }
+ return output_trajectory;
+ }
+
+ SimpleDiffIK::Reachability
+ GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp,
+ SimpleDiffIK::Parameters params) {
+ return SimpleDiffIK::CalculateReachability(getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
+ }
+
+ void
+ GraspTrajectory::updateKeypointMap() {
+ keypointMap.clear();
+ float t = 0;
+ for (size_t i = 0; i < keypoints.size(); i++)
+ {
+ t += getKeypoint(i)->dt;
+ keypointMap[t] = i;
+ }
+ }
+
+
+
+ void
+ GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr &prev, float dt) {
+ Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(prev->tcpTarget);
+ Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(prev->tcpTarget);
+ Eigen::Vector3f hnd0 = prev->handJointsTarget;
+
+ Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(tcpTarget);
+ Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(tcpTarget);
+ Eigen::Vector3f hnd1 = handJointsTarget;
+
+ Eigen::Vector3f dpos = pos1 - pos0;
+ Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1);
+ Eigen::Vector3f dhnd = hnd1 - hnd0;
+
+ this->dt = dt;
+ feedForwardPosVelocity = dpos / dt;
+ feedForwardOriVelocity = dori / dt;
+ feedForwardHandJointsVelocity = dhnd / dt;
+ }
+
+ Eigen::Vector3f
+ GraspTrajectory::Keypoint::getTargetPosition() const {
+ return ::math::Helpers::GetPosition(tcpTarget);
+ }
+
+ Eigen::Matrix3f
+ GraspTrajectory::Keypoint::getTargetOrientation() const {
+ return ::math::Helpers::GetOrientation(tcpTarget);
+ }
+
+ Eigen::Matrix4f
+ GraspTrajectory::Keypoint::getTargetPose() const {
+ return tcpTarget;
+ }
+
+ GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget,
+ float dt, const Eigen::Vector3f &feedForwardPosVelocity,
+ const Eigen::Vector3f &feedForwardOriVelocity,
+ const Eigen::Vector3f &feedForwardHandJointsVelocity)
+ : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(dt),
+ feedForwardPosVelocity(feedForwardPosVelocity), feedForwardOriVelocity(feedForwardOriVelocity),
+ feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
+ { }
+
+ GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget)
+ : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(0),
+ feedForwardPosVelocity(0, 0, 0), feedForwardOriVelocity(0, 0, 0), feedForwardHandJointsVelocity(0, 0, 0)
+ { }
+}
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
new file mode 100644
index 0000000000000000000000000000000000000000..18990ca21ae05f41fbe520c0da94374d4f2ae11e
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
@@ -0,0 +1,151 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+#include <memory>
+#include <map>
+#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+
+namespace armarx
+{
+ class RapidXmlReader;
+ using RapidXmlReaderPtr = std::shared_ptr<RapidXmlReader>;
+
+ class GraspTrajectory;
+ typedef std::shared_ptr<GraspTrajectory> GraspTrajectoryPtr;
+
+ class GraspTrajectory
+ {
+ public:
+ class Keypoint;
+ typedef std::shared_ptr<Keypoint> KeypointPtr;
+
+ class Keypoint
+ {
+ public:
+ Eigen::Matrix4f tcpTarget;
+ Eigen::Vector3f handJointsTarget;
+ float dt;
+ Eigen::Vector3f feedForwardPosVelocity;
+ Eigen::Vector3f feedForwardOriVelocity;
+ Eigen::Vector3f feedForwardHandJointsVelocity;
+
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget);
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt,
+ const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity
+ , const Eigen::Vector3f& feedForwardHandJointsVelocity);
+ Eigen::Vector3f getTargetPosition() const;
+ Eigen::Matrix3f getTargetOrientation() const;
+ Eigen::Matrix4f getTargetPose() const;
+ void updateVelocities(const KeypointPtr& prev, float dt);
+ };
+
+ struct Length
+ {
+ float pos = 0;
+ float ori = 0;
+ };
+
+
+ public:
+ GraspTrajectory() = default;
+ GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::Vector3f& handJointsStart);
+
+ void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+
+ size_t getKeypointCount() const;
+
+ void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+
+ void removeKeypoint(size_t index);
+
+ void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+
+ void setKeypointDt(size_t index, float dt);
+
+ KeypointPtr& lastKeypoint();
+ KeypointPtr& getKeypoint(int i);
+ Eigen::Matrix4f getStartPose();
+
+ void getIndex(float t, int& i1, int& i2, float& f);
+
+ Eigen::Vector3f GetPosition(float t);
+
+ Eigen::Matrix3f GetOrientation(float t);
+
+ Eigen::Matrix4f GetPose(float t);
+
+ std::vector<Eigen::Matrix4f> getAllKeypointPoses();
+ std::vector<Eigen::Vector3f> getAllKeypointPositions();
+ std::vector<Eigen::Matrix3f> getAllKeypointOrientations();
+
+ Eigen::Vector3f GetHandValues(float t);
+
+ Eigen::Vector3f GetPositionDerivative(float t);
+
+ Eigen::Vector3f GetOrientationDerivative(float t);
+
+ Eigen::Matrix<float, 6, 1> GetTcpDerivative(float t);
+
+ Eigen::Vector3f GetHandJointsDerivative(float t);
+
+ float getDuration() const;
+
+ Length calculateLength() const;
+
+
+ GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation);
+ GraspTrajectoryPtr getTransformed(const Eigen::Matrix4f& transform);
+ GraspTrajectoryPtr getTransformedToOtherHand();
+
+ GraspTrajectoryPtr getClone();
+
+ GraspTrajectoryPtr
+ getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
+
+ SimpleDiffIK::Reachability calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
+
+
+
+
+ void writeToFile(const std::string& filename);
+
+ static GraspTrajectoryPtr ReadFromReader(const RapidXmlReaderPtr& reader);
+ static GraspTrajectoryPtr ReadFromFile(const std::string& filename);
+ static GraspTrajectoryPtr ReadFromString(const std::string& xml);
+
+ private:
+
+ void updateKeypointMap();
+
+ private:
+ std::vector<KeypointPtr> keypoints;
+ std::map<float, size_t> keypointMap;
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
index fc83132850cfb652359c0ef7cca81d89e3ff3aac..41b48bdff28d2cdbd2e601ab7e9d6dadcdbc0e2b 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -13,6 +13,13 @@ namespace armarx::plugins
channelHeartbeatConfig.emplace(channel, args);
}
+ void HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel, const std::string& message)
+ {
+ auto args = heartbeatArgs;
+ args.message = message;
+ configureHeartbeatChannel(channel, args);
+ }
+
void HeartbeatComponentPlugin::heartbeat()
{
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
index 80cf546726085b6ca03406a7dc1abc91e647c11c..bbefa079c1d54e71444d9f0e90120f4db0250297 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
@@ -33,6 +33,14 @@ namespace armarx::plugins
public:
using ComponentPlugin::ComponentPlugin;
+ /**
+ * @brief Configures a heartbeat subchannel.
+ *
+ * @param channel Identifier of the heartbeat channel
+ * @param args Configuration of this channel's heartbeat properties
+ */
+ void configureHeartbeatChannel(const std::string& channel, const std::string& message);
+
/**
* @brief Configures a heartbeat subchannel.
*
@@ -66,7 +74,7 @@ namespace armarx::plugins
private:
//! heartbeat topic name (outgoing)
- std::string topicName{"DebugObserver"};
+ std::string topicName{"RobotHealthTopic"};
//! name of this component used as identifier for heartbeats
std::string componentName;
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 9c0c21175f5926a8db9676bc06e25d5b00dd7b84..8222d062291e990872750187ff6dc1d943dc5079 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -9,7 +9,7 @@
namespace armarx::plugins
{
RobotUnitDataStreamingReceiverPtr
- RobotUnitComponentPlugin::startDataSatreming(
+ RobotUnitComponentPlugin::startDataStreaming(
const RobotUnitDataStreaming::Config& cfg)
{
//ok to create smart ptr from parent, since ice handels this
@@ -194,5 +194,3 @@ namespace armarx
ARMARX_INFO << "Robot unit is up and running.";
}
}
-
-
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index 6edd2d83691362e3dfdebb372984ca05f497e586..ec497f88670d49e3e780114b2fcc3a9e78e3cc9e 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -74,7 +74,7 @@ namespace armarx
//datastreaming
public:
- RobotUnitDataStreamingReceiverPtr startDataSatreming(const RobotUnitDataStreaming::Config& cfg);
+ RobotUnitDataStreamingReceiverPtr startDataStreaming(const RobotUnitDataStreaming::Config& cfg);
private:
static constexpr const char* PROPERTY_NAME = "RobotUnitName";
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
index 10a524cb71f64e12add7d65870139b1c44707ea5..b83206f6fb670587dc418fdd0207382390b83543 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -22,6 +22,7 @@
#include <Ice/ObjectAdapter.h>
+#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/ArmarXManager.h>
#include "RobotUnitDataStreamingReceiver.h"
@@ -59,8 +60,8 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
static_assert(sizeof(std::uint64_t) == sizeof(msgSequenceNbr));
const auto seq = static_cast<std::uint64_t>(msgSequenceNbr);
std::lock_guard g{_data_mutex};
- ARMARX_INFO << deactivateSpam()
- << "received " << data.size() << " timesteps";
+ ARMARX_VERBOSE << deactivateSpam()
+ << "received " << data.size() << " timesteps";
_data[seq] = data;
}
@@ -173,8 +174,9 @@ namespace armarx
_last_iteration_id + 1 != step.iterationId
)
{
- ARMARX_ERROR << "Missing Iterations or iterations out of order!"
- " This should not happen";
+ ARMARX_ERROR << "Missing Iterations or iterations out of order! "
+ << "This should not happen. "
+ << VAROUT(_last_iteration_id) << ", " << VAROUT(step.iterationId);
}
_last_iteration_id = step.iterationId;
_data_buffer.emplace_back(std::move(step));
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index c17efd18644d5b515170b714d263f4b37a194f25..5d6ed01da11647da7bd64177aabb439d276efa96 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -164,6 +164,7 @@ set(LIB_HEADERS
mns/plugins/Plugin.h
mns/plugins/PluginUser.h
+ util/prediction_helpers.h
util/util.h
)
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
index ceb765e60db13404fbe170900342cff554b3affe..521d9ce6981e9503d90515b0fc10c518eedaae5a 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryListener.h
@@ -63,12 +63,16 @@ namespace armarx::armem::client::util
};
subscribe(subscriptionID, cb);
}
+
template <class CalleeT>
void subscribe(const MemoryID& subscriptionID, CalleeT* callee, MemberCallbackUpdatedOnly<CalleeT> callback)
{
auto cb = [callee, callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
{
- (callee->*callback)(updatedSnapshotIDs);
+ if(callee)
+ {
+ (callee->*callback)(updatedSnapshotIDs);
+ }
};
subscribe(subscriptionID, cb);
}
diff --git a/source/RobotAPI/libraries/armem/core/Prediction.h b/source/RobotAPI/libraries/armem/core/Prediction.h
index a8ec9ee52436b5b5d96c5c8013d058f8d6abc6e5..259ea375e546c64bfee9993939312fb3476ae60a 100644
--- a/source/RobotAPI/libraries/armem/core/Prediction.h
+++ b/source/RobotAPI/libraries/armem/core/Prediction.h
@@ -26,6 +26,7 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
namespace armarx::armem
{
@@ -66,6 +67,7 @@ namespace armarx::armem
armem::prediction::data::PredictionResult toIce() const;
};
+
void toIce(armem::prediction::data::PredictionEngine& ice,
const PredictionEngine& engine);
void fromIce(const armem::prediction::data::PredictionEngine& ice,
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
index 0b877909c4a35e5b867d21de7840b1625b5e74dc..02b2bcae49014e801c59d001fe9150dcae7a1b95 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.h
@@ -32,6 +32,12 @@ namespace armarx::armem::base
/// An optional confidence, may be used for things like decay.
float confidence = 1.0;
+ /// An optional value indicating the last access
+ Time lastAccessed = Time::Invalid();
+
+ /// A counter how often the instance has been accessed
+ unsigned long accessed = 0;
+
bool operator==(const EntityInstanceMetadata& other) const;
inline bool operator!=(const EntityInstanceMetadata& other) const
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index b6497f8ff421feb16b7f7b07ede1f2d3e57c2266..efc18d58c99ee298b71ce2e775a012d7e6050b04 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -261,12 +261,12 @@ namespace armarx::armem::base
* @param commit The commit.
* @return The resulting memory IDs.
*/
- std::vector<UpdateResult> update(const Commit& commit)
+ std::vector<UpdateResult> update(const Commit& commit, const bool addMissingCoreSegmentDuringUpdate = false, const bool checkMemoryName = true)
{
std::vector<UpdateResult> results;
for (const EntityUpdate& update : commit.updates)
{
- results.push_back(this->update(update));
+ results.push_back(this->update(update, addMissingCoreSegmentDuringUpdate, checkMemoryName));
}
return results;
}
@@ -276,11 +276,14 @@ namespace armarx::armem::base
* @param update The update.
* @return The resulting entity snapshot's ID.
*/
- UpdateResult update(const EntityUpdate& update)
+ UpdateResult update(const EntityUpdate& update, const bool addMissingCoreSegmentDuringUpdate = false, const bool checkMemoryName = true)
{
- this->_checkContainerName(update.entityID.memoryName, this->name());
+ if (checkMemoryName)
+ {
+ this->_checkContainerName(update.entityID.memoryName, this->name());
+ }
- auto [inserted, coreSeg] = _addCoreSegmentIfMissing(update.entityID.coreSegmentName);
+ auto [inserted, coreSeg] = _addCoreSegmentIfMissing(update.entityID.coreSegmentName, addMissingCoreSegmentDuringUpdate);
// Update entry.
UpdateResult ret(coreSeg->update(update));
@@ -349,14 +352,14 @@ namespace armarx::armem::base
protected:
- std::pair<bool, CoreSegmentT*> _addCoreSegmentIfMissing(const std::string& coreSegmentName)
+ std::pair<bool, CoreSegmentT*> _addCoreSegmentIfMissing(const std::string& coreSegmentName, const bool addMissingCoreSegmentDuringUpdate)
{
CoreSegmentT* coreSeg = nullptr;
auto it = this->_container.find(coreSegmentName);
if (it == this->_container.end())
{
- if (_addMissingCoreSegmentDuringUpdate)
+ if (addMissingCoreSegmentDuringUpdate)
{
// Insert into map.
coreSeg = &addCoreSegment(coreSegmentName);
@@ -373,13 +376,5 @@ namespace armarx::armem::base
return {false, coreSeg};
}
}
-
-
- public:
-
- // ToDo: Remove from base structure - this should be specific to the server versions.
- // If necessary at all.
- bool _addMissingCoreSegmentDuringUpdate = false;
-
};
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
index 3e22caa31456c6a2afab30a721d064bb661427f5..4b7fce0a6805fb1e7505f09cca0b9f802e994f61 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
@@ -9,7 +9,7 @@
namespace armarx::armem::base::detail
{
/**
- * @class Provides default implmentations of `MemoryContainer`, as well as
+ * Provides default implmentations of `MemoryContainer`, as well as
* iterators (which requires a template).
*/
template <class _ContainerT, class _DerivedT>
diff --git a/source/RobotAPI/libraries/armem/core/container_maps.h b/source/RobotAPI/libraries/armem/core/container_maps.h
index 625c92bb02548c66b96c18e349bbe23acc0385ef..6b836da0b833ea76ce24a1920e0247255e9b6e61 100644
--- a/source/RobotAPI/libraries/armem/core/container_maps.h
+++ b/source/RobotAPI/libraries/armem/core/container_maps.h
@@ -32,14 +32,6 @@ namespace armarx::armem
namespace detail
{
- template <class KeyT, class ValueT>
- struct MapRef
- {
- KeyT* key;
- ValueT* value;
- };
-
-
/**
* @brief Get the entry in the map for which the returned key is the longest prefix
@@ -81,6 +73,50 @@ namespace armarx::armem
return result;
}
+ /**
+ * @brief Get the entry in the map for which the returned key is the longest prefix
+ * of the given key among the keys in the map that satisfy the predicate.
+ *
+ * `prefixFunc` is used to successively calculate the prefixes of the given key.
+ * It must be pure and return an empty optional when there is no shorter
+ * prefix of the given key (for strings, this would be the case when passed the empty string).
+ * `predicate` is used to filter for entries that satisfy the desired condition.
+ * It must be pure.
+ *
+ * @param keyValMap the map that contains the key-value-pairs to search
+ * @param prefixFunc the function that returns the longest non-identical prefix of the key
+ * @param predicate the predicate to filter entries on
+ * @param key the key to calculate the prefixes of
+ *
+ * @return The iterator pointing to the found entry, or `keyValMap.end()`.
+ */
+ template <typename KeyT, typename ValueT>
+ typename std::map<KeyT, ValueT>::const_iterator
+ findEntryWithLongestPrefixAnd(
+ const std::map<KeyT, ValueT>& keyValMap,
+ const std::function<std::optional<KeyT>(KeyT&)>& prefixFunc,
+ const KeyT& key,
+ const std::function<bool(const KeyT&, const ValueT&)>& predicate)
+ {
+ std::optional<KeyT> curKey = key;
+
+ typename std::map<KeyT, ValueT>::const_iterator result = keyValMap.end();
+ do
+ {
+ auto iterator = keyValMap.find(curKey.value());
+ if (iterator != keyValMap.end() and predicate(iterator->first, iterator->second))
+ {
+ result = iterator;
+ }
+ else
+ {
+ curKey = prefixFunc(curKey.value());
+ }
+ }
+ while (result == keyValMap.end() and curKey.has_value());
+
+ return result;
+ }
/**
* @brief Accumulate all the values in a map for which the keys are prefixes of the given key.
@@ -202,6 +238,23 @@ namespace armarx::armem
}
+ /**
+ * @brief Find the entry with the most specific key that contains the given ID
+ * and satisfies the predicate, or `idMap.end()` if no key contains the ID and satisfies
+ * the predicate.
+ *
+ * @see `detail::findEntryWithLongestPrefixAnd()`
+ */
+ template <typename ValueT>
+ typename std::map<MemoryID, ValueT>::const_iterator
+ findMostSpecificEntryContainingIDAnd(const std::map<MemoryID, ValueT>& idMap, const MemoryID& id,
+ const std::function<bool(const MemoryID&, const ValueT&)>& predicate)
+ {
+ return detail::findEntryWithLongestPrefixAnd<MemoryID, ValueT>(
+ idMap, &getMemoryIDParent, id, predicate);
+ }
+
+
/**
* @brief Return all values of keys containing the given ID.
*
diff --git a/source/RobotAPI/libraries/armem/core/operations.h b/source/RobotAPI/libraries/armem/core/operations.h
index f1300da6a0d20f6685bc358494d9997c7e4bdb17..6f7ae4d48ffe65665affe83a9356858c57e29044 100644
--- a/source/RobotAPI/libraries/armem/core/operations.h
+++ b/source/RobotAPI/libraries/armem/core/operations.h
@@ -50,6 +50,20 @@ namespace armarx::armem
}
+ template <class ContainerT>
+ std::vector<MemoryID> getEntityIDs(const ContainerT& container)
+ {
+ std::vector<armem::MemoryID> entityIDs;
+
+ container.forEachEntity([&entityIDs](const auto& entity)
+ {
+ entityIDs.push_back(entity.id());
+ });
+
+ return entityIDs;
+ }
+
+
std::string print(const wm::Memory& data, int maxDepth = -1, int depth = 0);
std::string print(const wm::CoreSegment& data, int maxDepth = -1, int depth = 0);
std::string print(const wm::ProviderSegment& data, int maxDepth = -1, int depth = 0);
diff --git a/source/RobotAPI/libraries/armem/server/CMakeLists.txt b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
index 6d44b25f7e2a416df134df83a0efd170c5a99333..f2967422eaf548e5f040bd5b1685f7555e42bdb5 100644
--- a/source/RobotAPI/libraries/armem/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/server/CMakeLists.txt
@@ -176,6 +176,7 @@ set(LIB_HEADERS
wm/memory_definitions.h
wm/ice_conversions.h
wm/detail/MaxHistorySize.h
+ wm/detail/Prediction.h
plugins.h
plugins/Plugin.h
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 8f37824ff7ff96a8280813247bb17290ae48582b..63b9bba20d1575cc3377109b856d57a09bc4aae8 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -428,13 +428,23 @@ namespace armarx::armem::server
}
// PREDICTION
+ prediction::data::PredictionResultSeq
+ MemoryToIceAdapter::predict(prediction::data::PredictionRequestSeq requests)
+ {
+ auto res = workingMemory->dispatchPredictions(
+ armarx::fromIce<std::vector<PredictionRequest>>(requests));
+ return armarx::toIce<prediction::data::PredictionResultSeq>(res);
+ }
+
prediction::data::EngineSupportMap MemoryToIceAdapter::getAvailableEngines()
{
prediction::data::EngineSupportMap result;
armarx::toIce(result, workingMemory->getAllPredictionEngines());
- prediction::data::EngineSupportMap ltmMap;
- armarx::toIce(ltmMap, workingMemory->getAllPredictionEngines());
+ // Uncomment once LTM also supports prediction engines.
+
+ /*prediction::data::EngineSupportMap ltmMap;
+ armarx::toIce(ltmMap, longtermMemory->getAllPredictionEngines());
for (const auto& [memoryID, engines] : ltmMap)
{
auto entryIter = result.find(memoryID);
@@ -450,7 +460,7 @@ namespace armarx::armem::server
engineSet.insert(engines.begin(), engines.end());
entryIter->second.assign(engineSet.begin(), engineSet.end());
}
- }
+ }*/
return result;
}
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
index 6f19796395d85a1ff043e46c2172cba0940e5808..41b0312e6518ebcf78471f42efe23d5bf0eb9634 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.h
@@ -57,6 +57,9 @@ namespace armarx::armem::server
dto::RecordStatusResult getRecordStatus();
// PREDICTION
+ prediction::data::PredictionResultSeq
+ predict(prediction::data::PredictionRequestSeq requests);
+
prediction::data::EngineSupportMap getAvailableEngines();
public:
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
index f2baa4865ff5d2a832e61acc857863d2c6ca545a..1495b275baa88a14aecb9487db781ac0236b57d5 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/converter/object/json/JsonConverter.cpp
@@ -1,5 +1,6 @@
#include "JsonConverter.h"
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
namespace armarx::armem::server::ltm::converter::object
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
index b975b352f007c8ebd3364af641ef75c3a7fc6a08..ebe21f4c1c35a03ab8a0b5219d26e57417c4533a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/CoreSegmentBase.h
@@ -65,7 +65,7 @@ namespace armarx::armem::server::ltm
}
/// iterate over all provider segments of this ltm
- virtual bool forEachProviderSegment(std::function<void(ProviderSegmentT&)>&& func) const = 0;
+ virtual bool forEachProviderSegment(std::function<void(ProviderSegmentT&)> func) const = 0;
/// find provider segment
virtual std::shared_ptr<ProviderSegmentT> findProviderSegment(const std::string&) const = 0;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h
index 9dedb572fdc8b4ff31c6018f238009d45fde6763..15ebde328ec72b9f189f542087fd7753c2a3faf0 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/EntityBase.h
@@ -59,11 +59,11 @@ namespace armarx::armem::server::ltm
}
/// iterate over all entity snapshots of this ltm
- virtual bool forEachSnapshot(std::function<void(EntitySnapshotT&)>&& func) const = 0;
- virtual bool forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshotT&)>&& func) const = 0;
- virtual bool forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshotT&)>&& func) const = 0;
- virtual bool forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshotT&)>&& func) const = 0;
- virtual bool forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshotT&)>&& func) const = 0;
+ virtual bool forEachSnapshot(std::function<void(EntitySnapshotT&)> func) const = 0;
+ virtual bool forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshotT&)> func) const = 0;
+ virtual bool forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshotT&)> func) const = 0;
+ virtual bool forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshotT&)> func) const = 0;
+ virtual bool forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshotT&)> func) const = 0;
/// find entity snapshot segment
virtual std::shared_ptr<EntitySnapshotT> findSnapshot(const Time&) const = 0;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
index bec7576234c8f6bbc9064745f2d0cdc0f8ef333b..1b32ee2c9fc10bdb476b36b7046f46fc8518e462 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryBase.h
@@ -123,10 +123,10 @@ namespace armarx::armem::server::ltm
}
/// iterate over all core segments of this ltm
- virtual bool forEachCoreSegment(std::function<void(CoreSegmentT&)>&& func) const = 0;
+ virtual bool forEachCoreSegment(std::function<void(CoreSegmentT&)> func) const = 0;
/// find core segment
- virtual std::shared_ptr<CoreSegmentT> findCoreSegment(const std::string&) const = 0;
+ virtual std::shared_ptr<CoreSegmentT> findCoreSegment(const std::string&) const = 0; // TODO make unique!!
/// default parameters. Implementation should use the configuration to configure
virtual void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix)
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
index 6f38d6ea21317b44d37045c985f15d5efe2103da..d5318e1f1e6ec9933ddc7e6b50f3894044111917 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.cpp
@@ -33,4 +33,9 @@ namespace armarx::armem::server::ltm
{
return _id.getLeafItem();
}
+
+ void MemoryItem::setMemoryName(const std::string& memoryName)
+ {
+ _id.memoryName = memoryName;
+ }
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
index 570231509806a4f6a56d55ab4a4a2d7442de8807..b8d17fe471c17e62aa188406648515e5a32f384a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/MemoryItem.h
@@ -21,6 +21,7 @@ namespace armarx::armem::server::ltm
std::string name() const;
virtual void setMemoryID(const MemoryID&);
+ void setMemoryName(const std::string& memoryName);
protected:
std::shared_ptr<Processors> processors;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
index b5aa8f0ee95711001d378f06ef5c674266dc021b..c241d7889621f534991733baf82973e28428a641 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/base/detail/ProviderSegmentBase.h
@@ -65,7 +65,7 @@ namespace armarx::armem::server::ltm
}
/// iterate over all core segments of this ltm
- virtual bool forEachEntity(std::function<void(EntityT&)>&& func) const = 0;
+ virtual bool forEachEntity(std::function<void(EntityT&)> func) const = 0;
/// find entity segment
virtual std::shared_ptr<EntityT> findEntity(const std::string&) const = 0;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
index 785276d04bd1e366be172ba7a07560a96a0c7755..cfc859a5f89418247c1d7f383cfea1b5bce8aa2a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.cpp
@@ -16,7 +16,7 @@ namespace armarx::armem::server::ltm::disk
{
}
- bool CoreSegment::forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const
+ bool CoreSegment::forEachProviderSegment(std::function<void(ProviderSegment&)> func) const
{
auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
auto relPath = getRelativePathForMode(currentMode);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
index 6dfc00e7e5e37814027cfc10a3be53dd83dff603..78ff8be3f12d35eeebbed370cc04559b37a57822 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/CoreSegment.h
@@ -18,7 +18,7 @@ namespace armarx::armem::server::ltm::disk
CoreSegment(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
- bool forEachProviderSegment(std::function<void(ProviderSegment&)>&& func) const override;
+ bool forEachProviderSegment(std::function<void(ProviderSegment&)> func) const override;
std::shared_ptr<ProviderSegment> findProviderSegment(const std::string&) const override;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
index 6c87da45e576c10c18c6680141e00ed8c19aa3ba..8b00d8d39b8fa49de6c31d8b617549d97e6a45cb 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.cpp
@@ -21,7 +21,7 @@ namespace armarx::armem::server::ltm::disk
{
}
- bool Entity::forEachSnapshot(std::function<void(EntitySnapshot&)>&& func) const
+ bool Entity::forEachSnapshot(std::function<void(EntitySnapshot&)> func) const
{
auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
auto relPath = getRelativePathForMode(currentMode);
@@ -75,7 +75,7 @@ namespace armarx::armem::server::ltm::disk
return true;
}
- bool Entity::forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)>&& func) const
+ bool Entity::forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)> func) const
{
ARMARX_WARNING << "PLEASE NOTE THAT QUERYING THE LTM INDEX WISE MAY BE BUGGY BECAUSE THE FILESYSTEM ITERATOR IS UNSORTED!";
@@ -106,7 +106,7 @@ namespace armarx::armem::server::ltm::disk
return forEachSnapshot(std::move(f));
}
- bool Entity::forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshot&)>&& func) const
+ bool Entity::forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshot&)> func) const
{
auto f = [&](EntitySnapshot& e)
{
@@ -120,7 +120,7 @@ namespace armarx::armem::server::ltm::disk
return forEachSnapshot(std::move(f));
}
- bool Entity::forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshot&)>&& func) const
+ bool Entity::forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshot&)> func) const
{
auto f = [&](EntitySnapshot& e)
{
@@ -134,7 +134,7 @@ namespace armarx::armem::server::ltm::disk
return forEachSnapshot(std::move(f));
}
- bool Entity::forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)>&& func) const
+ bool Entity::forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)> func) const
{
auto f = [&](EntitySnapshot& e)
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
index 759b22733612c700ec0461541976cf3872d9df07..3d98ef1bf8abe315e005a110ad6fa807f373d7ba 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Entity.h
@@ -18,11 +18,11 @@ namespace armarx::armem::server::ltm::disk
public:
Entity(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
- bool forEachSnapshot(std::function<void(EntitySnapshot&)>&& func) const override;
- bool forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)>&& func) const override;
- bool forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshot&)>&& func) const override;
- bool forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshot&)>&& func) const override;
- bool forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)>&& func) const override;
+ bool forEachSnapshot(std::function<void(EntitySnapshot&)> func) const override;
+ bool forEachSnapshotInIndexRange(long first, long last, std::function<void(EntitySnapshot&)> func) const override;
+ bool forEachSnapshotInTimeRange(const Time& min, const Time& max, std::function<void(EntitySnapshot&)> func) const override;
+ bool forEachSnapshotBeforeOrAt(const Time& time, std::function<void(EntitySnapshot&)> func) const override;
+ bool forEachSnapshotBefore(const Time& time, std::function<void(EntitySnapshot&)> func) const override;
std::shared_ptr<EntitySnapshot> findSnapshot(const Time&) const override;
std::shared_ptr<EntitySnapshot> findLatestSnapshot() const override;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
index 4d24c2e6bd15c0696bbbefd757db1fc4a895dbde..f1706a545a0f3d81cee3637e3bef3f5eb7df0e17 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/EntitySnapshot.cpp
@@ -19,7 +19,7 @@ namespace armarx::armem::server::ltm::disk
/ EscapeSegmentName(id.providerSegmentName)
/ EscapeSegmentName(id.entityName)
/ std::to_string(id.timestamp.toSecondsSinceEpoch() / 3600 /* hours */)
- / std::to_string(id.timestamp.toSecondsSinceEpoch()) / id.timestampStr()),
+ / std::to_string(id.timestamp.toSecondsSinceEpoch()) /* seconds */ / id.timestampStr()),
currentMode(mode),
currentExport(e)
{
@@ -80,6 +80,7 @@ namespace armarx::armem::server::ltm::disk
if (std::find(allFilesInIndexFolder.begin(), allFilesInIndexFolder.end(), dataFilename) != allFilesInIndexFolder.end())
{
+ //ARMARX_INFO << "Convert data for entity " << id();
auto datafilecontent = util::readDataFromFile(mPath, relDataPath);
auto dataaron = dictConverter.convert(datafilecontent, "");
datadict = aron::data::Dict::DynamicCastAndCheck(dataaron);
@@ -110,6 +111,7 @@ namespace armarx::armem::server::ltm::disk
ARMARX_ERROR << "Could not find the data file '" << relDataPath.string() << "'. Continuing without data.";
}
+ //ARMARX_INFO << "Convert metadata for entity " << id();
if (std::find(allFilesInIndexFolder.begin(), allFilesInIndexFolder.end(), metadataFilename) != allFilesInIndexFolder.end())
{
auto metadatafilecontent = util::readDataFromFile(mPath, relMetadataPath);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
index 6d0b4e1fa18ee244643c5fc45755af0468d056db..a2a374c6de24e4fd1dca370149841d79f85de027 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.cpp
@@ -65,7 +65,7 @@ namespace armarx::armem::server::ltm::disk
}
}
- bool Memory::forEachCoreSegment(std::function<void(CoreSegment&)>&& func) const
+ bool Memory::forEachCoreSegment(std::function<void(CoreSegment&)> func) const
{
for (unsigned long i = 0; i <= maxExportIndex; ++i)
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
index c9f1e95b216a0dd0dac6ff67c9f28c9ef1b38d5c..afd3a09004eea72c406ebfea844ffa80b257fc84 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/Memory.h
@@ -29,7 +29,7 @@ namespace armarx::armem::server::ltm::disk
void configure(const nlohmann::json& config) final;
- bool forEachCoreSegment(std::function<void(CoreSegment&)>&& func) const final;
+ bool forEachCoreSegment(std::function<void(CoreSegment&)> func) const final;
std::shared_ptr<CoreSegment> findCoreSegment(const std::string& coreSegmentName) const final;
protected:
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
index ede13058b8a8abbca1eb348b3fa483234e4eae76..32500f7d6a799f678d5fdf1fce09024d93c4a796 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.cpp
@@ -18,7 +18,7 @@ namespace armarx::armem::server::ltm::disk
{
}
- bool ProviderSegment::forEachEntity(std::function<void(Entity&)>&& func) const
+ bool ProviderSegment::forEachEntity(std::function<void(Entity&)> func) const
{
auto mPath = getMemoryBasePathForMode(currentMode, currentExport);
auto relPath = getRelativePathForMode(currentMode);
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
index 0402f4b98408b73719ea2d2889d02c195186aac6..dfa7c42714310a042dc344c043794eca4536ea00 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/ProviderSegment.h
@@ -17,7 +17,7 @@ namespace armarx::armem::server::ltm::disk
public:
ProviderSegment(const std::filesystem::path& parentPath, const MemoryID& id, const std::shared_ptr<Processors>& p, const DiskMemoryItem::MemoryEncodingMode mode, const unsigned long e);
- bool forEachEntity(std::function<void(Entity&)>&& func) const override;
+ bool forEachEntity(std::function<void(Entity&)> func) const override;
std::shared_ptr<Entity> findEntity(const std::string&) const override;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp
index daea3fcfc439326545a79ad00ac53e9133afa195..1a40e26a04923bccb123b4b07f00ab9be190efa9 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/disk/detail/util/filesystem_util.cpp
@@ -1,5 +1,7 @@
#include "filesystem_util.h"
+#include <algorithm>
+
#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
@@ -97,6 +99,7 @@ namespace armarx::armem::server::ltm::disk
ret.push_back(subdirPath.filename());
}
}
+ std::sort(ret.begin(), ret.end());
return ret;
}
@@ -111,6 +114,7 @@ namespace armarx::armem::server::ltm::disk
ret.push_back(subdirPath.filename());
}
}
+ std::sort(ret.begin(), ret.end());
return ret;
}
}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
index 02c89e5f323b4376ca4493f6b0f1e8efa22f3b60..aa6afd8171ff50cf43b955ff145a4770cf8f24b7 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.h
@@ -41,7 +41,7 @@ namespace armarx::armem::server::plugins
public:
- /// Set the name of the wm and the ltm (if enabled).
+ /// Set the name of the wm and the ltm
void setMemoryName(const std::string& memoryName);
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
index 8d0b986443914d60c976da57ae6736c775bc1f54..299e8242ee540df8c5397d86a03a7cae2d1a0eb8 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -146,16 +146,7 @@ namespace armarx::armem::server::plugins
armem::prediction::data::PredictionResultSeq
ReadWritePluginUser::predict(const armem::prediction::data::PredictionRequestSeq& requests)
{
- armem::prediction::data::PredictionResultSeq result;
- for (auto request : requests)
- {
- armem::PredictionResult singleResult;
- singleResult.success = false;
- singleResult.errorMessage = "This memory does not implement predictions.";
- singleResult.prediction = nullptr;
- result.push_back(singleResult.toIce());
- }
- return result;
+ return iceAdapter().predict(requests);
}
armem::prediction::data::EngineSupportMap
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp
index 7999a6a5752cd1bc9432f9caec95a6f928645a20..edaa7bb5c5d92b57b0edf3918d1674db34337680 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp
@@ -31,9 +31,16 @@
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <RobotAPI/libraries/armem/server/ltm/disk/Memory.h>
//#include "../core/io/diskWriter/NlohmannJSON/NlohmannJSONDiskWriter.h"
+// TODO: REMOVE ME!!
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Import/SimoxXMLFactory.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+
#include <filesystem>
#include <iostream>
@@ -205,6 +212,61 @@ BOOST_AUTO_TEST_CASE(test_memory_export__easy_bool_setup)
run<aron::type::Bool>("TestMemory_BoolSetup", "theBool");
}
+BOOST_AUTO_TEST_CASE(test_memory_export__easy_fabian_setup)
+{
+ std::filesystem::path memoryExport = "/home/fabian/repos/projects/ltmtools/data/sim_2022_09_15";
+
+ armem::server::ltm::disk::Memory ltm(memoryExport, "Object");
+ auto ltm_c_seg = ltm.findCoreSegment("Instance");
+
+ ltm_c_seg->forEachProviderSegment([](const armem::server::ltm::disk::ProviderSegment& ltm_p_seg){
+ ltm_p_seg.forEachEntity([](const armem::server::ltm::disk::Entity& ltm_e){
+ ltm_e.forEachSnapshot([](const armem::server::ltm::disk::EntitySnapshot& ltm_snapshot){
+ armem::wm::EntitySnapshot s;
+ ltm_snapshot.loadAllReferences(s);
+ ltm_snapshot.resolve(s);
+
+ auto i = s.getInstance(0);
+ auto data = i.data();
+
+
+
+ auto p = data->getElement("pose");
+ auto p2 = armarx::aron::data::Dict::DynamicCastAndCheck(p);
+
+ armarx::objpose::arondto::ObjectPose px;
+ px.fromAron(p2);
+
+ auto a = p2->getElement("attachment");
+ auto a2 = armarx::aron::data::Dict::DynamicCastAndCheck(a);
+
+ armarx::objpose::arondto::ObjectAttachmentInfo ax;
+ ax.fromAron(a2);
+
+ if (ax.agentName != "")
+ {
+ ARMARX_INFO << (ax.poseInFrame);
+ }
+ });
+ });
+ });
+
+
+ //auto ltm_p_seg = ltm_c_seg->findProviderSegment("Armar3");
+ //auto ltm_e = ltm_p_seg->findEntity("Armar3");
+
+ //armem::wm::Entity e;
+ //ltm_e->loadAllReferences(e);
+ //ltm_e->resolve(e);
+
+ //std::function<void(armarx::armem::wm::EntitySnapshot)> f = [](armem::wm::EntitySnapshot s){ ARMARX_INFO << s.id(); };
+
+ //e.forEachSnapshot(f);
+ //auto wm = ltm.loadAllAndResolve();
+
+
+}
+
/*
BOOST_AUTO_TEST_CASE(test_memory_export__easy_rainer_setup)
{
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
index d90756691871b6ad444a67fa76a733d0086b5b29..2e94c2cdb47a1fb883a6e790240be4c76a21b5e6 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
@@ -1,7 +1,7 @@
#include "MaxHistorySize.h"
-namespace armarx::armem::server::detail
+namespace armarx::armem::server::wm::detail
{
void MaxHistorySize::setMaxHistorySize(long maxSize)
{
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
index 962179a68aeb601dc261eb6d496389295597a2ea..64eb07a63d4d8087378856623ea47c2edcee3b2f 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
@@ -1,6 +1,6 @@
#pragma once
-namespace armarx::armem::server::detail
+namespace armarx::armem::server::wm::detail
{
// TODO: Replace by ConstrainedHistorySize (not only max entries, e.g. delete oldest / delete least accessed / ...)
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h b/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
new file mode 100644
index 0000000000000000000000000000000000000000..3b2eccc022514313ceb0aaea7154e5c77258ea79
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
@@ -0,0 +1,295 @@
+/*
+ * 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 flied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::armem::core::base::detail
+ * @author phesch ( phesch at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <functional>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
+#include <RobotAPI/libraries/armem/core/base/detail/derived.h>
+#include <RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h>
+#include <RobotAPI/libraries/armem/core/base/detail/Predictive.h>
+#include <RobotAPI/libraries/armem/core/container_maps.h>
+
+
+namespace armarx::armem::server::wm::detail
+{
+
+ using Predictor = std::function<PredictionResult(const PredictionRequest&)>;
+
+ /**
+ * Can do predictions, but has no children it could delegate predictions to.
+ *
+ * This class is integrated with `armem::base::detail::Predictive`:
+ * If `DerivedT` is also a `Predictive`, the setters of this class also update the
+ * `Predictive` part.
+ */
+ template <class DerivedT>
+ class Prediction
+ {
+ using Predictive = armem::base::detail::Predictive<DerivedT>;
+
+ Predictive* _asPredictive()
+ {
+ return dynamic_cast<Predictive*>(&base::detail::derived<DerivedT>(this));
+ }
+
+ public:
+ explicit Prediction(const std::map<PredictionEngine, Predictor>& predictors = {})
+ {
+ this->setPredictors(predictors);
+ }
+
+ void
+ addPredictor(const PredictionEngine& engine, Predictor&& predictor)
+ {
+ _predictors.emplace(engine.engineID, predictor);
+
+ if (Predictive* predictive = this->_asPredictive())
+ {
+ predictive->addPredictionEngine(engine);
+ }
+ }
+
+ void
+ setPredictors(const std::map<PredictionEngine, Predictor>& predictors)
+ {
+ this->_predictors.clear();
+ for (const auto& [engine, predictor] : predictors)
+ {
+ _predictors.emplace(engine.engineID, predictor);
+ }
+
+ if (Predictive* predictive = this->_asPredictive())
+ {
+ predictive->setPredictionEngines(simox::alg::get_keys(predictors));
+ }
+ }
+
+ /**
+ * Resolves mapping of requests to predictors and dispatches them.
+ *
+ * In this case, the resolution is basically no-op because there are no children.
+ */
+ std::vector<PredictionResult>
+ dispatchPredictions(const std::vector<PredictionRequest>& requests)
+ {
+ const MemoryID ownID = base::detail::derived<DerivedT>(this).id();
+ std::vector<PredictionResult> results;
+ for (const auto& request : requests)
+ {
+ results.push_back(dispatchTargetedPrediction(request, ownID));
+ }
+ return results;
+ }
+
+ /**
+ * Dispatches a single prediction request (assuming resolution was done by the caller).
+ */
+ PredictionResult
+ dispatchTargetedPrediction(const PredictionRequest& request, const MemoryID& target)
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+
+ MemoryID ownID = base::detail::derived<DerivedT>(this).id();
+ if (ownID == target)
+ {
+ auto it = _predictors.find(request.predictionSettings.predictionEngineID);
+ if (it != _predictors.end())
+ {
+ const Predictor& predictor = it->second;
+ result = predictor(request);
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not dispatch prediction request for " << request.snapshotID
+ << " with engine '" << request.predictionSettings.predictionEngineID
+ << "' in " << ownID << ": Engine not registered.";
+ result.errorMessage = sstream.str();
+ }
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not dispatch prediction request for " << request.snapshotID
+ << " to " << target << " from " << ownID;
+ result.errorMessage = sstream.str();
+ }
+ return result;
+ }
+
+ private:
+ std::map<std::string, Predictor> _predictors; // NOLINT
+ };
+
+
+ /**
+ * Can do predictions itself and has children it could delegate predictions to.
+ */
+ template <class DerivedT>
+ class PredictionContainer : public Prediction<DerivedT>
+ {
+ public:
+ using Prediction<DerivedT>::Prediction;
+
+ explicit PredictionContainer(const std::map<PredictionEngine, Predictor>& predictors = {})
+ : Prediction<DerivedT>(predictors)
+ {
+ }
+
+ std::vector<PredictionResult>
+ dispatchPredictions(const std::vector<PredictionRequest>& requests)
+ {
+ const auto& derivedThis = base::detail::derived<DerivedT>(this);
+ const std::map<MemoryID, std::vector<PredictionEngine>> engines =
+ derivedThis.getAllPredictionEngines();
+
+ std::vector<PredictionResult> results;
+ for (const PredictionRequest& request : requests)
+ {
+ PredictionResult& result = results.emplace_back();
+ result.snapshotID = request.snapshotID;
+
+ auto iter =
+ armem::findMostSpecificEntryContainingIDAnd<std::vector<PredictionEngine>>(
+ engines, request.snapshotID,
+ [&request](const MemoryID& /*unused*/,
+ const std::vector<PredictionEngine>& supported) -> bool
+ {
+ for (const PredictionEngine& engine : supported)
+ {
+ if (engine.engineID == request.predictionSettings.predictionEngineID)
+ {
+ return true;
+ }
+ }
+ return false;
+ });
+
+ if (iter != engines.end())
+ {
+ const MemoryID& responsibleID = iter->first;
+
+ result = dispatchTargetedPrediction(request, responsibleID);
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not find segment offering prediction engine '"
+ << request.predictionSettings.predictionEngineID << "' for memory ID "
+ << request.snapshotID << ".";
+ result.errorMessage = sstream.str();
+ }
+ }
+ return results;
+ }
+
+
+ /**
+ * Semantics: This container or one of its children (target) is responsible
+ * for performing the prediction.
+ */
+ PredictionResult
+ dispatchTargetedPrediction(const PredictionRequest& request, const MemoryID& target)
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+
+ const auto& derivedThis = base::detail::derived<DerivedT>(this);
+ MemoryID ownID = derivedThis.id();
+ if (ownID == target)
+ {
+ // Delegate to base class.
+ result = Prediction<DerivedT>::dispatchTargetedPrediction(request, target);
+ }
+ // Check if of this' children is really responsible for the request.
+ else if (contains(ownID, target))
+ {
+ std::string childName = _getChildName(ownID, target);
+
+ // TODO(phesch): Looping over all the children just to find the one
+ // with the right name isn't nice, but it's the interface we've got.
+ // TODO(RainerKartmann): Try to add findChild() to loopup mixins.
+ typename DerivedT::ChildT* child = nullptr;
+ derivedThis.forEachChild(
+ [&child, &childName](auto& otherChild)
+ {
+ if (otherChild.name() == childName)
+ {
+ child = &otherChild;
+ }
+ });
+ if (child)
+ {
+ result = child->dispatchTargetedPrediction(request, target);
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not find memory item with ID " << target << ".";
+ result.errorMessage = sstream.str();
+ }
+ }
+ else
+ {
+ result.success = false;
+ std::stringstream sstream;
+ sstream << "Could not dispatch prediction request for " << request.snapshotID
+ << " to " << target << " from " << ownID << ".";
+ result.errorMessage = sstream.str();
+ }
+ return result;
+ }
+
+ private:
+
+ std::string _getChildName(const MemoryID& parent, const MemoryID& child)
+ {
+ ARMARX_CHECK(armem::contains(parent, child));
+ ARMARX_CHECK(parent != child);
+
+ size_t parentLength = parent.getItems().size();
+
+ // Get iterator to first entry of child ID (memory).
+ std::vector<std::string> childItems = child.getItems();
+
+ int index = parentLength;
+ ARMARX_CHECK_FITS_SIZE(index, childItems.size());
+
+ return childItems[index];
+ }
+
+
+ };
+
+} // namespace armarx::armem::server::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
index 35da4691d69dcb4a0290d84f19098608b7d3aab6..973e3ca5c21f1913f5abb84718789b4d4d27b2d2 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
@@ -3,6 +3,8 @@
#include "error.h"
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/armem/core/container_maps.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <map>
@@ -107,5 +109,4 @@ namespace armarx::armem::server::wm
result.memoryUpdateType = UpdateType::UpdatedExisting;
return result;
}
-
}
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
index e6889b7f8e0a6b40f16d28ad2bcc05f68250b1a5..35015353a2653ca957bb19fe29c7e8430790885d 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
@@ -1,6 +1,7 @@
#pragma once
#include "detail/MaxHistorySize.h"
+#include "detail/Prediction.h"
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
@@ -60,6 +61,7 @@ namespace armarx::armem::server::wm
public base::ProviderSegmentBase<Entity, ProviderSegment>
, public detail::MaxHistorySizeParent<ProviderSegment>
, public armem::wm::detail::FindInstanceDataMixin<ProviderSegment>
+ , public armem::server::wm::detail::Prediction<ProviderSegment>
{
public:
@@ -82,9 +84,10 @@ namespace armarx::armem::server::wm
/// @brief base::CoreSegmentBase
class CoreSegment :
- public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
- public detail::MaxHistorySizeParent<CoreSegment>
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>
+ , public detail::MaxHistorySizeParent<CoreSegment>
, public armem::wm::detail::FindInstanceDataMixin<CoreSegment>
+ , public armem::server::wm::detail::PredictionContainer<CoreSegment>
{
using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
@@ -125,6 +128,7 @@ namespace armarx::armem::server::wm
class Memory :
public base::MemoryBase<CoreSegment, Memory>
, public armem::wm::detail::FindInstanceDataMixin<Memory>
+ , public armem::server::wm::detail::PredictionContainer<Memory>
{
using Base = base::MemoryBase<CoreSegment, Memory>;
diff --git a/source/RobotAPI/libraries/armem/util/prediction_helpers.h b/source/RobotAPI/libraries/armem/util/prediction_helpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..1734dca8f20e488e789e0c39af380b24376a5e9f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/util/prediction_helpers.h
@@ -0,0 +1,135 @@
+/*
+ * 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 phesch ( ulila at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include <ArmarXCore/core/time.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::armem
+{
+ /**
+ * Holds info on snapshot data extracted from a time range.
+ * Used for performing predictions.
+ */
+ template <typename DataType, typename LatestType>
+ struct SnapshotRangeInfo
+ {
+ bool success = false;
+ std::string errorMessage = "";
+
+ std::vector<double> timestampsSec = {};
+ std::vector<DataType> values = {};
+ LatestType latestValue;
+ };
+
+ /*!
+ * @brief Get data points for the snapshots of an entity in a given time range.
+ *
+ * The resulting `SnapshotRangeInfo` is useful for performing predictions.
+ * Data from the individual snapshots is given to the conversion functions
+ * before being stored in the result.
+ * Timestamps in the `timestampsSec` vector are relative to the endTime parameter
+ * to this function.
+ * Data from a snapshot is always retrieved from the instance with index 0.
+ * If some data cannot be retrieved, `success` is set to `false`
+ * and the `errorMessage` in the return value is set accordingly.
+ *
+ * @param segment the segment to get the snapshot data from
+ * @param entityID the entity to get the data for
+ * @param startTime the beginning of the time range
+ * @param endTime the end of the time range. Timestamps are relative to this value
+ * @param dictToData the conversion function from `DictPtr` to `DataType`
+ * @param dictToLatest the conversion function from `DictPtr` to `LatestType`
+ * @return the corresponding `LatestSnapshotInfo`
+ */
+ template <typename SegmentType, typename DataType, typename LatestType>
+ SnapshotRangeInfo<DataType, LatestType>
+ getSnapshotsInRange(const SegmentType* segment,
+ const MemoryID& entityID,
+ const DateTime& startTime,
+ const DateTime& endTime,
+ std::function<DataType(const aron::data::DictPtr&)> dictToData,
+ std::function<LatestType(const aron::data::DictPtr&)> dictToLatest)
+ {
+ SnapshotRangeInfo<DataType, LatestType> result;
+ result.success = false;
+
+ const server::wm::Entity* entity = segment->findEntity(entityID);
+ if (entity == nullptr)
+ {
+ std::stringstream sstream;
+ sstream << "Could not find entity with ID " << entityID << ".";
+ result.errorMessage = sstream.str();
+ return result;
+ }
+
+ const int instanceIndex = 0;
+ bool querySuccess = true;
+ entity->forEachSnapshotInTimeRange(
+ startTime,
+ endTime,
+ [&](const wm::EntitySnapshot& snapshot)
+ {
+ const auto* instance = snapshot.findInstance(instanceIndex);
+ if (instance)
+ {
+ result.timestampsSec.push_back(
+ (instance->id().timestamp - endTime).toSecondsDouble());
+ result.values.emplace_back(dictToData(instance->data()));
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "Could not find instance with index " << instanceIndex
+ << " in snapshot " << snapshot.id() << ".";
+ result.errorMessage = sstream.str();
+ querySuccess = false;
+ }
+ });
+
+ if (querySuccess)
+ {
+ aron::data::DictPtr latest = entity->findLatestInstanceData(instanceIndex);
+ if (latest)
+ {
+ result.success = true;
+ result.latestValue = dictToLatest(latest);
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "Could not find instance with index " << instanceIndex << " for entity "
+ << entity->id() << ".";
+ result.errorMessage = sstream.str();
+ }
+ }
+
+ return result;
+ }
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index f1ad93a2c87396e2f178d2ed522d39dcd7de95b9..e5c40a12dbb73b0cef3569eb12d80fb1980f95f2 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -66,9 +66,10 @@ namespace armarx::armem::grasping::segment
retGrasp.quality = std::stof(graspNode.attribute_value("quality"));
retGrasp.creator = graspNode.attribute_value("Creation");
- ARMARX_CHECK_EQUAL(graspNode.nodes().size(), 1);
+ ARMARX_CHECK(graspNode.nodes().size() == 2 or graspNode.nodes().size() == 1);
RapidXmlReaderNode transformNode = graspNode.nodes()[0];
+ ARMARX_CHECK_EQUAL(transformNode.name(), "Transform");
ARMARX_CHECK_EQUAL(transformNode.nodes().size(), 1);
RapidXmlReaderNode matrixNode = transformNode.nodes()[0];
@@ -95,7 +96,7 @@ namespace armarx::armem::grasping::segment
retGrasp.pose(3, 2) = std::stof(row3.attribute_value("c3"));
retGrasp.pose(3, 3) = std::stof(row3.attribute_value("c4"));
- ARMARX_INFO << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
+ ARMARX_VERBOSE << "Found grasp '" << retGrasp.name << "' in set '" << retGraspSet.name << "' for obj '" << objectClassName << "' with pose \n" << retGrasp.pose;
retGraspSet.grasps.push_back(retGrasp);
}
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 622031b7aead835f8d91b9eeac4ac06362d731a1..b9c789398b0d8ce5c6fb17e0c2cb44fb5bc5b531 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -150,7 +150,7 @@ namespace armarx::armem::gui
connect(this, &This::connected, this, &This::startPeriodicUpdateTimer);
connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::startQueries);
- connect(periodicUpdateTimer, &QTimer::timeout, this, &This::updateAvailableMemories);
+ connect(periodicUpdateTimer, &QTimer::timeout, this, &This::updateListOfActiveMemories);
connect(periodicUpdateTimer, &QTimer::timeout, this, &This::processQueryResults);
connect(memoryGroup->queryWidget(), &armem::gui::QueryWidget::storeInLTM, this, &This::queryAndStoreInLTM);
@@ -267,7 +267,7 @@ namespace armarx::armem::gui
{
std::stringstream ss;
ss << "Memory name '" << memoryName
- << "' is unknown. Known are: " << simox::alg::get_keys(memoryData);
+ << "' is unknown. Known are: " << simox::alg::join(simox::alg::get_keys(memoryData), ", ");
statusLabel->setText(QString::fromStdString(ss.str()));
return nullptr;
}
@@ -288,7 +288,7 @@ namespace armarx::armem::gui
{
TIMING_START(MemoryStore);
- auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
+ auto enabledMemories = memoryGroup->queryWidget()->getEnabledMemories();
for (auto& [name, reader] : memoryReaders)
{
// skip if memory should not be queried
@@ -321,7 +321,7 @@ namespace armarx::armem::gui
TIMING_START(MemoryStartRecording);
- auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
+ auto enabledMemories = memoryGroup->queryWidget()->getEnabledMemories();
for (auto& [name, reader] : memoryReaders)
{
// skip if memory should not be queried
@@ -341,7 +341,7 @@ namespace armarx::armem::gui
TIMING_START(MemoryStopRecording);
- auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
+ auto enabledMemories = memoryGroup->queryWidget()->getEnabledMemories();
for (auto& [name, reader] : memoryReaders)
{
// skip if memory should not be queried
@@ -406,7 +406,8 @@ namespace armarx::armem::gui
TIMING_START(MemoryExport)
std::string status;
- diskControl->storeOnDisk(directory, memoryData, &status);
+ std::vector<wm::Memory> memoryDataVec = simox::alg::get_values(memoryData);
+ diskControl->storeOnDisk(directory, memoryDataVec, &status);
statusLabel->setText(QString::fromStdString(status));
TIMING_END_STREAM(MemoryExport, ARMARX_VERBOSE)
@@ -418,19 +419,29 @@ namespace armarx::armem::gui
{
std::string status;
- std::map<std::string, wm::Memory> data =
+ std::map<std::filesystem::path, wm::Memory> data =
diskControl->loadFromDisk(directory, memoryGroup->queryInput(), &status);
- for (auto& [name, memory] : data)
+ for (auto& [path, memory] : data)
{
+ std::string name = memory.id().memoryName;
+ auto commit = armem::toCommit(memory);
+
if (memoryWriters.count(name) > 0)
{
- auto commit = armem::toCommit(memory);
memoryWriters.at(name).commit(commit);
}
else
{
- ARMARX_WARNING << "No memory with name " << name << " available for commit.";
+ ARMARX_INFO << "No memory with name '" << name << "' available for commit. Create new virtual memory.";
+
+ // Please note: Here we assume that a memory server with the same name does not exist.
+ // I think this assumption is ok, since nobody should use filepaths as memory name.
+ // Nonetheless, we did not restrict the user to do so...
+ std::string virtualMemoryName = name + " (at " + path.string() + ")";
+ wm::Memory virtualMemory(virtualMemoryName);
+ virtualMemory.update(commit, true, false);
+ memoryData[virtualMemoryName] = virtualMemory;
}
}
@@ -487,7 +498,7 @@ namespace armarx::armem::gui
// Can't use a structured binding here because you can't capture those in a lambda
// according to the C++ standard.
- auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
+ auto enabledMemories = memoryGroup->queryWidget()->getEnabledMemories();
for (const auto& pair : memoryReaders)
{
// skip if memory should not be queried
@@ -592,7 +603,7 @@ namespace armarx::armem::gui
}
// Perhaps remove entries
- auto enabledMemories = memoryGroup->queryWidget()->enabledMemories();
+ auto enabledMemories = memoryGroup->queryWidget()->getEnabledMemories();
for (auto it = memoryData.begin(); it != memoryData.end();)
{
// Drop all entries in memoryData which are not in memoryReaders anymore.
@@ -764,7 +775,7 @@ namespace armarx::armem::gui
void
- MemoryViewer::updateAvailableMemories()
+ MemoryViewer::updateListOfActiveMemories()
{
if (is_connected and mns) // mns must be connected and mns must be available
{
@@ -773,11 +784,13 @@ namespace armarx::armem::gui
memoryReaders = mns.getAllReaders(true);
memoryWriters = mns.getAllWriters(true);
- std::vector<std::string> convVec;
- std::transform(memoryReaders.begin(), memoryReaders.end(), std::back_inserter(convVec), [](const auto& p){return p.first;});
+ std::vector<std::string> activeMemoryNames;
+
+ // add all active memories to update list
+ std::transform(memoryReaders.begin(), memoryReaders.end(), std::back_inserter(activeMemoryNames), [](const auto& p){return p.first;});
TIMING_START(GuiUpdateAvailableMemories);
- memoryGroup->queryWidget()->update(convVec);
+ memoryGroup->queryWidget()->update(activeMemoryNames);
TIMING_END_STREAM(GuiUpdateAvailableMemories, ARMARX_VERBOSE);
}
catch (...)
@@ -796,14 +809,16 @@ namespace armarx::armem::gui
void
MemoryViewer::updateMemoryTree()
{
- std::map<std::string, const armem::wm::Memory*> convMap;
+ std::map<std::string, const armem::wm::Memory*> memoriesToUpdate;
+
+ //auto checkboxStates = memoryGroup->queryWidget()->getAvailableMemoryStates();
for (auto& [name, data] : memoryData)
{
- convMap[name] = &data;
+ memoriesToUpdate[name] = &data;
}
TIMING_START(GuiUpdateMemoryTree)
- memoryGroup->tree()->update(convMap);
+ memoryGroup->tree()->update(memoriesToUpdate);
TIMING_END_STREAM(GuiUpdateMemoryTree, ARMARX_VERBOSE)
if (debugObserver)
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index 7be0874b265375827d22067e4325edcbf6c4bd12..69fb59d1c2f83b53ebd6587cfd63c148bcab9b7f 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -110,7 +110,7 @@ namespace armarx::armem::gui
void processQueryResults();
void updateMemoryTree();
- void updateAvailableMemories();
+ void updateListOfActiveMemories();
signals:
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
index fe443952ce26bf9546c9238eaf1fa234f13368f6..a490d63436a484cee322d167a02675710c695260 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
@@ -78,7 +78,7 @@ namespace armarx::armem::gui::disk
void
ControlWidget::storeOnDisk(
QString directory,
- const std::map<std::string, wm::Memory> memoryData,
+ const std::vector<wm::Memory> memoryData,
std::string* outStatus)
{
std::filesystem::path path(directory.toUtf8().constData());
@@ -92,8 +92,9 @@ namespace armarx::armem::gui::disk
else
{
int numStored = 0;
- for (const auto& [name, data] : memoryData)
+ for (const auto& data : memoryData)
{
+ std::string name = data.id().memoryName;
if (std::filesystem::is_regular_file(path / name))
{
status << "Could not export memory '" << name << "' to " << path << ": Cannot overwrite existing file.\n";
@@ -116,7 +117,7 @@ namespace armarx::armem::gui::disk
}
- std::map<std::string, wm::Memory>
+ std::map<std::filesystem::path, wm::Memory>
ControlWidget::loadFromDisk(
QString directory,
const armem::client::QueryInput& _queryInput,
@@ -124,7 +125,7 @@ namespace armarx::armem::gui::disk
{
std::filesystem::path path(directory.toUtf8().constData());
- std::map<std::string, wm::Memory> memoryData;
+ std::map<std::filesystem::path, wm::Memory> memoryData;
auto setStatus = [&](const std::string& s){
if (outStatus)
@@ -161,7 +162,7 @@ namespace armarx::armem::gui::disk
}
}
- // TODO: Only add data that matchs query?
+ // TODO: Only add data that matches query?
// We use LTM as query target for the disk
// armem::client::QueryInput queryInput = _queryInput;
// queryInput.addQueryTargetToAll(armem::query::data::QueryTarget::LTM);
@@ -172,8 +173,8 @@ namespace armarx::armem::gui::disk
if (std::filesystem::is_directory(p))
{
armem::server::ltm::disk::Memory ltm(p.parent_path(), p.filename());
- armem::wm::Memory memory = ltm.loadAllAndResolve(); // load list of references
- memoryData[memory.name()] = std::move(memory);
+ armem::wm::Memory memory = ltm.loadAllAndResolve(); // load list of references and load data
+ memoryData[p] = memory;
numLoaded++;
}
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
index b14c9b0a0810acbafba74638e6ebd87989e81165..5eb35b96057f0639a30d94df4dada8ecd38afd9a 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
@@ -5,6 +5,7 @@
#include <QWidget>
#include <QString>
+#include <filesystem>
class QPushButton;
@@ -26,10 +27,10 @@ namespace armarx::armem::gui::disk
void
storeOnDisk(
QString directory,
- const std::map<std::string, wm::Memory> memoryData,
+ const std::vector<wm::Memory> memoryData,
std::string* outStatus = nullptr);
- std::map<std::string, wm::Memory>
+ std::map<std::filesystem::path, wm::Memory>
loadFromDisk(
QString directory,
const armem::client::QueryInput& queryInput,
diff --git a/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp b/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp
index 0856ff1d24329de8c4718c569072e45bbce2116a..a32ced5e4ef945ff4426c75b074cc24434b889c3 100644
--- a/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp
@@ -41,7 +41,7 @@ namespace armarx::armem::gui
timestampLayout(new QHBoxLayout()),
instanceSpinner(new QSpinBox()),
predictionEngineSelector(new QComboBox()),
- predictButton(new QPushButton("Make prediction")),
+ predictButton(new QPushButton("Predict")),
entityInfoRetriever(entityInfoRetriever)
{
auto* vlayout = new QVBoxLayout();
@@ -51,13 +51,13 @@ namespace armarx::armem::gui
vlayout->addLayout(hlayout);
timestampInputSelector->addItems({"Absolute", "Relative to now"});
- timestampLayout->addWidget(new QLabel("Prediction time"));
+ timestampLayout->addWidget(new QLabel("Time"));
timestampLayout->addWidget(timestampInputSelector);
vlayout->addLayout(timestampLayout);
hlayout = new QHBoxLayout();
predictionEngineSelector->setSizeAdjustPolicy(QComboBox::AdjustToContents);
- hlayout->addWidget(new QLabel("Prediction engine"));
+ hlayout->addWidget(new QLabel("Engine"));
hlayout->addWidget(predictionEngineSelector);
hlayout->addStretch();
vlayout->addLayout(hlayout);
diff --git a/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.cpp b/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.cpp
index 04b28cde489ae3f1efb4804ef574cb1a40516fb9..51aade002b3e6c607cf951463801e3aa281667b5 100644
--- a/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.cpp
+++ b/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.cpp
@@ -43,7 +43,7 @@ namespace armarx::armem::gui
microseconds->setSuffix(" µs");
auto* hlayout = new QHBoxLayout();
- hlayout->addWidget(new QLabel("Time"));
+ // hlayout->addWidget(new QLabel("Time"));
hlayout->addWidget(dateTime);
hlayout->addWidget(new QLabel("."));
hlayout->addWidget(microseconds);
@@ -66,7 +66,7 @@ namespace armarx::armem::gui
seconds->setValue(0);
auto* hlayout = new QHBoxLayout();
- hlayout->addWidget(new QLabel("Seconds"));
+ // hlayout->addWidget(new QLabel("Seconds"));
hlayout->addWidget(seconds);
setLayout(hlayout);
}
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
index 54e5aaec313825ae63bf8f15351339fce9288129..a3ce95672ec49cf3a3cffb426f28a04be4b43ccc 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
@@ -162,8 +162,35 @@ namespace armarx::armem::gui
return _dropDisabledCheckBox->isChecked();
}
+ std::map<std::string, QueryWidget::ActiveMemoryState> QueryWidget::getAvailableMemoryStates() const
+ {
+ std::scoped_lock l(enabledMemoriesMutex);
+
+ std::map<std::string, QueryWidget::ActiveMemoryState> states;
+ int maxIndex = _availableMemoriesGroupBox->layout()->count();
+ for (int i = 0; i < maxIndex; ++i)
+ {
+ auto w = _availableMemoriesGroupBox->layout()->itemAt(i)->widget();
+ QCheckBox* box = static_cast<QCheckBox*>(w);
+ std::string memoryName = box->text().toStdString();
+ if (box->isEnabled() && box->isChecked())
+ {
+ states[memoryName] = ActiveMemoryState::FoundAndChecked;
+ }
+ else if (box->isEnabled() && !box->isChecked())
+ {
+ states[memoryName] = ActiveMemoryState::FoundAndNotChecked;
+ }
+ else
+ {
+ states[memoryName] = ActiveMemoryState::NotFound;
+ }
+ }
+ return states;
+ }
+
- std::vector<std::string> QueryWidget::enabledMemories() const
+ std::vector<std::string> QueryWidget::getEnabledMemories() const
{
std::scoped_lock l(enabledMemoriesMutex);
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
index d3d534e5b4896d51d5d0a104852c1d6c64af982f..623b78c4126b6a09acd636c93b9d4c9b6f4934cf 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h
@@ -22,6 +22,13 @@ namespace armarx::armem::gui
public:
+ enum class ActiveMemoryState
+ {
+ FoundAndChecked,
+ FoundAndNotChecked,
+ NotFound
+ };
+
QueryWidget();
armem::query::DataMode dataMode() const;
@@ -31,7 +38,9 @@ namespace armarx::armem::gui
int queryLinkRecursionDepth() const;
- std::vector<std::string> enabledMemories() const;
+ std::map<std::string, ActiveMemoryState> getAvailableMemoryStates() const;
+
+ std::vector<std::string> getEnabledMemories() const;
void update(const std::vector<std::string>& memoryNames);
diff --git a/source/RobotAPI/libraries/armem_index/CMakeLists.txt b/source/RobotAPI/libraries/armem_index/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e586cd150e061a690b1d18b672028e17ac85a3fa
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/CMakeLists.txt
@@ -0,0 +1,37 @@
+set(LIB_NAME armem_index)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # ArmarXGui
+ ArmarXGuiComponentPlugins
+ # RobotAPI
+ RobotAPI::ArViz
+ RobotAPI::armem
+ # RobotAPI::armem_robot
+
+ HEADERS
+ forward_declarations.h
+
+ aron_conversions.h
+ aron_forward_declarations.h
+ memory_ids.h
+
+ SOURCES
+ aron_conversions.cpp
+ memory_ids.cpp
+
+ ARON_FILES
+ aron/Named.xml
+ aron/Spatial.xml
+)
+
+
+add_library(${PROJECT_NAME}::armem_index ALIAS armem_index)
+
+add_subdirectory(server)
diff --git a/source/RobotAPI/libraries/armem_index/aron/Named.xml b/source/RobotAPI/libraries/armem_index/aron/Named.xml
new file mode 100644
index 0000000000000000000000000000000000000000..626b369674a04cd749bff99d79494c951b80971b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/aron/Named.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/Names.xml>" />
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name="armarx::armem::index::arondto::Named"
+ doc-brief="Index of a named entity in the memory.">
+
+ <ObjectChild key="id" doc-brief="Link to the memory element.">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <ObjectChild key="names" doc-brief="The entity's names.">
+ <armarx::arondto::Names />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_index/aron/Spatial.xml b/source/RobotAPI/libraries/armem_index/aron/Spatial.xml
new file mode 100644
index 0000000000000000000000000000000000000000..68e722cc7514fb5ff0ad9430b6adb5220b474969
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/aron/Spatial.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/AxisAlignedBoundingBox.xml>" />
+ <Include include="<RobotAPI/libraries/aron/common/aron/OrientedBox.xml>" />
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name="armarx::armem::index::arondto::Spatial"
+ doc-brief="Index of a spatial entity in the memory.">
+
+ <ObjectChild key="id" doc-brief="Link to the memory element.">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <ObjectChild key="aabbGlobal"
+ doc-brief="Axis-aligned bounding box in global frame.">
+ <simox::arondto::AxisAlignedBoundingBox />
+ </ObjectChild>
+
+ <ObjectChild key="oobbGlobal"
+ doc-brief="Object-oriented bounding box in global frame.">
+ <simox::arondto::OrientedBox optional="true" />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_index/aron_conversions.cpp b/source/RobotAPI/libraries/armem_index/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8b91c50408bf30b099307156ffb30de1dfc34643
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/aron_conversions.cpp
@@ -0,0 +1,9 @@
+#include "aron_conversions.h"
+
+#include <RobotAPI/libraries/armem_index/aron/Named.aron.generated.h>
+#include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
+
+
+namespace armarx::armem::index
+{
+} // namespace armarx::armem::index
diff --git a/source/RobotAPI/libraries/armem_index/aron_conversions.h b/source/RobotAPI/libraries/armem_index/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e774eaff84ee5e2de7f6e187603b497eefd53f2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/aron_conversions.h
@@ -0,0 +1,8 @@
+#pragma once
+
+#include "aron_forward_declarations.h"
+
+
+namespace armarx::armem::index
+{
+} // namespace armarx::armem::index
diff --git a/source/RobotAPI/libraries/armem_index/aron_forward_declarations.h b/source/RobotAPI/libraries/armem_index/aron_forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..dc7606a373e03b3e804c602899bfaa0738ff0104
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/aron_forward_declarations.h
@@ -0,0 +1,8 @@
+#pragma once
+
+
+namespace armarx::armem::index::arondto
+{
+ class Named;
+ class Spatial;
+} // namespace armarx::armem::index::arondto
diff --git a/source/RobotAPI/libraries/armem_index/forward_declarations.h b/source/RobotAPI/libraries/armem_index/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..c6468e5a0b6999c7a6625f7042c1abcbd9f310b4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/forward_declarations.h
@@ -0,0 +1,8 @@
+#pragma once
+
+#include "aron_forward_declarations.h"
+
+
+namespace armarx::armem::index
+{
+}
diff --git a/source/RobotAPI/libraries/armem_index/memory_ids.cpp b/source/RobotAPI/libraries/armem_index/memory_ids.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..246737408f13650d28f3f1290211a184c73305a5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/memory_ids.cpp
@@ -0,0 +1,34 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem_index
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "memory_ids.h"
+
+
+namespace armarx::armem
+{
+
+ const MemoryID index::memoryID { "Index" };
+
+ const MemoryID index::namedSegmentID { memoryID.withCoreSegmentName("Named") };
+ const MemoryID index::spatialSegmentID { memoryID.withCoreSegmentName("Spatial") };
+
+}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/test/SickLaserUnitTest.cpp b/source/RobotAPI/libraries/armem_index/memory_ids.h
similarity index 63%
rename from source/RobotAPI/drivers/SickLaserUnit/test/SickLaserUnitTest.cpp
rename to source/RobotAPI/libraries/armem_index/memory_ids.h
index d6a69c168b0f9018569070829af43a8bd3094563..6173e7a2a022710518934ddffe4f1d72c5c7b553 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/test/SickLaserUnitTest.cpp
+++ b/source/RobotAPI/libraries/armem_index/memory_ids.h
@@ -13,25 +13,24 @@
* 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::SickLaserUnit
- * @author Johann Mantel ( j-mantel at gmx dot net )
- * @date 2021
+ * @package RobotAPI::ArmarXObjects::armem_index
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::SickLaserUnit
+#pragma once
-#define ARMARX_BOOST_TEST
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/Test.h>
-#include "../SickLaserUnit.h"
-#include <iostream>
-
-BOOST_AUTO_TEST_CASE(testExample)
+namespace armarx::armem::index
{
- armarx::SickLaserUnit instance;
- BOOST_CHECK_EQUAL(true, true);
-}
+ extern const MemoryID memoryID;
+
+ extern const MemoryID spatialSegmentID;
+ extern const MemoryID namedSegmentID;
+
+} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..86a96909df32e14bbc90a95a9e2a600e86181c9f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/server/CMakeLists.txt
@@ -0,0 +1,33 @@
+set(ARMARX_LIB_NAME "")
+set(ARON_FILES "")
+
+set(LIB_NAME armem_index_server)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # ArmarXGui
+ ArmarXGuiComponentPlugins
+ # RobotAPI
+ RobotAPI::ArViz
+ RobotAPI::ComponentPlugins
+ RobotAPI::armem_server
+ RobotAPI::armem_index
+
+ # armem_robot_state
+ # armem_robot
+
+ HEADERS
+ server.h
+
+ SOURCES
+ server.cpp
+)
+
+
+add_library(${PROJECT_NAME}::armem_index_server ALIAS armem_index_server)
diff --git a/source/RobotAPI/libraries/armem_index/server/server.cpp b/source/RobotAPI/libraries/armem_index/server/server.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..963efb65f78d449683e5db8f2db44a97c8e08a8d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/server/server.cpp
@@ -0,0 +1,6 @@
+#include "server.h"
+
+
+namespace armarx::armem::index::server
+{
+} // namespace armarx::armem::index::server
diff --git a/source/RobotAPI/libraries/armem_index/server/server.h b/source/RobotAPI/libraries/armem_index/server/server.h
new file mode 100644
index 0000000000000000000000000000000000000000..b0ac2558419db6e938a3215afb9569ab20cfb5e8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_index/server/server.h
@@ -0,0 +1,6 @@
+#pragma once
+
+
+namespace armarx::armem::index::server
+{
+} // namespace armarx::armem::index::server
diff --git a/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt b/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1d452ddcb4d41e37379cf21364bd914e59e328c5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/CMakeLists.txt
@@ -0,0 +1,42 @@
+set(LIB_NAME armem_laser_scans)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ # This package
+ RobotAPI::Core
+ RobotAPI::armem
+ aroncommon
+ armem_robot_state
+ armem_robot
+ # System / External
+ Eigen3::Eigen
+ HEADERS
+ aron_conversions.h
+ client/common/Reader.h
+ client/common/Writer.h
+ server/Visu.h
+ constants.h
+ SOURCES
+ aron_conversions.cpp
+ client/common/Reader.cpp
+ client/common/Writer.cpp
+ server/Visu.cpp
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/LaserScan.xml
+)
+
+add_library(
+ RobotAPI::armem_laser_scans
+ ALIAS
+ armem_laser_scans
+)
diff --git a/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml b/source/RobotAPI/libraries/armem_laser_scans/aron/LaserScan.xml
similarity index 80%
rename from source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
rename to source/RobotAPI/libraries/armem_laser_scans/aron/LaserScan.xml
index 9ad92092e4f2fde9a14450e0bf36bacb8cc84326..dfaae40135956c8c342e8ebc043ce2a3775d10f6 100644
--- a/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
+++ b/source/RobotAPI/libraries/armem_laser_scans/aron/LaserScan.xml
@@ -8,7 +8,7 @@
<GenerateTypes>
- <Object name='armarx::armem::vision::arondto::LaserScannerInfo'>
+ <Object name='armarx::armem::laser_scans::arondto::LaserScannerInfo'>
<ObjectChild key='device'>
<string />
</ObjectChild>
@@ -26,7 +26,7 @@
</ObjectChild>
</Object>
- <Object name="armarx::armem::vision::arondto::SensorHeader">
+ <Object name="armarx::armem::laser_scans::arondto::SensorHeader">
<ObjectChild key="agent">
<string/>
</ObjectChild>
@@ -39,9 +39,9 @@
</Object>
- <Object name='armarx::armem::vision::arondto::LaserScanStamped'>
+ <Object name='armarx::armem::laser_scans::arondto::LaserScanStamped'>
<ObjectChild key="header">
- <armarx::armem::vision::arondto::SensorHeader />
+ <armarx::armem::laser_scans::arondto::SensorHeader />
</ObjectChild>
<!--
diff --git a/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.cpp b/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aaa39472e70e3152feee0f306aca66b81d3f7343
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.cpp
@@ -0,0 +1,100 @@
+#include "aron_conversions.h"
+
+#include <algorithm>
+#include <cstdint>
+#include <iterator>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/common/Converter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include "types.h"
+
+
+namespace armarx::armem::laser_scans
+{
+
+ /************ fromAron ************/
+ SensorHeader
+ fromAron(const arondto::SensorHeader& aronSensorHeader)
+ {
+ return {.agent = aronSensorHeader.agent,
+ .frame = aronSensorHeader.frame,
+ .timestamp = aron::fromAron<Time>(aronSensorHeader.timestamp)};
+ }
+
+ void
+ fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan)
+ {
+ laserScan.header = fromAron(aronLaserScan.header);
+ // laserScan.data = fromAron(aronLaserScan.data);
+ }
+
+ void
+ fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
+ std::string& agentName)
+ {
+ const auto header = fromAron(aronLaserScan.header);
+
+ // laserScan = fromAron(aronLaserScan.data);
+
+ timestamp = header.timestamp.toMicroSecondsSinceEpoch();
+ frame = header.frame;
+ agentName = header.agent;
+ }
+
+ /************ toAron ************/
+
+ // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
+ // {
+ // aronLaserScan.scan = toAron(laserScan);
+ // }
+
+ int64_t
+ toAron(const armem::Time& timestamp)
+ {
+ return timestamp.toMicroSecondsSinceEpoch();
+ }
+
+ arondto::SensorHeader
+ toAron(const SensorHeader& sensorHeader)
+ {
+ arondto::SensorHeader aronSensorHeader;
+
+ aronSensorHeader.agent = sensorHeader.agent;
+ aronSensorHeader.frame = sensorHeader.frame;
+ aron::toAron(aronSensorHeader.timestamp, sensorHeader.timestamp);
+
+ return aronSensorHeader;
+ }
+
+ void
+ toAron(const LaserScanStamped& laserScanStamped,
+ arondto::LaserScanStamped& aronLaserScanStamped)
+ {
+ aronLaserScanStamped.header = toAron(laserScanStamped.header);
+ // toAron(laserScanStamped.data, aronLaserScanStamped.data);
+ }
+
+ void
+ toAron(const LaserScan& laserScan,
+ const armem::Time& timestamp,
+ const std::string& frame,
+ const std::string& agentName,
+ arondto::LaserScanStamped& aronLaserScanStamped)
+ {
+ const SensorHeader header{.agent = agentName, .frame = frame, .timestamp = timestamp};
+
+ const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
+
+ toAron(laserScanStamped, aronLaserScanStamped);
+ }
+
+
+} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.h b/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..931cda8c82013b0d2ad9060bd39be4273e9fce51
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.h
@@ -0,0 +1,71 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_laser_scans/types.h>
+#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+namespace armarx::armem::laser_scans
+{
+
+ namespace arondto
+ {
+ struct LaserScanStamped;
+ } // namespace arondto
+
+ // struct LaserScan;
+ struct LaserScanStamped;
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
+ std::string& agentName);
+
+ template <typename T>
+ auto
+ fromAron(const aron::data::NDArrayPtr& navigator)
+ {
+ return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
+ }
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
+
+ void toAron(const LaserScan& laserScan,
+ const armem::Time& timestamp,
+ const std::string& frame,
+ const std::string& agentName,
+ arondto::LaserScanStamped& aronLaserScan);
+
+ inline aron::data::NDArrayPtr
+ toAron(const LaserScan& laserScan)
+ {
+ using aron::converter::AronVectorConverter;
+ return AronVectorConverter::ConvertFromVector(laserScan);
+ }
+
+
+} // namespace armarx::armem::laser_scans
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
similarity index 56%
rename from source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
rename to source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
index 396dd1bd8b4f40cfef062fa963e87c97f52f45b4..e7ceabba97898778af1c13d9ad9c805357f79ac2 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.cpp
@@ -6,11 +6,10 @@
#include <map>
#include <optional>
#include <ostream>
+#include <type_traits>
#include <utility>
#include <vector>
-#include <type_traits>
-
// ICE
#include <IceUtil/Handle.h>
#include <IceUtil/Time.h>
@@ -24,6 +23,7 @@
#include <ArmarXCore/core/logging/Logging.h>
// RobotAPI Interfaces
+#include "RobotAPI/libraries/armem_laser_scans/constants.h"
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
@@ -33,21 +33,19 @@
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
// RobotAPI Armem
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/util/util.h>
-
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/selectors.h>
-
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
+#include <RobotAPI/libraries/armem_laser_scans/types.h>
-namespace armarx::armem::vision::laser_scans::client
+namespace armarx::armem::laser_scans::client
{
Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
@@ -61,26 +59,22 @@ namespace armarx::armem::vision::laser_scans::client
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- registerPropertyDefinitions(def);
const std::string prefix = propertyPrefix;
-
- def->optional(properties.coreSegmentName,
- prefix + "CoreSegment",
- "Name of the mapping memory core segment to use.");
-
- def->optional(properties.memoryName, prefix + "MemoryName");
}
- void Reader::connect()
+ void
+ Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '"
- << properties.memoryName << "' ...";
+ ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << constants::memoryName
+ << "' ...";
try
{
- memoryReader = memoryNameSystem.useReader(MemoryID().withMemoryName(properties.memoryName));
- ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName << "'";
+ memoryReader =
+ memoryNameSystem.useReader(MemoryID().withMemoryName(constants::memoryName));
+ ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << constants::memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -94,28 +88,44 @@ namespace armarx::armem::vision::laser_scans::client
{
armarx::armem::client::query::Builder qb;
- ARMARX_INFO << "Query for agent: " << query.agent
- << " memory name: " << properties.memoryName;
+ ARMARX_VERBOSE << "Query for agent: " << query.agent
+ << " memory name: " << constants::memoryName;
if (query.sensorList.empty()) // all sensors
{
// clang-format off
- qb
- .coreSegments().withName(properties.memoryName)
+ auto& snapshots = qb
+ .coreSegments().withName(constants::memoryName)
.providerSegments().withName(query.agent)
.entities().all()
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ .snapshots();
// clang-format on
+ if (query.timeRange.has_value())
+ {
+ snapshots.timeRange(query.timeRange->min, query.timeRange->max);
+ }
+ else
+ {
+ snapshots.latest();
+ }
}
else
{
// clang-format off
- qb
- .coreSegments().withName(properties.memoryName)
+ auto& snapshots = qb
+ .coreSegments().withName(constants::memoryName)
.providerSegments().withName(query.agent)
.entities().withNames(query.sensorList)
- .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
+ .snapshots();
// clang-format on
+ if (query.timeRange.has_value())
+ {
+ snapshots.timeRange(query.timeRange->min, query.timeRange->max);
+ }
+ else
+ {
+ snapshots.latest();
+ }
}
return qb;
@@ -130,86 +140,85 @@ namespace armarx::armem::vision::laser_scans::client
ARMARX_WARNING << "No entities!";
}
- const auto convert =
- [](const auto& aronLaserScanStamped,
- const wm::EntityInstance & ei) -> LaserScanStamped
+ const auto convert = [](const auto& aronLaserScanStamped,
+ const wm::EntityInstance& ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
fromAron(aronLaserScanStamped, laserScanStamped);
const auto ndArrayNavigator =
- aron::data::NDArray::DynamicCast(
- ei.data()->getElement("scan"));
+ aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
- ARMARX_IMPORTANT << "4";
return laserScanStamped;
};
// loop over all entities and their snapshots
- providerSegment.forEachEntity([&outV, &convert](const wm::Entity & entity)
- {
- // If we don't need this warning, we could directly iterate over the snapshots.
- if (entity.empty())
+ providerSegment.forEachEntity(
+ [&outV, &convert](const wm::Entity& entity)
{
- ARMARX_WARNING << "Empty history for " << entity.id();
- }
- ARMARX_DEBUG << "History size: " << entity.size();
-
- entity.forEachInstance([&outV, &convert](const wm::EntityInstance & entityInstance)
- {
- if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
+ // If we don't need this warning, we could directly iterate over the snapshots.
+ if (entity.empty())
{
- outV.push_back(convert(*o, entityInstance));
+ ARMARX_WARNING << "Empty history for " << entity.id();
}
+ ARMARX_DEBUG << "History size: " << entity.size();
+
+ entity.forEachInstance(
+ [&outV, &convert](const wm::EntityInstance& entityInstance)
+ {
+ if (const auto o = tryCast<arondto::LaserScanStamped>(entityInstance))
+ {
+ outV.push_back(convert(*o, entityInstance));
+ }
+ return true;
+ });
return true;
});
- return true;
- });
return outV;
}
- Reader::Result Reader::queryData(const Query& query) const
+ Reader::Result
+ Reader::queryData(const Query& query) const
{
const auto qb = buildQuery(query);
ARMARX_DEBUG << "[MappingDataReader] query ... ";
- const armem::client::QueryResult qResult =
- memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
if (not qResult.success)
{
- ARMARX_WARNING << "Failed to query data from memory: "
- << qResult.errorMessage;
- return {.laserScans = {},
- .sensors = {},
- .status = Result::Status::Error,
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.laserScans = {},
+ .sensors = {},
+ .status = Result::Status::Error,
.errorMessage = qResult.errorMessage};
}
// now create result from memory
const wm::ProviderSegment& providerSegment =
- qResult.memory.getCoreSegment(properties.memoryName).getProviderSegment(query.agent);
+ qResult.memory.getCoreSegment(constants::memoryName).getProviderSegment(query.agent);
const auto laserScans = asLaserScans(providerSegment);
std::vector<std::string> sensors;
- providerSegment.forEachEntity([&sensors](const wm::Entity & entity)
- {
- sensors.push_back(entity.name());
- return true;
- });
+ providerSegment.forEachEntity(
+ [&sensors](const wm::Entity& entity)
+ {
+ sensors.push_back(entity.name());
+ return true;
+ });
- return {.laserScans = laserScans,
- .sensors = sensors,
- .status = Result::Status::Success,
+ return {.laserScans = laserScans,
+ .sensors = sensors,
+ .status = Result::Status::Success,
.errorMessage = ""};
}
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
similarity index 92%
rename from source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
rename to source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
index 349b65a5af3bea2fc6acb17324a42b87fbb3bf8d..55beab204b12a7f72432ef2e38789f1856058934 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Reader.h
@@ -27,11 +27,11 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include "RobotAPI/libraries/armem_laser_scans/types.h"
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
namespace armarx
@@ -39,7 +39,7 @@ namespace armarx
class ManagedIceObject;
}
-namespace armarx::armem::vision::laser_scans::client
+namespace armarx::armem::laser_scans::client
{
struct TimeRange
@@ -72,7 +72,8 @@ namespace armarx::armem::vision::laser_scans::client
{
std::string agent;
- TimeRange timeRange;
+ // if not provided, only latest is queried
+ std::optional<TimeRange> timeRange;
// if empty, all sensors will be queried
std::vector<std::string> sensorList;
@@ -109,8 +110,6 @@ namespace armarx::armem::vision::laser_scans::client
// Properties
struct Properties
{
- std::string memoryName = "Vision";
- std::string coreSegmentName = "LaserScans";
} properties;
const std::string propertyPrefix = "mem.vision.laser_scans.";
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
similarity index 54%
rename from source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
rename to source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
index 0cfe7fb95f4abdf1d5065a6d0f245e516644bc8f..08f456b165d08e52c9c3ce0aed649aa96ec8f863 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.cpp
@@ -1,15 +1,19 @@
#include "Writer.h"
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
+#include "RobotAPI/libraries/armem_laser_scans/constants.h"
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
-namespace armarx::armem::vision::laser_scans::client
+namespace armarx::armem::laser_scans::client
{
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem)
- : memoryNameSystem(memoryNameSystem) {}
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
Writer::~Writer() = default;
@@ -20,22 +24,20 @@ namespace armarx::armem::vision::laser_scans::client
const std::string prefix = propertyPrefix;
- def->optional(properties.coreSegmentName,
- prefix + "CoreSegment",
- "Name of the mapping memory core segment to use.");
-
- def->optional(properties.memoryName, prefix + "MemoryName");
}
- void Writer::connect()
+ void
+ Writer::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '"
- << properties.memoryName << "' ...";
+ ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
+ << "' ...";
try
{
- memoryWriter = memoryNameSystem.useWriter(MemoryID().withMemoryName(properties.memoryName));
- ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << properties.memoryName << "'";
+ memoryWriter =
+ memoryNameSystem.useWriter(MemoryID().withMemoryName(constants::memoryName));
+ ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << constants::memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -43,19 +45,20 @@ namespace armarx::armem::vision::laser_scans::client
return;
}
- ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '"
- << properties.memoryName;
+ ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << constants::memoryName;
}
- bool Writer::storeSensorData(const LaserScan& laserScan,
- const std::string& frame,
- const std::string& agentName,
- const std::int64_t& timestamp)
+ bool
+ Writer::storeSensorData(const LaserScan& laserScan,
+ const std::string& frame,
+ const std::string& agentName,
+ const armem::Time& timestamp)
{
std::lock_guard g{memoryWriterMutex};
- const auto result =
- memoryWriter.addSegment(properties.memoryName, agentName);
+ const auto result = memoryWriter.addSegment(constants::memoryName, agentName);
+
+ ARMARX_VERBOSE << "Storing scan with " << laserScan.size() << " elements";
if (not result.success)
{
@@ -65,26 +68,24 @@ namespace armarx::armem::vision::laser_scans::client
return false;
}
- const auto iceTimestamp = Time(Duration::MicroSeconds(timestamp));
const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID =
- providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+ const auto entityID = providerId.withEntityName(frame).withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
arondto::LaserScanStamped aronSensorData;
// currently only sets the header
- toAron(laserScan, iceTimestamp, frame, agentName, aronSensorData);
+ toAron(laserScan, timestamp, frame, agentName, aronSensorData);
auto dict = aronSensorData.toAron();
dict->addElement("scan", toAron(laserScan));
update.instancesData = {dict};
- update.timeCreated = iceTimestamp;
+ update.timeCreated = timestamp;
- ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
ARMARX_DEBUG << updateResult;
@@ -97,4 +98,4 @@ namespace armarx::armem::vision::laser_scans::client
return updateResult.success;
}
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
similarity index 85%
rename from source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
rename to source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
index b4f7502ef9493692fc82bf79740d2367fe165d4a..6a75f24b53d13c0ba2d885c588b5c3e944388a57 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/client/common/Writer.h
@@ -27,11 +27,11 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
-namespace armarx::armem::vision::laser_scans::client
+namespace armarx::armem::laser_scans::client
{
/**
@@ -48,7 +48,6 @@ namespace armarx::armem::vision::laser_scans::client
class Writer
{
public:
-
Writer(armem::client::MemoryNameSystem& memoryNameSystem);
virtual ~Writer();
@@ -60,13 +59,12 @@ namespace armarx::armem::vision::laser_scans::client
// void connect() override;
/// to be called in Component::addPropertyDefinitions
- void registerPropertyDefinitions(
- armarx::PropertyDefinitionsPtr& def) /*override*/;
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
bool storeSensorData(const LaserScan& laserScan,
const std::string& frame,
const std::string& agentName,
- const std::int64_t& timestamp);
+ const armem::Time& timestamp);
private:
armem::client::MemoryNameSystem& memoryNameSystem;
@@ -75,14 +73,11 @@ namespace armarx::armem::vision::laser_scans::client
// Properties
struct Properties
{
- std::string memoryName = "Vision";
- std::string coreSegmentName = "LaserScans";
} properties;
std::mutex memoryWriterMutex;
const std::string propertyPrefix = "mem.vision.laser_scans.";
-
};
-} // namespace armarx::armem::vision::laser_scans::client
+} // namespace armarx::armem::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_laser_scans/constants.h b/source/RobotAPI/libraries/armem_laser_scans/constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb934187159e74e0c13a8a0bc68c391d200c762b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/constants.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 Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+
+namespace armarx::armem::laser_scans::constants
+{
+ const inline std::string memoryName = "LaserScans";
+
+ // core segments
+ const inline std::string laserScanCoreSegment = "LaserScans";
+
+} // namespace armarx::armem::vision::constants
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..551e4792c2eba3b5e19d2ebf468f2d090a53af41
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.cpp
@@ -0,0 +1,332 @@
+#include "Visu.h"
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/color/ColorMap.h>
+#include <algorithm>
+#include <exception>
+#include <string>
+
+#include <Eigen/Geometry>
+
+#include <SimoxUtility/algorithm/apply.hpp>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/math/pose.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/interface/core/PackagePath.h>
+
+#include "RobotAPI/components/ArViz/Client/elements/Color.h"
+#include "RobotAPI/components/ArViz/Client/elements/PointCloud.h"
+#include "RobotAPI/libraries/armem/util/util.h"
+#include "RobotAPI/libraries/armem_laser_scans/types.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron_conversions.h>
+#include <RobotAPI/libraries/armem_laser_scans/util/laser_scanner_conversion.h>
+
+namespace armarx::armem::server::laser_scans
+{
+
+ Visu::Visu()
+ {
+ Logging::setTag("Visu");
+ }
+
+
+ void
+ Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(
+ p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
+ defs->optional(p.uniformColor, prefix + "uniformColor", "If enabled, points will be drawn in red.");
+ defs->optional(p.maxRobotAgeMs,
+ prefix + "maxRobotAgeMs",
+ "Maximum age of robot state before a new one is retrieved in milliseconds.");
+ }
+
+
+ void
+ Visu::init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader)
+ {
+ this->coreSegment = coreSegment;
+ this->virtualRobotReader = virtualRobotReader;
+ }
+
+
+ void
+ Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
+ {
+ this->arviz = arviz;
+ if (debugObserver)
+ {
+ bool batchMode = true;
+ this->debugObserver = DebugObserverHelper("LaserScansMemory", debugObserver, batchMode);
+ }
+
+ if (updateTask)
+ {
+ updateTask->stop();
+ updateTask->join();
+ updateTask = nullptr;
+ }
+ updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
+ updateTask->start();
+ }
+
+
+ void
+ Visu::visualizeRun()
+ {
+ CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
+ while (updateTask and not updateTask->isStopped())
+ {
+ if (p.enabled)
+ {
+ const Time timestamp = Time::Now();
+ ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
+
+ try
+ {
+ visualizeOnce(timestamp);
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
+ }
+
+ if (debugObserver.has_value())
+ {
+ debugObserver->sendDebugObserverBatch();
+ }
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
+
+ void
+ Visu::visualizeScan(const std::vector<Eigen::Vector3f>& points,
+ const std::string& sensorName,
+ const std::string& agentName, const viz::Color& color)
+ {
+ viz::PointCloud pointCloud("laser_scan");
+
+ ARMARX_VERBOSE << "Point cloud with " << points.size() << " points";
+
+ for (const auto& point : points)
+ {
+ pointCloud.addPoint(point.x(), point.y(), point.z(), color);
+ }
+
+ pointCloud.pointSizeInPixels(3);
+
+ viz::Layer l = arviz.layer(agentName + "/" + sensorName);
+ l.add(pointCloud);
+
+ arviz.commit(l);
+ }
+
+ std::vector<Eigen::Vector3f>
+ convertScanToGlobal(const armem::laser_scans::LaserScanStamped& scan,
+ const Eigen::Isometry3f& global_T_sensor)
+ {
+ auto scanCartesian =
+ armarx::armem::laser_scans::util::toCartesian<Eigen::Vector3f>(scan.data);
+
+ for (auto& point : scanCartesian)
+ {
+ point = global_T_sensor * point;
+ }
+
+ return scanCartesian;
+ }
+
+
+ // void Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
+ // {
+ // coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment & provSegment)
+ // {
+ // getLatestObjectPoses(provSegment, out);
+ // });
+ // }
+
+
+ // void Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
+ // {
+ // provSegment.forEachEntity([&out](const wm::Entity & entity)
+ // {
+ // if (!entity.empty())
+ // {
+ // ObjectPose pose = getLatestObjectPose(entity);
+ // // Try to insert. Fails and returns false if an entry already exists.
+ // const auto [it, success] = out.insert({pose.objectID, pose});
+ // if (!success)
+ // {
+ // // An entry with that ID already exists. We keep the newest.
+ // if (it->second.timestamp < pose.timestamp)
+ // {
+ // it->second = pose;
+ // }
+ // }
+ // }
+ // });
+ // }
+
+
+ // void Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
+ // {
+ // entity.getLatestSnapshot().forEachInstance([&out](const wm::EntityInstance & instance)
+ // {
+ // arondto::ObjectInstance dto;
+ // dto.fromAron(instance.data());
+
+ // fromAron(dto, out);
+ // });
+ // }
+
+ std::map<std::string, armem::laser_scans::LaserScanStamped>
+ Visu::getCurrentLaserScans()
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+
+ const auto convert = [this](const wm::EntityInstance& entityInstance)
+ -> ::armarx::armem::laser_scans::LaserScanStamped
+ {
+ const std::optional<armarx::armem::laser_scans::arondto::LaserScanStamped> dto =
+ tryCast<armarx::armem::laser_scans::arondto::LaserScanStamped>(entityInstance);
+
+ ARMARX_CHECK(dto.has_value());
+
+ ::armarx::armem::laser_scans::LaserScanStamped laserScanStamped;
+ fromAron(dto.value(), laserScanStamped);
+
+ const auto ndArrayNavigator =
+ aron::data::NDArray::DynamicCast(entityInstance.data()->getElement("scan"));
+
+ ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+ laserScanStamped.data =
+ armarx::armem::laser_scans::fromAron<LaserScanStep>(ndArrayNavigator);
+
+ ARMARX_VERBOSE << "Number of steps: " << laserScanStamped.data.size();
+
+ return laserScanStamped;
+ };
+
+ std::map<std::string, armem::laser_scans::LaserScanStamped> scans;
+
+ const auto applyToInstance = [&](const wm::EntityInstance& instance)
+ {
+ const auto scan = convert(instance);
+ scans[instance.id().providerSegmentName + "/" + instance.id().entityName] = scan;
+ };
+
+ const auto applyToEntity = [&](const wm::Entity& entity)
+ {
+ if (entity.empty())
+ {
+ return;
+ }
+
+ const auto& snapshot = entity.getLatestSnapshot();
+
+ snapshot.forEachInstance(applyToInstance);
+ };
+
+ const auto applyToProviderSegment = [&](const auto& providerSegment)
+ { providerSegment.forEachEntity(applyToEntity); };
+
+ coreSegment->forEachProviderSegment(applyToProviderSegment);
+
+ ARMARX_VERBOSE << scans.size() << " scans";
+ return scans;
+ }
+
+ void
+ Visu::visualizeOnce(const Time& timestamp)
+ {
+ std::map<std::string, armem::laser_scans::LaserScanStamped> currentLaserScans =
+ getCurrentLaserScans();
+
+ int i = 0;
+
+ for (const auto& [provider, scan] : currentLaserScans)
+ {
+ ARMARX_VERBOSE << "Visualizing `" << provider << "`";
+
+ const auto global_T_sensor = [&]() -> Eigen::Isometry3f{
+
+ const auto robot = getSynchronizedRobot(scan.header.agent, scan.header.timestamp);
+ if(not robot)
+ {
+ ARMARX_VERBOSE << deactivateSpam(1) << "Robot `" << scan.header.agent << "`" << "not available";
+ return Eigen::Isometry3f::Identity();
+ }
+
+ const auto sensorNode = robot->getRobotNode(scan.header.frame);
+ ARMARX_CHECK_NOT_NULL(sensorNode) << "No robot node `" << scan.header.frame
+ << "` for robot `" << scan.header.agent << "`";
+
+ ARMARX_VERBOSE << "Sensor position for sensor `" << scan.header.frame << "` is " << sensorNode->getGlobalPosition();
+ return Eigen::Isometry3f{sensorNode->getGlobalPose()};
+ }();
+
+ const std::vector<Eigen::Vector3f> points = convertScanToGlobal(scan, global_T_sensor);
+
+ const auto color = [&]() -> simox::Color{
+ if(p.uniformColor)
+ {
+ return simox::Color::red();
+ }
+
+ return simox::color::GlasbeyLUT::at(i++);
+ }();
+
+ visualizeScan(points, scan.header.frame, scan.header.agent, color);
+ }
+ }
+
+ VirtualRobot::RobotPtr
+ Visu::getSynchronizedRobot(const std::string& name, const DateTime& timestamp)
+ {
+ if (robots.count(name) == 0)
+ {
+ ARMARX_CHECK_NOT_NULL(virtualRobotReader);
+ const auto robot = virtualRobotReader->getRobot(name);
+
+ if (robot)
+ {
+ robots[name] = {robot, DateTime::Invalid()};
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+
+ auto& entry = robots.at(name);
+ if (entry.second.isInvalid() ||
+ (timestamp - entry.second) > Duration::MilliSeconds(p.maxRobotAgeMs))
+ {
+ if (virtualRobotReader->synchronizeRobot(*entry.first, timestamp))
+ {
+ entry.second = timestamp;
+ }
+ else
+ {
+ ARMARX_INFO << deactivateSpam(10) << "Failed to synchronize robot `" << name << "`";
+ }
+ }
+ return entry.first;
+ }
+
+} // namespace armarx::armem::server::laser_scans
diff --git a/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..d6c79dd12d9aaf7b85dfad6eacaf436e7e6f59c1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/server/Visu.h
@@ -0,0 +1,97 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <optional>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/core/time.h>
+#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+
+#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem_laser_scans/types.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+
+
+namespace armarx::armem::server::laser_scans
+{
+
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class Visu : public armarx::Logging
+ {
+ public:
+ Visu();
+
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "visu.");
+ void init(const wm::CoreSegment* coreSegment, armem::robot_state::VirtualRobotReader* virtualRobotReader);
+ void connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver = nullptr);
+
+
+ private:
+ void visualizeRun();
+ void visualizeOnce(const Time& timestamp);
+
+
+ viz::Client arviz;
+ std::optional<DebugObserverHelper> debugObserver;
+
+ struct Properties
+ {
+ bool enabled = true;
+ bool uniformColor = false;
+ float frequencyHz = 5;
+ int maxRobotAgeMs = 100;
+ } p;
+
+
+ SimpleRunningTask<>::pointer_type updateTask;
+
+ armem::robot_state::VirtualRobotReader* virtualRobotReader;
+
+ std::map<std::string, std::pair<VirtualRobot::RobotPtr, DateTime>> robots;
+
+ VirtualRobot::RobotPtr getSynchronizedRobot(const std::string& name,
+ const DateTime& timestamp);
+
+ void visualizeScan(const std::vector<Eigen::Vector3f>& points,
+ const std::string& sensorName,
+ const std::string& agentName,
+ const viz::Color& color);
+
+ std::map<std::string, armem::laser_scans::LaserScanStamped> getCurrentLaserScans();
+
+ const wm::CoreSegment* coreSegment;
+ };
+
+} // namespace armarx::armem::server::laser_scans
diff --git a/source/RobotAPI/libraries/armem_laser_scans/types.h b/source/RobotAPI/libraries/armem_laser_scans/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..34f58e01988301e7947de008d63a2865d3324382
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/types.h
@@ -0,0 +1,46 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <VirtualRobot/MathTools.h>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+namespace armarx::armem::laser_scans
+{
+ struct SensorHeader
+ {
+ std::string agent;
+ std::string frame;
+ armem::Time timestamp;
+ };
+
+ struct LaserScanStamped
+ {
+ SensorHeader header;
+ LaserScan data;
+ };
+
+} // namespace armarx::armem::laser_scans
diff --git a/source/RobotAPI/libraries/armem_laser_scans/util/laser_scanner_conversion.h b/source/RobotAPI/libraries/armem_laser_scans/util/laser_scanner_conversion.h
new file mode 100644
index 0000000000000000000000000000000000000000..11e0ee6e8a3f071117aa938bf3e5a23cc1ef6935
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_laser_scans/util/laser_scanner_conversion.h
@@ -0,0 +1,61 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+
+#include <Eigen/Geometry>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx::armem::laser_scans::util
+{
+
+ template <typename EigenVectorT>
+ EigenVectorT
+ toCartesian(const LaserScanStep& laserScanStep)
+ {
+ EigenVectorT point = EigenVectorT::Identity();
+
+ point.x() = laserScanStep.distance * std::cos(laserScanStep.angle);
+ point.y() = laserScanStep.distance * std::sin(laserScanStep.angle);
+
+ return point;
+ }
+
+ template <typename EigenVectorT>
+ std::vector<EigenVectorT>
+ toCartesian(const armarx::LaserScan& laserScan)
+ {
+ std::vector<EigenVectorT> points;
+ points.reserve(laserScan.size());
+
+ std::transform(laserScan.begin(),
+ laserScan.end(),
+ std::back_inserter(points),
+ [](const LaserScanStep& pt) { return toCartesian<EigenVectorT>(pt); });
+
+ return points;
+ }
+
+} // namespace armarx::armem::laser_scans::util
diff --git a/source/RobotAPI/libraries/armem_locations/CMakeLists.txt b/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..22d418142d784080f8f4f99410a25a272d5c628f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/CMakeLists.txt
@@ -0,0 +1,30 @@
+set(LIB_NAME armem_locations)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXCoreObservers
+
+ RobotAPI::Core
+ RobotAPI::armem
+ # aronjsonconverter
+ SOURCES
+ ./aron_conversions.cpp
+
+ HEADERS
+ ./aron_conversions.h
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/Location.xml
+)
+
+
+add_library(RobotAPI::armem_locations ALIAS armem_locations)
diff --git a/source/RobotAPI/libraries/armem_locations/aron/Location.xml b/source/RobotAPI/libraries/armem_locations/aron/Location.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9a0539d5d40a3b3b91dc32ad2034077a815437ff
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/aron/Location.xml
@@ -0,0 +1,44 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true"/>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <!--
+ ToDo: Model regions. Ideas:
+ - Polygon (convex, non-convex)
+ -
+ -->
+
+
+ <Object name='armarx::navigation::location::arondto::ObjectRelativeLocation'>
+
+ <ObjectChild key='objectInstanceID'>
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <ObjectChild key='relativeRobotPose'>
+ <Pose />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::navigation::location::arondto::Location'>
+
+ <ObjectChild key='globalRobotPose'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='relativeToObject'>
+ <armarx::navigation::location::arondto::ObjectRelativeLocation optional="true" />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_locations/aron_conversions.cpp b/source/RobotAPI/libraries/armem_locations/aron_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..50ef19ce29d8432970e47071ac3dc4358cef6745
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/aron_conversions.cpp
@@ -0,0 +1,6 @@
+#include "aron_conversions.h"
+
+namespace armarx::armem
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_locations/aron_conversions.h b/source/RobotAPI/libraries/armem_locations/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..93e57fbcecc4f1e8347559d300a7a9272b24555b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_locations/aron_conversions.h
@@ -0,0 +1,7 @@
+#pragma once
+
+
+namespace armarx::armem
+{
+
+}
diff --git a/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp
index 79f6c4a161b1941e44ba42cf6acc40607c0efa84..d173538b588e3ab711e78e941fb7447cab60e34a 100644
--- a/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp
@@ -20,9 +20,9 @@ namespace armarx::armem::server::motions::mdb::conversion
motionReference.associatedInstitution = json["associatedInstitution"];
motionReference.associatedProject = json["associatedProject"];
motionReference.comment = json["comment"];
- motionReference.createdDate = IceUtil::Time::seconds(json["createdDate"]);
+ motionReference.createdDate = DateTime(Duration::Seconds(json["createdDate"]));
motionReference.createdUser = json["createdUser"];
- motionReference.modifiedDate = IceUtil::Time::seconds(json["modifiedDate"]);
+ motionReference.modifiedDate = DateTime(Duration::Seconds(json["modifiedDate"]));
motionReference.modifiedUser = json["modifiedUser"];
motionReference.date = json["date"];
for (auto it = json["attachedFiles"].begin(); it != json["attachedFiles"].end(); ++it)
@@ -34,7 +34,7 @@ namespace armarx::armem::server::motions::mdb::conversion
{
armarx::motion::mdb::arondto::FileReference fileRef;
fileRef.attachedToId = attachedFile["attachedToId"];
- fileRef.createdDate = IceUtil::Time::microSeconds(attachedFile["createdDate"]);
+ fileRef.createdDate = DateTime(Duration::MicroSeconds(attachedFile["createdDate"]));
fileRef.createdUser = attachedFile["createdUser"];
fileRef.description = attachedFile["description"];
fileRef.fileName = attachedFile["fileName"];
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index ac67c9ad6cc34d051144df93def583c94ce63187..63da8444d40d90e6c4e73a1a4fd755082f4914f9 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -20,6 +20,9 @@ armarx_add_library(
HEADERS
aron_conversions.h
aron_forward_declarations.h
+ memory_ids.h
+ types.h
+ utils.h
server/class/FloorVis.h
server/class/Segment.h
@@ -48,6 +51,9 @@ armarx_add_library(
SOURCES
aron_conversions.cpp
+ memory_ids.cpp
+ types.cpp
+ utils.cpp
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
@@ -67,6 +73,8 @@ armarx_add_library(
aron/Attachment.xml
# aron/Constraint.xml
+
+ aron/Marker.xml
)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/Marker.xml b/source/RobotAPI/libraries/armem_objects/aron/Marker.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0fa6542098e8d8d6542845927564a5026e192de7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/aron/Marker.xml
@@ -0,0 +1,32 @@
+<!--
+Core segment type of Object/Marker
+-->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/aron/common/aron/framed.xml>" autoinclude="true" />
+ </AronIncludes>
+ <GenerateTypes>
+ <Object name="armarx::armem::arondto::Marker">
+ <ObjectChild key="name">
+ <String />
+ </ObjectChild>
+ <ObjectChild key="markerPose">
+ <armarx::arondto::FramedPose />
+ </ObjectChild>
+ <ObjectChild key="markerGlobal">
+ <armarx::arondto::FramedPose />
+ </ObjectChild>
+ <ObjectChild key="robotGlobal">
+ <armarx::arondto::FramedPose />
+ </ObjectChild>
+ <ObjectChild key="rgbCamera">
+ <armarx::arondto::FramedPose />
+ </ObjectChild>
+ <ObjectChild key="depthCamera">
+ <armarx::arondto::FramedPose />
+ </ObjectChild>
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
+
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
index a90821817037a2deb733474743f33221048ab24b..e2d695429d3c4193ad97df106821f5bb912a0028 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -27,6 +27,22 @@ Core segment type of Object/Class.
<armarx::arondto::PackagePath />
</ObjectChild>
+ <ObjectChild key="urdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="articulatedUrdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="sdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
+ <ObjectChild key="articulatedSdfPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
<ObjectChild key="meshWrlPath">
<armarx::arondto::PackagePath />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 8602178224b696fdcf026f50184628ee7a870dae..23b84e31aef6972059feb8cdf26f75fe3d4271ab 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -1,50 +1,55 @@
#include "aron_conversions.h"
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem
{
- void fromAron(const arondto::ObjectInstance& dto,
- objpose::arondto::ObjectPose& bo)
+ void
+ fromAron(const arondto::ObjectInstance& dto, objpose::arondto::ObjectPose& bo)
{
bo = dto.pose;
}
- void toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
+ void
+ toAron(arondto::ObjectInstance& dto, const objpose::arondto::ObjectPose& bo)
{
dto.pose = bo;
}
- void fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
+ void
+ fromAron(const arondto::ObjectInstance& dto, objpose::ObjectPose& bo)
{
objpose::fromAron(dto.pose, bo);
}
- void toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
+ void
+ toAron(arondto::ObjectInstance& dto, const objpose::ObjectPose& bo)
{
objpose::toAron(dto.pose, bo);
}
/* Attachments */
- void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo)
+ void
+ 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)
+ void
+ 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)
+ void
+ fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo)
{
fromAron(dto.agent, bo.agent);
aron::fromAron(dto.transformation, bo.transformation);
@@ -53,7 +58,8 @@ namespace armarx::armem
// TODO aron::fromAron(dto.timestamp, bo.timestamp);
}
- void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo)
+ void
+ toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo)
{
toAron(dto.agent, bo.agent);
aron::toAron(dto.transformation, bo.transformation);
@@ -63,7 +69,9 @@ namespace armarx::armem
}
- void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo)
+ void
+ fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
+ attachment::ArticulatedObjectAttachment& bo)
{
fromAron(dto.agent, bo.agent);
aron::fromAron(dto.transformation, bo.transformation);
@@ -72,7 +80,9 @@ namespace armarx::armem
// TODO aron::fromAron(dto.timestamp, bo.timestamp);
}
- void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo)
+ void
+ toAron(arondto::attachment::ArticulatedObjectAttachment& dto,
+ const attachment::ArticulatedObjectAttachment& bo)
{
toAron(dto.agent, bo.agent);
aron::toAron(dto.transformation, bo.transformation);
@@ -81,16 +91,34 @@ namespace armarx::armem
// TODO aron::toAron(dto.timestamp, bo.timestamp);
}
+ void
+ toAron(arondto::Marker& dto, const marker::Marker& bo)
+ {
+ dto.name = bo.name;
+ dto.robotGlobal = bo.robotGlobal;
+ dto.rgbCamera = bo.rgbCamera;
+ dto.depthCamera = bo.depthCamera;
+ dto.markerPose = bo.markerPose;
+ dto.markerGlobal = bo.getGlobalMarkerPose();
+ }
+ void
+ fromAron(const arondto::Marker& dto, marker::Marker& bo)
+ {
+ bo.name = dto.name;
+ bo.robotGlobal = dto.robotGlobal;
+ bo.rgbCamera = dto.rgbCamera;
+ bo.depthCamera = dto.depthCamera;
+ bo.markerPose = dto.markerPose;
+ }
-
-} // namespace armarx::armem
+} // namespace armarx::armem
armarx::armem::MemoryID
armarx::armem::obj::makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose)
{
return MemoryID("Object/Instance")
- .withProviderSegmentName(objectPose.providerName)
- .withEntityName(objectPose.objectID.str())
- .withTimestamp(objectPose.timestamp);
+ .withProviderSegmentName(objectPose.providerName)
+ .withEntityName(objectPose.objectID.str())
+ .withTimestamp(objectPose.timestamp);
}
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index 61e89c37ec00aaabed94e1b20a2ba1b50be976a8..e27ed16c4cede302e2eecf2c013440d240162550 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -4,6 +4,7 @@
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.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/types.h>
@@ -26,6 +27,8 @@ namespace armarx::armem
void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo);
void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo);
+ void fromAron(const arondto::Marker& dto, marker::Marker&bo);
+ void toAron(arondto::Marker& dto, const marker::Marker&bo);
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h b/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
index 9416a033d713f5c8dab28506538c1a56534dbb53..6a57255f1019c433f87d7f1f66faa3919a9087c8 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
@@ -5,4 +5,6 @@ namespace armarx::armem::arondto
{
class ObjectClass;
class ObjectInstance;
+ class Marker;
}
+
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index c16ff257603b9fded0a5ea72737b92b40667fdb6..a2b0ddd6b2985997156cfa6c82998de249409e5a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -47,7 +47,7 @@ namespace armarx::armem::articulated_object
ARMARX_DEBUG << "Object " << name << " available";
auto obj = VirtualRobot::RobotIO::loadRobot(
- ArmarXDataPath::resolvePath(it->xml.serialize().path),
+ it->xml.toSystemPath(),
VirtualRobot::RobotIO::eStructure);
if (not obj)
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 6cbb3ecdcfee0334df79a9d6728080c5bce55f5f..0b466d404fbc41d5eee48819418dd5b5cae35d1f 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -3,44 +3,53 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <VirtualRobot/Robot.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
-
-#include <VirtualRobot/Robot.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
namespace armarx::armem::articulated_object
{
- armem::articulated_object::ArticulatedObject convert(
- const VirtualRobot::Robot& obj,
- const armem::Time& timestamp)
+ armem::articulated_object::ArticulatedObject
+ convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
{
ARMARX_DEBUG << "Filename is " << obj.getFilename();
- // TODO(fabian.reister): remove "PriorKnowledgeData" below
+ const std::vector<std::string> packages =
+ armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const std::string package = armarx::ArmarXDataPath::getProject(packages, obj.getFilename());
- return armem::articulated_object::ArticulatedObject
+ // make sure that the relative path is without the 'package/' prefix
+ const std::string fileRelPath = [&obj, &package]() -> std::string
{
- .description = {
- .name = obj.getType(),
- .xml = PackagePath(armarx::ArmarXDataPath::getProject(
- {"PriorKnowledgeData"}, obj.getFilename()),
- obj.getFilename())
- },
- .instance = "", // TODO(fabian.reister):
- .config = {
- .timestamp = timestamp,
- .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
- .jointMap = obj.getJointValues()
- },
- .timestamp = timestamp};
+ if (simox::alg::starts_with(obj.getFilename(), package))
+ {
+ // remove "package" + "/"
+ const std::string fixedFilename = obj.getFilename().substr(package.size() + 1, -1);
+ return fixedFilename;
+ }
+
+ return obj.getFilename();
+ }();
+
+ return armem::articulated_object::ArticulatedObject{
+ .description = {.name = obj.getType(),
+ .xml = PackagePath(armarx::ArmarXDataPath::getProject(
+ {package}, fileRelPath),
+ obj.getFilename())},
+ .instance = obj.getName(),
+ .config = {.timestamp = timestamp,
+ .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+ .jointMap = obj.getJointValues()},
+ .timestamp = timestamp};
}
bool
- ArticulatedObjectWriter::storeArticulatedObject(
- const VirtualRobot::RobotPtr& articulatedObject,
- const armem::Time& timestamp)
+ ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp)
{
ARMARX_CHECK_NOT_NULL(articulatedObject);
diff --git a/source/RobotAPI/libraries/armem_objects/memory_ids.cpp b/source/RobotAPI/libraries/armem_objects/memory_ids.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ad77419a2e5d3288aefae77a038cca62808e81dd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/memory_ids.cpp
@@ -0,0 +1,35 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem_objects
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "memory_ids.h"
+
+
+namespace armarx::armem
+{
+
+ const MemoryID objects::memoryID { "Object" };
+
+ const MemoryID objects::attachmentsSegmentID = memoryID.withCoreSegmentName("Attachments");
+ const MemoryID objects::classSegmentID = memoryID.withCoreSegmentName("Class");
+ const MemoryID objects::instaceSegmentID = memoryID.withCoreSegmentName("Instance");
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/memory_ids.h b/source/RobotAPI/libraries/armem_objects/memory_ids.h
new file mode 100644
index 0000000000000000000000000000000000000000..50408b82f550fe2ecf80b4cc5d4930ee9b69b012
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/memory_ids.h
@@ -0,0 +1,38 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem_objects
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::armem::objects
+{
+
+ extern const MemoryID memoryID;
+
+ extern const MemoryID attachmentsSegmentID;
+ extern const MemoryID classSegmentID;
+ extern const MemoryID instaceSegmentID;
+
+
+} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
index 9f977a7beaa67789b181cbabb18752cbf4242fef..74e29b855363472523ae328b2698059a8f9d108f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
@@ -36,6 +36,8 @@ armarx_add_library(
instance/Visu.h
instance/ArticulatedObjectVisu.h
+ instance/visu/LinearPredictionsVisu.h
+
attachments/Segment.h
SOURCES
@@ -49,6 +51,8 @@ armarx_add_library(
instance/Visu.cpp
instance/ArticulatedObjectVisu.cpp
+ instance/visu/LinearPredictionsVisu.cpp
+
attachments/Segment.cpp
)
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
index 4f8cd20ccd9a0ce456bb664f5f5eb8a25f1981e0..9f4636fa9484d8c9d4fdd4d0a825442f06340637 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
@@ -25,6 +25,7 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem/server/forward_declarations.h>
#include <RobotAPI/libraries/armem_objects/types.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -57,7 +58,7 @@ namespace armarx::armem::server::obj::attachments
struct Properties
{
- std::string coreSegmentName = "Attachments";
+ std::string coreSegmentName = objects::attachmentsSegmentID.coreSegmentName;
int64_t maxHistorySize = -1;
};
Properties p;
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 3250a1cca34d47df08e4d9218c0bfeda729163b0..4dcf3262724cd6a9626a967f9582d4514a478d89 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -5,6 +5,7 @@
#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>
@@ -22,7 +23,7 @@ namespace armarx::armem::server::obj::clazz
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- SpecializedCoreSegment(memoryToIceAdapter, "Class", arondto::ObjectClass::ToAronType(), -1)
+ SpecializedCoreSegment(memoryToIceAdapter, objects::classSegmentID.coreSegmentName, arondto::ObjectClass::ToAronType(), -1)
{
}
@@ -190,7 +191,11 @@ namespace armarx::armem::server::obj::clazz
}
};
setPathIfExists(data.simoxXmlPath, info.simoxXML());
+ setPathIfExists(data.urdfPath, info.urdf());
+ setPathIfExists(data.sdfPath, info.sdf());
setPathIfExists(data.articulatedSimoxXmlPath, info.articulatedSimoxXML());
+ setPathIfExists(data.articulatedUrdfPath, info.articulatedUrdf());
+ setPathIfExists(data.articulatedSdfPath, info.articulatedSdf());
setPathIfExists(data.meshObjPath, info.wavefrontObj());
setPathIfExists(data.meshWrlPath, info.meshWrl());
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 1bac179ed97c29d431f3f47107ad6e367677b3da..c17ce69020b2b7f2c363219339754919fc805723 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -6,6 +6,7 @@
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
@@ -15,6 +16,7 @@
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/util/util.h>
+
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
@@ -49,12 +51,20 @@
namespace armarx::armem::server::obj::instance
{
+ void Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(robotName, prefix + "robotName",
+ "Name of robot whose note can be calibrated.\n"
+ "If not given, the 'fallbackName' is used.");
+ defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
+ defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
+ }
+
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
SpecializedCoreSegment(memoryToIceAdapter,
- "Instance",
+ objects::instaceSegmentID.coreSegmentName,
arondto::ObjectInstance::ToAronType(),
- 64,
- predictionEngines)
+ 64)
{
oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
{
@@ -109,10 +119,20 @@ namespace armarx::armem::server::obj::instance
"Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
defs->optional(p.sceneSnapshotsDirectory, prefix + "scene.11_Directory",
"Directory in Package/data/Package/ containing the scene snapshots.");
- defs->optional(p.sceneSnapshotToLoad, prefix + "scene.12_SnapshotToLoad",
- "Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').\n"
- "You can also specify paths relative to 'Package/scenes/'. \n"
- "You can also specify a ; separated list of scenes.");
+
+ std::vector<std::string> sceneSnapshotToLoadDescription =
+ {
+ "Scene(s) to load on startup.",
+ "Specify multiple scenes in a ; separated list.",
+ "Each entry must be one of the following:",
+ "(1) A scene file in 'Package/scenes/' (with or without '.json' extension), "
+ "e.g. 'MyScene', 'MyScene.json'",
+ "(2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' extension), "
+ "e.g. 'path/to/MyScene', 'path/to/MyScene.json'",
+ "(3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'",
+ };
+ defs->optional(p.sceneSnapshotsToLoad, prefix + "scene.12_SnapshotToLoad",
+ simox::alg::join(sceneSnapshotToLoadDescription, " \n"));
decay.defineProperties(defs, prefix + "decay.");
}
@@ -122,10 +142,10 @@ namespace armarx::armem::server::obj::instance
{
SpecializedCoreSegment::init();
- if (not p.sceneSnapshotToLoad.empty())
+ if (not p.sceneSnapshotsToLoad.empty())
{
bool trim = true;
- const std::vector<std::string> scenes = simox::alg::split(p.sceneSnapshotToLoad, ";", trim);
+ const std::vector<std::string> scenes = simox::alg::split(p.sceneSnapshotsToLoad, ";", trim);
for (const std::string& scene : scenes)
{
const bool lockMemory = false;
@@ -149,6 +169,7 @@ namespace armarx::armem::server::obj::instance
Segment::CommitStats Segment::commitObjectPoses(
const std::string& providerName,
const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ const Calibration& calibration,
std::optional<armem::Time> discardUpdatesUntil)
{
CommitStats stats;
@@ -157,7 +178,7 @@ namespace armarx::armem::server::obj::instance
objpose::ObjectPoseSeq newObjectPoses;
stats.numUpdated = 0;
- // timestamp used to reduce the rpc calls for robot sync
+ // timestamp used to reduce the rpc calls for robot sync
Time robotSyncTimestamp = Time::Invalid();
for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
@@ -201,13 +222,59 @@ namespace armarx::armem::server::obj::instance
stats.numUpdated++;
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!
// Update the robot to obtain correct local -> global transformation
if (robot and robotSyncTimestamp != timestamp)
{
- ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, 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.";
+
robotSyncTimestamp = timestamp;
+
+
+ // Apply calibration offset after synchronizing the robot.
+ {
+ if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
+ {
+ VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode(calibration.robotNode);
+
+ 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);
+ }
+ }
}
objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
@@ -886,7 +953,7 @@ namespace armarx::armem::server::obj::instance
void Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
{
- if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
+ if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
{
ARMARX_INFO << "Storing scene snapshot at: \n" << path.value();
@@ -905,7 +972,7 @@ namespace armarx::armem::server::obj::instance
std::optional<armarx::objects::Scene>
Segment::loadScene(const std::string& filename)
{
- if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
+ if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
{
return loadScene(path.value());
}
@@ -935,17 +1002,18 @@ namespace armarx::armem::server::obj::instance
std::optional<std::filesystem::path>
- Segment::resolveSceneFilename(const std::string& _filename)
+ Segment::resolveSceneFilepath(const std::string& _filename)
{
- std::string filename = _filename;
+ std::string filepath = _filename;
- filename = simox::alg::replace_all(filename, timestampPlaceholder,
+ filepath = simox::alg::replace_all(filepath, timestampPlaceholder,
Time::Now().toString("%Y-%m-%d_%H-%M-%S"));
- if (not simox::alg::ends_with(filename, ".json"))
+ if (not simox::alg::ends_with(filepath, ".json"))
{
- filename += ".json";
+ filepath += ".json";
}
+ // Try to interprete it as relative to 'Package/scenes/'.
if (!finder)
{
finder.reset(new CMakePackageFinder(p.sceneSnapshotsPackage));
@@ -956,42 +1024,78 @@ namespace armarx::armem::server::obj::instance
}
if (finder->packageFound())
{
- std::filesystem::path dataDir = finder->getDataDir();
- std::filesystem::path path = dataDir / p.sceneSnapshotsPackage / p.sceneSnapshotsDirectory / filename;
- return path;
+ std::filesystem::path absDataDir = finder->getDataDir();
+ std::filesystem::path absPath = absDataDir / p.sceneSnapshotsPackage
+ / p.sceneSnapshotsDirectory / filepath;
+ if (std::filesystem::is_regular_file(absPath))
+ {
+ return absPath;
+ }
}
- else
+
+ // Try to interprete it as ArmarXDataPath.
{
- return std::nullopt;
+ std::string resolved = ArmarXDataPath::resolvePath(filepath);
+ if (resolved != filepath)
+ {
+ return resolved;
+ }
}
+
+ // Else: Fail
+ return std::nullopt;
}
armarx::objects::Scene Segment::getSceneSnapshot() const
{
- armarx::objects::Scene scene;
- segmentPtr->forEachEntity([&scene](wm::Entity & entity)
+ using armarx::objects::SceneObject;
+
+ // We only store the latest version of each objectID.
+
+ struct StampedSceneObject
{
- try
+ SceneObject object;
+ Time timestamp;
+ };
+
+ std::map<ObjectID, StampedSceneObject> objects;
+ segmentPtr->forEachEntity([&objects](wm::Entity & entity)
+ {
+ const wm::EntityInstance* entityInstance = entity.findLatestInstance();
+ if (entityInstance)
{
- const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
- std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(entityInstance);
+ std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(*entityInstance);
if (objectInstance)
{
- armarx::objects::SceneObject& object = scene.objects.emplace_back();
- // object.instanceID = entityInstance.id();
- object.className = ObjectID(objectInstance->classID.entityName).getClassID().str();
- object.collection = "";
- object.position = simox::math::position(objectInstance->pose.objectPoseGlobal);
- object.orientation = simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+ const ObjectID objectID = ObjectID::FromString(objectInstance->classID.entityName);
+
+ auto it = objects.find(objectID);
+ if (it == objects.end() or objectInstance->pose.timestamp > it->second.timestamp)
+ {
+ StampedSceneObject& stamped = objects[objectID];
+ stamped.timestamp = objectInstance->pose.timestamp;
+
+ SceneObject& object = stamped.object;
+ object.className = objectID.getClassID().str();
+ object.instanceName = objectID.instanceName();
+ object.collection = "";
+
+ object.position = simox::math::position(objectInstance->pose.objectPoseGlobal);
+ object.orientation = simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+
+ object.isStatic = objectInstance->pose.isStatic;
+ object.jointValues = objectInstance->pose.objectJointValues;
+ }
}
}
- catch (const armem::error::ArMemError& e)
- {
- ARMARX_WARNING_S << e.what();
- }
});
+ armarx::objects::Scene scene;
+ for (const auto& [id, stamped] : objects)
+ {
+ scene.objects.emplace_back(std::move(stamped.object));
+ }
return scene;
}
@@ -1041,25 +1145,40 @@ namespace armarx::armem::server::obj::instance
void Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
{
- ARMARX_INFO << "Loading scene snapshot '" << filename << "' ...";
- if (auto path = resolveSceneFilename(filename))
+ std::stringstream ss;
+ ss << "Loading scene '" << filename << "' ...";
+ if (std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
{
+ ARMARX_INFO << ss.str() << "\nfrom " << path.value();
+
if (const auto snapshot = loadScene(path.value()))
{
- std::filesystem::path filename = path->filename();
- filename.replace_extension(); // Removes extension
+ auto makeSceneName = [](const std::filesystem::path& path)
+ {
+ std::filesystem::path filename = path.filename();
+ filename.replace_extension(); // Removes extension
+ return filename.string();
+ };
+ std::string sceneName = makeSceneName(path.value());
// The check seems useless?
if (lockMemory)
{
- commitSceneSnapshot(snapshot.value(), filename.string());
+ segmentPtr->doLocked([this,&snapshot, &sceneName]()
+ {
+ commitSceneSnapshot(snapshot.value(), sceneName);
+ });
}
else
{
- commitSceneSnapshot(snapshot.value(), filename.string());
+ commitSceneSnapshot(snapshot.value(), sceneName);
}
}
}
+ else
+ {
+ ARMARX_INFO << ss.str() << " failed: Could not resolve scene name or path.";
+ }
}
@@ -1122,7 +1241,8 @@ namespace armarx::armem::server::obj::instance
segment.storeScene(storeLoadLine.getValue(), scene);
}
- if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+ if (infiniteHistory.hasValueChanged()
+ || maxHistorySize.hasValueChanged()
|| discardSnapshotsWhileAttached.hasValueChanged())
{
segment.doLocked([this, &segment]()
@@ -1184,20 +1304,21 @@ namespace armarx::armem::server::obj::instance
// Lookup in cache.
if (auto it = loaded.find(robotName); it != loaded.end())
{
+ ARMARX_CHECK_NOT_NULL(it->second);
return it->second;
}
else
{
// Try to fetch the robot.
ARMARX_CHECK_NOT_NULL(reader);
- bool warnings = false;
VirtualRobot::RobotPtr robot = reader->getRobot(
robotName, Clock::Now(),
- VirtualRobot::RobotIO::RobotDescription::eStructure, warnings);
- reader->synchronizeRobot(*robot, Clock::Now());
- // Store robot if valid.
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
+
if (robot)
{
+ ARMARX_CHECK(reader->synchronizeRobot(*robot, Clock::Now()));
+ // Store robot if valid.
loaded.emplace(robotName, robot);
}
return robot; // valid or nullptr
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 08163aee236405e1da5c3f99ef748f6e4cdc3e48..c5e62d975ca161773c9ae4e8b7d7f5352f0f1922 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -47,6 +47,15 @@ namespace armarx::armem::server::obj::instance
using ObjectPoseSeq = objpose::ObjectPoseSeq;
using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
+ struct Calibration
+ {
+ std::string robotName = "";
+ std::string robotNode = "Neck_2_Pitch";
+ float offset = 0.0f;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
+ };
+
public:
@@ -63,6 +72,7 @@ namespace armarx::armem::server::obj::instance
CommitStats commitObjectPoses(
const std::string& providerName,
const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ const Calibration& calibration,
std::optional<Time> discardUpdatesUntil = std::nullopt);
void commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName = "");
@@ -150,7 +160,7 @@ namespace armarx::armem::server::obj::instance
void storeScene(const std::string& filename, const armarx::objects::Scene& scene);
std::optional<armarx::objects::Scene> loadScene(const std::string& filename);
std::optional<armarx::objects::Scene> loadScene(const std::filesystem::path& path);
- std::optional<std::filesystem::path> resolveSceneFilename(const std::string& filename);
+ std::optional<std::filesystem::path> resolveSceneFilepath(const std::string& filename);
armarx::objects::Scene getSceneSnapshot() const;
void commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName);
@@ -190,7 +200,7 @@ namespace armarx::armem::server::obj::instance
/// Package containing the scene snapshots
std::string sceneSnapshotsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
std::string sceneSnapshotsDirectory = "scenes";
- std::string sceneSnapshotToLoad = "";
+ std::string sceneSnapshotsToLoad = "";
};
Properties p;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 00456e20382f6d6ad4d573bd7fc6e1820d656433..b3ea262d12dad291a55733bfa7914104fd30a739 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -175,21 +175,9 @@ namespace armarx::armem::server::obj::instance
{
const auto timestamp = armarx::Clock::Now();
- VirtualRobot::RobotPtr calibrationRobot = segment.robots.get(calibration.robotName);
- if (calibrationRobot)
- {
- ARMARX_CHECK(segment.robots.reader->synchronizeRobot(*calibrationRobot, timestamp));
- if (calibrationRobot->hasRobotNode(calibration.robotNode))
- {
- VirtualRobot::RobotNodePtr robotNode = calibrationRobot->getRobotNode(calibration.robotNode);
- float value = robotNode->getJointValue();
- robotNode->setJointValue(value + calibration.offset);
- }
- }
-
TIMING_START(tCommitObjectPoses);
Segment::CommitStats stats =
- segment.commitObjectPoses(providerName, providedPoses, discard.updatesUntil);
+ segment.commitObjectPoses(providerName, providedPoses, calibration, discard.updatesUntil);
TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
if (debugObserver)
@@ -572,7 +560,7 @@ namespace armarx::armem::server::obj::instance
poseHistories[id] = instance::Segment::getObjectPosesInRange(
*entity,
Time::Now() - Duration::SecondsDouble(
- visu.linearPredictionTimeWindowSeconds),
+ visu.linearPredictions.timeWindowSeconds),
Time::Invalid());
}
}
@@ -606,16 +594,6 @@ namespace armarx::armem::server::obj::instance
const std::vector<PredictionEngine> SegmentAdapter::predictionEngines{{linearPredictionEngineID}};
- void SegmentAdapter::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(robotName, prefix + "robotName",
- "Name of robot whose note can be calibrated.\n"
- "If not given, the 'fallbackName' is used.");
- defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
- defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
- }
-
-
void SegmentAdapter::RemoteGui::setup(const SegmentAdapter& adapter)
{
using namespace armarx::RemoteGui::Client;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
index 26295b4513340b361c7661a75eb9f26b5688d3b9..88ab9ca34e6dd554ed536d732e00da71cb7d8ada 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
@@ -149,16 +149,7 @@ namespace armarx::armem::server::obj::instance
instance::Visu visu;
std::mutex visuMutex;
-
- struct Calibration
- {
- std::string robotName = "";
- std::string robotNode = "Neck_2_Pitch";
- float offset = 0.0f;
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
- };
- Calibration calibration;
+ Segment::Calibration calibration;
public:
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index eb7d90cd267566d10487ce4d2b390ccc63eee586..514096731b83167b902dfe4198deae82725837ed 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -35,27 +35,28 @@ namespace armarx::armem::server::obj::instance
defs->optional(objectFramesScale, prefix + "objectFramesScale",
"Scaling of object frames.");
- defs->optional(gaussiansPosition, prefix + "gaussians.position",
+ gaussians.defineProperties(defs, prefix + "gaussians.");
+
+ defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
+ "Prefer articulated object models if available.");
+
+ linearPredictions.defineProperties(defs, prefix + "predictions.linear.");
+ }
+
+
+ void Visu::Gaussians::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(position, prefix + "position",
"Enable showing pose gaussians (position part).");
- defs->optional(gaussiansPositionScale, prefix + "gaussians.positionScale",
+ defs->optional(positionScale, prefix + "positionScale",
"Scaling of pose gaussians (position part).");
- defs->optional(gaussiansOrientation, prefix + "gaussians.position",
+ defs->optional(orientation, prefix + "position",
"Enable showing pose gaussians (orientation part).");
- defs->optional(gaussiansOrientationScale, prefix + "gaussians.positionScale",
+ defs->optional(orientationScale, prefix + "positionScale",
"Scaling of pose gaussians (orientation part).");
- defs->optional(gaussiansOrientationDisplaced, prefix + "gaussians.positionDisplaced",
+ defs->optional(orientationDisplaced, prefix + "positionDisplaced",
"Displace center orientation (co)variance circle arrows along their rotation axis.");
-
- defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
- "Prefer articulated object models if available.");
-
- defs->optional(showLinearPredictions, prefix + "predictions.linear.show",
- "Show arrows linearly predicting object positions.");
- defs->optional(linearPredictionTimeOffsetSeconds, prefix + "predictions.linear.timeOffset",
- "The offset (in seconds) to the current time to make predictions for.");
- defs->optional(linearPredictionTimeWindowSeconds, prefix + "predictions.linear.timeWindow",
- "The time window (in seconds) into the past to perform the regression on.");
}
@@ -168,35 +169,41 @@ namespace armarx::armem::server::obj::instance
const std::string key = id.str();
const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
+
+ auto makeObject = [&objectInfo, &id](const std::string& key)
{
- std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
+ viz::Object object(key);
+ if (objectInfo)
+ {
+ object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
+ }
+ else
+ {
+ object.fileByObjectFinder(id);
+ }
+ return object;
+ };
+ bool hasObject = false;
+ {
bool done = false;
- if (useArticulatedModels && objectInfo)
+ if (useArticulatedModels and objectInfo)
{
if (std::optional<PackageFileLocation> model = objectInfo->getArticulatedModel())
{
viz::Robot robot(key);
robot.pose(pose);
- robot.file(model->package, model->relativePath);
+ robot.file(model->package, model->package + "/" + model->relativePath);
robot.joints(objectPose.objectJointValues);
layer.add(robot);
done = true;
}
}
- if (!done)
+ if (not done)
{
- viz::Object object(key);
- object.pose(pose);
- if (objectInfo)
- {
- object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
- }
- else
- {
- object.fileByObjectFinder(id);
- }
+ viz::Object object = makeObject(key).pose(pose);
if (alphaByConfidence && objectPose.confidence < 1.0f)
{
object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
@@ -206,10 +213,11 @@ namespace armarx::armem::server::obj::instance
object.overrideColor(simox::Color::white().with_alpha(alpha));
}
layer.add(object);
+ hasObject = true;
}
}
- if (oobbs && objectPose.localOOBB)
+ if (oobbs and objectPose.localOOBB)
{
const simox::OrientedBoxf oobb = inGlobalFrame
? objectPose.oobbGlobal().value()
@@ -220,83 +228,85 @@ namespace armarx::armem::server::obj::instance
{
layer.add(viz::Pose(key + " Pose").pose(pose).scale(objectFramesScale));
}
- if (objectPose.objectPoseGlobalGaussian.has_value() and (gaussiansPosition or gaussiansOrientation))
+ if (gaussians.showAny()
+ and (inGlobalFrame ? objectPose.objectPoseGlobalGaussian.has_value() : objectPose.objectPoseRobotGaussian.has_value()))
{
- const objpose::PoseManifoldGaussian& gaussian = objectPose.objectPoseGlobalGaussian.value();
- objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getPositionEllipsoid();
+ gaussians.draw(layer, objectPose, inGlobalFrame);
+ }
+ if (linearPredictions.showAny() and hasObject)
+ {
+ linearPredictions.draw(layer, makeObject, objectPose, poseHistory, inGlobalFrame);
+ }
+ }
- if (gaussiansPosition)
- {
- layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
- .position(ellipsoid.center)
- .orientation(ellipsoid.orientation)
- .axisLengths(gaussiansPositionScale * ellipsoid.size)
- .color(viz::Color::azure(255, 32))
- );
+ bool Visu::Gaussians::showAny() const
+ {
+ return position or orientation;
+ }
- if (false) // Arrows can be visualized for debugging.
- {
- for (int i = 0; i < 3; ++i)
- {
- Eigen::Vector3i color = Eigen::Vector3i::Zero();
- color(i) = 255;
-
- layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
- .fromTo(ellipsoid.center,
- ellipsoid.center + gaussiansPositionScale * ellipsoid.size(i)
- * ellipsoid.orientation.col(i)
- )
- .width(5)
- .color(simox::Color(color))
- );
- }
- }
- }
- if (gaussiansOrientation)
+ void Visu::Gaussians::draw(
+ viz::Layer& layer,
+ const objpose::ObjectPose& objectPose,
+ bool inGlobalFrame) const
+ {
+ const std::string key = objectPose.objectID.str();
+
+ const objpose::PoseManifoldGaussian& gaussian =
+ inGlobalFrame
+ ? objectPose.objectPoseGlobalGaussian.value()
+ : objectPose.objectPoseRobotGaussian.value()
+ ;
+ objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getPositionEllipsoid();
+
+ if (position)
+ {
+ layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
+ .position(ellipsoid.center)
+ .orientation(ellipsoid.orientation)
+ .axisLengths(positionScale * ellipsoid.size)
+ .color(viz::Color::azure(255, 32))
+ );
+
+ if (false) // Arrows can be visualized for debugging.
{
- const float pi = static_cast<float>(M_PI);
for (int i = 0; i < 3; ++i)
{
- const bool global = true;
- Eigen::AngleAxisf rot = gaussian.getScaledRotationAxis(i, global);
-
- Eigen::Vector4i color = Eigen::Vector4i::Zero();
+ Eigen::Vector3i color = Eigen::Vector3i::Zero();
color(i) = 255;
- color(3) = 64;
-
- layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
- .position(gaussiansOrientationDisplaced
- ? ellipsoid.center + gaussiansOrientationScale * rot.axis()
- : ellipsoid.center)
- .normal(rot.axis())
- .radius(gaussiansOrientationScale)
- .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
- .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
+
+ layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
+ .fromTo(ellipsoid.center,
+ ellipsoid.center + positionScale * ellipsoid.size(i)
+ * ellipsoid.orientation.col(i)
+ )
+ .width(5)
.color(simox::Color(color))
);
}
}
}
- if (showLinearPredictions)
+ if (orientation)
{
- auto predictionResult = objpose::predictObjectPoseLinear(
- poseHistory,
- Time::Now() + Duration::SecondsDouble(linearPredictionTimeOffsetSeconds),
- objectPose);
- if (predictionResult.success)
+ const float pi = static_cast<float>(M_PI);
+ for (int i = 0; i < 3; ++i)
{
- auto predictedPose =
- armarx::fromIce<objpose::ObjectPose>(predictionResult.prediction);
- Eigen::Vector3f predictedPosition = simox::math::position(
- inGlobalFrame ? predictedPose.objectPoseGlobal : predictedPose.objectPoseRobot);
- layer.add(viz::Arrow(key + " Linear Prediction")
- .fromTo(simox::math::position(pose), predictedPosition)
- .color(viz::Color::blue()));
- }
- else
- {
- ARMARX_INFO << "Linear prediction for visualization failed: "
- << predictionResult.errorMessage;
+ const bool global = true;
+ Eigen::AngleAxisf rot = gaussian.getScaledRotationAxis(i, global);
+
+ Eigen::Vector4i color = Eigen::Vector4i::Zero();
+ color(i) = 255;
+ color(3) = 64;
+
+ layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
+ .position(orientationDisplaced
+ ? ellipsoid.center + orientationScale * rot.axis()
+ : ellipsoid.center)
+ .normal(rot.axis())
+ .radius(orientationScale)
+ .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
+ .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
+ .color(simox::Color(color))
+ );
}
}
}
@@ -324,12 +334,12 @@ namespace armarx::armem::server::obj::instance
objectFrames.setValue(visu.objectFrames);
initScale(objectFramesScale, visu.objectFramesScale, 10);
- gaussians.position.setValue(visu.gaussiansPosition);
- initScale(gaussians.positionScale, visu.gaussiansPositionScale, 10);
+ gaussians.position.setValue(visu.gaussians.position);
+ initScale(gaussians.positionScale, visu.gaussians.positionScale, 10);
- gaussians.orientation.setValue(visu.gaussiansOrientation);
- initScale(gaussians.orientationScale, visu.gaussiansOrientationScale, 0.5);
- gaussians.orientationDisplaced.setValue(visu.gaussiansOrientationDisplaced);
+ gaussians.orientation.setValue(visu.gaussians.orientation);
+ initScale(gaussians.orientationScale, visu.gaussians.orientationScale, 0.5);
+ gaussians.orientationDisplaced.setValue(visu.gaussians.orientationDisplaced);
useArticulatedModels.setValue(visu.useArticulatedModels);
@@ -358,7 +368,7 @@ namespace armarx::armem::server::obj::instance
grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
row++;
- linearPredictions.setup(visu);
+ linearPredictions.setup(visu.linearPredictions);
grid.add(linearPredictions.group, {row, 0}, {1, 4});
row++;
@@ -390,6 +400,17 @@ namespace armarx::armem::server::obj::instance
}
+ void Visu::RemoteGui::Gaussians::update(Visu::Gaussians& data)
+ {
+ data.position = position.getValue();
+ data.positionScale = positionScale.getValue();
+
+ data.orientation = orientation.getValue();
+ data.orientationScale = orientationScale.getValue();
+ data.orientationDisplaced = orientationDisplaced.getValue();
+ }
+
+
void Visu::RemoteGui::update(Visu& visu)
{
visu.enabled = enabled.getValue();
@@ -401,56 +422,11 @@ namespace armarx::armem::server::obj::instance
visu.objectFrames = objectFrames.getValue();
visu.objectFramesScale = objectFramesScale.getValue();
- visu.gaussiansPosition = gaussians.position.getValue();
- visu.gaussiansPositionScale = gaussians.positionScale.getValue();
-
- visu.gaussiansOrientation = gaussians.orientation.getValue();
- visu.gaussiansOrientationScale = gaussians.orientationScale.getValue();
- visu.gaussiansOrientationDisplaced = gaussians.orientationDisplaced.getValue();
+ gaussians.update(visu.gaussians);
visu.useArticulatedModels = useArticulatedModels.getValue();
- linearPredictions.update(visu);
- }
-
-
- void Visu::RemoteGui::LinearPredictions::setup(const Visu& visu)
- {
- using namespace armarx::RemoteGui::Client;
-
- show.setValue(visu.showLinearPredictions);
- timeOffsetSeconds.setValue(visu.linearPredictionTimeOffsetSeconds);
- timeOffsetSeconds.setRange(-1e6, 1e6);
- timeOffsetSeconds.setSteps(2 * 2 * 1000 * 1000);
-
- timeWindowSeconds.setValue(visu.linearPredictionTimeWindowSeconds);
- timeWindowSeconds.setRange(0, 1e6);
- timeWindowSeconds.setSteps(2 * 1000 * 1000);
-
-
- GridLayout grid;
- int row = 0;
-
- grid.add(Label("Show"), {row, 0}).add(show, {row, 1});
- row++;
- grid.add(Label("Time (seconds from now):"), {row, 0})
- .add(timeOffsetSeconds, {row, 1});
- row++;
- grid.add(Label("Time Window (seconds):"), {row, 0})
- .add(timeWindowSeconds, {row, 1});
- row++;
-
- group = GroupBox();
- group.setLabel("Linear Predictions");
- group.addChild(grid);
- }
-
-
- void Visu::RemoteGui::LinearPredictions::update(Visu& visu)
- {
- visu.showLinearPredictions = show.getValue();
- visu.linearPredictionTimeOffsetSeconds = timeOffsetSeconds.getValue();
- visu.linearPredictionTimeWindowSeconds = timeWindowSeconds.getValue();
+ linearPredictions.update(visu.linearPredictions);
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index 0341f514421e10a9158a446c1cb2c4a7eb3625e6..13a27228aa1198d91d1c0db4c1f809ce93b6f5f1 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -9,6 +9,7 @@
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h>
namespace armarx
@@ -83,19 +84,28 @@ namespace armarx::armem::server::obj::instance
bool objectFrames = false;
float objectFramesScale = 1.0;
- bool gaussiansPosition = true;
- float gaussiansPositionScale = 1.0;
- bool gaussiansOrientation = false;
- float gaussiansOrientationScale = 100;
- bool gaussiansOrientationDisplaced = false;
+ struct Gaussians
+ {
+ bool position = true;
+ float positionScale = 1.0;
+ bool orientation = false;
+ float orientationScale = 100;
+ bool orientationDisplaced = false;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix);
+
+ bool showAny() const;
+ void draw(viz::Layer& layer,
+ const objpose::ObjectPose& objectPose,
+ bool inGlobalFrame) const;
+ };
+ Gaussians gaussians;
/// Prefer articulated models if available.
bool useArticulatedModels = true;
- /// Linear prediction arrows for object positions.
- bool showLinearPredictions = false;
- float linearPredictionTimeOffsetSeconds = 1;
- float linearPredictionTimeWindowSeconds = 2;
+ /// Linear predictions for objects.
+ visu::LinearPredictions linearPredictions;
SimpleRunningTask<>::pointer_type updateTask;
@@ -125,25 +135,13 @@ namespace armarx::armem::server::obj::instance
armarx::RemoteGui::Client::GroupBox group;
void setup(const Visu& data);
+ void update(Visu::Gaussians& data);
};
Gaussians gaussians;
armarx::RemoteGui::Client::CheckBox useArticulatedModels;
-
- struct LinearPredictions
- {
- armarx::RemoteGui::Client::CheckBox show;
-
- armarx::RemoteGui::Client::FloatSpinBox timeOffsetSeconds;
- armarx::RemoteGui::Client::FloatSpinBox timeWindowSeconds;
-
- armarx::RemoteGui::Client::GroupBox group;
-
- void setup(const Visu& data);
- void update(Visu& data);
- };
- LinearPredictions linearPredictions;
+ visu::LinearPredictions::RemoteGui linearPredictions;
void setup(const Visu& visu);
void update(Visu& visu);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..789b597d51ab1845ca75375bb13a7ec105dadc3d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp
@@ -0,0 +1,146 @@
+#include "LinearPredictionsVisu.h"
+
+#include <SimoxUtility/math/pose.h>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/ice_conversions.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
+
+
+namespace armarx::armem::server::obj::instance::visu
+{
+
+ void LinearPredictions::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(showGhost, prefix + "showGhost",
+ "Show ghosts at linearly predicted object poses.");
+ defs->optional(ghostAlpha, prefix + "ghostAlpha",
+ "Alpha of linear prediction ghosts.");
+ defs->optional(showFrame, prefix + "showFrame",
+ "Show frames at linearly predicted object poses.");
+ defs->optional(showArrow, prefix + "showArrow",
+ "Show arrows from current object poses to the linearly predicted ones.");
+ defs->optional(timeOffsetSeconds, prefix + "timeOffset",
+ "The offset (in seconds) to the current time to make predictions for.");
+ defs->optional(timeWindowSeconds, prefix + "timeWindow",
+ "The time window (in seconds) into the past to perform the regression on.");
+
+ }
+
+
+ bool LinearPredictions::showAny() const
+ {
+ return showGhost or showFrame or showArrow;
+ }
+
+ void LinearPredictions::draw(
+ viz::Layer& layer,
+ std::function<viz::Object(const std::string& key)> makeObjectFn,
+ const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
+ bool inGlobalFrame
+ ) const
+ {
+ const std::string key = objectPose.objectID.str();
+ const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+
+ auto predictionResult = objpose::predictObjectPoseLinear(
+ poseHistory,
+ DateTime::Now() + Duration::SecondsDouble(timeOffsetSeconds),
+ objectPose);
+ if (predictionResult.success)
+ {
+ auto predictedObjectPose =
+ armarx::fromIce<objpose::ObjectPose>(predictionResult.prediction);
+ const Eigen::Matrix4f& predictedPose =
+ inGlobalFrame ? predictedObjectPose.objectPoseGlobal : predictedObjectPose.objectPoseRobot;
+
+ if (showGhost)
+ {
+ layer.add(makeObjectFn(key + " Linear Prediction Ghost")
+ .pose(predictedPose)
+ .overrideColor(simox::Color::white().with_alpha(ghostAlpha)));
+ }
+ if (showFrame)
+ {
+ layer.add(viz::Pose(key + " Linear Prediction Pose")
+ .pose(predictedPose));
+ }
+ if (showArrow)
+ {
+ layer.add(viz::Arrow(key + " Linear Prediction Arrw")
+ .fromTo(simox::math::position(pose), simox::math::position(predictedPose))
+ .width(10)
+ .color(viz::Color::azure()));
+ }
+ }
+ else
+ {
+ ARMARX_INFO << deactivateSpam(60) << "Linear prediction for visualization failed: "
+ << predictionResult.errorMessage;
+ }
+ }
+
+
+ void LinearPredictions::RemoteGui::setup(const LinearPredictions& data)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ showGhost.setName("Show Ghost");
+ showGhost.setValue(data.showGhost);
+ showFrame.setValue(data.showFrame);
+ showArrow.setValue(data.showArrow);
+
+ ghostAlpha.setRange(0, 1);
+ ghostAlpha.setValue(0.5);
+
+ timeOffsetSeconds.setValue(data.timeOffsetSeconds);
+ timeOffsetSeconds.setRange(-1e6, 1e6);
+ timeOffsetSeconds.setSteps(2 * 2 * 1000 * 1000);
+
+ timeWindowSeconds.setValue(data.timeWindowSeconds);
+ timeWindowSeconds.setRange(0, 1e6);
+ timeWindowSeconds.setSteps(2 * 1000 * 1000);
+
+ GridLayout grid;
+ int row = 0;
+
+ HBoxLayout showBoxes(
+ {Label("Ghost"), showGhost,
+ Label(" Frame"), showFrame,
+ Label(" Arrow"), showArrow,
+ Label(" Ghost Alpha"), ghostAlpha
+ });
+ grid.add(showBoxes, {row, 0}, {1, 2});
+ row++;
+
+ grid.add(Label("Prediction time (sec from now):"), {row, 0})
+ .add(timeOffsetSeconds, {row, 1});
+ row++;
+
+ grid.add(Label("Model time window (sec before now):"), {row, 0})
+ .add(timeWindowSeconds, {row, 1});
+ row++;
+
+ group = GroupBox();
+ group.setLabel("Linear Predictions");
+ group.addChild(grid);
+ }
+
+
+ void LinearPredictions::RemoteGui::update(LinearPredictions& data)
+ {
+ data.showGhost = showGhost.getValue();
+ data.ghostAlpha = ghostAlpha.getValue();
+ data.showFrame = showFrame.getValue();
+ data.showArrow = showArrow.getValue();
+ data.timeOffsetSeconds = timeOffsetSeconds.getValue();
+ data.timeWindowSeconds = timeWindowSeconds.getValue();
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ede135125c6d44ac62bdd5ecef2fc6eeb2f353c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h
@@ -0,0 +1,59 @@
+#pragma once
+
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+
+
+namespace armarx::armem::server::obj::instance::visu
+{
+
+ /// Visualization control for linear predictions for objects.
+ struct LinearPredictions : public armarx::Logging
+ {
+ bool showGhost = false;
+ float ghostAlpha = 0.7;
+
+ bool showFrame = false;
+ bool showArrow = false;
+
+ float timeOffsetSeconds = 1;
+ float timeWindowSeconds = 2;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix);
+
+ bool showAny() const;
+ void draw(viz::Layer& layer,
+ std::function<viz::Object(const std::string& key)> makeObjectFn,
+ const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
+ bool inGlobalFrame) const;
+
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::CheckBox showGhost;
+ armarx::RemoteGui::Client::FloatSpinBox ghostAlpha;
+
+ armarx::RemoteGui::Client::CheckBox showFrame;
+ armarx::RemoteGui::Client::CheckBox showArrow;
+
+ armarx::RemoteGui::Client::FloatSpinBox timeOffsetSeconds;
+ armarx::RemoteGui::Client::FloatSpinBox timeWindowSeconds;
+
+ armarx::RemoteGui::Client::GroupBox group;
+
+ void setup(const LinearPredictions& data);
+ void update(LinearPredictions& data);
+ };
+
+ };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/types.cpp b/source/RobotAPI/libraries/armem_objects/types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..58c02ecd6d2392a4367ec2731dc0a8d8d9a1d4f7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/types.cpp
@@ -0,0 +1,64 @@
+/*
+ * 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::marker_pose_data
+ * @author Hawo Höfer ( uuujt at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "types.h"
+
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
+
+#include "aron_forward_declarations.h"
+
+
+namespace armarx::armem::marker
+{
+ Marker::Marker(const std::string& name,
+ const FramedPose& framedMarkerPose,
+ const FramedPose& framedRobotPose,
+ const FramedPose& framedRGBCameraPose,
+ const FramedPose& framedDepthCameraPose) :
+ name(name),
+ robotGlobal(framedRobotPose),
+ rgbCamera(framedRGBCameraPose),
+ depthCamera(framedDepthCameraPose),
+ markerPose(framedMarkerPose)
+ {
+ markerGlobal = getGlobalMarkerPose();
+ }
+
+ Marker::Marker(const armarx::armem::arondto::Marker& dto) :
+ name(dto.name),
+ robotGlobal(dto.robotGlobal),
+ rgbCamera(dto.rgbCamera),
+ depthCamera(dto.depthCamera),
+ markerPose(dto.markerPose)
+ {
+ markerGlobal = getGlobalMarkerPose();
+ }
+ armarx::FramedPose
+ Marker::getGlobalMarkerPose() const
+ {
+ return {robotGlobal.toEigen() * depthCamera.toEigen() * markerPose.toEigen(),
+ GlobalFrame,
+ "marker"};
+ }
+
+} // namespace armarx::armem::marker
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 5ca91ff285e5fc92d1d3523a504379d582dda8b5..79fe9ac956f65869ceb03b35db8545155c4c963c 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -23,10 +23,24 @@
#include <Eigen/Geometry>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include "aron_forward_declarations.h"
+
+
+namespace armarx::armem
+{
+ struct ObjectInstance
+ {
+ objpose::ObjectPose pose;
+ MemoryID classID;
+ MemoryID sourceID;
+ };
+}
namespace armarx::armem::attachment
{
@@ -84,15 +98,39 @@ namespace armarx::armem::attachment
armem::Time timestamp;
bool active;
-
};
-} // namespace armarx::armem::attachment
+} // namespace armarx::armem::attachment
namespace armarx::armem::articulated_object
{
using ArticulatedObjectDescription = armarx::armem::robot::RobotDescription;
- using ArticulatedObject = armarx::armem::robot::Robot;
+ using ArticulatedObject = armarx::armem::robot::Robot;
using ArticulatedObjects = armarx::armem::robot::Robots;
} // namespace armarx::armem::articulated_object
+
+
+namespace armarx::armem::marker
+{
+ class Marker
+ {
+
+ public:
+ Marker(const armarx::armem::arondto::Marker& dto);
+ Marker(const std::string& name,
+ const FramedPose& markerPose,
+ const FramedPose& robotPose,
+ const FramedPose& rgbCameraPose,
+ const FramedPose& depthCameraPose);
+
+ std::string name;
+ FramedPose robotGlobal;
+ FramedPose rgbCamera;
+ FramedPose depthCamera;
+ FramedPose markerPose;
+ FramedPose markerGlobal;
+
+ FramedPose getGlobalMarkerPose() const;
+ };
+} // namespace armarx::armem::marker
diff --git a/source/RobotAPI/libraries/armem_objects/utils.cpp b/source/RobotAPI/libraries/armem_objects/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e3025f5264ebcbc5a50de330cb229f792b6f421f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/utils.cpp
@@ -0,0 +1,45 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem_objects
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "utils.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
+
+
+namespace armarx::armem
+{
+
+ MemoryID
+ objects::reconstructObjectInstanceID(const objpose::ObjectPose& objectPose)
+ {
+ armem::MemoryID id =
+ armem::objects::instaceSegmentID.withProviderSegmentName(objectPose.providerName)
+ .withEntityName(objectPose.objectID.str())
+ .withTimestamp(objectPose.timestamp)
+ .withInstanceIndex(0);
+ return id;
+ }
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_objects/utils.h b/source/RobotAPI/libraries/armem_objects/utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc0068234ac8c55d4985d25ab17fab8040c987d7
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/utils.h
@@ -0,0 +1,36 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem_objects
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+
+
+namespace armarx::armem::objects
+{
+
+ armem::MemoryID
+ reconstructObjectInstanceID(const objpose::ObjectPose& objectPose);
+
+
+} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 345564d4b859af306acecdc82d00096c4a559351..2bc45c144b53e3900e8c7041c54d33ee0d205f56 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -40,6 +40,15 @@ namespace armarx::armem::robot
Eigen::Vector3f torque;
};
+ using ToFArray = Eigen::MatrixXf;
+
+ struct ToF
+ {
+ ToFArray array;
+ std::string frame;
+ };
+
+
struct RobotState
{
using JointMap = std::map<std::string, float>;
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index aafe0978d9d41e95b51dfabfe49d33783e2791ea..8dda90015cd030197cd4af43fd2b9d534968fc19 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -38,6 +38,7 @@ armarx_add_library(
client/localization/TransformWriter.h
aron_conversions.h
+ memory_ids.h
utils.h
SOURCES
@@ -52,6 +53,7 @@ armarx_add_library(
client/localization/TransformWriter.cpp
aron_conversions.cpp
+ memory_ids.cpp
utils.cpp
)
@@ -63,6 +65,7 @@ armarx_enable_aron_file_generation_for_target(
ARON_FILES
aron/JointState.xml
aron/Proprioception.xml
+ aron/Exteroception.xml
aron/TransformHeader.xml
aron/Transform.xml
)
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
new file mode 100644
index 0000000000000000000000000000000000000000..80f72e06059a39e60e6709a5e7acb28c63ee3395
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <SystemInclude include="Eigen/Core" />
+ <SystemInclude include="Eigen/Geometry" />
+ </CodeIncludes>
+
+ <GenerateTypes>
+
+ <Object name="armarx::armem::exteroception::arondto::ToF">
+
+ <ObjectChild key="array">
+ <Matrix rows="-1" cols="-1" type="float32" />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name="armarx::armem::arondto::Exteroception">
+
+ <ObjectChild key="iterationID">
+ <long />
+ </ObjectChild>
+
+ <ObjectChild key="tof">
+ <Dict>
+ <armarx::armem::exteroception::arondto::ToF/>
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index c7ac7670d399329bb6674dad3243a048153534f8..d5472cbde89434e346f116e6b6f461bf4f2ca34e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -234,8 +234,6 @@
</ObjectChild>
</Object>
-
-
<Object name="armarx::armem::arondto::Proprioception">
@@ -257,7 +255,6 @@
</Dict>
</ObjectChild>
-
<ObjectChild key="extraFloats">
<Dict>
<Float />
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 1c18fe5b78799b1b2f9555a33a8997e7edb60d28..3749c892d5e46e6ce0f7a848a86e8059ed70991c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -6,6 +6,7 @@
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
@@ -97,4 +98,14 @@ namespace armarx::armem
aron::toAron(dto.torque, bo.torque);
}
+
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToFArray& bo){
+ bo = dto.array;
+ }
+
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToFArray& bo){
+ dto.array = bo;
+ }
+
+
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 75c339c7fc4aad7f24b5b52befdf2e7f1451b345..f78efc200ee18205523ddf7ea5c60e8c6949bf9f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -40,6 +40,11 @@ namespace armarx::armem
struct ForceTorque;
}
+ namespace exteroception::arondto
+ {
+ struct ToF;
+ }
+
namespace arondto
{
struct Transform;
@@ -65,5 +70,8 @@ namespace armarx::armem
void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToFArray& bo);
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToFArray& bo);
+
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 611fa0e0d3f5bb931f5d64a60c70735d698c623f..eaa2e9ca07b063eaba3a71887eda2d14fa839c16 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -22,6 +22,7 @@
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
@@ -121,7 +122,7 @@ namespace armarx::armem::robot_state
const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
if (elapsedTime > syncTimeout)
{
- ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
return false;
}
@@ -135,11 +136,7 @@ namespace armarx::armem::robot_state
}
std::optional<robot::RobotDescription>
- RobotReader::queryDescription(
- const std::string& name,
- const armem::Time& timestamp,
- bool warnings
- )
+ RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
@@ -159,11 +156,8 @@ namespace armarx::armem::robot_state
if (not memoryReader)
{
- if (warnings)
- {
- ARMARX_WARNING << "Memory reader is null. Did you forget to call "
- "RobotReader::connect() in onConnectComponent()?";
- }
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
return std::nullopt;
}
@@ -182,10 +176,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- if (warnings)
- {
- ARMARX_WARNING << "query description failure" << GetHandledExceptionString();
- }
+ ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
}
return std::nullopt;
@@ -198,14 +189,14 @@ namespace armarx::armem::robot_state
const auto jointMap = queryJointState(description, timestamp);
if (not jointMap)
{
- ARMARX_WARNING << "Failed to query joint state for robot '" << description.name << "'.";
+ ARMARX_VERBOSE << "Failed to query joint state for robot '" << description.name << "'.";
return std::nullopt;
}
const auto globalPose = queryGlobalPose(description, timestamp);
if (not globalPose)
{
- ARMARX_WARNING << "Failed to query global pose for robot " << description.name;
+ ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
return std::nullopt;
}
@@ -235,7 +226,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << GetHandledExceptionString();
+ ARMARX_VERBOSE << GetHandledExceptionString();
return std::nullopt;
}
}
@@ -268,7 +259,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -276,7 +267,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << deactivateSpam(1) << "Failed to query joint state. Reason: "
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query joint state. Reason: "
<< GetHandledExceptionString();
return std::nullopt;
}
@@ -306,7 +297,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return {};
}
@@ -339,7 +330,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -352,7 +343,8 @@ namespace armarx::armem::robot_state
{
try
{
- const auto result = transformReader.getGlobalPose(description.name, constants::robotRootNodeName, timestamp);
+ const auto result = transformReader.getGlobalPose(
+ description.name, constants::robotRootNodeName, timestamp);
if (not result)
{
return std::nullopt;
@@ -362,7 +354,7 @@ namespace armarx::armem::robot_state
}
catch (...)
{
- ARMARX_WARNING << deactivateSpam(1) << "Failed to query global pose. Reason: "
+ ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query global pose. Reason: "
<< GetHandledExceptionString();
return std::nullopt;
}
@@ -381,7 +373,7 @@ namespace armarx::armem::robot_state
if (providerSegment.empty())
{
- ARMARX_WARNING << "No entity found";
+ ARMARX_VERBOSE << "No entity found";
return std::nullopt;
}
@@ -394,9 +386,10 @@ namespace armarx::armem::robot_state
instance = &i;
return false; // break
});
- if (!instance)
+
+ if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found";
+ ARMARX_VERBOSE << "No entity snapshots found";
return std::nullopt;
}
@@ -436,6 +429,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&jointMap](const wm::Entity& entity)
{
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -525,7 +523,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -558,7 +556,7 @@ namespace armarx::armem::robot_state
if (not qResult.success) /* c++20 [[unlikely]] */
{
- ARMARX_WARNING << qResult.errorMessage;
+ ARMARX_VERBOSE << qResult.errorMessage;
return std::nullopt;
}
@@ -566,6 +564,37 @@ namespace armarx::armem::robot_state
}
+ std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
+ RobotReader::queryToF(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ ARMARX_DEBUG << "Querying ToF data for robot: " << description;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(constants::exteroceptionCoreSegment)
+ .providerSegments().withName(description.name) // agent
+ .entities().all() // TODO
+ .snapshots().beforeOrAtTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << qResult.errorMessage;
+ return std::nullopt;
+ }
+
+ return getToF(qResult.memory, description.name);
+ }
+
+
std::optional<robot::PlatformState>
RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
const std::string& name) const
@@ -580,6 +609,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&platformState](const wm::Entity& entity)
{
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -623,6 +657,11 @@ namespace armarx::armem::robot_state
coreSegment.forEachEntity(
[&forceTorques](const wm::Entity& entity)
{
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto proprioception =
@@ -675,6 +714,47 @@ namespace armarx::armem::robot_state
return forceTorques;
}
+ std::map<RobotReader::Hand, robot::ToFArray>
+ RobotReader::getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const
+ {
+ std::map<RobotReader::Hand, robot::ToFArray> tofs;
+
+ // clang-format off
+ const armem::wm::CoreSegment& coreSegment = memory
+ .getCoreSegment(constants::exteroceptionCoreSegment);
+ // clang-format on
+
+ coreSegment.forEachEntity(
+ [&tofs](const wm::Entity& entity)
+ {
+ ARMARX_DEBUG << "Processing ToF element";
+
+ if (not entity.getLatestSnapshot().hasInstance(0))
+ {
+ return;
+ }
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+
+ const auto exteroception =
+ tryCast<::armarx::armem::arondto::Exteroception>(entityInstance);
+ ARMARX_CHECK(exteroception.has_value());
+
+
+ for (const auto& [handName, dtoFt] : exteroception->tof)
+ {
+ ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
+
+ robot::ToFArray tof;
+ fromAron(dtoFt, tof);
+
+ const auto hand = fromHandName(handName);
+ tofs.emplace(hand, tof);
+ }
+ });
+
+ return tofs;
+ }
+
std::optional<robot::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
@@ -691,11 +771,77 @@ namespace armarx::armem::robot_state
{ instance = &i; });
if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found in provider segment `" << name << "`";
+ ARMARX_VERBOSE << "No entity snapshots found in provider segment `" << name << "`";
return std::nullopt;
}
return robot::convertRobotDescription(*instance);
}
+ std::vector<robot::RobotDescription>
+ RobotReader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ {
+ const armem::wm::CoreSegment& coreSegment =
+ memory.getCoreSegment(constants::descriptionCoreSegment);
+
+ std::vector<robot::RobotDescription> descriptions;
+
+ coreSegment.forEachInstance(
+ [&descriptions](const wm::EntityInstance& instance)
+ {
+ if (const std::optional<robot::RobotDescription> desc =
+ robot::convertRobotDescription(instance))
+ {
+ descriptions.push_back(desc.value());
+ }
+ });
+
+ return descriptions;
+ }
+
+ std::vector<robot::RobotDescription>
+ RobotReader::queryDescriptions(const armem::Time& timestamp)
+ {
+ const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
+
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(constants::descriptionCoreSegment)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().beforeOrAtTime(sanitizedTimestamp);
+ // clang-format on
+
+ ARMARX_DEBUG << "Lookup query in reader";
+
+ if (not memoryReader)
+ {
+ ARMARX_WARNING << "Memory reader is null. Did you forget to call "
+ "RobotReader::connect() in onConnectComponent()?";
+ return {};
+ }
+
+ try
+ {
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getRobotDescriptions(qResult.memory);
+ }
+ catch (...)
+ {
+ ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
+ }
+
+ return {};
+ }
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index bb63c714180325961c434d61e4a8e31b78a84045..a9cddc6b2afd5258271f5718b4f1c82d0dd6a995 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -23,6 +23,7 @@
#include <mutex>
#include <optional>
+#include <vector>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
@@ -50,7 +51,7 @@ namespace armarx::armem::robot_state
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
+ [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
std::optional<robot::Robot> get(const std::string& name,
const armem::Time& timestamp) override;
@@ -58,8 +59,9 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) override;
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp,
- bool warnings = true);
+ const armem::Time& timestamp);
+
+ std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
const armem::Time& timestamp);
@@ -100,6 +102,10 @@ namespace armarx::armem::robot_state
const armem::Time& start,
const armem::Time& end) const;
+
+ std::optional<std::map<Hand, robot::ToFArray>>
+ queryToF(const robot::RobotDescription& description, const armem::Time& timestamp) const;
+
/**
* @brief retrieve the robot's pose in the odometry frame.
*
@@ -125,6 +131,9 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotDescription>
getRobotDescription(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::vector<robot::RobotDescription>
+ getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
@@ -142,6 +151,9 @@ namespace armarx::armem::robot_state
std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
+ std::map<RobotReader::Hand, robot::ToFArray> getToF(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
+
struct Properties
{
} properties;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 726895f8350d49bc3166558f8126005997f2adb9..225e048864e69a7c349b9e53163d41ed1e0eef6b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -38,16 +38,16 @@ namespace armarx::armem::robot_state
bool
VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp)
{
- const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
+ // const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ // const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
const robot::RobotDescription robotDescription{
- .name = robot.getName(), .xml = PackagePath{package, robot.getFilename()}};
+ .name = robot.getName(), .xml = PackagePath{"", ""}};
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << "` "
+ ARMARX_VERBOSE << "Querying robot state failed for robot `" << robot.getName() << "` "
<< "(type `"<< robot.getType() << "`)!";
return false;
}
@@ -61,24 +61,22 @@ namespace armarx::armem::robot_state
VirtualRobot::RobotPtr
VirtualRobotReader::getRobot(const std::string& name,
const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode,
- bool warnings)
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
{
ARMARX_INFO << deactivateSpam(60)
<< "Querying robot description for robot '" << name << "'";
- const auto description = queryDescription(name, timestamp, warnings);
+ const auto description = queryDescription(name, timestamp);
if (not description)
{
- if (warnings)
- {
- ARMARX_WARNING << "The description of robot `" << name << "` is not a available!";
- }
+ ARMARX_VERBOSE << "The description of robot `" << name << "` is not a available!";
+
return nullptr;
}
- const std::string xmlFilename =
- ArmarXDataPath::resolvePath(description->xml.serialize().path);
+ const std::string xmlFilename = description->xml.toSystemPath();
+ ARMARX_CHECK(std::filesystem::exists(xmlFilename)) << xmlFilename;
+
ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '"
<< xmlFilename << "'";
@@ -133,7 +131,7 @@ namespace armarx::armem::robot_state
Clock::WaitFor(sleepAfterFailure);
}
- ARMARX_WARNING << "Failed to get synchronized robot `" << name << "`";
+ ARMARX_VERBOSE << "Failed to get synchronized robot `" << name << "`";
return nullptr;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index d19e0483fdfa04f00579ba85b1e830f58feee241..ab30cbc1317d98f802dc24cbe5d844da02b038e9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -27,7 +27,6 @@
#include "RobotReader.h"
-
namespace armarx::armem::robot_state
{
/**
@@ -47,14 +46,14 @@ namespace armarx::armem::robot_state
void connect();
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- bool synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp);
+ [[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
+ const armem::Time& timestamp);
[[nodiscard]] VirtualRobot::RobotPtr
getRobot(const std::string& name,
const armem::Time& timestamp = armem::Time::Invalid(),
const VirtualRobot::RobotIO::RobotDescription& loadMode =
- VirtualRobot::RobotIO::RobotDescription::eStructure,
- bool warnings = true);
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
[[nodiscard]] VirtualRobot::RobotPtr
getSynchronizedRobot(const std::string& name,
@@ -71,14 +70,12 @@ namespace armarx::armem::robot_state
private:
-
[[nodiscard]] VirtualRobot::RobotPtr
_getSynchronizedRobot(const std::string& name,
const armem::Time& timestamp = armem::Time::Invalid(),
const VirtualRobot::RobotIO::RobotDescription& loadMode =
- VirtualRobot::RobotIO::RobotDescription::eStructure,
+ VirtualRobot::RobotIO::RobotDescription::eStructure,
bool blocking = true);
-
};
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
index 15f2e8776698f9078b6b0535c1b6282476802589..47519748cad8fe1c5a0ca37db35f215e0359197b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.cpp
@@ -47,7 +47,7 @@ namespace armarx::armem::robot_state
PackagePath
resolvePackagePath(const std::string& filename)
{
- const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
PackagePath packagePath(armarx::ArmarXDataPath::getProject(packages, filename), filename);
return packagePath;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h b/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
index d9efcbcf795b46ddb4e9647919d38c0c617c1234..5d093c195addda7c64c378d919d38f09987cdfc9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
@@ -28,6 +28,7 @@ namespace armarx::armem::robot_state::constants
inline const std::string descriptionCoreSegment = "Description";
inline const std::string localizationCoreSegment = "Localization";
inline const std::string proprioceptionCoreSegment = "Proprioception";
+ inline const std::string exteroceptionCoreSegment = "Exteroception";
inline const std::string descriptionEntityName = "description";
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index fda00f26806d70a922b9487a008099117150d9e4..529cd6d28a198d14779682d27826cb4d6b0f3cd4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -1,4 +1,5 @@
#include "TransformHelper.h"
+#include <optional>
#include <string>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
@@ -48,12 +49,23 @@ namespace armarx::armem::common::robot_state::localization
const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
- const armem::Time sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+ const std::optional<armem::Time> sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+
+ if(not sanitizedTimestamp.has_value())
+ {
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::Error,
+ .errorMessage = "Error: Issue with timestamp"};
+ }
+
+
auto header = query.header;
+ ARMARX_CHECK(sanitizedTimestamp.has_value());
+
// ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
- header.timestamp = sanitizedTimestamp;
+ header.timestamp = sanitizedTimestamp.value();
if (transforms.empty())
{
@@ -178,27 +190,43 @@ namespace armarx::armem::common::robot_state::localization
}
template <class ...Args>
- armarx::core::time::DateTime
+ std::optional<armarx::core::time::DateTime>
TransformHelper::_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp)
{
// first we check which the newest timestamp is
- int64_t timeSinceEpochUs = 0;
+ std::optional<int64_t> timeSinceEpochUs = std::nullopt;
localizationCoreSegment.forEachEntity([&timeSinceEpochUs, ×tamp](const auto& entity){
auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
+
+ if(snapshot == nullptr)
+ {
+ return;
+ }
+
+ if(not snapshot->hasInstance(0))
+ {
+ return;
+ }
+
const armem::wm::EntityInstance& item = snapshot->getInstance(0);
const auto tf = _convertEntityToTransform(item);
const auto& dataTs = tf.header.timestamp;
- timeSinceEpochUs = std::max(timeSinceEpochUs, dataTs.toMicroSecondsSinceEpoch());
+ timeSinceEpochUs = std::max(timeSinceEpochUs.value_or(0), dataTs.toMicroSecondsSinceEpoch());
});
+ if(not timeSinceEpochUs.has_value())
+ {
+ return std::nullopt;
+ }
+
// then we ensure that the timestamp is not more recent than the query timestamp
- timeSinceEpochUs = std::min(timeSinceEpochUs, timestamp.toMicroSecondsSinceEpoch());
+ timeSinceEpochUs = std::min(timeSinceEpochUs.value(), timestamp.toMicroSecondsSinceEpoch());
- return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs));
+ return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
}
@@ -234,11 +262,15 @@ namespace armarx::armem::common::robot_state::localization
}
catch (const ::armarx::exceptions::local::ExpressionException& ex)
{
- ARMARX_WARNING << "local exception: " << ex.what();
+ ARMARX_WARNING << "Local expression exception: " << ex.what();
+ }
+ catch (const ::armarx::LocalException& ex)
+ {
+ ARMARX_WARNING << "Local exception: " << ex.what();
}
catch (...)
{
- ARMARX_WARNING << "Error: " << GetHandledExceptionString();
+ ARMARX_WARNING << "Unexpected error: " << GetHandledExceptionString();
}
return {};
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
index 6e2611f7a178ba64ab3e25082b5e74a6b968dc23..80cdea39921497a5c2c369416d59115d7dffc4c5 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -22,6 +22,7 @@
#pragma once
#include <vector>
+#include <optional>
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -113,7 +114,7 @@ namespace armarx::armem::common::robot_state::localization
template <class ...Args>
static
- armarx::core::time::DateTime
+ std::optional<armarx::core::time::DateTime>
_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp);
static
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2f219eff39376ed3ecc44e47b0780fd1328785a0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
@@ -0,0 +1,37 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem_robot_state
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "memory_ids.h"
+
+
+namespace armarx::armem
+{
+
+ const MemoryID robot_state::memoryID { "RobotState" };
+
+ const MemoryID robot_state::descriptionSegmentID { memoryID.withCoreSegmentName("Description") };
+ const MemoryID robot_state::proprioceptionSegmentID { memoryID.withCoreSegmentName("Proprioception") };
+ const MemoryID robot_state::exteroceptionSegmentID { memoryID.withCoreSegmentName("Exteroception") };
+ const MemoryID robot_state::localizationSegmentID { memoryID.withCoreSegmentName("Localization") };
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
new file mode 100644
index 0000000000000000000000000000000000000000..f653236b1f62ec8aef905d45a7ee86af079b05b9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
@@ -0,0 +1,38 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::armem_robot_state
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::armem::robot_state
+{
+
+ extern const MemoryID memoryID;
+
+ extern const MemoryID descriptionSegmentID;
+ extern const MemoryID proprioceptionSegmentID;
+ extern const MemoryID exteroceptionSegmentID;
+ extern const MemoryID localizationSegmentID;
+
+} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 848efe75e0b615f367f5602c9cdfdfb3cdbbd7ea..3ad1d50684d89c246e6123a6ef5d2055779eab6f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -7,7 +7,7 @@ armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-
+armarx_build_if(manif_FOUND "manif not available")
armarx_add_library(
LIBS
@@ -27,6 +27,7 @@ armarx_add_library(
# System / External
Eigen3::Eigen
+ ${manif_LIBRARIES}
HEADERS
forward_declarations.h
@@ -42,10 +43,16 @@ armarx_add_library(
proprioception/RobotUnitData.h
proprioception/RobotUnitReader.h
+ proprioception/converters/ConverterInterface.h
proprioception/converters/Armar6Converter.h
proprioception/converters/ConverterTools.h
proprioception/converters/ConverterRegistry.h
- proprioception/converters/ConverterInterface.h
+
+ exteroception/converters/ConverterInterface.h
+ exteroception/converters/ConverterTools.h
+ exteroception/converters/ConverterRegistry.h
+ exteroception/converters/ArmarDEConverter.h
+ exteroception/Segment.h
description/Segment.h
@@ -66,8 +73,17 @@ armarx_add_library(
proprioception/converters/ConverterRegistry.cpp
proprioception/converters/ConverterInterface.cpp
+ exteroception/converters/ConverterInterface.cpp
+ exteroception/converters/ConverterTools.cpp
+ exteroception/converters/ConverterRegistry.cpp
+ exteroception/converters/ArmarDEConverter.cpp
+ exteroception/Segment.cpp
+
description/Segment.cpp
)
+if(manif_FOUND)
+ target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
+endif()
add_library(RobotAPI::armem_robot_state_server ALIAS armem_robot_state_server)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index e68119affae5664f232dba25c679a8cce9fbdd10..b8b3190d1bae586226c60b490bb399abe5799272 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -42,8 +42,8 @@ namespace armarx::armem::server::robot_state
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(
- p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(p.framesEnabled, prefix + "famesEnabled", "Enable or disable visualization of frames.");
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
}
@@ -84,7 +84,7 @@ namespace armarx::armem::server::robot_state
// clang-format off
viz::Robot robotVisu = viz::Robot(robot.description.name)
- .file(xmlPath.package, xmlPath.path)
+ .file(xmlPath.package, xmlPath.package + "/" + xmlPath.path)
.joints(robot.config.jointMap)
.pose(robot.config.globalPose);
@@ -110,6 +110,8 @@ namespace armarx::armem::server::robot_state
Eigen::Affine3f from = pose;
Eigen::Affine3f to = pose * frame;
+ pose = to;
+
layerFrames.add(viz::Arrow(robotName + std::to_string(++i)).fromTo(from.translation(), to.translation()));
}
}
@@ -199,10 +201,15 @@ namespace armarx::armem::server::robot_state
ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
viz::Layer layer = arviz.layer("Robots");
visualizeRobots(layer, robots);
+ std::vector layers{layer};
ARMARX_DEBUG << "Visualize frames ...";
- viz::Layer layerFrames = arviz.layer("Frames");
- visualizeFrames(layerFrames, frames);
+ if(p.framesEnabled)
+ {
+ viz::Layer layerFrames = arviz.layer("Frames");
+ visualizeFrames(layerFrames, frames);
+ layers.push_back(layerFrames);
+ }
TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
@@ -211,7 +218,7 @@ namespace armarx::armem::server::robot_state
ARMARX_DEBUG << "Commit visualization ...";
TIMING_START(tVisuCommit);
- arviz.commit({layer, layerFrames});
+ arviz.commit(layers);
TIMING_END_STREAM(tVisuCommit, ARMARX_DEBUG);
TIMING_END_STREAM(tVisuTotal, ARMARX_DEBUG);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index ce69772c1b1c55553c62e39d3db5e8e4503d2592..66222e0cf8dc6cf6043e6db35ebb7ba00e0cab00 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -84,6 +84,7 @@ namespace armarx::armem::server::robot_state
struct Properties
{
bool enabled = true;
+ bool framesEnabled = false;
float frequencyHz = 25;
} p;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index f6cba1d8c22086497f7eb537c6a3e16fa2e2faf4..af2381c78d334e62ea39eb22978492ad66a32f1a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -1,4 +1,6 @@
#include "Segment.h"
+#include <filesystem>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -20,13 +22,14 @@
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
namespace armarx::armem::server::robot_state::description
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- Base(memoryToIceAdapter, "Description", arondto::RobotDescription::ToAronType())
+ Base(memoryToIceAdapter, armem::robot_state::descriptionSegmentID.coreSegmentName, arondto::RobotDescription::ToAronType())
{
}
@@ -47,7 +50,11 @@ namespace armarx::armem::server::robot_state::description
const Time now = Time::Now();
const MemoryID providerID = segmentPtr->id().withProviderSegmentName(robotDescription.name);
- segmentPtr->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::ToAronType());
+ if (not segmentPtr->hasProviderSegment(providerID.providerSegmentName))
+ {
+ segmentPtr->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::ToAronType());
+ }
+
EntityUpdate update;
update.entityID = providerID.withEntityName("description");
@@ -78,13 +85,29 @@ namespace armarx::armem::server::robot_state::description
const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
- ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
+ // make sure that the relative path is without the 'package/' prefix
+ const std::string robotFileRelPath = [&robotFilename, &package]()-> std::string {
+
+ if(simox::alg::starts_with(robotFilename, package))
+ {
+ // remove "package" + "/"
+ const std::string fixedRobotFilename = robotFilename.substr(package.size() + 1, -1);
+ return fixedRobotFilename;
+ }
+
+ return robotFilename;
+ }();
+
+ ARMARX_INFO << "Robot description '" << VAROUT(robotFileRelPath) << "' found in package " << package;
const robot::RobotDescription robotDescription
{
.name = kinematicUnit->getRobotName(),
- .xml = {package, kinematicUnit->getRobotFilename()}
- }; // FIXME
+ .xml = {package, robotFileRelPath}
+ };
+
+ // make sure that the package path is valid
+ ARMARX_CHECK(std::filesystem::exists(robotDescription.xml.toSystemPath()));
commitRobotDescription(robotDescription);
}
@@ -128,7 +151,7 @@ namespace armarx::armem::server::robot_state::description
}
});
- ARMARX_INFO << deactivateSpam(60) << "Number of known robot descriptions: " << robotDescriptions.size();
+ ARMARX_VERBOSE << deactivateSpam(60) << "Number of known robot descriptions: " << robotDescriptions.size();
return robotDescriptions;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..63f24d0f46a156adc023b3b64b4f27d344b151c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
@@ -0,0 +1,50 @@
+#include "Segment.h"
+
+#include <filesystem>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter,
+ armem::robot_state::exteroceptionSegmentID.coreSegmentName,
+ arondto::Exteroception::ToAronType())
+ {
+ }
+
+ Segment::~Segment() = default;
+
+
+ // void
+ // Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
+ // {
+ // robotUnit = robotUnitPrx;
+
+ // // store the robot description linked to the robot unit in this segment
+ // }
+
+
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8471563d640800a44d4516d6eccdd7d7e355ccd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
@@ -0,0 +1,55 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+#include <string>
+#include <unordered_map>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ class Segment : public segment::SpecializedCoreSegment
+ {
+ using Base = segment::SpecializedCoreSegment;
+
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ ~Segment() override;
+
+
+ // void onConnect(const RobotUnitInterfacePrx& robotUnitPrx);
+
+
+ private:
+ // RobotUnitInterfacePrx robotUnit;
+ };
+
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d71bb045b32028df15bb37150892175e1ec8ec72
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
@@ -0,0 +1,130 @@
+#include "ArmarDEConverter.h"
+#include <cmath>
+#include <string>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
+
+#include "ConverterTools.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ArmarDEConverter::ArmarDEConverter() = default;
+ ArmarDEConverter::~ArmarDEConverter() = default;
+
+
+ aron::data::DictPtr
+ ArmarDEConverter::convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description)
+ {
+ arondto::Exteroception dto;
+ dto.iterationID = data.iterationId;
+
+ for (const auto& [dataEntryName, dataEntry] : description.entries)
+ {
+ process(dto, dataEntryName, {data, dataEntry});
+ }
+
+ // resize to square
+ for(auto& [_, tof] : dto.tof)
+ {
+ const int sr = std::sqrt(tof.array.size());
+ const bool isSquare = (sr * sr == tof.array.size());
+
+ if(tof.array.size() > 0 and isSquare)
+ {
+ tof.array.resize(sr, sr);
+ }
+ }
+
+
+ return dto.toAron();
+ }
+
+
+ void ArmarDEConverter::process(
+ arondto::Exteroception& dto,
+ const std::string& entryName,
+ const ConverterValue& value)
+ {
+ const std::vector<std::string> split = simox::alg::split(entryName, ".", false, false);
+ ARMARX_CHECK_GREATER_EQUAL(split.size(), 3);
+ const std::set<size_t> acceptedSizes{3, 4, 5};
+ ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
+ << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
+ << "\n- split: '" << split << "'";
+
+ const std::string& category = split.at(0);
+ // const std::string& name = split.at(1);
+ const std::string& field = split.at(2);
+ ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << entryName;
+
+ if (simox::alg::contains(field, "tofDepth"))
+ {
+ // ARMARX_DEBUG << "Processing ToF sensor data";
+ processToFEntry(dto.tof, split, value);
+ }
+
+ }
+
+
+
+
+ void ArmarDEConverter::processToFEntry(
+ std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
+
+ ARMARX_CHECK_EQUAL(split.size(), 4);
+ ARMARX_CHECK_EQUAL(split.at(2), "tofDepth");
+
+ const std::string& name = split.at(1);
+
+ // element 0 sens
+ // element 1 is either LeftHand or RightHand
+
+ const std::map<std::string, std::string> sidePrefixMap
+ {
+ {"LeftHand", "Left"},
+ {"RightHand", "Right"}
+ };
+
+ auto it = sidePrefixMap.find(name);
+ ARMARX_CHECK(it != sidePrefixMap.end()) << name;
+
+ const std::string& side = it->second;
+ processToFEntry(tofs[side], split, value);
+ }
+
+ void ArmarDEConverter::processToFEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ // split, e.g., element_12
+ const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
+ ARMARX_CHECK_EQUAL(elements.size(), 2);
+
+ const int idx = std::stoi(elements.at(1));
+
+ if(tof.array.size() < (idx +1))
+ {
+ tof.array.resize(idx+1, 1);
+ }
+
+ tof.array(idx) = getValueAs<float>(value);
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf6c482a06bfd4be24fca5ad78a320b5b6834042
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ struct ConverterValue;
+ // class ConverterTools;
+
+
+ class ArmarDEConverter : public ConverterInterface
+ {
+ public:
+ ArmarDEConverter();
+ ~ArmarDEConverter() override;
+
+
+ aron::data::DictPtr
+ convert(const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) override;
+
+
+ protected:
+ void process(arondto::Exteroception& dtoExteroception,
+ const std::string& entryName,
+ const ConverterValue& value);
+
+
+ private:
+
+
+ void processToFEntry(std::map<std::string, armarx::armem::exteroception::arondto::ToF>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ void processToFEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+
+ // std::unique_ptr<ConverterTools> tools;
+ };
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ba33e6c5f308cd6213c8ccf02018ad63ba9440b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
@@ -0,0 +1,11 @@
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ConverterInterface::~ConverterInterface()
+ {
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ba0ecc3c8aeaf5c2b3f3c03d9f36ed83269fb5e
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <memory>
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::RobotUnitDataStreaming
+{
+ struct TimeStep;
+ struct DataStreamingDescription;
+ struct DataEntry;
+}
+namespace armarx::armem::arondto
+{
+ class Proprioception;
+}
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ class ConverterInterface
+ {
+ public:
+
+ virtual ~ConverterInterface();
+
+ virtual
+ aron::data::DictPtr convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ef7296b669504e0f8d0aff8b4cd3991fe6ec10b3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
@@ -0,0 +1,31 @@
+#include "ConverterRegistry.h"
+
+#include "ArmarDEConverter.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ConverterRegistry::ConverterRegistry()
+ {
+ add<ArmarDEConverter>("ArmarDE");
+ // add<Armar7Converter>("Armar7");
+ }
+
+
+ ConverterInterface*
+ ConverterRegistry::get(const std::string& key) const
+ {
+ auto it = converters.find(key);
+ return it != converters.end() ? it->second.get() : nullptr;
+ }
+
+
+ std::vector<std::string> ConverterRegistry::getKeys() const
+ {
+ return simox::alg::get_keys(converters);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e1cac9a18eb3d901b0ca0f61eccf08395a279b5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ class ConverterRegistry
+ {
+ public:
+
+ ConverterRegistry();
+
+
+ template <class ConverterT, class ...Args>
+ void add(const std::string& key, Args... args)
+ {
+ converters[key].reset(new ConverterT(args...));
+ }
+
+
+ ConverterInterface* get(const std::string& key) const;
+ std::vector<std::string> getKeys() const;
+
+
+ private:
+
+ std::map<std::string, std::unique_ptr<ConverterInterface>> converters;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6086a9aee5f2adb8d997a65302084e0c902d747c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
@@ -0,0 +1,179 @@
+#include "ConverterTools.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ std::optional<std::string>
+ exteroception::findByPrefix(const std::string& key, const std::set<std::string>& prefixes)
+ {
+ for (const auto& prefix : prefixes)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return prefix;
+ }
+ }
+ return std::nullopt;
+ }
+}
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ ConverterTools::ConverterTools()
+ {
+ {
+ vector3fSetters["X"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.x() = f;
+ };
+ vector3fSetters["Y"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.y() = f;
+ };
+ vector3fSetters["Z"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.z() = f;
+ };
+ vector3fSetters["x"] = vector3fSetters["X"];
+ vector3fSetters["y"] = vector3fSetters["Y"];
+ vector3fSetters["z"] = vector3fSetters["Z"];
+ vector3fSetters["Rotation"] = vector3fSetters["Z"];
+ }
+ {
+ platformPoseGetters["acceleration"] = [](prop::arondto::Platform & p)
+ {
+ return &p.acceleration;
+ };
+ platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
+ {
+ return &p.relativePosition;
+ };
+ platformPoseGetters["velocity"] = [](prop::arondto::Platform & p)
+ {
+ return &p.velocity;
+ };
+ platformIgnored.insert("absolutePosition");
+ }
+ {
+ ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationForce;
+ };
+ ftGetters["gravCompT"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationTorque;
+ };
+ ftGetters["f"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.force;
+ };
+ ftGetters["t"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.torque;
+ };
+ }
+ {
+ jointGetters["acceleration"] = [](prop::arondto::Joints & j)
+ {
+ return &j.acceleration;
+ };
+ jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.gravityTorque;
+ };
+ jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inertiaTorque;
+ };
+ jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inverseDynamicsTorque;
+ };
+ jointGetters["position"] = [](prop::arondto::Joints & j)
+ {
+ return &j.position;
+ };
+ jointGetters["torque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.torque;
+ };
+ jointGetters["velocity"] = [](prop::arondto::Joints & j)
+ {
+ return &j.velocity;
+ };
+ }
+ {
+
+#define ADD_SCALAR_SETTER(container, name, type) \
+ container [ #name ] = []( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue & value) \
+ { \
+ dto. name [split.at(1)] = getValueAs< type >(value); \
+ }
+
+ ADD_SCALAR_SETTER(jointSetters, position, float);
+ ADD_SCALAR_SETTER(jointSetters, velocity, float);
+ ADD_SCALAR_SETTER(jointSetters, acceleration, float);
+
+ ADD_SCALAR_SETTER(jointSetters, relativePosition, float);
+ ADD_SCALAR_SETTER(jointSetters, filteredVelocity, float);
+
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, velocityTarget, float);
+
+ ADD_SCALAR_SETTER(jointSetters, torque, float);
+ ADD_SCALAR_SETTER(jointSetters, inertiaTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityCompensatedTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, inverseDynamicsTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, torqueTicks, int);
+
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+
+ // "temperature" handled below
+ // ADD_SCALAR_SETTER(jointSetters, temperature, float);
+
+ ADD_SCALAR_SETTER(jointSetters, motorCurrent, float);
+ ADD_SCALAR_SETTER(jointSetters, maxTargetCurrent, float);
+
+ ADD_SCALAR_SETTER(jointSetters, sensorBoardUpdateRate, float);
+ ADD_SCALAR_SETTER(jointSetters, I2CUpdateRate, float);
+
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEmergencyStop, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEnabled, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusError, int);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusOperation, int);
+
+
+#define ADD_VECTOR3_SETTER(container, name, type) \
+ container [ #name ] = [this]( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue & value) \
+ { \
+ auto& vec = dto. name [split.at(1)]; \
+ auto& setter = this->vector3fSetters.at(split.at(3)); \
+ setter(vec, getValueAs< type >(value)); \
+ }
+
+ ADD_VECTOR3_SETTER(jointSetters, angularVelocity, float);
+ ADD_VECTOR3_SETTER(jointSetters, linearAcceleration, float);
+
+ // ADD_GETTER(jointVectorGetters, orientation, float);
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
new file mode 100644
index 0000000000000000000000000000000000000000..7460ebb133a01387c0f29a472961366877254506
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
@@ -0,0 +1,106 @@
+#pragma once
+
+#include <map>
+#include <set>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ struct ConverterValue
+ {
+ const RobotUnitDataStreaming::TimeStep& data;
+ const RobotUnitDataStreaming::DataEntry& entry;
+ };
+
+
+ template <class T>
+ T
+ getValueAs(const ConverterValue& value)
+ {
+ return RobotUnitDataStreamingReceiver::GetAs<T>(value.data, value.entry);
+ }
+
+
+ /**
+ * @brief Search
+ * @param key
+ * @param prefixes
+ * @return
+ */
+ std::optional<std::string>
+ findByPrefix(const std::string& key, const std::set<std::string>& prefixes);
+
+
+ template <class ValueT>
+ ValueT
+ findByPrefix(const std::string& key, const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [prefix, value] : map)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+ template <class ValueT>
+ ValueT
+ findBySuffix(const std::string& key, const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [suffix, value] : map)
+ {
+ if (simox::alg::ends_with(key, suffix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+
+ class ConverterTools
+ {
+ public:
+
+ ConverterTools();
+
+
+ public:
+
+ std::map<std::string, std::function<void(Eigen::Vector3f&, float)> > vector3fSetters;
+
+ std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
+ std::map<std::string, std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)> > jointVectorGetters;
+
+ using JointSetter = std::function<void(prop::arondto::Joints& dto, const std::vector<std::string>& split, const ConverterValue& value)>;
+ std::map<std::string, JointSetter> jointSetters;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)> > platformPoseGetters;
+ std::set<std::string> platformIgnored;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)> > ftGetters;
+
+
+ std::map<std::string, std::string> sidePrefixMap
+ {
+ { "R", "Right" },
+ { "L", "Left" },
+ };
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index dc92366e7ab6eb45468bde0a046f32b888e66a6e..3fbf097937386a7d4973b55a22f76419d3f5d5eb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -3,6 +3,11 @@
// STL
#include <iterator>
+#include <manif/SE3.h>
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
+
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/core/FramedPose.h>
@@ -12,6 +17,7 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
@@ -21,13 +27,14 @@
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
namespace armarx::armem::server::robot_state::localization
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- Base(memoryToIceAdapter, "Localization", arondto::Transform::ToAronType(), 1024)
+ Base(memoryToIceAdapter, armem::robot_state::localizationSegmentID.coreSegmentName, arondto::Transform::ToAronType(), 1024)
{
}
@@ -36,6 +43,24 @@ namespace armarx::armem::server::robot_state::localization
{
}
+ void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+
+ defs->optional(properties.predictionTimeWindow,
+ "prediction.TimeWindow",
+ "Duration of time window into the past to use for predictions"
+ " when requested via the PredictingMemoryInterface (in seconds).");
+ }
+
+ void Segment::init()
+ {
+ Base::init();
+
+ segmentPtr->addPredictor(armem::PredictionEngine{.engineID = "Linear"},
+ [this](const PredictionRequest& request){ return this->predictLinear(request); });
+ }
+
void Segment::onConnect()
{
@@ -81,8 +106,8 @@ namespace armarx::armem::server::robot_state::localization
frames.emplace(robotName, result.transforms);
}
- ARMARX_INFO << deactivateSpam(60)
- << "Number of known robot pose chains: " << frames.size();
+ ARMARX_VERBOSE << deactivateSpam(60)
+ << "Number of known robot pose chains: " << frames.size();
return frames;
}
@@ -128,8 +153,8 @@ namespace armarx::armem::server::robot_state::localization
}
}
- ARMARX_INFO << deactivateSpam(60)
- << "Number of known robot poses: " << robotGlobalPoses.size();
+ ARMARX_VERBOSE << deactivateSpam(60)
+ << "Number of known robot poses: " << robotGlobalPoses.size();
return robotGlobalPoses;
}
@@ -171,4 +196,78 @@ namespace armarx::armem::server::robot_state::localization
return update;
}
+
+ PredictionResult Segment::predictLinear(const PredictionRequest& request)
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+ if (request.predictionSettings.predictionEngineID != "Linear")
+ {
+ result.success = false;
+ result.errorMessage = "Prediction engine " +
+ request.predictionSettings.predictionEngineID +
+ " is not supported in Proprioception.";
+ return result;
+ }
+
+ static const int tangentDims = 6;
+ using Vector6d = Eigen::Matrix<double, tangentDims, 1>;
+
+ const DateTime timeOrigin = DateTime::Now();
+ const armarx::Duration timeWindow =
+ Duration::SecondsDouble(properties.predictionTimeWindow);
+ SnapshotRangeInfo<Vector6d, arondto::Transform> info;
+
+ doLocked(
+ [&, this]()
+ {
+ info = getSnapshotsInRange<server::wm::CoreSegment, Vector6d, arondto::Transform>(
+ segmentPtr,
+ request.snapshotID,
+ timeOrigin - timeWindow,
+ timeOrigin,
+ [](const aron::data::DictPtr& data)
+ {
+ Eigen::Matrix4d mat =
+ arondto::Transform::FromAron(data).transform.cast<double>();
+ manif::SE3d se3(simox::math::position(mat),
+ Eigen::Quaterniond(simox::math::orientation(mat)));
+ return se3.log().coeffs();
+ },
+ [](const aron::data::DictPtr& data)
+ { return arondto::Transform::FromAron(data); });
+ });
+
+ if (info.success)
+ {
+ Eigen::Matrix4f prediction;
+ if (info.timestampsSec.size() <= 1)
+ {
+ prediction = info.latestValue.transform;
+ }
+ else
+ {
+ using simox::math::LinearRegression;
+ const bool inputOffset = false;
+ const LinearRegression<tangentDims> model = LinearRegression<tangentDims>::Fit(
+ info.timestampsSec, info.values, inputOffset);
+ const auto predictionTime = request.snapshotID.timestamp;
+ Vector6d linearPred =
+ model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ prediction = manif::SE3Tangentd(linearPred).exp().transform().cast<float>();
+ }
+
+ info.latestValue.transform = prediction;
+ result.success = true;
+ result.prediction = info.latestValue.toAron();
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage = info.errorMessage;
+ }
+
+ return result;
+ }
+
} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index ba7138fd0c8a4bbe6467ebc00c68e3e743643f29..0f1fe71345bf1d2767f5cfc7b0c268e1f41e7eba 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -49,6 +49,9 @@ namespace armarx::armem::server::robot_state::localization
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+
+ void init() override;
void onConnect();
@@ -67,6 +70,13 @@ namespace armarx::armem::server::robot_state::localization
EntityUpdate makeUpdate(const armem::robot_state::Transform& transform) const;
+ PredictionResult predictLinear(const PredictionRequest& request);
+
+ struct Properties
+ {
+ double predictionTimeWindow = 2;
+ };
+ Properties properties;
};
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 82ba252a400bf57e561dd2ba45a063a49182dbd1..847ec98c82966408545a50bf9d7fa93fc7081d9f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -29,152 +29,168 @@
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
// ArmarX
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::server::robot_state::proprioception
{
- void RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver)
+ void
+ RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin)
{
// Thread-local copy of debug observer helper.
this->debugObserver =
- DebugObserverHelper(Logging::tag.tagName, debugObserver.getDebugObserver(), true);
+ 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)
+ static float
+ toDurationMs(std::chrono::high_resolution_clock::time_point start,
+ std::chrono::high_resolution_clock::time_point end)
{
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
return duration.count() / 1000.f;
}
- void RobotStateWriter::run(
- float pollFrequency,
- std::queue<RobotUnitData>& dataQueue,
- std::mutex& dataMutex,
- MemoryToIceAdapter& memory,
- localization::Segment& localizationSegment)
+ void
+ RobotStateWriter::run(float pollFrequency,
+ Queue& dataBuffer,
+ MemoryToIceAdapter& memory,
+ localization::Segment& localizationSegment)
{
- CycleUtil cycle(static_cast<int>(1000.f / pollFrequency));
while (task and not task->isStopped())
{
- size_t queueSize = 0;
- std::queue<RobotUnitData> batch;
- {
- std::lock_guard lock{dataMutex};
- queueSize = dataQueue.size();
- if (!dataQueue.empty())
- {
- std::swap(batch, dataQueue);
- }
- }
- if (debugObserver)
- {
- debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", queueSize);
- }
- if (not batch.empty())
+
+ // if (debugObserver)
+ // {
+ // debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", dataBuffer.size());
+ // }
+
+ RobotUnitData robotUnitData;
+ if (const auto status = dataBuffer.wait_pull(robotUnitData);
+ status == boost::queue_op_status::success)
{
auto start = std::chrono::high_resolution_clock::now();
auto endBuildUpdate = start, endProprioception = start, endLocalization = start;
- Update update = buildUpdate(batch);
+ Update update = buildUpdate(robotUnitData);
endBuildUpdate = std::chrono::high_resolution_clock::now();
// Commits lock the core segments.
- // Proprioception
- armem::CommitResult result = memory.commitLocking(update.proprioception);
+ // Proprioception + Exteroception
+ armem::CommitResult resultProprioception = memory.commitLocking(update.proprioception);
+
+ ARMARX_DEBUG << deactivateSpam(1) << VAROUT(update.exteroception.updates.size());
+ armem::CommitResult resultExteroception = memory.commitLocking(update.exteroception);
endProprioception = std::chrono::high_resolution_clock::now();
// Localization
for (const armem::robot_state::Transform& transform : update.localization)
{
- localizationSegment.doLocked([&localizationSegment, &transform]()
- {
- localizationSegment.commitTransform(transform);
- });
+ localizationSegment.doLocked(
+ [&localizationSegment, &transform]()
+ { localizationSegment.commitTransform(transform); });
}
endLocalization = std::chrono::high_resolution_clock::now();
- if (not result.allSuccess())
+ if (not resultProprioception.allSuccess())
{
- ARMARX_WARNING << "Could not commit data to memory. Error message: " << result.allErrorMessages();
+ 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: "
+ << resultExteroception.allErrorMessages();
+ }
+
if (debugObserver)
{
auto end = std::chrono::high_resolution_clock::now();
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)", toDurationMs(start, end));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 1. Build Update (ms)", toDurationMs(start, endBuildUpdate));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 2. Proprioception (ms)", toDurationMs(endBuildUpdate, endProprioception));
- debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 3. Localization (ms)", toDurationMs(endProprioception, endLocalization));
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)",
+ toDurationMs(start, end));
+ debugObserver->setDebugObserverDatafield(
+ "RobotStateWriter | t Commit 1. Build Update (ms)",
+ toDurationMs(start, endBuildUpdate));
+ debugObserver->setDebugObserverDatafield(
+ "RobotStateWriter | t Commit 2. Proprioception (ms)",
+ toDurationMs(endBuildUpdate, endProprioception));
+ debugObserver->setDebugObserverDatafield(
+ "RobotStateWriter | t Commit 3. Localization (ms)",
+ toDurationMs(endProprioception, endLocalization));
}
}
+ else
+ {
+ ARMARX_WARNING << "Queue pull was not successful. "
+ << static_cast<std::underlying_type<boost::queue_op_status>::type>(
+ status);
+ }
if (debugObserver)
{
debugObserver->sendDebugObserverBatch();
}
- cycle.waitForCycleDuration();
}
}
- RobotStateWriter::Update RobotStateWriter::buildUpdate(std::queue<RobotUnitData>& dataQueue)
+ RobotStateWriter::Update
+ RobotStateWriter::buildUpdate(const RobotUnitData& data)
{
- ARMARX_CHECK_GREATER(dataQueue.size(), 0);
- ARMARX_DEBUG << "RobotStateWriter: Commit batch of " << dataQueue.size() << " timesteps to memory...";
-
// Send batch to memory
Update update;
- while (dataQueue.size() > 0)
- {
- const RobotUnitData& data = dataQueue.front();
-
- {
- armem::EntityUpdate& up = update.proprioception.add();
- up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
- up.timeCreated = data.timestamp;
- up.instancesData = { 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.timeCreated = data.timestamp;
+ up.instancesData = {data.proprioception};
+ }
- // Extract odometry data.
- const std::string platformKey = "platform";
- if (data.proprioception->hasElement(platformKey))
- {
- ARMARX_DEBUG << "Found odometry data.";
- auto platformData = aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
- update.localization.emplace_back(getTransform(platformData, data.timestamp));
- }
- else
- {
- ARMARX_INFO << "No odometry data! "
- << "(No element '" << platformKey << "' in proprioception data.)"
- << "\nIf you are using a mobile platform this should not have happened."
- << "\nThis error is only logged once."
- << "\nThese keys exist: " << data.proprioception->getAllKeys()
- ;
- noOdometryDataLogged = true;
- }
+ // 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.timeCreated = data.timestamp;
+ up.instancesData = {data.exteroception};
+ }
- dataQueue.pop();
+ // Extract odometry data.
+ ARMARX_CHECK_NOT_NULL(data.proprioception);
+ const std::string platformKey = "platform";
+ if (data.proprioception->hasElement(platformKey))
+ {
+ ARMARX_DEBUG << "Found odometry data.";
+ auto platformData =
+ aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
+ update.localization.emplace_back(getTransform(platformData, data.timestamp));
+ }
+ else
+ {
+ ARMARX_INFO << "No odometry data! "
+ << "(No element '" << platformKey << "' in proprioception data.)"
+ << "\nIf you are using a mobile platform this should not have happened."
+ << "\nThis error is only logged once."
+ << "\nThese keys exist: " << data.proprioception->getAllKeys();
+ noOdometryDataLogged = true;
}
return update;
@@ -182,9 +198,8 @@ namespace armarx::armem::server::robot_state::proprioception
armem::robot_state::Transform
- RobotStateWriter::getTransform(
- const aron::data::DictPtr& platformData,
- const Time& timestamp) const
+ RobotStateWriter::getTransform(const aron::data::DictPtr& platformData,
+ const Time& timestamp) const
{
prop::arondto::Platform platform;
platform.fromAron(platformData);
@@ -192,7 +207,7 @@ namespace armarx::armem::server::robot_state::proprioception
const Eigen::Vector3f& relPose = platform.relativePosition;
Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
- odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
+ odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
odometryPose.linear() = simox::math::rpy_to_mat3f(0.f, 0.f, relPose.z());
armem::robot_state::Transform transform;
@@ -205,4 +220,4 @@ namespace armarx::armem::server::robot_state::proprioception
return transform;
}
-}
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 4a4f14672b038132243bdbd5fc5a5617f3f5d668..78863697a61ecd20c4947188a04965d5990c6f7b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -25,6 +25,7 @@
#include <optional>
+#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
@@ -54,11 +55,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
public:
- void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver);
+ void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin);
+
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
+
/// Reads data from `dataQueue` and commits to the memory.
void run(float pollFrequency,
- std::queue<RobotUnitData>& dataQueue, std::mutex& dataMutex,
+ Queue& dataBuffer,
MemoryToIceAdapter& memory,
localization::Segment& localizationSegment
);
@@ -67,9 +71,10 @@ namespace armarx::armem::server::robot_state::proprioception
struct Update
{
armem::Commit proprioception;
+ armem::Commit exteroception;
std::vector<armem::robot_state::Transform> localization;
};
- Update buildUpdate(std::queue<RobotUnitData>& dataQueue);
+ Update buildUpdate(const RobotUnitData& dataQueue);
private:
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 c0029b4fbccece40156be49f05ca1efa4215850c..8d904f249029cf9fd5ce010657cb4324f3ba5551 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
@@ -2,8 +2,10 @@
#include <memory>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <boost/thread/concurrent_queues/sync_queue.hpp>
+
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::armem::server::robot_state::proprioception
{
@@ -12,6 +14,8 @@ namespace armarx::armem::server::robot_state::proprioception
{
Time timestamp;
aron::data::DictPtr proprioception;
+ aron::data::DictPtr exteroception;
};
-}
+ using Queue = boost::sync_queue<RobotUnitData>;
+} // namespace armarx::armem::server::robot_state::proprioception
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 f50c4e5619d70b7910d6d3ef544c2ca4f9d7fdc9..53021dc7d88f9362a967f395b1381d07934f271e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -1,82 +1,81 @@
#include "RobotUnitReader.h"
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h>
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <filesystem>
+#include <istream>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <RobotAPI/libraries/aron/core/data/variant/primitive/Long.h>
-#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
-#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
-
-#include <ArmarXCore/core/time/CycleUtil.h>
+#include "ArmarXCore/core/time/Frequency.h"
+#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-#include <istream>
-#include <filesystem>
-#include <fstream>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::armem::server::robot_state::proprioception
{
- RobotUnitReader::RobotUnitReader()
- {
- }
+ RobotUnitReader::RobotUnitReader() = default;
- void RobotUnitReader::connect(
- armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
- armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
- const std::string& robotTypeName)
+ void
+ RobotUnitReader::connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
+ const std::string& robotTypeName)
{
{
- converter = converterRegistry.get(robotTypeName);
- ARMARX_CHECK_NOT_NULL(converter)
- << "No converter for robot type '" << robotTypeName << "' available. \n"
- << "Known are: " << converterRegistry.getKeys();
+ converterProprioception = proprioceptionConverterRegistry.get(robotTypeName);
+ ARMARX_CHECK_NOT_NULL(converterProprioception)
+ << "No converter for robot type '" << robotTypeName << "' available. \n"
+ << "Known are: " << proprioceptionConverterRegistry.getKeys();
config.loggingNames.push_back(properties.sensorPrefix);
- receiver = robotUnitPlugin.startDataSatreming(config);
+ receiver = robotUnitPlugin.startDataStreaming(config);
description = receiver->getDataDescription();
}
+
+ {
+ converterExteroception = exteroceptionConverterRegistry.get(robotTypeName);
+ ARMARX_CHECK_NOT_NULL(converterProprioception)
+ << "No converter for robot type '" << robotTypeName << "' available. \n"
+ << "Known are: " << exteroceptionConverterRegistry.getKeys();
+ }
+
{
// Thread-local copy of debug observer helper.
- debugObserver =
- DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
+ debugObserver = DebugObserverHelper(
+ Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
}
}
- void RobotUnitReader::run(
- float pollFrequency,
- std::queue<RobotUnitData>& dataQueue,
- std::mutex& dataMutex)
+ void
+ RobotUnitReader::run(float pollFrequency, Queue& dataBuffer)
{
- CycleUtil cycle(static_cast<int>(1000.f / pollFrequency));
+ Metronome metronome(Frequency::HertzDouble(pollFrequency));
+
while (task and not task->isStopped())
{
if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
- std::lock_guard g{dataMutex};
- dataQueue.push(std::move(commit.value()));
+ // will lock a mutex
+ dataBuffer.push(std::move(commit.value()));
}
if (debugObserver)
{
debugObserver->sendDebugObserverBatch();
}
- cycle.waitForCycleDuration();
+
+ metronome.waitForNextTick();
}
}
- std::optional<RobotUnitData> RobotUnitReader::fetchAndConvertLatestRobotUnitData()
+ std::optional<RobotUnitData>
+ RobotUnitReader::fetchAndConvertLatestRobotUnitData()
{
- ARMARX_CHECK_NOT_NULL(converter);
+ ARMARX_CHECK_NOT_NULL(converterProprioception);
const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
if (not data.has_value())
@@ -88,23 +87,36 @@ namespace armarx::armem::server::robot_state::proprioception
auto start = std::chrono::high_resolution_clock::now();
RobotUnitData result;
- result.proprioception = converter->convert(data.value(), description);
+
+ if(converterProprioception != nullptr)
+ {
+ result.proprioception = converterProprioception->convert(data.value(), description);
+ }
+
+ if(converterExteroception != nullptr)
+ {
+ result.exteroception = converterExteroception->convert(data.value(), description);
+ }
+
result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit: " << duration;
+ ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit: "
+ << duration;
if (debugObserver)
{
- debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]", duration.count() / 1000.f);
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]",
+ duration.count() / 1000.f);
}
return result;
}
- std::optional<RobotUnitDataStreaming::TimeStep> RobotUnitReader::fetchLatestData()
+ std::optional<RobotUnitDataStreaming::TimeStep>
+ RobotUnitReader::fetchLatestData()
{
std::deque<RobotUnitDataStreaming::TimeStep>& data = receiver->getDataBuffer();
if (debugObserver)
@@ -124,5 +136,4 @@ namespace armarx::armem::server::robot_state::proprioception
}
-}
-
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
index efb7a0d8175de36ebf963fd035d176442df822f2..88313402f217eb8f46470aafd796465354f6cc68 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
@@ -1,9 +1,9 @@
#pragma once
-#include <queue>
#include <map>
#include <memory>
#include <optional>
+#include <queue>
#include <string>
#include <ArmarXCore/core/logging/Logging.h>
@@ -14,52 +14,51 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include "RobotUnitData.h"
-#include "converters/ConverterRegistry.h"
#include "converters/ConverterInterface.h"
+#include "../exteroception/converters/ConverterInterface.h"
+#include "converters/ConverterRegistry.h"
+#include "../exteroception/converters/ConverterRegistry.h"
namespace armarx::plugins
{
class RobotUnitComponentPlugin;
class DebugObserverComponentPlugin;
-}
+} // namespace armarx::plugins
namespace armarx
{
using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
}
+
+// TODO move this out of proprioception. it is now for proprioception and exteroception
namespace armarx::armem::server::robot_state::proprioception
{
class RobotUnitReader : public armarx::Logging
{
public:
-
RobotUnitReader();
+ using Queue = armarx::armem::server::robot_state::proprioception::Queue;
- void connect(
- armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
- armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
- const std::string& robotTypeName);
+ void connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
+ armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
+ const std::string& robotTypeName);
/// Reads data from `handler` and fills `dataQueue`.
- void run(float pollFrequency,
- std::queue<RobotUnitData>& dataQueue,
- std::mutex& dataMutex);
+ void run(float pollFrequency, Queue& dataBuffer);
std::optional<RobotUnitData> fetchAndConvertLatestRobotUnitData();
private:
-
/// Fetch the latest timestep and clear the robot unit buffer.
std::optional<RobotUnitDataStreaming::TimeStep> fetchLatestData();
public:
-
struct Properties
{
std::string sensorPrefix = "sens.*";
@@ -73,12 +72,12 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitDataStreamingReceiverPtr receiver;
RobotUnitDataStreaming::DataStreamingDescription description;
- ConverterRegistry converterRegistry;
- ConverterInterface* converter = nullptr;
+ ConverterRegistry proprioceptionConverterRegistry;
+ exteroception::ConverterRegistry exteroceptionConverterRegistry;
+ proprioception::ConverterInterface* converterProprioception = nullptr;
+ exteroception::ConverterInterface* converterExteroception = nullptr;
armarx::SimpleRunningTask<>::pointer_type task = nullptr;
-
};
-}
-
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index ca9e43ed9d4a7022dd4c6a4eecba530e81bf7b21..99c330e7661d35c5b161c26e1d18b45af2d91ce9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -1,5 +1,7 @@
#include "Segment.h"
+#include <SimoxUtility/math/regression/linear.hpp>
+
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -9,19 +11,43 @@
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
namespace armarx::armem::server::robot_state::proprioception
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- Base(memoryToIceAdapter, "Proprioception", arondto::Proprioception::ToAronType(), 1024)
+ Base(memoryToIceAdapter,
+ armem::robot_state::proprioceptionSegmentID.coreSegmentName,
+ arondto::Proprioception::ToAronType(),
+ 1024)
{
}
Segment::~Segment() = default;
+ void
+ Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ Base::defineProperties(defs, prefix);
+
+ defs->optional(properties.predictionTimeWindow,
+ "prediction.TimeWindow",
+ "Duration of time window into the past to use for predictions"
+ " when requested via the PredictingMemoryInterface (in seconds).");
+ }
+
+ void Segment::init()
+ {
+ Base::init();
+
+ segmentPtr->addPredictor(
+ armem::PredictionEngine{.engineID = "Linear"},
+ [this](const PredictionRequest& request) { return this->predictLinear(request); });
+ }
void Segment::onConnect(RobotUnitInterfacePrx robotUnitPrx)
{
@@ -129,6 +155,127 @@ namespace armarx::armem::server::robot_state::proprioception
return robotUnitProviderID;
}
+ Eigen::VectorXd readJointData(const wm::EntityInstanceData& data)
+ {
+ namespace adn = aron::data;
+
+ std::vector<double> values;
+
+ auto addData =
+ [&](adn::DictPtr dict) // NOLINT
+ {
+ for (const auto& [name, value] : dict->getElements())
+ {
+ values.push_back(
+ static_cast<double>(adn::Float::DynamicCastAndCheck(value)->getValue()));
+ }
+ };
+
+ if (adn::DictPtr joints = getDictElement(data, "joints"))
+ {
+ if (adn::DictPtr jointsPosition = getDictElement(*joints, "position"))
+ {
+ addData(jointsPosition);
+ }
+ if (adn::DictPtr jointsVelocity = getDictElement(*joints, "velocity"))
+ {
+ addData(jointsVelocity);
+ }
+ if (adn::DictPtr jointsTorque = getDictElement(*joints, "torque"))
+ {
+ addData(jointsTorque);
+ }
+ }
+ Eigen::VectorXd vec =
+ Eigen::Map<Eigen::VectorXd>(values.data(), static_cast<Eigen::Index>(values.size()));
+ return vec;
+ }
+
+ void
+ emplaceJointData(const Eigen::VectorXd& jointData,
+ arondto::Proprioception& dataTemplate)
+ {
+ Eigen::Index row = 0;
+ for (auto& [joint, value] : dataTemplate.joints.position)
+ {
+ value = static_cast<float>(jointData(row++));
+ }
+ for (auto& [joint, value] : dataTemplate.joints.velocity)
+ {
+ value = static_cast<float>(jointData(row++));
+ }
+ for (auto& [joint, value] : dataTemplate.joints.torque)
+ {
+ value = static_cast<float>(jointData(row++));
+ }
+ }
+
+ armem::PredictionResult
+ Segment::predictLinear(const armem::PredictionRequest& request) const
+ {
+ PredictionResult result;
+ result.snapshotID = request.snapshotID;
+ if (request.predictionSettings.predictionEngineID != "Linear")
+ {
+ result.success = false;
+ result.errorMessage = "Prediction engine " +
+ request.predictionSettings.predictionEngineID +
+ " is not supported in Proprioception.";
+ return result;
+ }
+
+ const DateTime timeOrigin = DateTime::Now();
+ const armarx::Duration timeWindow = Duration::SecondsDouble(properties.predictionTimeWindow);
+ SnapshotRangeInfo<Eigen::VectorXd, aron::data::DictPtr> info;
+
+ doLocked(
+ // Default capture because the number of variables was getting out of hand
+ [&, this]()
+ {
+ info = getSnapshotsInRange<server::wm::CoreSegment,
+ Eigen::VectorXd,
+ aron::data::DictPtr>(
+ segmentPtr,
+ request.snapshotID,
+ timeOrigin - timeWindow,
+ timeOrigin,
+ [](const aron::data::DictPtr& data) { return readJointData(*data); },
+ [](const aron::data::DictPtr& data) { return data; });
+ });
+
+ if (info.success)
+ {
+ Eigen::VectorXd latestJoints = readJointData(*info.latestValue);
+ Eigen::VectorXd prediction(latestJoints.size());
+ if (info.timestampsSec.size() <= 1)
+ {
+ prediction = latestJoints;
+ }
+ else
+ {
+ using simox::math::LinearRegression;
+ const bool inputOffset = false;
+ const LinearRegression model = LinearRegression<Eigen::Dynamic>::Fit(
+ info.timestampsSec, info.values, inputOffset);
+ const auto predictionTime = request.snapshotID.timestamp;
+ prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ }
+
+ arondto::Proprioception templateData =
+ arondto::Proprioception::FromAron(info.latestValue);
+ emplaceJointData(prediction, templateData);
+ result.success = true;
+ result.prediction = templateData.toAron();
+ }
+ else
+ {
+ result.success = false;
+ result.errorMessage = info.errorMessage;
+ }
+
+ return result;
+ }
+
std::map<std::string, float>
Segment::readJointPositions(const wm::EntityInstanceData& data)
@@ -151,4 +298,4 @@ namespace armarx::armem::server::robot_state::proprioception
return jointPositions;
}
-} // namespace armarx::armem::server::robot_state::proprioception
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index 808fbd7905c1ec6e44e51296ea959e7e3af755d5..8123aea55ea50a11d02b8bf1d50aee0fbe5b09dc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -32,6 +32,7 @@
// RobotAPI
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
@@ -52,6 +53,10 @@ namespace armarx::armem::server::robot_state::proprioception
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+
+ void init() override;
+
void onConnect(RobotUnitInterfacePrx robotUnitPrx);
@@ -62,6 +67,8 @@ namespace armarx::armem::server::robot_state::proprioception
const armem::MemoryID& getRobotUnitProviderID() const;
+ armem::PredictionResult predictLinear(const armem::PredictionRequest& request) const;
+
private:
@@ -75,6 +82,12 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitInterfacePrx robotUnit;
armem::MemoryID robotUnitProviderID;
+ struct Properties
+ {
+ double predictionTimeWindow = 2;
+ };
+ Properties properties;
+
// Debug Observer prefix
const std::string dp = "Proprioception::getRobotJointPositions() | ";
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
index 02325df8c3333ffa5671bcdc109e04ff6edc4ceb..4dda540b2596801f1492c4ae8ac532455af002a4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
@@ -164,7 +164,23 @@ namespace armarx::armem::server::robot_state::proprioception
std::string key = split.size() == 4
? (fieldName + "." + split.at(3))
: fieldName;
- dto.extra[key] = getValueAs<float>(value);
+
+ switch (value.entry.type)
+ {
+ case RobotUnitDataStreaming::NodeTypeFloat:
+ dto.extra[key] = getValueAs<float>(value);
+ break;
+ case RobotUnitDataStreaming::NodeTypeInt:
+ dto.extra[key] = getValueAs<int>(value);
+ break;
+ case RobotUnitDataStreaming::NodeTypeLong:
+ dto.extra[key] = getValueAs<long>(value);
+ break;
+ default:
+ ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
+ << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
+ break;
+ }
}
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
index 3b492f8e13bcb8ec0110ed6ac9b2d0cea3a9f1cd..0c6baa3b770e2136543be7885b1e856a7e93bb78 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
@@ -12,6 +12,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
add<Armar6Converter>("Armar6");
add<Armar6Converter>("ArmarDE");
+ add<Armar6Converter>("Armar7");
}
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
index f8bbe5ec1db7641c9f634ff51ba6416e1d2dbb81..4d179ca04bddc9e1eb11ebb5b08aea520e520d83 100644
--- a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -13,10 +13,9 @@ armarx_add_library(
aroncommon
# System / External
Eigen3::Eigen
+ armem_laser_scans
HEADERS
./aron_conversions.h
- ./client/laser_scans/Reader.h
- ./client/laser_scans/Writer.h
./client/occupancy_grid/Reader.h
./client/occupancy_grid/Writer.h
./client/laser_scanner_features/Reader.h
@@ -25,8 +24,6 @@ armarx_add_library(
constants.h
SOURCES
./aron_conversions.cpp
- ./client/laser_scans/Reader.cpp
- ./client/laser_scans/Writer.cpp
./client/occupancy_grid/Reader.cpp
./client/occupancy_grid/Writer.cpp
./client/laser_scanner_features/Reader.cpp
@@ -38,7 +35,6 @@ armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
ARON_FILES
- aron/LaserScan.xml
aron/LaserScannerFeatures.xml
aron/OccupancyGrid.xml
)
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index 6b0ea82d8ab18f6aa885d075949caa75b288d279..814ec54638ae165734b5b5e629684328d8085277 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -6,7 +6,7 @@
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/common/Converter.h>
@@ -18,37 +18,6 @@
namespace armarx::armem::vision
{
- /************ fromAron ************/
- SensorHeader
- fromAron(const arondto::SensorHeader& aronSensorHeader)
- {
- return {.agent = aronSensorHeader.agent,
- .frame = aronSensorHeader.frame,
- .timestamp = aron::fromAron<Time>(aronSensorHeader.timestamp)};
- }
-
- void
- fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan)
- {
- laserScan.header = fromAron(aronLaserScan.header);
- // laserScan.data = fromAron(aronLaserScan.data);
- }
-
- void
- fromAron(const arondto::LaserScanStamped& aronLaserScan,
- LaserScan& laserScan,
- std::int64_t& timestamp,
- std::string& frame,
- std::string& agentName)
- {
- const auto header = fromAron(aronLaserScan.header);
-
- // laserScan = fromAron(aronLaserScan.data);
-
- timestamp = header.timestamp.toMicroSecondsSinceEpoch();
- frame = header.frame;
- agentName = header.agent;
- }
/************ toAron ************/
@@ -62,40 +31,7 @@ namespace armarx::armem::vision
{
return timestamp.toMicroSecondsSinceEpoch();
}
-
- arondto::SensorHeader
- toAron(const SensorHeader& sensorHeader)
- {
- arondto::SensorHeader aronSensorHeader;
-
- aron::toAron(aronSensorHeader.agent, sensorHeader.agent);
- aron::toAron(aronSensorHeader.frame, sensorHeader.frame);
- aron::toAron(aronSensorHeader.timestamp, sensorHeader.timestamp);
-
- return aronSensorHeader;
- }
-
- void
- toAron(const LaserScanStamped& laserScanStamped,
- arondto::LaserScanStamped& aronLaserScanStamped)
- {
- aronLaserScanStamped.header = toAron(laserScanStamped.header);
- // toAron(laserScanStamped.data, aronLaserScanStamped.data);
- }
-
- void
- toAron(const LaserScan& laserScan,
- const armem::Time& timestamp,
- const std::string& frame,
- const std::string& agentName,
- arondto::LaserScanStamped& aronLaserScanStamped)
- {
- const SensorHeader header{.agent = agentName, .frame = frame, .timestamp = timestamp};
-
- const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
-
- toAron(laserScanStamped, aronLaserScanStamped);
- }
+
void
toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
index e9cf78c698b31345a9fcc17262bb7a23a25cd7fc..216088b410231671dd49a22a95f9fa2b6ae9362b 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
@@ -40,7 +40,7 @@
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
#include <RobotAPI/libraries/armem_vision/types.h>
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
index 5746bffa1880c3031e183acf5cecd74f2d6d515a..0b24561dd5507efeef77793a5285b5a3eb2b00c9 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
@@ -2,7 +2,7 @@
#include "RobotAPI/libraries/armem_vision/constants.h"
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
index 91505d40e5ea0046fc2419ac532db85010062011..a98495d192583b101299c8b6b0b365f569427a38 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -42,7 +42,6 @@
#include <RobotAPI/libraries/armem/client/query/selectors.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
#include <RobotAPI/libraries/armem_vision/types.h>
diff --git a/source/RobotAPI/libraries/armem_vision/types.h b/source/RobotAPI/libraries/armem_vision/types.h
index 85322d188ce8fdbd9b065ce476e281408d38f254..fe584e3dc235a19333fd9b174cfab390c1a5d435 100644
--- a/source/RobotAPI/libraries/armem_vision/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -31,18 +31,6 @@
namespace armarx::armem::vision
{
- struct SensorHeader
- {
- std::string agent;
- std::string frame;
- armem::Time timestamp;
- };
-
- struct LaserScanStamped
- {
- SensorHeader header;
- LaserScan data;
- };
// template<typename _ValueT = float>
struct OccupancyGrid
diff --git a/source/RobotAPI/libraries/aron/CMakeLists.txt b/source/RobotAPI/libraries/aron/CMakeLists.txt
index ce4f7bc99cb8d923decfdd7fdb79390837e79150..e1ccc8560ff75b018fbc958d0f1febf0512d9590 100644
--- a/source/RobotAPI/libraries/aron/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/CMakeLists.txt
@@ -2,3 +2,4 @@ add_subdirectory(core)
add_subdirectory(converter)
add_subdirectory(codegeneration)
add_subdirectory(common)
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp
index 72a657218790d389d14283f7c22fb02ce183c357..85c6403e5e4452848c31ffbc3f59aa39c3fe46ee 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp
@@ -253,7 +253,6 @@ namespace armarx::aron::codegenerator::cpp
c->addInherit("public armarx::aron::cpp::AronGeneratedClass");
// ctor
- c->addCtor(generator.toCopyCtor(c->getName()));
c->addCtor(generator.toInnerEnumCtor(c->getName()));
// Specific methods
@@ -336,6 +335,10 @@ namespace armarx::aron::codegenerator::cpp
// ctor
c->addCtor(gen.toCtor(c->getName()));
+ c->addCtor(gen.toCopyCtor(c->getName()));
+
+ // dtor
+ c->addMethod(gen.toDtor(c->getName()));
// Generic methods
//std::cout << "Generate equals method" << std::endl;
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
index 8b2d2cf94cbb3334341b3159b4c73bb9dc7cdc76..be87200f51f745b314ef7f3f7c2a67beb18d332a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
@@ -41,6 +41,7 @@ namespace armarx::aron::codegenerator::cpp
const std::string Generator::ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_tmpls";
const std::string Generator::ARON_VARIANT_RETURN_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_variant";
const std::string Generator::ARON_PATH_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_p";
+ const std::string Generator::ARON_OTHER_ACCESSOR = Generator::ARON_VARIABLE_PREFIX + "_o";
const SerializerFactoryPtr Generator::FACTORY = SerializerFactoryPtr(new GeneratorFactory());
@@ -168,17 +169,55 @@ namespace armarx::aron::codegenerator::cpp
CppCtorPtr Generator::toCtor(const std::string& name) const
{
- CppCtorPtr c = CppCtorPtr(new CppCtor(name + "()"));
- std::vector<std::pair<std::string, std::string>> initList = this->getCtorInitializers("");
- CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("resetHard();");
- b->appendBlock(this->getCtorBlock(""));
- c->addInitListEntries(initList);
- c->setBlock(b);
+ CppBlockPtr b = this->getCtorBlock("");
+ auto initList = this->getCtorInitializers("");
+
+ if (b->size() > 0 || initList.second)
+ {
+ CppCtorPtr c = CppCtorPtr(new CppCtor(name + "()"));
+ c->addInitListEntries(initList.first);
+ c->setBlock(b);
+ return c;
+ }
+
+ CppCtorPtr c = CppCtorPtr(new CppCtor(name + "() = default;"));
+ return c;
+ }
+ CppCtorPtr Generator::toCopyCtor(const std::string& name) const
+ {
+ CppBlockPtr b = this->getCopyCtorBlock("");
+ auto initList = this->getCopyCtorInitializers("");
+
+ if (b->size() > 0 || initList.second)
+ {
+ CppCtorPtr c = CppCtorPtr(new CppCtor(name + "(const " + name + "& " + ARON_OTHER_ACCESSOR + ")"));
+ c->addInitListEntries(initList.first);
+ c->setBlock(b);
+ return c;
+ }
+
+ CppCtorPtr c = CppCtorPtr(new CppCtor(name + "(const " + name + "& " + ARON_OTHER_ACCESSOR + ") = default;"));
return c;
}
+ CppMethodPtr Generator::toDtor(const std::string& name) const
+ {
+ CppBlockPtr b = this->getDtorBlock("");
+ if (b->size() > 0)
+ {
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual ~" + name + "()"));
+ m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
+ return m;
+ }
+
+ CppMethodPtr m = CppMethodPtr(new CppMethod("virtual ~" + name + "() = default;"));
+ m->setEnforceBlockGeneration(false);
+ return m;
+ }
+
CppMethodPtr Generator::toResetSoftMethod() const
{
std::stringstream doc;
@@ -188,6 +227,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetSoft() override", doc.str()));
CppBlockPtr b = this->getResetSoftBlock("");
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -200,6 +241,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void resetHard() override", doc.str()));
CppBlockPtr b = this->getResetHardBlock("");
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -209,13 +252,15 @@ namespace armarx::aron::codegenerator::cpp
doc << "@brief writeType() - This method returns a new type from the class structure using a type writer implementation. This function is static. \n";
doc << "@return - the result of the writer implementation";
- CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\nstatic typename WriterT::ReturnType writeType(WriterT& " + ARON_WRITER_ACCESSOR + ", std::vector<std::string> " + ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + " = {}, armarx::aron::type::Maybe "+ ARON_MAYBE_TYPE_ACCESSOR +" = armarx::aron::type::Maybe::NONE, const armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path())", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\nstatic typename WriterT::ReturnType writeType(WriterT& " + ARON_WRITER_ACCESSOR + ", std::vector<std::string> " + ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR + " = {}, armarx::aron::type::Maybe "+ ARON_MAYBE_TYPE_ACCESSOR +" = ::armarx::aron::type::Maybe::NONE, const ::armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = ::armarx::aron::Path())", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("using _Aron_T [[maybe_unused]] = typename WriterT::ReturnType;");
std::string dummy;
b->appendBlock(this->getWriteTypeBlock("", "", Path(), dummy));
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -226,7 +271,7 @@ namespace armarx::aron::codegenerator::cpp
doc << "@param w - The writer implementation\n";
doc << "@return - the result of the writer implementation";
- CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\ntypename WriterT::ReturnType write(WriterT& " + ARON_WRITER_ACCESSOR + ", const armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path()) const", doc.str()));
+ CppMethodPtr m = CppMethodPtr(new CppMethod("template<class WriterT>\ntypename WriterT::ReturnType write(WriterT& " + ARON_WRITER_ACCESSOR + ", const ::armarx::aron::Path& "+ARON_PATH_ACCESSOR+" = armarx::aron::Path()) const", doc.str()));
CppBlockPtr b = std::make_shared<CppBlock>();
b->addLine("using _Aron_T [[maybe_unused]] = typename WriterT::ReturnType;");
@@ -234,9 +279,10 @@ namespace armarx::aron::codegenerator::cpp
std::string dummy;
b->addBlock(this->getWriteBlock("", Path(), dummy));
b->addLine("catch(const std::exception& " + ARON_VARIABLE_PREFIX + "_e)");
- b->addLineAsBlock("throw armarx::aron::error::AronException(__PRETTY_FUNCTION__, std::string(\"An error occured during the write method of an aron generated class. The full error log was:\\n\") + " + ARON_VARIABLE_PREFIX + "_e.what());");
+ b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, std::string(\"An error occured during the write method of an aron generated class. The full error log was:\\n\") + " + ARON_VARIABLE_PREFIX + "_e.what());");
m->setBlock(b);
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -253,14 +299,15 @@ namespace armarx::aron::codegenerator::cpp
b->addLine("using _Aron_TNonConst [[maybe_unused]] = typename ReaderT::InputTypeNonConst;");
b->addLine("this->resetSoft();");
- b->addLine("if (" + ARON_READER_ACCESSOR + ".readNull(input))");
- b->addLineAsBlock("throw armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"The input to the read method must not be null.\");");
+ b->addLine("ARMARX_CHECK_AND_THROW(!" + ARON_READER_ACCESSOR + ".readNull(input), ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"The input to the read method must not be null.\"));");
b->addLine("try");
b->addBlock(this->getReadBlock("", "input"));
b->addLine("catch(const std::exception& " + ARON_VARIABLE_PREFIX + "_e)");
- b->addLineAsBlock("throw armarx::aron::error::AronException(__PRETTY_FUNCTION__, std::string(\"An error occured during the read method of an aron generated class. The full error log was:\\n\") + " + ARON_VARIABLE_PREFIX + "_e.what());");
+ b->addLineAsBlock("throw ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, std::string(\"An error occured during the read method of an aron generated class. The full error log was:\\n\") + " + ARON_VARIABLE_PREFIX + "_e.what());");
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -273,6 +320,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod(info.returnType + " " + info.methodName + "() const", doc.str()));
m->addLine(info.writerClassType + " writer;");
m->addLine("return " + info.enforceConversion + "(this->write(writer))" + info.enforceMemberAccess + ";");
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -285,6 +334,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("void " + info.methodName + "(const " + info.argumentType + "& input)", doc.str()));
m->addLine(info.readerClassType + " reader;");
m->addLine("this->read(reader, " + info.enforceConversion + "(input)" + info.enforceMemberAccess + ");");
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -298,6 +349,8 @@ namespace armarx::aron::codegenerator::cpp
m->addLine(info.returnType + " t;");
m->addLine("t.fromAron(input);");
m->addLine("return t;");
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -310,6 +363,8 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr m = CppMethodPtr(new CppMethod("static " + info.returnType + " " + info.methodName + "()", doc.str()));
m->addLine(info.writerClassType + " writer;");
m->addLine("return " + info.enforceConversion + "(writeType(writer))" + info.enforceMemberAccess + ";");
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -324,6 +379,8 @@ namespace armarx::aron::codegenerator::cpp
CppBlockPtr b = this->getEqualsBlock("", "i");
b->addLine("return true;");
m->setBlock(b);
+
+ m->setEnforceBlockGeneration(true);
return m;
}
@@ -338,9 +395,9 @@ namespace armarx::aron::codegenerator::cpp
return {field};
}
- std::vector<std::pair<std::string, std::string>> Generator::getCtorInitializers(const std::string&) const
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool> Generator::getCtorInitializers(const std::string&) const
{
- return {};
+ return {{}, false};
}
CppBlockPtr Generator::getCtorBlock(const std::string&) const
@@ -348,6 +405,27 @@ namespace armarx::aron::codegenerator::cpp
return std::make_shared<CppBlock>();
}
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool> Generator::getCopyCtorInitializers(const std::string& name) const
+ {
+ const auto& t = getType();
+ if (t.getMaybe() == type::Maybe::UNIQUE_PTR || t.getMaybe() == type::Maybe::RAW_PTR)
+ {
+ return {{{name, ARON_OTHER_ACCESSOR + "." + name + " ? " + resolveMaybeGenerator("*" + ARON_OTHER_ACCESSOR + "." + name) + " : nullptr"}}, true};
+ }
+
+ return {{{name, ARON_OTHER_ACCESSOR + "." + name}}, false};
+ }
+
+ CppBlockPtr Generator::getCopyCtorBlock(const std::string&) const
+ {
+ return std::make_shared<CppBlock>();
+ }
+
+ CppBlockPtr Generator::getDtorBlock(const std::string&) const
+ {
+ return std::make_shared<CppBlock>();
+ }
+
CppBlockPtr Generator::getResetHardBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
@@ -389,24 +467,35 @@ namespace armarx::aron::codegenerator::cpp
return s;
}
- std::string Generator::resolveMaybeGenerator(const std::string& s) const
+ std::string Generator::resolveMaybeGenerator(const std::string& args) const
{
const auto& t = getType();
if (t.getMaybe() == type::Maybe::OPTIONAL)
{
- return s + " = std::make_optional<" + getInstantiatedCppTypename() + ">();";
+ return "std::make_optional<" + getInstantiatedCppTypename() + ">(" + args + ")";
}
if (t.getMaybe() == type::Maybe::RAW_PTR)
{
- return s + " = new " + getInstantiatedCppTypename() + "();";
+ return "new " + getInstantiatedCppTypename() + "(" + args + ")";
}
if (t.getMaybe() == type::Maybe::SHARED_PTR)
{
- return s + " = std::make_shared<" + getInstantiatedCppTypename() + ">();";
+ return "std::make_shared<" + getInstantiatedCppTypename() + ">(" + args + ")";
}
if (t.getMaybe() == type::Maybe::UNIQUE_PTR)
{
- return s + " = std::make_unique<" + getInstantiatedCppTypename() + ">();";
+ return "std::make_unique<" + getInstantiatedCppTypename() + ">(" + args + ")";
+ }
+ return "";
+
+ }
+
+ std::string Generator::resolveMaybeGeneratorWithSetter(const std::string& s, const std::string& args) const
+ {
+ auto gen = resolveMaybeGenerator(args);
+ if (!gen.empty())
+ {
+ return s + " = " + gen + ";";
}
return "";
}
@@ -497,7 +586,7 @@ namespace armarx::aron::codegenerator::cpp
b->addLine("if (not (" + ARON_READER_ACCESSOR + ".readNull(" + variantAccessor + "))) // if aron contains data");
{
CppBlockPtr ifb = std::make_shared<CppBlock>();
- ifb->addLine(resolveMaybeGenerator(cppAccessor));
+ ifb->addLine(resolveMaybeGeneratorWithSetter(cppAccessor));
ifb->appendBlock(block_if_data);
b->addBlock(ifb);
}
@@ -521,5 +610,3 @@ namespace armarx::aron::codegenerator::cpp
return block_if_data;
}
}
-
-
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.h
index 7b9c15a7d4c6f1409db4999b906933bff64f3370..18bec1e225a6fbae97c71c10a7bd82a21ba5604b 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.h
@@ -91,9 +91,16 @@ namespace armarx::aron::codegenerator::cpp
virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const;
CppCtorPtr toCtor(const std::string&) const;
- virtual std::vector<std::pair<std::string, std::string>> getCtorInitializers(const std::string&) const;
+ virtual std::pair<std::vector<std::pair<std::string, std::string>>, bool> getCtorInitializers(const std::string&) const;
virtual CppBlockPtr getCtorBlock(const std::string&) const;
+ CppCtorPtr toCopyCtor(const std::string&) const;
+ virtual std::pair<std::vector<std::pair<std::string, std::string>>, bool> getCopyCtorInitializers(const std::string&) const;
+ virtual CppBlockPtr getCopyCtorBlock(const std::string&) const;
+
+ CppMethodPtr toDtor(const std::string&) const;
+ virtual CppBlockPtr getDtorBlock(const std::string&) const;
+
CppMethodPtr toResetSoftMethod() const;
virtual CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const;
@@ -127,7 +134,8 @@ namespace armarx::aron::codegenerator::cpp
std::string toPointerAccessor(const std::string&) const;
std::string resolveMaybeAccessor(const std::string&) const;
- std::string resolveMaybeGenerator(const std::string&) const;
+ std::string resolveMaybeGenerator(const std::string& args = "") const;
+ std::string resolveMaybeGeneratorWithSetter(const std::string&, const std::string& args = "") const;
CppBlockPtr resolveMaybeResetHardBlock(const CppBlockPtr&, const std::string&) const;
CppBlockPtr resolveMaybeResetSoftBlock(const CppBlockPtr&, const std::string&) const;
@@ -144,6 +152,7 @@ namespace armarx::aron::codegenerator::cpp
static const std::string ARON_WRITER_ACCESSOR;
static const std::string ARON_TEMPLATE_INSTANTIATIONS_ACCESSOR;
static const std::string ARON_VARIANT_RETURN_ACCESSOR;
+ static const std::string ARON_OTHER_ACCESSOR;
private:
static const SerializerFactoryPtr FACTORY;
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp
index b81341ae2bf19f38cbb9928eafdbf9d3f0f9ffe5..e3c2913dd52b5924ea7d02f9c5a402626788f7e2 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp
@@ -59,7 +59,7 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string escaped_accessor = this->EscapeAccessor(cppAccessor);
variantAccessor = Generator::ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- block_if_data->addLine(variantAccessor + " = armarx::aron::data::readAndWrite<armarx::aron::data::FromVariantConverter<WriterT>>(" + cppAccessor + "); // of " + cppAccessor);
+ block_if_data->addLine(variantAccessor + " = ::armarx::aron::data::readAndWrite<::armarx::aron::data::FromVariantConverter<WriterT>>(" + cppAccessor + "); // of " + cppAccessor);
return block_if_data;
}
@@ -69,7 +69,7 @@ namespace armarx::aron::codegenerator::cpp::generator
auto block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = this->EscapeAccessor(cppAccessor);
- block_if_data->addLine(cppAccessor + " = armarx::aron::data::Dict::DynamicCastAndCheck(armarx::aron::data::readAndWrite<aron::data::ToVariantConverter<ReaderT>>(" + variantAccessor + ")); // of " + cppAccessor);
+ block_if_data->addLine(cppAccessor + " = ::armarx::aron::data::Dict::DynamicCastAndCheck(::armarx::aron::data::readAndWrite<::armarx::aron::data::ToVariantConverter<ReaderT>>(" + variantAccessor + ")); // of " + cppAccessor);
return block_if_data;
}
@@ -81,7 +81,7 @@ namespace armarx::aron::codegenerator::cpp::generator
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeAnyObject(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
+ "::armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
return b;
}
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.cpp
index 5b734427b84bbd9fa139f6518110b55b7daf48ae..617a658199e8380a3c6fe02ad13a5a13fad1e886 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.cpp
@@ -57,7 +57,7 @@ namespace armarx::aron::codegenerator::cpp::generator
bool b = std::find(x.disallowedBases.begin(), x.disallowedBases.end(), t.getPath().getRootIdentifier()) == x.disallowedBases.end();
return b;
}
- return true;
+ return false;
}
std::string checkForInstantiationTypenameReplacement(const type::Object& t)
@@ -99,7 +99,8 @@ namespace armarx::aron::codegenerator::cpp::generator
checkForInstantiationTypenameReplacement(e),
checkForClassNameReplacement(e),
simox::meta::get_type_name<data::dto::Dict>(),
- simox::meta::get_type_name<type::dto::AronObject>(), e)
+ simox::meta::get_type_name<type::dto::AronObject>(), e),
+ has_been_replaced(checkForAllowedReplacement(e))
{
}
@@ -160,7 +161,7 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string escaped_accessor = EscapeAccessor(cppAccessor);
std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
- if (const auto reset = resolveMaybeGenerator(cppAccessor); !reset.empty())
+ if (const auto reset = resolveMaybeGeneratorWithSetter(cppAccessor); !reset.empty())
{
block_if_data->addLine(reset);
}
@@ -169,5 +170,31 @@ namespace armarx::aron::codegenerator::cpp::generator
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
+
+
+
+ CppBlockPtr Object::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ if (has_been_replaced)
+ {
+ return Base::getResetHardBlock(cppAccessor);
+ }
+
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + nextEl() + "resetHard();");
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Object::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ if (has_been_replaced)
+ {
+ return Base::getResetHardBlock(cppAccessor);
+ }
+
+ auto block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + nextEl() + "resetSoft();");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.h
index b1a2995c443f4e680725fdcbfbf4ac6374d7c712..942dc161a8792b9b9455c08db5c40eba79260605 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.h
@@ -37,12 +37,14 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string replacedInstantiatedTypename;
std::vector<std::string> additionalIncludes; // additional includes for the replaced type and aron conversions
- std::vector<std::string> disallowedBases; // disallow replacement if the used in a specific class
+ std::vector<std::string> disallowedBases; // disallow replacement if used in a specific class
};
class Object :
public detail::ContainerGenerator<type::Object, Object>
{
+ using Base = detail::ContainerGenerator<type::Object, Object>;
+
public:
// constructors
Object(const type::Object&);
@@ -53,5 +55,10 @@ namespace armarx::aron::codegenerator::cpp::generator
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
+
+ private:
+ bool has_been_replaced = false;
};
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
index c818db64a05d881f8f3efee3ae9d2a767c8383ed..00a9ab35d7f52eb757963c97355403377459737e 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/IntEnum.cpp
@@ -88,7 +88,7 @@ namespace armarx::aron::codegenerator::cpp::generator
CppBlockPtr IntEnum::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
- if (const auto reset = resolveMaybeGenerator(cppAccessor); !reset.empty())
+ if (const auto reset = resolveMaybeGeneratorWithSetter(cppAccessor); !reset.empty())
{
block_if_data->addLine(reset);
}
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 2fbfad0e89159185d4481abde6fada3e9d79d14c..569d5be67dfd601cf09157655ff0542f022680ae 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
@@ -101,7 +101,27 @@ namespace armarx::aron::codegenerator::cpp::generator
block_if_data->addLine("std::string " + type + ";");
block_if_data->addLine("std::vector<int> " + dims + ";");
block_if_data->addLine("std::vector<unsigned char> " + data + ";");
+
block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readNDArray("+variantAccessor+", "+dims+", "+type+", "+data+"); // of " + cppAccessor);
+
+ if((this->type.getRows() == -1 or this->type.getRows() == Eigen::Dynamic) and this->type.getCols() != -1)
+ {
+ block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + ".at(0));");
+ }
+
+ if(this->type.getRows() != -1 and (this->type.getCols() == -1 or this->type.getCols() == Eigen::Dynamic))
+ {
+ block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + ".at(1));");
+ }
+
+ if((this->type.getRows() == -1 or this->type.getRows() == Eigen::Dynamic) and (this->type.getCols() == -1 or this->type.getCols() == Eigen::Dynamic))
+ {
+ block_if_data->addLine(cppAccessor + nextEl() + "resize(" + dims + ".at(0), " + dims + ".at(1));");
+ }
+
+ block_if_data->addLine("ARMARX_CHECK_AND_THROW(" + cppAccessor + nextEl() + "rows() == " + dims + ".at(0) and " + cppAccessor + nextEl() + "cols() == " + dims + ".at(1), ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Received wrong dimensions for member '"+cppAccessor+"'.\"));");
+ block_if_data->addLine("ARMARX_CHECK_AND_THROW(" + type + " == \"" + std::get<0>(ElementType2Cpp.at(this->type.getElementType())) + "\", ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Received wrong type for member '"+cppAccessor+"'.\"));");
+
block_if_data->addLine("std::memcpy(reinterpret_cast<unsigned char*>(" + cppAccessor + nextEl() + "data()), "+data+".data(), "+data+".size());");
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp
index fb3572e26adb9a61cf7bcde1396a1a2cb4bfff26..7dcadbb33ae84d70a180e89763ca37a634946268 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.cpp
@@ -49,4 +49,18 @@ namespace armarx::aron::codegenerator::cpp::generator
"armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
return b;
}
+
+ CppBlockPtr Bool::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = false;");
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Bool::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ auto block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = false;");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.h
index 85172f3de8745ac5b2feb465cd9ff8408e94cd56..381adec7bc759c7a20aedd54229dd499253110d1 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Bool.h
@@ -40,5 +40,7 @@ namespace armarx::aron::codegenerator::cpp::generator
/* virtual implementations */
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.cpp
index 7076794550d8e149f496677c079da4a3b7517e03..735f3ad9dc4efdf0f00a34093671644aebb0eb3f 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.cpp
@@ -50,4 +50,18 @@ namespace armarx::aron::codegenerator::cpp::generator
"armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
return b;
}
+
+ CppBlockPtr Double::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = 0.0;");
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Double::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ auto block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = 0.0;");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.h
index e109cc626500ce4004f2b69b670ed9574c947ea5..b9a96a077e225aad5b0871882e86e20f85fb3685 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Double.h
@@ -39,5 +39,7 @@ namespace armarx::aron::codegenerator::cpp::generator
/* virtual implementations */
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.cpp
index 6c740a79206d323e0b6e9bf0b66c19d82eb1593a..d3d27ea6c34713b3d81074c1ab64e37ed696d69c 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.cpp
@@ -50,4 +50,18 @@ namespace armarx::aron::codegenerator::cpp::generator
"armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
return b;
}
+
+ CppBlockPtr Float::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = 0.0f;");
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Float::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ auto block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = 0.0f;");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.h
index a2f52f4fb7d78ebcefcd96ba3eba8d0b31fc7dcb..f1c88c502f92b7d86afc7668eda0710f0f3351b7 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Float.h
@@ -39,5 +39,7 @@ namespace armarx::aron::codegenerator::cpp::generator
/* virtual implementations */
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.cpp
index 9242126f54ae10d3ee26fd6e242de8014c6a23f7..7a976bd83fffa4ad52bec05c2a5392bbf7400798 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.cpp
@@ -50,4 +50,18 @@ namespace armarx::aron::codegenerator::cpp::generator
"armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
return b;
}
+
+ CppBlockPtr Int::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = 0;");
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Int::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ auto block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = 0;");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.h
index eb18e445473f402c2be373da523ba92336a4ab48..b21abee9cef1850a1d55da0369a530b65585c210 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Int.h
@@ -39,5 +39,7 @@ namespace armarx::aron::codegenerator::cpp::generator
/* virtual implementations */
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.cpp
index 2231f3305264746bb5c31855cac0e6a5c3d712b0..fe6cd5e87f6f3db26873c351987f91d86556d27b 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.cpp
@@ -50,4 +50,18 @@ namespace armarx::aron::codegenerator::cpp::generator
"armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
return b;
}
+
+ CppBlockPtr Long::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = 0;");
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr Long::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ auto block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = 0;");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.h
index 8e74bf85b421ff3101af4f86a3edcdc91858559a..09d923e487108a7c2c0927156af77c21fddf79ec 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/Long.h
@@ -39,5 +39,7 @@ namespace armarx::aron::codegenerator::cpp::generator
/* virtual implementations */
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.cpp
index a335fafcbd33ffca5c0a1658aaa1da5ccfeb4cc6..d51ee9ed159a5abb82442a5501ffebb3500aad15 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.cpp
@@ -50,4 +50,18 @@ namespace armarx::aron::codegenerator::cpp::generator
"armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
return b;
}
+
+ CppBlockPtr String::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = \"\";");
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
+ CppBlockPtr String::getResetSoftBlock(const std::string& cppAccessor) const
+ {
+ auto block_if_data = std::make_shared<CppBlock>();
+ block_if_data->addLine(cppAccessor + " = \"\";");
+ return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
+ }
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.h
index 7db96c8713e04a479d4f1623043f633d4d429f84..fd854ccee807d549fbe1448643eb31a4f069bcc8 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/String.h
@@ -39,5 +39,7 @@ namespace armarx::aron::codegenerator::cpp::generator
/* virtual implementations */
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
};
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp
index c1c77424ba3d97dc217bd846bb1335023761fa2a..f5463258db1423ba317c428b2d604907b913755a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.cpp
@@ -134,7 +134,10 @@ namespace armarx::aron::codegenerator::cpp::generator
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("int " + INT_ENUM_TMP_VALUE + ";");
block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readPrimitive("+variantAccessor+", "+INT_ENUM_TMP_VALUE+"); // of top level enum " + getInstantiatedCppTypename());
- block_if_data->addLine("value = ValueToEnumMap.at("+INT_ENUM_TMP_VALUE+");");
+
+ block_if_data->addLine("auto valueToEnumMap_iterator = ValueToEnumMap.find("+INT_ENUM_TMP_VALUE+");");
+ block_if_data->addLine("ARMARX_CHECK_AND_THROW(valueToEnumMap_iterator != ValueToEnumMap.end(), ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Missing enum for value '\" + std::to_string(" + INT_ENUM_TMP_VALUE + ") + \"' in aron enum '" + getFullClassCppTypename() + "'.\"))");
+ block_if_data->addLine("value = valueToEnumMap_iterator->second;");
return block_if_data;
}
@@ -147,12 +150,18 @@ namespace armarx::aron::codegenerator::cpp::generator
}
- CppCtorPtr IntEnumClass::toCopyCtor(const std::string& name) const
+ /*CppCtorPtr IntEnumClass::toCopyCtor(const std::string& name) const
{
CppCtorPtr c = std::make_shared<CppCtor>(name + "(const " + getFullInstantiatedCppTypename() + "& i)");
std::vector<std::pair<std::string, std::string>> initList = {{"value", "i.value"}};
c->addInitListEntries(initList);
+ c->setBlock(std::make_shared<CppBlock>());
return c;
+ }*/
+
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool> IntEnumClass::getCopyCtorInitializers(const std::string&) const
+ {
+ return {{{"value", ARON_OTHER_ACCESSOR + ".value"}}, false};
}
CppCtorPtr IntEnumClass::toInnerEnumCtor(const std::string& name) const
@@ -160,6 +169,7 @@ namespace armarx::aron::codegenerator::cpp::generator
CppCtorPtr c = std::make_shared<CppCtor>(name + "(const " + std::string(IMPL_ENUM) + " e)");
std::vector<std::pair<std::string, std::string>> initList = {{"value", "e"}};
c->addInitListEntries(initList);
+ c->setBlock(std::make_shared<CppBlock>());
return c;
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h
index b03310d425bb6783f618e9548d573a87b87c77dc..10d6ca0c14d8100e71d78944f24fd3f46870b8e1 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h
@@ -42,6 +42,9 @@ namespace armarx::aron::codegenerator::cpp::generator
// virtual implementations
std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const final;
+
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool> getCopyCtorInitializers(const std::string&) const final;
+
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
@@ -50,7 +53,7 @@ namespace armarx::aron::codegenerator::cpp::generator
CppBlockPtr getEqualsBlock(const std::string&, const std::string&) const final;
// TODO: Move some of those methods to upper class for enums (if we want to support multiple enums)
- CppCtorPtr toCopyCtor(const std::string&) const;
+ //CppCtorPtr toCopyCtor(const std::string&) const;
CppCtorPtr toInnerEnumCtor(const std::string&) const;
CppEnumPtr toInnerEnumDefinition() const;
CppMethodPtr toIntMethod() const;
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
index 0d54dc8bf173b1262ecbc938a6c1678fb1dff8b2..e3af89daa12ecc2de2f4fe60d9269dd48f729c9a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.cpp
@@ -213,8 +213,10 @@ namespace armarx::aron::codegenerator::cpp::generator
for (const auto& [key, child] : type.getMemberTypes())
{
const auto child_s = FromAronType(*child);
- const std::string child_variant_accessor = OBJECT_MEMBERS_ACCESSOR + ".at(\"" + key + "\")";
- block_if_data->appendBlock(child_s->getReadBlock(key, child_variant_accessor));
+ std::string child_accessor = OBJECT_MEMBERS_ACCESSOR + "_" + key + "_iterator";
+ block_if_data->addLine("auto " + child_accessor + " = " + OBJECT_MEMBERS_ACCESSOR + ".find(\"" + key + "\");");
+ block_if_data->addLine("ARMARX_CHECK_AND_THROW(" + child_accessor + " != " + OBJECT_MEMBERS_ACCESSOR + ".end(), ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, \"Missing member '" + key + "' in aron object '" + getFullClassCppTypename() + "'.\"));");
+ block_if_data->appendBlock(child_s->getReadBlock(key, child_accessor + "->second"));
}
return block_if_data;
}
@@ -236,5 +238,25 @@ namespace armarx::aron::codegenerator::cpp::generator
}
return block_if_data;
}
-}
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool> ObjectClass::getCopyCtorInitializers(const std::string&) const
+ {
+ std::vector<std::pair<std::string, std::string>> ret;
+
+ if (type.getExtends() != nullptr)
+ {
+ const auto extends_s = FromAronType(*type.getExtends());
+ ret.push_back({extends_s->getFullInstantiatedCppTypename(), ARON_OTHER_ACCESSOR});
+ }
+
+ bool anyComplex = false;
+ for (const auto& [key, child] : type.getMemberTypes())
+ {
+ auto child_s = FromAronType(*child);
+ auto initList = child_s->getCopyCtorInitializers(key);
+ simox::alg::append(ret, initList.first);
+ anyComplex = anyComplex || initList.second;
+ }
+ return {ret, anyComplex};
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h
index dd8e7d3d95bb291a54aeba45d46bae8bbf336834..772d138cb7cc521753ce6bd3086bb076d475b462 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h
@@ -40,6 +40,9 @@ namespace armarx::aron::codegenerator::cpp::generator
// virtual implementations
std::vector<std::string> getRequiredIncludes() const final;
std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const final;
+
+ std::pair<std::vector<std::pair<std::string, std::string>>, bool> getCopyCtorInitializers(const std::string&) const final;
+
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt
index e57da47b6d56c9d33a146a609c6dcba9c6c06f30..d7add84ada2fcafe2c8bf8f9f9a8d2c40e663c7d 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/CMakeLists.txt
@@ -106,6 +106,25 @@ armarx_add_test(
${Simox_INCLUDE_DIR}
)
+######################
+# ARON JSON EXPORT TEST
+######################
+armarx_add_test(
+ TEST_NAME
+ aronJsonExportTest
+ TEST_FILE
+ aronJsonExportTest.cpp
+ LIBS
+ SimoxUtility # Simox::SimoxUtility
+ ArmarXCore
+ RobotAPI::aron
+ ARON_FILES
+ aron/OptionalTest.xml
+ aron/MatrixTest.xml
+ INCLUDE_DIRECTORIES
+ ${Simox_INCLUDE_DIR}
+)
+
########################
# ARON RANDOMIZED TEST #
########################
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aronJsonExportTest.cpp b/source/RobotAPI/libraries/aron/codegeneration/test/aronJsonExportTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dea56801557a7890824c8f03d5b2cf5b2711d9a0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aronJsonExportTest.cpp
@@ -0,0 +1,86 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::aron
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::aron
+
+#define ARMARX_BOOST_TEST
+
+// STD/STL
+#include <iostream>
+#include <cstdlib>
+#include <ctime>
+#include <numeric>
+#include <fstream>
+
+// Test
+#include <RobotAPI/Test.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+
+// Aron
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
+#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
+#include <RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
+
+// Generated File
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/MatrixTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/codegeneration/test/aron/OptionalTest.aron.generated.h>
+
+using namespace armarx;
+using namespace aron;
+
+BOOST_AUTO_TEST_CASE(AronJsonExportTest)
+{
+ std::cout << "Aron json export test" << std::endl;
+ OptionalTest clazz;
+
+ {
+ auto t = clazz.ToAronType();
+
+ auto type_writer = type::writer::NlohmannJSONWriter();
+ auto type_json = clazz.writeType(type_writer);
+
+ std::ofstream type_ofs("/tmp/aronJsonExportTestType.json");
+ type_ofs << type_json.dump(2);
+
+ auto data_writer = data::writer::NlohmannJSONWriter();
+ auto data_json = clazz.write(data_writer);
+
+ std::ofstream data_ofs("/tmp/aronJsonExportTestData.json");
+ data_ofs << data_json.dump(2);
+ }
+
+
+ // Try to read data again
+ auto data_reader = data::reader::NlohmannJSONReader();
+ std::ifstream data_ifs("/tmp/aronJsonExportTestData.json");
+ auto data_json = nlohmann::json::parse(data_ifs);
+
+ clazz.read(data_reader, data_json);
+}
+
diff --git a/source/RobotAPI/libraries/aron/converter/binary/CMakeLists.txt b/source/RobotAPI/libraries/aron/converter/binary/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7fa2684a23d3ef1e887a5f11b1c5aa83cbbe4b8f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/binary/CMakeLists.txt
@@ -0,0 +1,34 @@
+set(LIB_NAME aronivtconverter)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_build_if(IVT_FOUND "IVT not available")
+armarx_build_if(OpenCV_FOUND "OpenCV not available")
+
+set(LIBS
+ aron
+ ivt
+ ivtopencv
+
+ ${IVT_LIBRARIES}
+)
+
+set(LIB_FILES
+ IVTConverter.cpp
+)
+
+set(LIB_HEADERS
+ IVTConverter.h
+)
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+if(IVT_FOUND)
+ target_include_directories(aronivtconverter
+ SYSTEM PUBLIC
+ ${IVT_INCLUDE_DIRS}
+ )
+endif()
+
+add_library(RobotAPI::aron::converter::ivt ALIAS aronivtconverter)
diff --git a/source/RobotAPI/libraries/aron/converter/binary/LTMVectorRepresentationConverter.cpp b/source/RobotAPI/libraries/aron/converter/binary/LTMVectorRepresentationConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..999359d6b79ef56bbe7352263556ad71316e7384
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/binary/LTMVectorRepresentationConverter.cpp
@@ -0,0 +1,53 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, 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/>.
+ *
+ * @author Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+// STD/STL
+#include <numeric>
+
+// Header
+#include "IVTConverter.h"
+
+namespace armarx::aron::converter
+{
+ std::shared_ptr<CByteImage> AronIVTConverter::ConvertToCByteImage(const data::NDArrayPtr& nav)
+ {
+ ARMARX_CHECK_NOT_NULL(nav);
+
+ if (nav->getShape().size() != 3) // +1 for bytes per pixel
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "The size of an NDArray does not match.", nav->getPath());
+ }
+ auto dims = nav->getShape();
+
+ auto ret = std::make_shared<CByteImage>();
+ ret->Set(dims[0], dims[1], static_cast<CByteImage::ImageType>(std::stoi(nav->getType())));
+ memcpy(reinterpret_cast<unsigned char*>(ret->pixels), nav->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ return ret;
+ }
+
+ data::NDArrayPtr ConvertFromCByteImage(const std::shared_ptr<CByteImage>& img)
+ {
+ // TODO:
+ return nullptr;
+ }
+}
diff --git a/source/RobotAPI/libraries/aron/converter/binary/LTMVectorRepresentationConverter.h b/source/RobotAPI/libraries/aron/converter/binary/LTMVectorRepresentationConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..6d0c23d3b598c7dc55334c7181fa39885ee15ce4
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/converter/binary/LTMVectorRepresentationConverter.h
@@ -0,0 +1,45 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @author Fabian Peller (fabian dot peller at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+// STD/STL
+#include <memory>
+#include <string>
+
+// IVT
+#include <Image/ByteImage.h>
+
+// ArmarX
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+
+namespace armarx::aron::converter
+{
+ class AronIVTConverter
+ {
+ AronIVTConverter() = delete;
+ public:
+ static std::shared_ptr<CByteImage> ConvertToCByteImage(const data::NDArrayPtr&);
+ static data::NDArrayPtr ConvertFromCByteImage(const std::shared_ptr<CByteImage>&);
+ };
+}
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 6c45ffbdb7400b4d3bbc365559c3fa6563e1fd89..9d83042a1ce7e0254c827350a87d515f05761665 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -135,6 +135,16 @@ namespace armarx::aron::converter
}
}
+ template<typename T>
+ static data::NDArrayPtr ConvertFromQuaternion(
+ const Eigen::Quaternion<T>& quat)
+ {
+ data::NDArrayPtr ndArr(new data::NDArray);
+ ndArr->setShape({1, 4});
+ ndArr->setData(sizeof(T) * 4, reinterpret_cast <const unsigned char* >(quat.coeffs().data()));
+
+ return ndArr;
+ }
template<typename T>
static data::NDArrayPtr ConvertFromMatrix(
@@ -148,6 +158,18 @@ namespace armarx::aron::converter
return ndArr;
}
+ template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
+ static data::NDArrayPtr ConvertFromMatrix(
+ const Eigen::Matrix < T, Rows, Cols >& mat)
+ {
+ data::NDArrayPtr ndArr(new data::NDArray);
+
+ ndArr->setShape({Rows, Cols});
+ ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+ return ndArr;
+ }
+
// Eigen::Array
diff --git a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
index e6429e7fe75ba54fea8a38c6f41398c0d149d2a5..3729e90a5812a192c256f2deaaa13bbecad71bab 100644
--- a/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
+++ b/source/RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.cpp
@@ -1,5 +1,7 @@
#include "NLohmannJSONConverter.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx::aron::converter
{
nlohmann::json AronNlohmannJSONConverter::ConvertToNlohmannJSON(const data::VariantPtr& aron)
diff --git a/source/RobotAPI/libraries/aron/core/Exception.h b/source/RobotAPI/libraries/aron/core/Exception.h
index 52de8098900e7a8dc5538aa4d53453cffe52a7e5..fc080bafd09e2d7424bf0fc18538539f680329da 100644
--- a/source/RobotAPI/libraries/aron/core/Exception.h
+++ b/source/RobotAPI/libraries/aron/core/Exception.h
@@ -26,6 +26,7 @@
// STD/STL
// ArmarX
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/interface/aron.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/exceptions/Exception.h>
@@ -52,6 +53,14 @@ namespace armarx::aron::error
LocalException(prettymethod + ": " + reason + ". The path was: " + path.toString())
{
}
+
+ /// call operator to append a message to the exception. Used by ARMARX_CHECK_AND_THROW
+ AronException& operator()(const std::string& additionalMessage = "")
+ {
+ auto currentReason = getReason();
+ setReason(currentReason + ". Additional Message: " + additionalMessage);
+ return *this;
+ }
};
/**
diff --git a/source/RobotAPI/libraries/aron/core/aron_conversions.h b/source/RobotAPI/libraries/aron/core/aron_conversions.h
index d0f88cab840be59b86bbeb22d7229afb9f9f0b48..d27d6415877d4c15f9ddf36310e656590f8efd7e 100644
--- a/source/RobotAPI/libraries/aron/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/core/aron_conversions.h
@@ -3,12 +3,29 @@
#include <map>
#include <memory>
#include <optional>
+#include <type_traits>
#include <vector>
#include "Path.h"
namespace armarx::aron
{
+
+namespace detail
+{
+
+ // Helper concept to avoid ambiguities
+ template<typename DtoT, typename BoT>
+ concept DtoAndBoAreSame = std::is_same<DtoT, BoT>::value;
+
+ template <class ...>
+ struct is_optional : public std::false_type {};
+
+ template <class ...Ts>
+ struct is_optional<std::optional<Ts...>> : public std::true_type {};
+
+}
+
/**
* Framework for converting ARON DTOs (Data Transfer Objects) to C++ BOs
* (Business Objects) and back.
@@ -51,12 +68,6 @@ namespace armarx::aron
* }
* @endcode
*/
-
- // Helper concept to avoid ambiguities
- template<typename DtoT, typename BoT>
- concept DtoAndBoAreSame = std::is_same<DtoT, BoT>::value;
-
-
// Same type
template <class T>
void toAron(T& dto, const T& bo)
@@ -139,6 +150,24 @@ namespace armarx::aron
}
}
+
+ // One-sided optional
+ template <class DtoT, class BoT>
+ requires (not detail::is_optional<BoT>::value)
+ void toAron(std::optional<DtoT>& dto, const BoT& bo)
+ {
+ dto = DtoT{};
+ toAron(*dto, bo);
+ }
+ template <class DtoT, class BoT>
+ requires (not detail::is_optional<DtoT>::value)
+ void fromAron(DtoT& dto, const std::optional<BoT>& bo)
+ {
+ bo = BoT{};
+ fromAron(dto, *bo);
+ }
+
+
// Flag-controlled optional
template <class DtoT, class BoT>
void toAron(DtoT& dto, bool& dtoValid, const BoT& bo, bool boValid)
@@ -154,6 +183,7 @@ namespace armarx::aron
}
}
+
template <class DtoT, class BoT>
void fromAron(const DtoT& dto, bool dtoValid, BoT& bo, bool& boValid)
{
@@ -222,7 +252,7 @@ namespace armarx::aron
// std::map
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(DtoAndBoAreSame<DtoKeyT, BoKeyT> and DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void toAron(std::map<DtoKeyT, DtoValueT>& dtoMap,
const std::map<BoKeyT, BoValueT>& boMap)
{
@@ -236,7 +266,7 @@ namespace armarx::aron
}
}
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(DtoAndBoAreSame<DtoKeyT, BoKeyT> and DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap,
std::map<BoKeyT, BoValueT>& boMap)
{
@@ -252,7 +282,7 @@ namespace armarx::aron
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(DtoAndBoAreSame<DtoKeyT, BoKeyT> and DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
std::map<DtoKeyT, DtoValueT> toAron(const std::map<BoKeyT, BoValueT>& boMap)
{
std::map<DtoKeyT, DtoValueT> dtoMap;
@@ -343,20 +373,20 @@ namespace armarx
// std::vector
template <class DtoT, class BoT>
- requires (!aron::DtoAndBoAreSame<DtoT, BoT>)
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void toAron(std::vector<DtoT>& dtos, const std::vector<BoT>& bos)
{
armarx::aron::toAron(dtos, bos);
}
template <class DtoT, class BoT>
- requires (!aron::DtoAndBoAreSame<DtoT, BoT>)
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
void fromAron(const std::vector<DtoT>& dtos, std::vector<BoT>& bos)
{
armarx::aron::fromAron(dtos, bos);
}
template <class DtoT, class BoT>
- requires (!aron::DtoAndBoAreSame<DtoT, BoT>)
+ requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
std::vector<DtoT> toAron(const std::vector<BoT>& bos)
{
return armarx::aron::toAron<DtoT, BoT>(bos);
@@ -365,13 +395,13 @@ namespace armarx
// std::map
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void toAron(std::map<DtoKeyT, DtoValueT>& dtoMap, const std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::toAron(dtoMap, boMap);
}
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
void fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap, std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::fromAron(dtoMap, boMap);
@@ -379,7 +409,7 @@ namespace armarx
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
std::map<DtoKeyT, DtoValueT> toAron(const std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::toAron<DtoKeyT, DtoValueT, BoKeyT, BoValueT>(boMap);
diff --git a/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h b/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h
index 552c653a9bb983f1066a7ef87737fa69377c8a8a..8372d89c318cf821a9056c6e5f00f58f372492db 100644
--- a/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h
@@ -30,6 +30,8 @@
#include <map>
// ArmarX
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
index 21fdfc82b0fdec76910890100721d6f5ee19654c..e6beb0e9aab36bc5aa245155b7a42b3d0acb82ab 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -52,7 +52,8 @@ namespace armarx::aron::data::reader
data::Descriptor NlohmannJSONReader::getDescriptor(InputType& input)
{
- return ConstNlohmannJSONVisitor::GetDescriptor(input);
+ auto ret = ConstNlohmannJSONVisitor::GetDescriptor(input);
+ return ret;
}
void NlohmannJSONReader::readList(const nlohmann::json& input, std::vector<nlohmann::json>& elements, Path& p)
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
index e773c517d9ceee0c8c166f327b639a2c8f799f6b..985f914d16d8c501992aeab2b44a0c7a00d9b8e6 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
@@ -19,13 +19,21 @@
*/
// STD/STL
+#include <cmath>
+#include <cstdint>
+#include <cstring>
#include <memory>
#include <numeric>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
// Header
#include "NlohmannJSONReaderWithoutTypeCheck.h"
// ArmarX
+#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h>
@@ -56,6 +64,26 @@ namespace armarx::aron::data::reader
elements = input.get<std::map<std::string, nlohmann::json>>();
}
+
+ const std::map<std::string, type::matrix::ElementType> ElementTypeAsString = {
+ {"short", ::armarx::aron::type::matrix::INT16},
+ {"int", ::armarx::aron::type::matrix::INT32},
+ {"long", ::armarx::aron::type::matrix::INT64},
+ {"float", ::armarx::aron::type::matrix::FLOAT32},
+ {"double", ::armarx::aron::type::matrix::FLOAT64}};
+
+ template <typename T>
+ void
+ readTo(const nlohmann::json& input, std::vector<unsigned char>& data)
+ {
+ const std::vector<T> d = input.at("data").get<std::vector<T>>();
+
+ const std::size_t bufferLen = d.size() * sizeof(T);
+
+ data.resize(bufferLen);
+ memcpy(data.data(), d.data(), bufferLen);
+ }
+
void
NlohmannJSONReaderWithoutTypeCheck::readNDArray(const nlohmann::json& input,
std::vector<int>& shape,
@@ -64,7 +92,37 @@ namespace armarx::aron::data::reader
Path& p)
{
shape = input.at("dims").get<std::vector<int>>();
- data = input.at("data").get<std::vector<unsigned char>>();
+
+ typeAsString = input.at("type").get<std::string>();
+
+ ARMARX_CHECK(ElementTypeAsString.count(typeAsString) > 0)
+ << "Invalid element `" << typeAsString << "`. Valid elements are "
+ << simox::alg::get_keys(ElementTypeAsString) << ".";
+
+ const type::matrix::ElementType elementType = ElementTypeAsString.at(typeAsString);
+
+ // as we need to return a vector<uchar>, we first need to read the json array as the specific type
+ // and then convert it to the vector<uchar>
+ switch (elementType)
+ {
+ case type::matrix::INT16:
+ readTo<std::int16_t>(input, data);
+ break;
+ case type::matrix::INT32:
+ readTo<std::int32_t>(input, data);
+ break;
+ case type::matrix::INT64:
+ readTo<std::int64_t>(input, data);
+ break;
+ case type::matrix::FLOAT32:
+ readTo<std::float_t>(input, data);
+ break;
+ case type::matrix::FLOAT64:
+ readTo<std::double_t>(input, data);
+ break;
+ }
+
+ ARMARX_INFO << VAROUT(typeAsString);
}
void
@@ -104,4 +162,5 @@ namespace armarx::aron::data::reader
{
i = input;
}
+
} // namespace armarx::aron::data::reader
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h
index e0bed7b71869e2abc074484fe3fb9774e0ef6730..2d17af7f5ead72cc51f04623a816e1fb8236c8ad 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h
@@ -69,5 +69,6 @@ namespace armarx::aron::data::reader
void readDouble(InputType& input, double& i, Path& p) override;
void readString(InputType& input, std::string& i, Path& p) override;
void readBool(InputType& input, bool& i, Path& p) override;
+
};
} // namespace armarx::aron::data::reader
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
index 111068b393c86b681179544c4d60fd02953f3f36..483088aa7ffaf706271072d5e13d55f26b8e4ec4 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -142,6 +142,7 @@ namespace armarx::aron::data
{
if (t.empty())
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "The type cannot be empty", getPath());
}
@@ -166,6 +167,7 @@ namespace armarx::aron::data
type::VariantPtr NDArray::recalculateType() const
{
+ ARMARX_TRACE;
throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
}
@@ -179,29 +181,35 @@ namespace armarx::aron::data
case type::Descriptor::MATRIX:
{
auto casted = type::Matrix::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
return (aron->shape.size() == 3 && aron->shape[0] == casted->getRows() && aron->shape[1] == casted->getCols());
}
case type::Descriptor::QUATERNION:
{
auto casted = type::Quaternion::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
return (aron->shape.size() == 3 && aron->shape[0] == 1 && aron->shape[1] == 4);
}
case type::Descriptor::POINTCLOUD:
{
auto casted = type::PointCloud::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
return (aron->shape.size() == 3);
}
case type::Descriptor::IMAGE:
{
auto casted = type::Image::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
return (aron->shape.size() == 3);
}
case type::Descriptor::NDARRAY:
{
auto casted = type::NDArray::DynamicCastAndCheck(type);
+ ARMARX_TRACE;
return (aron->shape.size() == (unsigned int) casted->getNumberDimensions());
}
default:
+ ARMARX_TRACE;
return false;
}
}
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 478c6a2c80a4984fcc4fb19ee9dba611eb384af9..9724ae6279a2b2a2140d5964d7d101f6e9fbabc8 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -25,10 +25,12 @@
#include "Dict.h"
// ArmarX
-#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
-
#include <SimoxUtility/algorithm/string/string_conversion.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
+
namespace armarx::aron::data
{
@@ -135,6 +137,7 @@ namespace armarx::aron::data
{
if (hasElement(key))
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "The key '"+key+"' already exists in a aron dict.");
}
setElement(key, data);
@@ -150,6 +153,7 @@ namespace armarx::aron::data
auto it = childrenNavigators.find(key);
if (it == childrenNavigators.end())
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "Could not find key '" + key + "'. But I found the following keys: [" + simox::alg::join(this->getAllKeys(), ", ") + "]", getPath());
}
return it->second;
@@ -201,8 +205,10 @@ namespace armarx::aron::data
return "armarx::aron::data::Dict";
}
+ // TODO
type::VariantPtr Dict::recalculateType() const
{
+ ARMARX_TRACE;
throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
}
@@ -216,42 +222,67 @@ namespace armarx::aron::data
{
case type::Descriptor::OBJECT:
{
+ ARMARX_TRACE;
auto objectTypeNav = type::Object::DynamicCastAndCheck(type);
for (const auto& [key, nav] : childrenNavigators)
{
if (!objectTypeNav->hasMemberType(key))
{
+ ARMARX_TRACE;
return false;
}
- if (!nav || !objectTypeNav->getMemberType(key))
+
+ auto childTypeNav = objectTypeNav->getMemberType(key);
+ if (!nav)
{
- return false;
+ ARMARX_TRACE;
+ if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
+ return true;
}
- if (!nav->fullfillsType(objectTypeNav->getMemberType(key)))
+ if (!nav->fullfillsType(childTypeNav))
{
+ ARMARX_TRACE;
return false;
}
}
+ ARMARX_TRACE;
return true;
}
case type::Descriptor::DICT:
{
+ ARMARX_TRACE;
auto dictTypeNav = type::Dict::DynamicCastAndCheck(type);
for (const auto& [key, nav] : childrenNavigators)
{
(void) key;
- if (!nav || !dictTypeNav->getAcceptedType())
+ auto childTypeNav = dictTypeNav->getAcceptedType();
+ if (!nav)
{
- return false;
+ ARMARX_TRACE;
+ if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
+ return true;
}
- if (!nav->fullfillsType(dictTypeNav->getAcceptedType()))
+ if (!nav->fullfillsType(childTypeNav))
{
+ ARMARX_TRACE;
return false;
}
}
+ ARMARX_TRACE;
return true;
}
default:
+ ARMARX_TRACE;
return false;
}
}
@@ -275,11 +306,13 @@ namespace armarx::aron::data
{
if (!path.hasElement())
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate without a valid path", path);
}
std::string el = path.getFirstElement();
if (!hasElement(el))
{
+ ARMARX_TRACE;
throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", el, path);
}
@@ -292,6 +325,7 @@ namespace armarx::aron::data
Path next = path.withDetachedFirstElement();
if (!childrenNavigators.at(el))
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
}
return childrenNavigators.at(el)->navigateAbsolute(next);
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
index 84bf1a2005b2d57f03065de58f65cf31870c8ea8..c53b51ee1b04c3e903b4ddd718b9793d75303412 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/List.cpp
@@ -26,10 +26,12 @@
// ArmarX
#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+
#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/List.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Pair.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
namespace armarx::aron::data
{
@@ -125,7 +127,8 @@ namespace armarx::aron::data
{
if (i > aron->elements.size())
{
- error::AronException(__PRETTY_FUNCTION__, "Cannot set a listelement at index " + std::to_string(i) + " because this list has size " + std::to_string(aron->elements.size()));
+ ARMARX_TRACE;
+ throw error::AronException(__PRETTY_FUNCTION__, "Cannot set a listelement at index " + std::to_string(i) + " because this list has size " + std::to_string(aron->elements.size()));
}
if (i == aron->elements.size())
@@ -163,6 +166,7 @@ namespace armarx::aron::data
{
if (i >= childrenNavigators.size())
{
+ ARMARX_TRACE;
throw error::ValueNotValidException(__PRETTY_FUNCTION__, "The index is out of bounds (size = " + std::to_string(childrenNavigators.size()) + ")", std::to_string(i), getPath());
}
return childrenNavigators[i];
@@ -206,6 +210,7 @@ namespace armarx::aron::data
// TODO
type::VariantPtr List::recalculateType() const
{
+ ARMARX_TRACE;
throw error::NotImplementedYetException(__PRETTY_FUNCTION__);
}
@@ -218,18 +223,28 @@ namespace armarx::aron::data
{
case type::Descriptor::LIST:
{
+ ARMARX_TRACE;
auto listTypeNav = type::List::DynamicCastAndCheck(type);
for (const auto& nav : childrenNavigators)
{
- if (!nav && !listTypeNav->getAcceptedType())
+ auto childTypeNav = listTypeNav->getAcceptedType();
+ if (!nav)
{
- return false;
+ if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
+ return true;
}
- if (!nav->fullfillsType(listTypeNav->getAcceptedType()))
+ if (!nav->fullfillsType(childTypeNav))
{
+ ARMARX_TRACE;
return false;
}
}
+ ARMARX_TRACE;
return true;
}
case type::Descriptor::TUPLE:
@@ -238,15 +253,30 @@ namespace armarx::aron::data
unsigned int i = 0;
for (const auto& nav : childrenNavigators)
{
- if (!nav && !tupleTypeNav->getAcceptedType(i))
+ if (!tupleTypeNav->hasAcceptedType(i))
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+
+ auto childTypeNav = tupleTypeNav->getAcceptedType(i);
+ if (!nav)
{
+ if (childTypeNav && childTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
return false;
}
if (!nav->fullfillsType(tupleTypeNav->getAcceptedType(i)))
{
+ ARMARX_TRACE;
return false;
}
}
+ ARMARX_TRACE;
return true;
}
case type::Descriptor::PAIR:
@@ -254,19 +284,36 @@ namespace armarx::aron::data
auto pairTypeNav = type::Pair::DynamicCastAndCheck(type);
if (childrenSize() != 2)
{
+ ARMARX_TRACE;
return false;
}
- if (!childrenNavigators[0] && !pairTypeNav->getFirstAcceptedType())
+ auto firstChildTypeNav = pairTypeNav->getFirstAcceptedType();
+ if (!childrenNavigators[0])
{
+ if (firstChildTypeNav && firstChildTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
return false;
}
- if (!childrenNavigators[1] && !pairTypeNav->getSecondAcceptedType())
+ auto secondChildTypeNav = pairTypeNav->getSecondAcceptedType();
+ if (!childrenNavigators[1])
{
+ if (secondChildTypeNav && secondChildTypeNav->getMaybe() == type::Maybe::NONE)
+ {
+ ARMARX_TRACE;
+ return false;
+ }
+ ARMARX_TRACE;
return false;
}
- return childrenNavigators[0]->fullfillsType(pairTypeNav->getFirstAcceptedType()) && childrenNavigators[1]->fullfillsType(pairTypeNav->getSecondAcceptedType());
+ ARMARX_TRACE;
+ return childrenNavigators[0]->fullfillsType(firstChildTypeNav) && childrenNavigators[1]->fullfillsType(secondChildTypeNav);
}
default:
+ ARMARX_TRACE;
return false;
}
}
@@ -290,6 +337,7 @@ namespace armarx::aron::data
unsigned int i = std::stoi(path.getFirstElement());
if (!hasElement(i))
{
+ ARMARX_TRACE;
throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Could not find an element of a path.", std::to_string(i), std::to_string(childrenSize()));
}
@@ -302,6 +350,7 @@ namespace armarx::aron::data
Path next = path.withDetachedFirstElement();
if (!childrenNavigators.at(i))
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate into a NULL member. Seems like the member is optional and not set.", next);
}
return childrenNavigators.at(i)->navigateAbsolute(next);
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
index 0e68bbc6dc16c373dd63c4626a45f5b1377afad4..2f7fe6fc8e486c43c32875c1cf072e721146c402 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
@@ -95,6 +95,7 @@ namespace armarx::aron::data
}
else
{
+ ARMARX_TRACE;
throw error::AronException(__PRETTY_FUNCTION__, "Could not set from string. Got: '" + setter + "'");
}
}
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
index 84adf5c1ea7643ece74e2bc3e224e5df075f7a12..e80e7d37e071585f8b6f3ae0e7d0f81d280b49e4 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
@@ -33,6 +33,7 @@ namespace armarx::aron::data
return data::Descriptor::UNKNOWN;
}
std::string t = n[armarx::aron::data::rw::json::constantes::TYPE_SLUG];
+
return armarx::aron::data::rw::json::conversion::String2Descriptor.at(t);
}
diff --git a/source/RobotAPI/libraries/aron/core/rw.h b/source/RobotAPI/libraries/aron/core/rw.h
index 6257f2422687591b0991afed8a0577ee127207ef..95b6aac606f47d3c46de34ac5ec077ded28c41a4 100644
--- a/source/RobotAPI/libraries/aron/core/rw.h
+++ b/source/RobotAPI/libraries/aron/core/rw.h
@@ -26,7 +26,7 @@ namespace armarx::aron
}
template<class ReaderT, class DtoT, class BoT>
- requires (data::isReader<ReaderT> && !DtoAndBoAreSame<DtoT, BoT>)
+ requires (data::isReader<ReaderT> && !detail::DtoAndBoAreSame<DtoT, BoT>)
inline void read(ReaderT& aron_r, typename ReaderT::InputType& input, BoT& ret)
{
DtoT aron;
@@ -36,7 +36,7 @@ namespace armarx::aron
}
template<class WriterT, class DtoT, class BoT>
- requires (data::isWriter<WriterT> && !DtoAndBoAreSame<DtoT, BoT>)
+ requires (data::isWriter<WriterT> && !detail::DtoAndBoAreSame<DtoT, BoT>)
inline void write(WriterT& aron_w, const BoT& input, typename WriterT::ReturnType& ret, const armarx::aron::Path& aron_p = armarx::aron::Path())
{
DtoT aron;
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
index 0879de17b8e9258a211646e2b7c8d7c4bdc37ace..51d45e3975bb162460fd06893e91f388ac52b9f1 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.cpp
@@ -64,6 +64,11 @@ namespace armarx::aron::type
return acceptedTypes;
}
+ bool Tuple::hasAcceptedType(unsigned int i) const
+ {
+ return i < acceptedTypes.size();
+ }
+
VariantPtr Tuple::getAcceptedType(unsigned int i) const
{
return acceptedTypes[i];
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
index d464f83269d5074e1ec6e759119b433c76de0f7d..3281584c15d6b4bb3e50c83ce598634a2bbec324 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/container/Tuple.h
@@ -46,6 +46,7 @@ namespace armarx::aron::type
// public member functions
std::vector<VariantPtr> getAcceptedTypes() const;
+ bool hasAcceptedType(unsigned int i) const;
VariantPtr getAcceptedType(unsigned int i) const;
void addAcceptedType(const VariantPtr&);
diff --git a/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp b/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aac543a04b6ba1696dedc7a21c7d4ea125896e4e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp
@@ -0,0 +1,38 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::aron_cpp_to_python_conv_test
+ * @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 "AronConversionTester.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+
+namespace armarx::aron::test
+{
+
+ AronConversionTester::AronConversionTester(dti::AronConversionTestInterfacePrx python) : interface(python)
+ {
+ ARMARX_CHECK(python);
+ }
+
+
+} // namespace armarx::aron::test
diff --git a/source/RobotAPI/libraries/aron/test/AronConversionTester.h b/source/RobotAPI/libraries/aron/test/AronConversionTester.h
new file mode 100644
index 0000000000000000000000000000000000000000..b7ad4b8dfdcf55a37f8efc3116c27440d925d428
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/AronConversionTester.h
@@ -0,0 +1,111 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::aron_cpp_to_python_conv_test
+ * @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/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/interface/aron/test/AronConversionTestInterface.h>
+
+
+namespace armarx::aron::test
+{
+
+ /**
+ * @brief Helper class for implementing distributed ARON conversion tests
+ * based on the `dti::AronConversionTestInterfacePrx`.
+ *
+ * Example usage:
+ *
+ * @code
+ *
+ * armarx::aron::test::dti::AronConversionTestInterfacePrx pythonComponent = ...;
+ *
+ * auto myAronClassProbeFn = []()
+ * {
+ * my::arondto::MyAronClass probe;
+ * probe.data = "42";
+ * return probe,
+ * };
+ *
+ * armarx::aron::test::AronConversionTester tester(pythonComponent);
+ *
+ * tester.test<my::arondto::MyAronClass>(myAronClassProbeFn, "MyAronClass");
+ *
+ * @endcode
+ *
+ * Note that the `pythonComponent` must point to a corresponding
+ * implementation of the `AronConversionTestInterfacePrx`.
+ */
+ class AronConversionTester
+ {
+ public:
+ template <class AronClassT>
+ using ProbeFn = std::function<AronClassT()>;
+
+
+ public:
+ AronConversionTester(dti::AronConversionTestInterfacePrx interface);
+
+ /**
+ * @brief Test the conversion of a specific ARON class.
+ *
+ * @param probeFn A factory function creating a test instance of the
+ * ARON class.
+ * @param aronClassName The name of the ARON class. Can be used by the
+ * other component to decide which class to convert to.
+ */
+ template <class AronClassT>
+ void
+ test(ProbeFn<AronClassT> probeFn, const std::string& aronClassName)
+ {
+ std::stringstream ss;
+ ss << "Test for ARON class '" << aronClassName << "': ";
+
+ const AronClassT probe = probeFn();
+
+ dto::TestAronConversionRequest req;
+ req.aronClassName = aronClassName;
+ req.probe = probe.toAronDTO();
+
+ dto::TestAronConversionResponse res = interface->testAronConversion(req);
+
+ if (res.success)
+ {
+ const AronClassT probeOut = AronClassT::FromAron(res.probe);
+ ARMARX_CHECK(probeOut == probe);
+
+ ARMARX_IMPORTANT << ss.str() << "Success";
+ }
+ else
+ {
+ ARMARX_WARNING << ss.str() << "Conversion in Python component failed: \n" << res.errorMessage;
+ }
+ }
+
+ public:
+ dti::AronConversionTestInterfacePrx interface;
+ };
+
+} // namespace armarx::aron::test
diff --git a/source/RobotAPI/libraries/aron/test/CMakeLists.txt b/source/RobotAPI/libraries/aron/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3a199acf0f19c13f6253e66947e540bfe3950efd
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/test/CMakeLists.txt
@@ -0,0 +1,25 @@
+set(LIB_NAME arontest)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ # ArmarXCore
+ ArmarXCore
+ # RobotAPI
+ RobotAPICore
+ RobotAPIInterfaces
+ aron
+
+ HEADERS
+ AronConversionTester.h
+
+ SOURCES
+ AronConversionTester.cpp
+)
+
+
+add_library(aron::test ALIAS arontest)
+add_library(${PROJECT_NAME}::Aron::test ALIAS arontest)
diff --git a/source/RobotAPI/libraries/aron_component_config/CMakeLists.txt b/source/RobotAPI/libraries/aron_component_config/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5b6b176ca9b2e9d303251db890ab28939ac1544b
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/CMakeLists.txt
@@ -0,0 +1,60 @@
+#armarx_add_library(AronComponentConfig
+# DEPENDENCIES
+# ArmarXCoreInterfaces ArmarXCore ArmarXGuiComponentPlugins
+# RobotAPIInterfaces RobotAPICore RobotAPIComponentPlugins
+# SOURCES
+# PropertyDefinitionVisitors.cpp
+# RemoteGuiVisitors.cpp
+# HEADERS
+# PropertyDefinitionVisitors.h
+# RemoteGui.h
+# RemoteGuiVisitors.h
+# Util.h
+# ComponentPlugin.h
+# )
+
+set(LIB_NAME aron_component_config)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+find_package(Simox QUIET)
+armarx_build_if(Simox_FOUND "Simox not available")
+
+set(LIBS
+ ArmarXCoreInterfaces ArmarXCore ArmarXGuiComponentPlugins
+ RobotAPIInterfaces RobotAPICore RobotAPIComponentPlugins aroneigenconverter
+)
+
+set(LIB_FILES
+ PropertyDefinitionVisitors.cpp
+ RemoteGuiVisitors.cpp
+ VariantHelperFactory.cpp
+)
+
+set(LIB_HEADERS
+ PropertyDefinitionVisitors.h
+ RemoteGui.h
+ RemoteGuiVisitors.h
+ Util.h
+ ComponentPlugin.h
+ VariantHelperFactory.h
+)
+
+
+armarx_add_library(
+ LIB_NAME
+ "${LIB_NAME}"
+ SOURCES
+ "${LIB_FILES}"
+ HEADERS
+ "${LIB_HEADERS}"
+ LIBS
+ "${LIBS}"
+)
+
+add_library(
+ RobotAPI::aron_component_config
+ ALIAS
+ aron_component_config
+)
diff --git a/source/RobotAPI/libraries/aron_component_config/ComponentPlugin.h b/source/RobotAPI/libraries/aron_component_config/ComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..288462223d51f039e7b38316c0e24a6f2037801f
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/ComponentPlugin.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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 07.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <experimental/memory>
+
+#include <ArmarXCore/core/ComponentPlugin.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/RemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h>
+
+#include "PropertyDefinitionVisitors.h"
+#include "RemoteGui.h"
+
+namespace armarx::plugins
+{
+ template <typename T>
+ concept isAronGenerated = std::is_base_of<armarx::aron::cpp::AronGeneratedClass, T>::value;
+
+ template <typename AronStructT>
+ requires isAronGenerated<AronStructT>
+ class AronComponentConfigPlugin : public ComponentPlugin
+ {
+ protected:
+ void
+ preOnInitComponent() override
+ {
+ ARMARX_TRACE;
+ armarx::aron::component_config::PropertyDefinitionGetterVisitor vis(
+ parent<armarx::PropertyUser>());
+ auto data = std::static_pointer_cast<armarx::aron::data::Variant>(
+ config_.getUpToDateReadBuffer().toAron());
+ auto type = config_.getUpToDateReadBuffer().ToAronType();
+ armarx::aron::data::visitRecursive(vis, data, type);
+ config_.getWriteBuffer().fromAron(
+ std::static_pointer_cast<armarx::aron::data::Dict>(data));
+ config_.commitWrite();
+ ManagedIceObjectPlugin::preOnInitComponent();
+ }
+
+ void
+ postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) override
+ {
+ ARMARX_TRACE;
+ armarx::aron::component_config::PropertyDefinitionSetterVisitor vis(properties);
+ const auto& config = config_.getUpToDateReadBuffer();
+ armarx::aron::data::visitRecursive(vis, config.toAron(), config.ToAronType());
+ ComponentPlugin::postCreatePropertyDefinitions(properties);
+ }
+
+ public:
+ RemoteGui::detail::GroupBoxBuilder
+ buildRemoteGui(const std::string& name)
+ {
+ ARMARX_TRACE;
+ return RemoteGui::makeConfigGui(name, config_.getUpToDateReadBuffer());
+ }
+
+ bool
+ updateRemoteGui(armarx::RemoteGui::TabProxy&
+ prx) // prx has to be already updated; otherwise nothing will change
+ {
+ ARMARX_TRACE;
+ armarx::aron::component_config::GetValueFromMapVisitor vis(&prx);
+ auto& cfg = config_.getWriteBuffer();
+ auto data = std::static_pointer_cast<armarx::aron::data::Variant>(cfg.toAron());
+ auto type = cfg.ToAronType();
+ armarx::aron::data::visitRecursive(vis, data, type);
+ cfg.fromAron(std::static_pointer_cast<armarx::aron::data::Dict>(data));
+ config_.commitWrite();
+ return vis.tabRebuildRequired();
+ }
+
+ AronComponentConfigPlugin(ManagedIceObject& parent, const std::string& prefix) :
+ ComponentPlugin(parent, prefix)
+ {
+ ARMARX_TRACE;
+ }
+
+ armarx::WriteBufferedTripleBuffer<AronStructT> config_;
+ };
+} // namespace armarx::plugins
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ee7d67c73021d5f72b5e3e266d24938b092fea0e
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.cpp
@@ -0,0 +1,396 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 06.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "PropertyDefinitionVisitors.h"
+
+#include <ArmarXCore/core/application/properties/PropertyDefinition.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/PropertyUser.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron_component_config/VariantHelperFactory.h>
+
+#include "Util.h"
+
+#define INPUT_GUARD(i) \
+ ARMARX_TRACE; \
+ ARMARX_CHECK_NOT_NULL(i); \
+ if (in_list_) \
+ return;
+
+inline std::string
+joinNamespaces(const std::vector<std::string>& namespaces)
+{
+ std::string result = "";
+ std::for_each(
+ namespaces.begin(), namespaces.end(), [&result](const auto& i) { result += i + "."; });
+ return result;
+}
+
+namespace armarx::aron::component_config
+{
+
+ void
+ PropertyDefinitionGetterVisitor::visitInt(DataInput& i, TypeInput& elementType)
+ {
+ INPUT_GUARD(i);
+ auto value = data::Int::DynamicCastAndCheck(i);
+ auto name = global_namespace_ + pathToName(i);
+ property_user_->getProperty(value->getValue(), name);
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitFloat(DataInput& f, TypeInput& elementType)
+ {
+ INPUT_GUARD(f);
+ auto value = data::Float::DynamicCastAndCheck(f);
+ auto name = global_namespace_ + pathToName(f);
+ property_user_->getProperty(value->getValue(), name);
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitDouble(DataInput& d, TypeInput& elementType)
+ {
+ INPUT_GUARD(d);
+ auto value = data::Double::DynamicCastAndCheck(d);
+ auto name = global_namespace_ + pathToName(d);
+ property_user_->getProperty(value->getValue(), name);
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitBool(DataInput& b, TypeInput& elementType)
+ {
+ INPUT_GUARD(b);
+ auto value = data::Bool::DynamicCastAndCheck(b);
+ auto name = global_namespace_ + pathToName(b);
+ property_user_->getProperty(value->getValue(), name);
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitString(DataInput& string, TypeInput& elementType)
+ {
+ INPUT_GUARD(string);
+ auto value = data::String::DynamicCastAndCheck(string);
+ auto name = global_namespace_ + pathToName(string);
+ auto property = property_user_->getProperty<std::string>(name);
+ if (not property.getValue().empty())
+ {
+ value->getValue() = property.getValueAndReplaceAllVars();
+ }
+ else
+ {
+ value->getValue() = "";
+ }
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitListOnEnter(DataInput& o, TypeInput& t)
+ {
+ ARMARX_TRACE;
+ in_list_ = true;
+ const auto& name = global_namespace_ + pathToName(o);
+ const auto& type = type::List::DynamicCast(t)->getAcceptedType()->getDescriptor();
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
+ implementedListDescriptors.end())
+ {
+ return;
+ }
+ auto old_list = data::List::DynamicCast(o);
+ old_list->clear();
+ auto get_list = [this, &name]() -> std::vector<std::string>
+ {
+ std::string list;
+ property_user_->getProperty(list, name);
+ return simox::alg::split(list, ",");
+ };
+ int i = 0;
+ std::vector<std::string> vector = get_list();
+ std::for_each(vector.begin(),
+ vector.end(),
+ [&old_list, &i, &type](const auto& el)
+ {
+ old_list->addElement(factories::VariantHelper::make(type)->from_string(
+ el, old_list->getPath().withIndex(i)));
+ i++;
+ });
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitListOnExit(DataInput& o, TypeInput& t)
+ {
+ in_list_ = false;
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitIntEnum(DataInput& enumData, TypeInput& elementType)
+ {
+ ARMARX_TRACE;
+ auto data = data::Int::DynamicCastAndCheck(enumData);
+ auto name = global_namespace_ + pathToName(enumData);
+ property_user_->getProperty(data->getValue(), name);
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitUnknown(DataInput& unknown, TypeInput& elementType)
+ {
+ ARMARX_WARNING << "Unknown data encountered: "
+ << (unknown ? unknown->getFullName() : "nullptr");
+ }
+
+ PropertyDefinitionGetterVisitor::PropertyDefinitionGetterVisitor(
+ const armarx::PropertyUser& defs) :
+ property_user_(std::experimental::make_observer(&defs)), in_list_(false)
+ {
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitDictOnEnter(std::shared_ptr<data::Variant>& o,
+ const std::shared_ptr<type::Variant>& t)
+ {
+ ARMARX_TRACE;
+ in_list_ = true;
+ const auto& name = global_namespace_ + pathToName(o);
+ const auto& type = type::Dict::DynamicCast(t)->getAcceptedType()->getDescriptor();
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
+ implementedListDescriptors.end())
+ {
+ return;
+ }
+ auto old_dict = data::Dict::DynamicCast(o);
+ old_dict->clear();
+ auto get_list = [this, &name]() -> std::vector<std::string>
+ {
+ std::string list;
+ property_user_->getProperty(list, name);
+ return simox::alg::split(list, ",");
+ };
+ std::vector<std::string> vector = get_list();
+ std::for_each(
+ vector.begin(),
+ vector.end(),
+ [&old_dict, &type](const auto& el)
+ {
+ auto key_value = simox::alg::split(el, ":");
+ old_dict->addElement(
+ key_value.front(),
+ factories::VariantHelper::make(type)->from_string(
+ key_value.back(), old_dict->getPath().withElement(key_value.front())));
+ });
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitDictOnExit(
+ std::shared_ptr<data::Variant>& elementData,
+ const std::shared_ptr<type::Variant>& elementType)
+ {
+ in_list_ = false;
+ }
+
+ PropertyDefinitionGetterVisitor::MapElements
+ PropertyDefinitionGetterVisitor::getObjectElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getObjectElements(o, t);
+ }
+
+ PropertyDefinitionGetterVisitor::MapElements
+ PropertyDefinitionGetterVisitor::getDictElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getDictElements(o, t);
+ }
+
+ PropertyDefinitionGetterVisitor::ListElements
+ PropertyDefinitionGetterVisitor::getListElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getListElements(o, t);
+ }
+
+ PropertyDefinitionGetterVisitor::PairElements
+ PropertyDefinitionGetterVisitor::getPairElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getPairElements(o, t);
+ }
+
+ PropertyDefinitionGetterVisitor::TupleElements
+ PropertyDefinitionGetterVisitor::getTupleElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getTupleElements(o, t);
+ }
+
+ type::Descriptor
+ PropertyDefinitionGetterVisitor::getDescriptor(DataInput& o, TypeInput& t)
+ {
+ return data::ConstTypedVariantVisitor::GetDescriptor(o, t);
+ }
+
+ void
+ PropertyDefinitionGetterVisitor::visitObjectOnEnter(std::shared_ptr<data::Variant>& i,
+ const std::shared_ptr<type::Variant>& j)
+ {
+ INPUT_GUARD(i);
+ auto t = type::Object::DynamicCastAndCheck(j);
+ if (global_namespace_.empty())
+ {
+ global_namespace_ = t->getObjectNameWithoutNamespace() + ".";
+ }
+ }
+
+ PropertyDefinitionSetterVisitor::PropertyDefinitionSetterVisitor(
+ const PropertyDefinitionsPtr& defs) :
+ property_definitions_(std::experimental::make_observer(defs.get()))
+ {
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::IntPtr& i, const type::IntPtr&)
+ {
+ INPUT_GUARD(i);
+ auto name = global_namespace_ + pathToName(i);
+ property_definitions_->defineOptionalProperty<int>(name, i->getValue());
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::FloatPtr& f,
+ const type::FloatPtr&)
+ {
+ INPUT_GUARD(f);
+ auto name = global_namespace_ + pathToName(f);
+ property_definitions_->defineOptionalProperty<float>(name, f->getValue());
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::DoublePtr& d,
+ const type::DoublePtr&)
+ {
+ INPUT_GUARD(d);
+ auto name = global_namespace_ + pathToName(d);
+ property_definitions_->defineOptionalProperty<double>(name, d->getValue());
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::BoolPtr& b, const type::BoolPtr&)
+ {
+ INPUT_GUARD(b);
+ auto name = global_namespace_ + pathToName(b);
+ property_definitions_->defineOptionalProperty<bool>(name, b->getValue());
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::StringPtr& string,
+ const type::StringPtr& type)
+ {
+ INPUT_GUARD(string);
+ auto name = global_namespace_ + pathToName(string);
+ property_definitions_->defineOptionalProperty<std::string>(name, string->getValue());
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariant(const data::IntPtr& i,
+ const type::IntEnumPtr& t)
+ {
+ INPUT_GUARD(i);
+ auto name = global_namespace_ + pathToName(i);
+ property_definitions_->defineOptionalProperty<int>(name, i->getValue())
+ .map(t->getAcceptedValueMap());
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariantOnEnter(const data::ListPtr& l,
+ const type::ListPtr& t)
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(l);
+ in_list_ = true;
+ const auto& name = global_namespace_ + pathToName(l);
+ const auto& type = t->getAcceptedType()->getDescriptor();
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
+ implementedListDescriptors.end())
+ {
+ return;
+ }
+ const auto& input = l->getElements();
+ std::string str;
+ std::vector<std::string> vector;
+ std::transform(input.begin(),
+ input.end(),
+ std::back_inserter(vector),
+ [&type](const auto& el)
+ { return factories::VariantHelper::make(type)->to_string(el); });
+ str = simox::alg::to_string(vector, ", ");
+ property_definitions_->defineOptionalProperty<std::string>(name, str);
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::ListPtr&,
+ const type::ListPtr&)
+ {
+ in_list_ = false;
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariantOnEnter(const data::DictPtr& d,
+ const type::DictPtr& t)
+ {
+ ARMARX_TRACE;
+ in_list_ = true;
+ const auto& name = global_namespace_ + pathToName(d);
+ const auto& type = t->getAcceptedType()->getDescriptor();
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
+ implementedListDescriptors.end())
+ {
+ return;
+ }
+ const auto& input = d->getElements();
+ std::stringstream ss;
+ for (const auto& [key, el] : input)
+ {
+ ss << key << ":";
+ ss << factories::VariantHelper::make(type)->to_string(el);
+ ss << ",";
+ }
+ std::string value = ss.str();
+ if (not value.empty())
+ {
+ value.pop_back();
+ }
+ property_definitions_->defineOptionalProperty<std::string>(name, value);
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariantOnExit(const data::DictPtr&,
+ const type::DictPtr&)
+ {
+ in_list_ = false;
+ }
+
+ void
+ PropertyDefinitionSetterVisitor::visitAronVariantOnEnter(const data::DictPtr& obj,
+ const type::ObjectPtr& type)
+ {
+ INPUT_GUARD(obj);
+ if (global_namespace_.empty())
+ {
+ global_namespace_ = type->getObjectNameWithoutNamespace() + ".";
+ }
+ }
+} // namespace armarx::aron::component_config
+
+#undef INPUT_GUARD
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h
new file mode 100644
index 0000000000000000000000000000000000000000..3d48904146e5e7e5d01b90015df70d40db3a7b98
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h
@@ -0,0 +1,116 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 06.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <experimental/memory>
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+namespace armarx::aron::component_config
+{
+
+ class PropertyDefinitionSetterVisitor : public aron::data::RecursiveConstTypedVariantVisitor
+ {
+ public:
+ explicit PropertyDefinitionSetterVisitor(const armarx::PropertyDefinitionsPtr& defs);
+
+ void visitAronVariant(const data::IntPtr&, const type::IntEnumPtr&) override;
+
+ void visitAronVariant(const data::IntPtr&, const type::IntPtr&) override;
+
+ void visitAronVariant(const data::FloatPtr&, const type::FloatPtr&) override;
+
+ void visitAronVariant(const data::DoublePtr&, const type::DoublePtr&) override;
+
+ void visitAronVariant(const data::BoolPtr&, const type::BoolPtr&) override;
+
+ void visitAronVariant(const data::StringPtr&, const type::StringPtr&) override;
+
+ void visitAronVariantOnEnter(const data::ListPtr&, const type::ListPtr&) override;
+
+ void visitAronVariantOnExit(const data::ListPtr&, const type::ListPtr&) override;
+
+ void visitAronVariantOnEnter(const data::DictPtr&, const type::DictPtr&) override;
+
+ void visitAronVariantOnExit(const data::DictPtr&, const type::DictPtr&) override;
+
+ void visitAronVariantOnEnter(const data::DictPtr& ptr,
+ const type::ObjectPtr& objectPtr) override;
+
+ private:
+ std::experimental::observer_ptr<PropertyDefinitionContainer> property_definitions_;
+ std::string global_namespace_{""};
+ std::atomic<bool> in_list_{false};
+ };
+
+ class PropertyDefinitionGetterVisitor :
+ public aron::data::RecursiveTypedVisitor<data::VariantPtr, const type::VariantPtr>
+ {
+ public:
+ explicit PropertyDefinitionGetterVisitor(const armarx::PropertyUser& defs);
+
+ type::Descriptor getDescriptor(DataInput& o, TypeInput& t) override;
+
+ MapElements getObjectElements(DataInput& o, TypeInput& t) override;
+
+ MapElements getDictElements(DataInput& o, TypeInput& t) override;
+
+ ListElements getListElements(DataInput& o, TypeInput& t) override;
+
+ PairElements getPairElements(DataInput& o, TypeInput& t) override;
+
+ TupleElements getTupleElements(DataInput& o, TypeInput& t) override;
+
+ void visitInt(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitFloat(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitDouble(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitBool(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitString(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitUnknown(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitListOnEnter(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitListOnExit(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitDictOnEnter(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitDictOnExit(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitObjectOnEnter(DataInput& elementData, TypeInput& elementType) override;
+
+ void visitIntEnum(DataInput& elementData, TypeInput& elementType) override;
+
+ private:
+ std::experimental::observer_ptr<const armarx::PropertyUser> property_user_;
+ std::string global_namespace_{""};
+ std::atomic<bool> in_list_;
+ };
+
+} // namespace armarx::aron::component_config
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/RemoteGui.h b/source/RobotAPI/libraries/aron_component_config/RemoteGui.h
new file mode 100644
index 0000000000000000000000000000000000000000..ddc281c6d9b22fccdb3774fbfc418e0ea15de236
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/RemoteGui.h
@@ -0,0 +1,60 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 07.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXGui/libraries/RemoteGui/RemoteGui.h>
+
+#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
+
+#include "RemoteGuiVisitors.h"
+
+namespace armarx::RemoteGui
+{
+ template <typename T>
+ concept isAronGenerated = std::is_base_of<armarx::aron::cpp::AronGeneratedClass, T>::value;
+
+ template <typename AronStructT>
+ requires isAronGenerated<AronStructT>
+ detail::GroupBoxBuilder
+ makeConfigGui(const std::string& name, const AronStructT& val)
+ {
+ aron::component_config::MakeConfigGuiVisitor vis(name);
+ armarx::aron::data::visitRecursive(vis, val.toAron(), val.ToAronType());
+ return vis.getGroupBoxBuilder();
+ }
+
+ // template <typename AronStructT>
+ // requires isAronGenerated<AronStructT>
+ // void getValueFromMap(AronStructT& cfg,
+ // RemoteGui::ValueMap const& values, std::string const& name)
+ // {
+ // armarx::aron::component_config::GetValueFromMapVisitor vis(&values);
+ // auto data = std::static_pointer_cast<armarx::aron::data::Variant>(cfg.toAron());
+ // auto type = cfg.ToAronType();
+ // armarx::aron::data::visitRecursive(vis, data, type);
+ // cfg.fromAron(std::static_pointer_cast<armarx::aron::data::Dict>(data));
+ // }
+
+} // namespace armarx::RemoteGui
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fc6ed525f1ff9239f9eb4f3078e66203e0baa44d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.cpp
@@ -0,0 +1,684 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 07.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RemoteGuiVisitors.h"
+
+#include <SimoxUtility/math/convert/quat_to_rpy.h>
+#include <SimoxUtility/math/convert/rpy_to_quat.h>
+
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+
+#include <ArmarXGui/libraries/RemoteGui/RemoteGui.h>
+
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron_component_config/VariantHelperFactory.h>
+
+#include "Util.h"
+
+#define INPUT_GUARD(i) \
+ ARMARX_TRACE; \
+ ARMARX_CHECK_NOT_NULL(i); \
+ if (in_list_) \
+ return;
+
+namespace armarx::aron::component_config
+{
+
+ void
+ MakeConfigGuiVisitor::visitObjectOnEnter(DataInput& dict, TypeInput& /*unused*/)
+ {
+ INPUT_GUARD(dict);
+
+ if (dict->getPath().hasElement())
+ {
+ std::string name = pathToName(dict);
+ group_hierarchy_.emplace_back(std::make_shared<RemoteGui::detail::GroupBoxBuilder>(
+ RemoteGui::makeGroupBox(name)));
+ }
+ else
+ {
+ group_hierarchy_.push_back(builder_);
+ }
+ }
+
+ void
+ MakeConfigGuiVisitor::visitObjectOnExit(DataInput& dict, TypeInput& /*j*/)
+ {
+ INPUT_GUARD(dict);
+ auto builder = *group_hierarchy_.back();
+ group_hierarchy_.pop_back();
+ if (!group_hierarchy_.empty())
+ {
+ group_hierarchy_.back()->addChild(builder);
+ }
+ }
+
+ void
+ MakeConfigGuiVisitor::visitInt(DataInput& i, TypeInput& /*unused*/)
+ {
+ INPUT_GUARD(i);
+ auto value = data::Int::DynamicCastAndCheck(i);
+ const auto& name = pathToName(i);
+ auto group = RemoteGui::makeHBoxLayout(name + "_layout");
+ group.addChild(RemoteGui::makeLabel(name + "_label").value(i->getPath().getLastElement()));
+ group.addHSpacer();
+ group.addChild(RemoteGui::makeIntSpinBox(name)
+ .value(value->getValue())
+ .min(-1000)
+ .max(1000)
+ .toolTip(name));
+ group_hierarchy_.back()->addChild(group);
+ }
+
+ void
+ MakeConfigGuiVisitor::visitFloat(DataInput& f, TypeInput& /*unused*/)
+ {
+ INPUT_GUARD(f);
+ auto value = data::Float::DynamicCastAndCheck(f);
+ const auto& name = pathToName(f);
+ auto group = RemoteGui::makeHBoxLayout(name + "_layout");
+ group.addChild(RemoteGui::makeLabel(name + "_label").value(f->getPath().getLastElement()));
+ group.addHSpacer();
+ group.addChild(RemoteGui::makeFloatSpinBox(name)
+ .value(value->getValue())
+ .min(-1000)
+ .max(1000)
+ .toolTip(name));
+ group_hierarchy_.back()->addChild(group);
+ }
+
+ void
+ MakeConfigGuiVisitor::visitDouble(DataInput& d, TypeInput& /*unused*/)
+ {
+ INPUT_GUARD(d);
+ auto value = data::Double::DynamicCastAndCheck(d);
+ const auto& name = pathToName(d);
+ auto group = RemoteGui::makeHBoxLayout(name + "_layout");
+ group.addChild(RemoteGui::makeLabel(name + "_label").value(d->getPath().getLastElement()));
+ group.addHSpacer();
+ group.addChild(RemoteGui::makeFloatSpinBox(name)
+ .value(value->getValue())
+ .min(-1000)
+ .max(1000)
+ .toolTip(name));
+ group_hierarchy_.back()->addChild(group);
+ }
+
+ void
+ MakeConfigGuiVisitor::visitBool(DataInput& b, TypeInput& /*unused*/)
+ {
+ INPUT_GUARD(b);
+ auto value = data::Bool::DynamicCastAndCheck(b);
+ const auto& name = pathToName(b);
+ auto group = RemoteGui::makeHBoxLayout(name + "_layout");
+ group.addChild(RemoteGui::makeLabel(name + "_label").value(b->getPath().getLastElement()));
+ group.addHSpacer();
+ group.addChild(
+ RemoteGui::makeCheckBox(name).label("").value(value->getValue()).toolTip(name));
+ group_hierarchy_.back()->addChild(group);
+ }
+
+ void
+ MakeConfigGuiVisitor::visitString(DataInput& string, TypeInput& /*unused*/)
+ {
+ INPUT_GUARD(string);
+ auto value = data::String::DynamicCastAndCheck(string);
+ const auto& name = pathToName(string);
+ auto group = RemoteGui::makeHBoxLayout(name + "_layout");
+ group.addChild(
+ RemoteGui::makeLabel(name + "_label").value(string->getPath().getLastElement()));
+ group.addHSpacer();
+ group.addChild(RemoteGui::makeLineEdit(name).value(value->getValue()).toolTip(name));
+ group_hierarchy_.back()->addChild(group);
+ }
+
+ MakeConfigGuiVisitor::MakeConfigGuiVisitor(const std::string& name) :
+ builder_(std::make_unique<RemoteGui::detail::GroupBoxBuilder>(name))
+ {
+ }
+
+ RemoteGui::detail::GroupBoxBuilder
+ MakeConfigGuiVisitor::getGroupBoxBuilder() const
+ {
+ return *builder_;
+ }
+
+ void
+ MakeConfigGuiVisitor::visitListOnEnter(DataInput& o, TypeInput& t)
+ {
+ in_list_ = true;
+ auto group = RemoteGui::makeSimpleGridLayout(pathToName(o) + "_grid").cols(20);
+ auto data = data::List::DynamicCastAndCheck(o);
+ auto type = type::List::DynamicCast(t)->getAcceptedType()->getDescriptor();
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
+ implementedListDescriptors.end())
+ {
+ return;
+ }
+ for (const auto& el : data->getElements())
+ {
+ group.addChild(RemoteGui::makeLineEdit(pathToName(el))
+ .value(factories::VariantHelper::make(type)->to_string(el)),
+ 10);
+ group.addHSpacer(8);
+ group.addChild(RemoteGui::makeButton(pathToName(el) + "_button")
+ .label("-")
+ .toolTip("Remove List Element"),
+ 2);
+ }
+ group_hierarchy_.back()->addChild(
+ RemoteGui::makeGroupBox(pathToName(o) + "_grp")
+ .label(o->getPath().getLastElement())
+ .collapsed(true)
+ .addChild(group)
+ .addChild(RemoteGui::makeButton(pathToName(o) + "_add")
+ .label("+")
+ .toolTip("Add new list entry.")));
+ }
+
+ void
+ MakeConfigGuiVisitor::visitListOnExit(DataInput& /*unused*/, TypeInput& /*unused*/)
+ {
+ in_list_ = false;
+ }
+
+ void
+ MakeConfigGuiVisitor::visitIntEnum(DataInput& o, TypeInput& t)
+ {
+ INPUT_GUARD(o);
+ auto data = data::Int::DynamicCastAndCheck(o);
+ auto type = type::IntEnum::DynamicCast(t);
+ const auto& name = pathToName(o);
+ auto group = RemoteGui::makeHBoxLayout(name + "_layout");
+ group.addChild(RemoteGui::makeLabel(name + "_label").value(o->getPath().getLastElement()));
+ group.addHSpacer();
+ group.addChild(RemoteGui::makeComboBox(name)
+ .options(type->getAcceptedValueNames())
+ .value(type->getValueName(data->getValue()))
+ .toolTip(name));
+ group_hierarchy_.back()->addChild(group);
+ }
+
+ void
+ MakeConfigGuiVisitor::visitMatrix(const std::shared_ptr<data::Variant>& q,
+ const std::shared_ptr<type::Variant>& t)
+ {
+ auto data = data::NDArray::DynamicCastAndCheck(q);
+ auto type = type::Matrix::DynamicCast(t);
+ const auto& cols = type->getCols();
+ const auto& rows = type->getRows();
+ const auto& name = pathToName(q);
+ auto group = RemoteGui::makeHBoxLayout(name + "_layout");
+ group.addChild(RemoteGui::makeLabel(name + "_label").value(q->getPath().getLastElement()));
+ group.addHSpacer();
+
+ if (cols == 4 && rows == 4)
+ {
+ // Poses
+ const auto& matrix = aron::converter::AronEigenConverter::ConvertToMatrix4f(data);
+ group.addChild(RemoteGui::makePosRPYSpinBoxes(name).value(matrix).toolTip(name));
+ }
+ else if ((cols == 3 and rows == 1) or (rows == 3 and cols == 1))
+ {
+ // Positions
+ const auto& vector = aron::converter::AronEigenConverter::ConvertToVector3f(data);
+ group.addChild(RemoteGui::makeVector3fSpinBoxes(name).value(vector).toolTip(name));
+ }
+ group_hierarchy_.back()->addChild(group);
+ }
+
+ void
+ MakeConfigGuiVisitor::visitQuaternion(const std::shared_ptr<data::Variant>& q,
+ const std::shared_ptr<type::Variant>& t)
+ {
+ INPUT_GUARD(q);
+ auto data = data::NDArray::DynamicCastAndCheck(q);
+ auto type = type::Quaternion::DynamicCast(t);
+ const auto& quat = simox::math::quat_to_rpy(
+ aron::converter::AronEigenConverter::ConvertToQuaternionf(data));
+ const auto& name = pathToName(q);
+ auto group = RemoteGui::makeHBoxLayout(name + "_layout");
+ group.addChild(RemoteGui::makeLabel(name + "_label").value(q->getPath().getLastElement()));
+ group.addHSpacer();
+ group.addChild(RemoteGui::makeVector3fSpinBoxes(name)
+ .value(quat)
+ .min(-M_PI)
+ .max(M_PI)
+ .steps(601)
+ .decimals(2)
+ .toolTip(name));
+ group_hierarchy_.back()->addChild(group);
+ }
+
+ void
+ MakeConfigGuiVisitor::visitDictOnEnter(const std::shared_ptr<data::Variant>& o,
+ const std::shared_ptr<type::Variant>& t)
+ {
+ in_list_ = true;
+ auto group = RemoteGui::makeSimpleGridLayout(pathToName(o) + "_grid").cols(20);
+ auto data = data::Dict::DynamicCastAndCheck(o);
+ auto type = type::Dict::DynamicCast(t)->getAcceptedType()->getDescriptor();
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
+ implementedListDescriptors.end())
+ {
+ return;
+ }
+ for (const auto& el : data->getElements())
+ {
+ group.addChild(RemoteGui::makeLineEdit(pathToName(el.second) + "_lbl").value(el.first),
+ 5);
+ group.addHSpacer(2);
+ group.addChild(RemoteGui::makeLineEdit(pathToName(el.second))
+ .value(factories::VariantHelper::make(type)->to_string(el.second)),
+ 5);
+ group.addHSpacer(6);
+ group.addChild(RemoteGui::makeButton(pathToName(el.second) + "_button")
+ .label("-")
+ .toolTip("Remove List Element"),
+ 2);
+ }
+ group_hierarchy_.back()->addChild(
+ RemoteGui::makeGroupBox(pathToName(o) + "_grp")
+ .label(o->getPath().getLastElement())
+ .collapsed(true)
+ .addChild(group)
+ .addChild(RemoteGui::makeButton(pathToName(o) + "_add")
+ .label("+")
+ .toolTip("Add new dict entry.")));
+ }
+
+ void
+ MakeConfigGuiVisitor::visitDictOnExit(const std::shared_ptr<data::Variant>&,
+ const std::shared_ptr<type::Variant>&)
+ {
+ in_list_ = false;
+ }
+
+ GetValueFromMapVisitor::GetValueFromMapVisitor(armarx::RemoteGui::TabProxy* proxy) :
+ proxy_(std::experimental::make_observer(proxy))
+ {
+ }
+
+ void
+ GetValueFromMapVisitor::visitInt(DataInput& i, TypeInput&)
+ {
+ INPUT_GUARD(i);
+ auto value = data::Int::DynamicCastAndCheck(i);
+ const std::string name = pathToName(i);
+ auto gui_value = proxy_->getValue<int>(name);
+ if (value->getValue() != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ value->setValue(gui_value.get());
+ i = value;
+ }
+ else
+ {
+ proxy_->setValue(value->getValue(), name);
+ }
+ }
+ }
+
+ void
+ GetValueFromMapVisitor::visitFloat(DataInput& f, TypeInput&)
+ {
+ INPUT_GUARD(f);
+ auto value = data::Float::DynamicCastAndCheck(f);
+ const std::string name = pathToName(f);
+ auto gui_value = proxy_->getValue<float>(name);
+ if (value->getValue() != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ value->setValue(gui_value.get());
+ f = value;
+ }
+ else
+ {
+ proxy_->setValue(value->getValue(), name);
+ }
+ }
+ }
+
+ void
+ GetValueFromMapVisitor::visitDouble(DataInput& d, TypeInput&)
+ {
+ INPUT_GUARD(d);
+ auto value = data::Double::DynamicCastAndCheck(d);
+ const std::string name = pathToName(d);
+ // TODO: does double work here?
+ auto gui_value = proxy_->getValue<float>(name);
+ if (value->getValue() != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ value->setValue(gui_value.get());
+ d = value;
+ }
+ else
+ {
+ proxy_->setValue(static_cast<float>(value->getValue()), name);
+ }
+ }
+ }
+
+ void
+ GetValueFromMapVisitor::visitBool(DataInput& b, TypeInput&)
+ {
+ INPUT_GUARD(b);
+ auto value = data::Bool::DynamicCastAndCheck(b);
+ const std::string name = pathToName(b);
+ auto gui_value = proxy_->getValue<bool>(name);
+ if (value->getValue() != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ value->setValue(gui_value.get());
+ b = value;
+ }
+ else
+ {
+ proxy_->setValue(value->getValue(), name);
+ }
+ }
+ }
+
+ void
+ GetValueFromMapVisitor::visitString(DataInput& string, TypeInput&)
+ {
+ INPUT_GUARD(string);
+ auto value = data::String::DynamicCastAndCheck(string);
+ const std::string name = pathToName(string);
+ auto gui_value = proxy_->getValue<std::string>(name);
+ if (value->getValue() != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ value->setValue(gui_value.get());
+ string = value;
+ }
+ else
+ {
+ proxy_->setValue(value->getValue(), name);
+ }
+ }
+ }
+
+ type::Descriptor
+ GetValueFromMapVisitor::getDescriptor(DataInput& o, TypeInput& t)
+ {
+ return data::ConstTypedVariantVisitor::GetDescriptor(o, t);
+ }
+
+ GetValueFromMapVisitor::MapElements
+ GetValueFromMapVisitor::getObjectElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getObjectElements(o, t);
+ }
+
+ GetValueFromMapVisitor::MapElements
+ GetValueFromMapVisitor::getDictElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getDictElements(o, t);
+ }
+
+ GetValueFromMapVisitor::ListElements
+ GetValueFromMapVisitor::getListElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getListElements(o, t);
+ }
+
+ GetValueFromMapVisitor::PairElements
+ GetValueFromMapVisitor::getPairElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getPairElements(o, t);
+ }
+
+ GetValueFromMapVisitor::TupleElements
+ GetValueFromMapVisitor::getTupleElements(DataInput& o, TypeInput& t)
+ {
+ return component_config::getTupleElements(o, t);
+ }
+
+ void
+ GetValueFromMapVisitor::visitIntEnum(DataInput& o, TypeInput& t)
+ {
+ INPUT_GUARD(o);
+ auto data = data::Int::DynamicCastAndCheck(o);
+ auto type = type::IntEnum::DynamicCastAndCheck(t);
+ std::string str;
+ const std::string name = pathToName(o);
+ proxy_->getValue(str, pathToName(o));
+ if (data->getValue() != type->getValue(str))
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ data->getValue() = type->getValue(str);
+ o = data;
+ }
+ else
+ {
+ proxy_->setValue(type->getValueName(data->getValue()), name);
+ }
+ }
+ }
+
+ void
+ GetValueFromMapVisitor::visitListOnEnter(DataInput& o, TypeInput& t)
+ {
+ in_list_ = true;
+ auto data = data::List::DynamicCastAndCheck(o);
+ auto type = type::List::DynamicCast(t)->getAcceptedType()->getDescriptor();
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
+ implementedListDescriptors.end())
+ {
+ return;
+ }
+ const auto& elements = data->getElements();
+ for (const auto& [idx, el] : armarx::MakeIndexedContainer(elements))
+ {
+ if (proxy_->getButtonClicked(pathToName(el) + "_button"))
+ {
+ data->removeElement(idx);
+ tab_rebuild_required_ = true;
+ }
+ auto gui_value = proxy_->getValue<std::string>(pathToName(el)).get();
+ factories::VariantHelper::make(type)->set_value_from_string(el, gui_value);
+ }
+ if (proxy_->getButtonClicked(pathToName(o) + "_add"))
+ {
+ data->addElement(factories::VariantHelper::make(type)->from_string(
+ "", o->getPath().withIndex(data->childrenSize())));
+ tab_rebuild_required_ = true;
+ }
+ }
+
+ void
+ GetValueFromMapVisitor::visitListOnExit(DataInput&, TypeInput&)
+ {
+ in_list_ = false;
+ }
+
+ bool
+ GetValueFromMapVisitor::tabRebuildRequired() const
+ {
+ return tab_rebuild_required_;
+ }
+
+ void
+ GetValueFromMapVisitor::visitMatrix(std::shared_ptr<data::Variant>& o,
+ const std::shared_ptr<type::Variant>& t)
+ {
+ INPUT_GUARD(o);
+ auto value = data::NDArray::DynamicCastAndCheck(o);
+ auto type = type::Matrix::DynamicCastAndCheck(t);
+ const std::string name = pathToName(o);
+ const auto& cols = type->getCols();
+ const auto& rows = type->getRows();
+ // TODO: does double work here?
+ if (cols == 4 && rows == 4)
+ {
+ auto gui_value = proxy_->getValue<Eigen::Matrix4f>(name);
+ auto config_value = converter::AronEigenConverter::ConvertToMatrix4f(value);
+
+ if (config_value != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ auto variant =
+ converter::AronEigenConverter::ConvertFromMatrix(gui_value.get());
+ value->setData(gui_value.get().size() * sizeof(float), variant->getData());
+ }
+ else
+ {
+ proxy_->setValue(config_value, name);
+ }
+ }
+ }
+ else if ((cols == 3 and rows == 1) or (cols == 1 and rows == 3))
+ {
+ auto gui_value = proxy_->getValue<Eigen::Vector3f>(name);
+ auto config_value = converter::AronEigenConverter::ConvertToVector3f(value);
+
+ if (config_value != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ auto variant =
+ converter::AronEigenConverter::ConvertFromMatrix(gui_value.get());
+ value->setData(gui_value.get().size() * sizeof(float), variant->getData());
+ }
+ else
+ {
+ proxy_->setValue(config_value, name);
+ }
+ }
+ }
+ }
+
+ void
+ GetValueFromMapVisitor::visitQuaternion(std::shared_ptr<data::Variant>& o,
+ const std::shared_ptr<type::Variant>& t)
+ {
+ INPUT_GUARD(o);
+ auto value = data::NDArray::DynamicCastAndCheck(o);
+ const std::string name = pathToName(o);
+ // TODO: does double work here?
+ auto gui_value = proxy_->getValue<Eigen::Vector3f>(name);
+ const auto& quat =
+ simox::math::quat_to_rpy(converter::AronEigenConverter::ConvertToQuaternionf(value));
+ if (quat != gui_value.get())
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ auto variant = converter::AronEigenConverter::ConvertFromQuaternion(
+ simox::math::rpy_to_quat(gui_value.get()));
+ value->setData(4 * sizeof(float), variant->getData());
+ }
+ else
+ {
+ proxy_->setValue(quat, name);
+ }
+ }
+ }
+
+ void
+ GetValueFromMapVisitor::visitDictOnEnter(std::shared_ptr<data::Variant>& o,
+ const std::shared_ptr<type::Variant>& t)
+ {
+ in_list_ = true;
+ auto data = data::Dict::DynamicCastAndCheck(o);
+ auto type = type::Dict::DynamicCast(t)->getAcceptedType()->getDescriptor();
+ if (std::find(implementedListDescriptors.begin(), implementedListDescriptors.end(), type) ==
+ implementedListDescriptors.end())
+ {
+ return;
+ }
+ const auto& elements = data->getElements();
+ std::map<std::string, std::string> changed_labels;
+ for (const auto& [idx, el] : elements)
+ {
+ const std::string name = pathToName(el);
+ if (proxy_->getButtonClicked(name + "_button"))
+ {
+ data->removeElement(idx);
+ tab_rebuild_required_ = true;
+ }
+ auto gui_value = proxy_->getValue<std::string>(name).get();
+ auto gui_key = proxy_->getValue<std::string>(name + "_lbl").get();
+ auto config_value = factories::VariantHelper::make(type)->to_string(el);
+ if (gui_value != config_value)
+ {
+ if (proxy_->hasValueChanged(name))
+ {
+ factories::VariantHelper::make(type)->set_value_from_string(el, gui_value);
+ }
+ else
+ {
+ proxy_->setValue(config_value, name);
+ }
+ }
+ if (gui_key != idx)
+ {
+ if (proxy_->hasValueChanged(name + "_lbl"))
+ {
+ changed_labels.emplace(idx, gui_key);
+ }
+ else
+ {
+ proxy_->setValue(idx, name + "_lbl");
+ }
+ }
+ }
+ // replace changed keys in map
+ for (const auto& [old_label, new_label] : changed_labels)
+ {
+ auto element = data->getElement(old_label);
+ data->removeElement(old_label);
+ auto variantHelper = factories::VariantHelper::make(type);
+ data->addElement(new_label,
+ variantHelper->from_string(
+ variantHelper->to_string(element),
+ o->getPath().withDetachedLastElement().withElement(new_label)));
+ tab_rebuild_required_ = true;
+ }
+
+ if (proxy_->getButtonClicked(pathToName(o) + "_add"))
+ {
+ data->addElement("defaultKey",
+ factories::VariantHelper::make(type)->from_string(
+ "", o->getPath().withElement("defaultKey")));
+ tab_rebuild_required_ = true;
+ }
+ }
+
+ void
+ GetValueFromMapVisitor::visitDictOnExit(std::shared_ptr<data::Variant>&,
+ const std::shared_ptr<type::Variant>&)
+ {
+ in_list_ = false;
+ }
+} // namespace armarx::aron::component_config
+
+#undef INPUT_GUARD
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.h b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.h
new file mode 100644
index 0000000000000000000000000000000000000000..93871934083414333bc7af05210b468af0601637
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/RemoteGuiVisitors.h
@@ -0,0 +1,136 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 07.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <experimental/memory>
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
+namespace armarx::RemoteGui
+{
+ namespace detail
+ {
+ class GroupBoxBuilder;
+ }
+ class TabProxy;
+
+ class ValueVariant;
+
+ using ValueMap = std::map<std::string, ValueVariant>;
+} // namespace armarx::RemoteGui
+
+namespace armarx::aron::component_config
+{
+
+ class MakeConfigGuiVisitor : public data::RecursiveConstTypedVariantVisitor
+ {
+ public:
+ explicit MakeConfigGuiVisitor(const std::string& name);
+
+ void visitObjectOnEnter(DataInput&, TypeInput&) override;
+
+ void visitObjectOnExit(DataInput&, TypeInput&) override;
+
+ void visitListOnEnter(DataInput&, TypeInput&) override;
+
+ void visitListOnExit(DataInput&, TypeInput&) override;
+
+ void visitDictOnEnter(DataInput&, TypeInput&) override;
+
+ void visitDictOnExit(DataInput&, TypeInput&) override;
+
+ void visitInt(DataInput&, TypeInput&) override;
+
+ void visitIntEnum(DataInput&, TypeInput&) override;
+
+ void visitFloat(DataInput&, TypeInput&) override;
+
+ void visitDouble(DataInput&, TypeInput&) override;
+
+ void visitBool(DataInput&, TypeInput&) override;
+
+ void visitString(DataInput&, TypeInput&) override;
+
+ void visitQuaternion(DataInput& input, TypeInput& typeInput) override;
+
+ void visitMatrix(DataInput& input, TypeInput& typeInput) override;
+
+ [[nodiscard]] RemoteGui::detail::GroupBoxBuilder getGroupBoxBuilder() const;
+
+ private:
+ std::shared_ptr<RemoteGui::detail::GroupBoxBuilder> builder_;
+ std::vector<std::shared_ptr<RemoteGui::detail::GroupBoxBuilder>> group_hierarchy_;
+ std::atomic_bool in_list_{false};
+ };
+
+ class GetValueFromMapVisitor :
+ public aron::data::RecursiveTypedVisitor<data::VariantPtr, const type::VariantPtr>
+ {
+ public:
+ explicit GetValueFromMapVisitor(armarx::RemoteGui::TabProxy* proxy);
+
+ type::Descriptor getDescriptor(DataInput&, TypeInput&) override;
+
+ MapElements getObjectElements(DataInput&, TypeInput&) override;
+
+ MapElements getDictElements(DataInput&, TypeInput&) override;
+
+ ListElements getListElements(DataInput&, TypeInput&) override;
+
+ PairElements getPairElements(DataInput&, TypeInput&) override;
+
+ TupleElements getTupleElements(DataInput&, TypeInput&) override;
+
+ void visitListOnEnter(DataInput&, TypeInput&) override;
+
+ void visitListOnExit(DataInput&, TypeInput&) override;
+
+ void visitDictOnEnter(DataInput&, TypeInput&) override;
+
+ void visitDictOnExit(DataInput&, TypeInput&) override;
+
+ void visitInt(DataInput&, TypeInput&) override;
+
+ void visitIntEnum(DataInput&, TypeInput&) override;
+
+ void visitFloat(DataInput&, TypeInput&) override;
+
+ void visitDouble(DataInput&, TypeInput&) override;
+
+ void visitBool(DataInput&, TypeInput&) override;
+
+ void visitString(DataInput&, TypeInput&) override;
+ void visitMatrix(DataInput& elementData, TypeInput& elementType) override;
+ void visitQuaternion(DataInput& elementData, TypeInput& elementType) override;
+
+ bool tabRebuildRequired() const;
+
+ private:
+ std::experimental::observer_ptr<armarx::RemoteGui::TabProxy> proxy_;
+ bool in_list_{false};
+ bool tab_rebuild_required_{false};
+ };
+
+} // namespace armarx::aron::component_config
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/Util.h b/source/RobotAPI/libraries/aron_component_config/Util.h
new file mode 100644
index 0000000000000000000000000000000000000000..f9b6889eff84ef6cac3d73607e13bc6eaa6b10dc
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/Util.h
@@ -0,0 +1,162 @@
+/*
+ * 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 robdekon
+ * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
+ * @date 07.09.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <memory>
+#include <numeric>
+
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+
+#include <cxxabi.h>
+
+namespace armarx::aron::component_config
+{
+
+ inline std::string
+ pathToName(const std::shared_ptr<data::Variant>& v)
+ {
+ auto vector = v->getPath().getPath();
+ auto path = v->getPath();
+ path.setDelimeter(".");
+ path.setRootIdentifier("");
+ auto string = path.toString();
+ return string.substr(1, string.size());
+ }
+
+ template <typename DataInputT, typename TypeInputT>
+ std::map<std::string,
+ std::pair<typename std::remove_const<DataInputT>::type,
+ typename std::remove_const<TypeInputT>::type>>
+ getObjectElements(DataInputT& o, TypeInputT& t)
+ {
+ std::map<std::string, std::pair<data::VariantPtr, type::VariantPtr>> ret;
+ auto x = data::Dict::DynamicCastAndCheck(o);
+ auto y = type::Object::DynamicCastAndCheck(t);
+
+ ARMARX_CHECK_NOT_NULL(y);
+
+ if (x)
+ {
+ for (const auto& [key, e] : x->getElements())
+ {
+ auto ct = y->getMemberType(key);
+ ret.insert({key, {e, ct}});
+ }
+ }
+ return ret;
+ }
+
+ template <typename DataInputT, typename TypeInputT>
+ std::map<std::string,
+ std::pair<typename std::remove_const<DataInputT>::type,
+ typename std::remove_const<TypeInputT>::type>>
+ getDictElements(DataInputT& o, TypeInputT& t)
+ {
+ std::map<std::string,
+ std::pair<typename std::remove_const<DataInputT>::type,
+ typename std::remove_const<TypeInputT>::type>>
+ ret;
+ auto x = data::Dict::DynamicCastAndCheck(o);
+ auto y = type::Dict::DynamicCastAndCheck(t);
+
+ auto ac = y ? y->getAcceptedType() : nullptr;
+
+ if (x)
+ {
+ for (const auto& [key, e] : x->getElements())
+ {
+ ret.insert({key, {e, ac}});
+ }
+ }
+ return ret;
+ }
+
+ template <typename DataInputT, typename TypeInputT>
+ std::vector<std::pair<typename std::remove_const<DataInputT>::type,
+ typename std::remove_const<TypeInputT>::type>>
+ getListElements(DataInputT& o, TypeInputT& t)
+ {
+ std::vector<std::pair<typename std::remove_const<DataInputT>::type,
+ typename std::remove_const<TypeInputT>::type>>
+ ret;
+ auto x = data::List::DynamicCastAndCheck(o);
+ auto y = type::List::DynamicCastAndCheck(t);
+
+ auto ac = y ? y->getAcceptedType() : nullptr;
+
+ if (x)
+ {
+ for (const auto& e : x->getElements())
+ {
+ ret.emplace_back(e, ac);
+ }
+ }
+ return ret;
+ }
+
+ template <typename DataInputT, typename TypeInputT>
+ std::pair<std::pair<typename std::remove_const<DataInputT>::type,
+ typename std::remove_const<TypeInputT>::type>,
+ std::pair<typename std::remove_const<DataInputT>::type,
+ typename std::remove_const<TypeInputT>::type>>
+ getPairElements(DataInputT& o, TypeInputT& t)
+ {
+ auto x = data::List::DynamicCastAndCheck(o);
+ auto y = type::Pair::DynamicCastAndCheck(t);
+
+ ARMARX_CHECK_NOT_NULL(y);
+
+ if (x)
+ {
+ auto e0 = x->getElement(0);
+ auto ac0 = y->getFirstAcceptedType();
+ auto e1 = x->getElement(1);
+ auto ac1 = y->getSecondAcceptedType();
+ return {{e0, ac0}, {e1, ac1}};
+ }
+ return {};
+ }
+
+ template <typename DataInputT, typename TypeInputT>
+ std::vector<std::pair<typename std::remove_const<DataInputT>::type,
+ typename std::remove_const<TypeInputT>::type>>
+ getTupleElements(DataInputT& o, TypeInputT& t)
+ {
+ std::vector<std::pair<data::VariantPtr, type::VariantPtr>> ret;
+ auto x = data::List::DynamicCastAndCheck(o);
+ auto y = type::Tuple::DynamicCastAndCheck(t);
+
+ ARMARX_CHECK_NOT_NULL(y);
+
+ if (x)
+ {
+ unsigned int i = 0;
+ for (const auto& e : x->getElements())
+ {
+ auto ac = y->getAcceptedType(i++);
+ ret.emplace_back(e, ac);
+ }
+ }
+ return ret;
+ }
+} // namespace armarx::aron::component_config
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/VariantHelperFactory.cpp b/source/RobotAPI/libraries/aron_component_config/VariantHelperFactory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..10e021861bbc16a6464ce29c95946dbbd3170888
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/VariantHelperFactory.cpp
@@ -0,0 +1,225 @@
+/*
+ * 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 04.11.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "VariantHelperFactory.h"
+
+#include <ArmarXCore/core/application/properties/Property.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+namespace armarx::aron::component_config::products
+{
+ template <>
+ std::string
+ VariantHelper<type::Descriptor::INT>::to_string(const data::VariantPtr& ptr) const
+ {
+ ARMARX_TRACE;
+ return simox::alg::to_string(data::Int::DynamicCast(ptr)->getValue());
+ }
+
+ template <>
+ std::string
+ VariantHelper<type::Descriptor::BOOL>::to_string(const data::VariantPtr& ptr) const
+ {
+ ARMARX_TRACE;
+ return simox::alg::to_string(data::Bool::DynamicCast(ptr)->getValue());
+ }
+
+ template <>
+ std::string
+ VariantHelper<type::Descriptor::STRING>::to_string(const data::VariantPtr& ptr) const
+ {
+ ARMARX_TRACE;
+ return data::String::DynamicCast(ptr)->getValue();
+ }
+
+ template <>
+ std::string
+ VariantHelper<type::Descriptor::FLOAT>::to_string(const data::VariantPtr& ptr) const
+ {
+ ARMARX_TRACE;
+ return simox::alg::to_string(data::Float::DynamicCast(ptr)->getValue());
+ }
+
+ template <>
+ std::string
+ VariantHelper<type::Descriptor::INT_ENUM>::to_string(const data::VariantPtr& ptr) const
+ {
+ ARMARX_TRACE;
+ // TODO: this is should use the name of the enum value
+ return simox::alg::to_string(data::Int::DynamicCast(ptr)->getValue());
+ }
+
+ template <>
+ std::string
+ VariantHelper<type::Descriptor::DOUBLE>::to_string(const data::VariantPtr& ptr) const
+ {
+ ARMARX_TRACE;
+ return simox::alg::to_string(data::Double::DynamicCast(ptr)->getValue());
+ }
+
+ template <>
+ aron::data::VariantPtr
+ products::VariantHelper<type::Descriptor::INT>::from_string(
+ const std::string& string,
+ const armarx::aron::Path& path) const
+ {
+ ARMARX_TRACE;
+ int i = string.empty() ? 0 : std::stoi(string);
+ return make_int(i, path);
+ }
+
+ template <>
+ aron::data::VariantPtr
+ products::VariantHelper<type::Descriptor::FLOAT>::from_string(
+ const std::string& string,
+ const armarx::aron::Path& path) const
+ {
+ ARMARX_TRACE;
+ float f = string.empty() ? 0 : std::stof(string);
+ return make_float(f, path);
+ }
+
+ template <>
+ aron::data::VariantPtr
+ products::VariantHelper<type::Descriptor::DOUBLE>::from_string(
+ const std::string& string,
+ const armarx::aron::Path& path) const
+ {
+ ARMARX_TRACE;
+ double d = string.empty() ? 0 : std::stod(string);
+ return make_double(d, path);
+ }
+
+ template <>
+ aron::data::VariantPtr
+ products::VariantHelper<type::Descriptor::BOOL>::from_string(
+ const std::string& string,
+ const armarx::aron::Path& path) const
+ {
+ ARMARX_TRACE;
+ if (string == "true")
+ {
+ return make_bool(true, path);
+ }
+ else if (string == "false" or string.empty())
+ {
+ return make_bool(false, path);
+ }
+ throw armarx::InvalidArgumentException("Boolean string has to be either true or false");
+ }
+
+ template <>
+ aron::data::VariantPtr
+ products::VariantHelper<type::Descriptor::INT_ENUM>::from_string(
+ const std::string& string,
+ const armarx::aron::Path& path) const
+ {
+ // TODO: this might not work
+ ARMARX_TRACE;
+ return make_int(string.empty() ? 0 : std::stoi(string), path);
+ }
+
+ template <>
+ aron::data::VariantPtr
+ products::VariantHelper<type::Descriptor::STRING>::from_string(
+ const std::string& string,
+ const armarx::aron::Path& path) const
+ {
+ ARMARX_TRACE;
+ std::string formatted_string = "";
+ if (not string.empty())
+ {
+ formatted_string =
+ armarx::FileSystemPathBuilder_ApplyFormattingAndResolveEnvAndCMakeVars(string);
+ }
+ return make_string(formatted_string, path);
+ }
+
+ template <>
+ void
+ products::VariantHelper<type::Descriptor::INT>::set_value_from_string(
+ const armarx::aron::data::VariantPtr& variant,
+ const std::string& string) const
+ {
+ ARMARX_TRACE;
+ data::Int::DynamicCast(variant)->getValue() = std::stoi(string);
+ }
+
+ template <>
+ void
+ products::VariantHelper<type::Descriptor::FLOAT>::set_value_from_string(
+ const armarx::aron::data::VariantPtr& variant,
+ const std::string& string) const
+ {
+ ARMARX_TRACE;
+ data::Float::DynamicCast(variant)->getValue() = std::stof(string);
+ }
+
+ template <>
+ void
+ products::VariantHelper<type::Descriptor::DOUBLE>::set_value_from_string(
+ const armarx::aron::data::VariantPtr& variant,
+ const std::string& string) const
+ {
+ ARMARX_TRACE;
+ data::Double::DynamicCast(variant)->getValue() = std::stod(string);
+ }
+
+ template <>
+ void
+ products::VariantHelper<type::Descriptor::BOOL>::set_value_from_string(
+ const armarx::aron::data::VariantPtr& variant,
+ const std::string& string) const
+ {
+ ARMARX_TRACE;
+ data::Bool::DynamicCast(variant)->getValue() = string == "true";
+ }
+
+ template <>
+ void
+ products::VariantHelper<type::Descriptor::STRING>::set_value_from_string(
+ const armarx::aron::data::VariantPtr& variant,
+ const std::string& string) const
+ {
+ ARMARX_TRACE;
+ data::String::DynamicCast(variant)->getValue() = string;
+ }
+
+ template <>
+ void
+ products::VariantHelper<type::Descriptor::INT_ENUM>::set_value_from_string(
+ const armarx::aron::data::VariantPtr& variant,
+ const std::string& string) const
+ {
+ ARMARX_TRACE;
+ data::Int::DynamicCast(variant)->getValue() = std::stoi(string);
+ }
+
+ template struct products::VariantHelper<type::Descriptor::INT>;
+ template struct products::VariantHelper<type::Descriptor::STRING>;
+ template struct products::VariantHelper<type::Descriptor::BOOL>;
+ template struct products::VariantHelper<type::Descriptor::FLOAT>;
+ template struct products::VariantHelper<type::Descriptor::DOUBLE>;
+ template struct products::VariantHelper<type::Descriptor::INT_ENUM>;
+} // namespace armarx::aron::component_config::products
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/aron_component_config/VariantHelperFactory.h b/source/RobotAPI/libraries/aron_component_config/VariantHelperFactory.h
new file mode 100644
index 0000000000000000000000000000000000000000..21a352fe268161758d1a6b20aa96b49c34f1d6c0
--- /dev/null
+++ b/source/RobotAPI/libraries/aron_component_config/VariantHelperFactory.h
@@ -0,0 +1,170 @@
+/*
+ * 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 04.11.22
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <ArmarXCore/util/CPPUtility/SelfRegisteringFactory.h>
+
+#include <RobotAPI/libraries/aron/core/Descriptor.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+
+namespace armarx::aron::component_config
+{
+ namespace factories
+ {
+ struct VariantHelper : armarx::Factory<VariantHelper, type::Descriptor>
+ {
+ explicit VariantHelper(Key){};
+
+ virtual ~VariantHelper() = default;
+
+ virtual std::string to_string(const armarx::aron::data::VariantPtr&) const = 0;
+
+ virtual void set_value_from_string(const armarx::aron::data::VariantPtr&,
+ const std::string& string) const = 0;
+
+ virtual aron::data::VariantPtr from_string(const std::string&,
+ const armarx::aron::Path& path) const = 0;
+ };
+ } // namespace factories
+
+ namespace products
+ {
+ template <type::Descriptor DescT>
+ struct VariantHelper : factories::VariantHelper::Registrar<VariantHelper<DescT>>
+ {
+ using RegistrarT = factories::VariantHelper::Registrar<VariantHelper<DescT>>;
+ static constexpr type::Descriptor id = DescT;
+
+ explicit VariantHelper() : RegistrarT(typename RegistrarT::Registration()){};
+
+ [[nodiscard]] std::string to_string(const data::VariantPtr& ptr) const override;
+
+ void set_value_from_string(const armarx::aron::data::VariantPtr& variant,
+ const std::string& string) const override;
+
+ [[nodiscard]] aron::data::VariantPtr
+ from_string(const std::string&, const armarx::aron::Path& path) const override;
+ };
+
+ } // namespace products
+
+ template <>
+ std::string
+ products::VariantHelper<type::Descriptor::INT>::to_string(const data::VariantPtr& ptr) const;
+
+ template <>
+ aron::data::VariantPtr products::VariantHelper<type::Descriptor::INT>::from_string(
+ const std::string&,
+ const armarx::aron::Path& path) const;
+
+ template <>
+ void products::VariantHelper<type::Descriptor::INT>::set_value_from_string(
+ const armarx::aron::data::VariantPtr&,
+ const std::string&) const;
+
+ template <>
+ std::string
+ products::VariantHelper<type::Descriptor::FLOAT>::to_string(const data::VariantPtr& ptr) const;
+
+ template <>
+ aron::data::VariantPtr products::VariantHelper<type::Descriptor::FLOAT>::from_string(
+ const std::string&,
+ const armarx::aron::Path& path) const;
+
+ template <>
+ void products::VariantHelper<type::Descriptor::FLOAT>::set_value_from_string(
+ const armarx::aron::data::VariantPtr&,
+ const std::string&) const;
+
+ template <>
+ std::string
+ products::VariantHelper<type::Descriptor::BOOL>::to_string(const data::VariantPtr& ptr) const;
+
+ template <>
+ aron::data::VariantPtr products::VariantHelper<type::Descriptor::BOOL>::from_string(
+ const std::string&,
+ const armarx::aron::Path& path) const;
+
+ template <>
+ void products::VariantHelper<type::Descriptor::BOOL>::set_value_from_string(
+ const armarx::aron::data::VariantPtr&,
+ const std::string&) const;
+
+ template <>
+ std::string
+ products::VariantHelper<type::Descriptor::STRING>::to_string(const data::VariantPtr& ptr) const;
+
+ template <>
+ aron::data::VariantPtr products::VariantHelper<type::Descriptor::STRING>::from_string(
+ const std::string&,
+ const armarx::aron::Path& path) const;
+
+ template <>
+ void products::VariantHelper<type::Descriptor::STRING>::set_value_from_string(
+ const armarx::aron::data::VariantPtr&,
+ const std::string&) const;
+
+ template <>
+ std::string
+ products::VariantHelper<type::Descriptor::DOUBLE>::to_string(const data::VariantPtr& ptr) const;
+
+ template <>
+ aron::data::VariantPtr products::VariantHelper<type::Descriptor::DOUBLE>::from_string(
+ const std::string&,
+ const armarx::aron::Path& path) const;
+
+ template <>
+ void products::VariantHelper<type::Descriptor::DOUBLE>::set_value_from_string(
+ const armarx::aron::data::VariantPtr&,
+ const std::string&) const;
+
+ template <>
+ std::string products::VariantHelper<type::Descriptor::INT_ENUM>::to_string(
+ const data::VariantPtr& ptr) const;
+
+ template <>
+ aron::data::VariantPtr products::VariantHelper<type::Descriptor::INT_ENUM>::from_string(
+ const std::string&,
+ const armarx::aron::Path& path) const;
+
+ template <>
+ void products::VariantHelper<type::Descriptor::INT_ENUM>::set_value_from_string(
+ const armarx::aron::data::VariantPtr&,
+ const std::string&) const;
+
+ static inline const std::list<type::Descriptor> implementedListDescriptors = {
+ type::Descriptor::INT_ENUM,
+ type::Descriptor::INT,
+ type::Descriptor::STRING,
+ type::Descriptor::FLOAT,
+ type::Descriptor::DOUBLE,
+ type::Descriptor::BOOL,
+ };
+ extern template struct products::VariantHelper<type::Descriptor::INT>;
+ extern template struct products::VariantHelper<type::Descriptor::FLOAT>;
+ extern template struct products::VariantHelper<type::Descriptor::DOUBLE>;
+ extern template struct products::VariantHelper<type::Descriptor::STRING>;
+ extern template struct products::VariantHelper<type::Descriptor::BOOL>;
+ extern template struct products::VariantHelper<type::Descriptor::INT_ENUM>;
+} // namespace armarx::aron::component_config
diff --git a/source/RobotAPI/libraries/diffik/CMakeLists.txt b/source/RobotAPI/libraries/diffik/CMakeLists.txt
index fce8840d1b6643ea53cc63d18f2703358eb37dde..358be1f028e92c8dd4f3f54ddf8715dbecb90246 100644
--- a/source/RobotAPI/libraries/diffik/CMakeLists.txt
+++ b/source/RobotAPI/libraries/diffik/CMakeLists.txt
@@ -13,16 +13,16 @@ set(LIBS
set(LIB_FILES
NaturalDiffIK.cpp
SimpleDiffIK.cpp
- RobotPlacement.cpp
- GraspTrajectory.cpp
+# RobotPlacement.cpp
+# GraspTrajectory.cpp
CompositeDiffIK.cpp
)
set(LIB_HEADERS
NaturalDiffIK.h
SimpleDiffIK.h
DiffIKProvider.h
- RobotPlacement.h
- GraspTrajectory.h
+# RobotPlacement.h
+# GraspTrajectory.h
CompositeDiffIK.h
)
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt b/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dfd09c4a46abb303d29f4a0e5385fb6aa0a16b7b
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CMakeLists.txt
@@ -0,0 +1,28 @@
+set(LIB_NAME obstacle_avoidance)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+ LIBS
+ # ArmarX
+ ArmarXCore
+ # This package
+ RobotAPI::Core
+ RobotAPI::armem
+ RobotAPI::armem_vision
+ RobotAPI::ArmarXObjects
+ aroncommon
+ # System / External
+# Eigen3::Eigen
+ HEADERS
+ CollisionModelHelper.h
+ SOURCES
+ CollisionModelHelper.cpp
+)
+
+add_library(
+ "RobotAPI::${LIB_NAME}"
+ ALIAS
+ ${LIB_NAME}
+)
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d53a8dd6a89f0cd37f609c9a4742d97d86f4deba
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -0,0 +1,179 @@
+/*
+ * 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 17.02.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CollisionModelHelper.h"
+
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/ArViz/Client/elements/Mesh.h>
+
+
+namespace armarx::obstacle_avoidance
+{
+
+ VirtualRobot::ManipulationObjectPtr
+ CollisionModelHelper::asManipulationObject(const objpose::ObjectPose& objectPose)
+ {
+ const ObjectFinder finder;
+
+ const VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ return obstacle;
+ }
+
+ ARMARX_WARNING << "Failed to load scene object `" << objectPose.objectID << "`";
+ return nullptr;
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ const ObjectFinder finder;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+ for (const auto& objectPose : objectPoses)
+ {
+ if (auto obstacle = finder.loadManipulationObject(objectPose))
+ {
+ obstacle->setGlobalPose(objectPose.objectPoseGlobal);
+ sceneObjects->addSceneObject(obstacle);
+ }
+ }
+
+ return sceneObjects;
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::asSceneObjects(const OccupancyGrid& occupancyGrid,
+ const OccupancyGridHelper::Params& params)
+ {
+ const OccupancyGridHelper ocHelper(occupancyGrid, params);
+ const auto obstacles = ocHelper.obstacles();
+
+ const float boxSize = occupancyGrid.resolution;
+ const float resolution = occupancyGrid.resolution;
+
+ VirtualRobot::SceneObjectSetPtr sceneObjects(new VirtualRobot::SceneObjectSet);
+
+ ARMARX_CHECK_EQUAL(occupancyGrid.frame, GlobalFrame)
+ << "Only occupancy grid in global frame supported.";
+
+ VirtualRobot::CoinVisualizationFactory factory;
+
+ const auto& world_T_map = occupancyGrid.pose;
+
+ for (int x = 0; x < obstacles.rows(); x++)
+ {
+ for (int y = 0; y < obstacles.cols(); y++)
+ {
+ if (obstacles(x, y))
+ {
+ const Eigen::Vector3f pos{
+ static_cast<float>(x) * resolution, static_cast<float>(y) * resolution, 0};
+
+ // FIXME: change to Isometry3f
+ Eigen::Affine3f map_T_obj = Eigen::Affine3f::Identity();
+ map_T_obj.translation() = pos;
+
+ Eigen::Affine3f world_T_obj = world_T_map * map_T_obj;
+
+ // ARMARX_INFO << world_T_obj.translation();
+
+ auto cube = factory.createBox(boxSize, boxSize, boxSize);
+
+ const VirtualRobot::CollisionModelPtr collisionModel(
+ new VirtualRobot::CollisionModel(cube));
+
+ const VirtualRobot::SceneObjectPtr sceneObject(new VirtualRobot::SceneObject(
+ "box_" + std::to_string(sceneObjects->getSize()), cube, collisionModel));
+ sceneObject->setGlobalPose(world_T_obj.matrix());
+
+ sceneObjects->addSceneObject(sceneObject);
+ }
+ }
+ }
+
+ return sceneObjects;
+ }
+
+ CollisionModelHelper::CollisionModelHelper(const objpose::ObjectPoseClient& client) :
+ objectPoseClient_(client)
+ {
+ }
+
+ VirtualRobot::SceneObjectSetPtr
+ CollisionModelHelper::fetchSceneObjects()
+ {
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient_.fetchObjectPoses();
+ return asSceneObjects(objectPoses);
+ }
+
+ CollisionModelHelper::ManipulationObjectSetPtr
+ CollisionModelHelper::fetchManipulationObjects()
+ {
+ const objpose::ObjectPoseSeq objectPoses = objectPoseClient_.fetchObjectPoses();
+ return asManipulationObjects(objectPoses);
+ }
+
+ CollisionModelHelper::ManipulationObjectSetPtr
+ CollisionModelHelper::asManipulationObjects(const objpose::ObjectPoseSeq& objectPoses)
+ {
+ ManipulationObjectSet set;
+
+ for (const auto& pose : objectPoses)
+ {
+ set.emplace_back(*asManipulationObject(pose));
+ }
+
+ return std::make_shared<ManipulationObjectSet>(set);
+ }
+
+ void
+ CollisionModelHelper::visualizeCollisionModel(const VirtualRobot::CollisionModelPtr& model,
+ viz::Client& arviz)
+ {
+ armarx::viz::Mesh mesh(model->getName());
+ auto faces = model->getTriMeshModel()->faces;
+ std::vector<armarx::viz::data::Face> viz_faces;
+ std::transform(
+ faces.begin(),
+ faces.end(),
+ std::back_inserter(viz_faces),
+ [](const auto& face)
+ {
+ return armarx::viz::data::Face(
+ face.id1, face.id2, face.id3, face.idColor1, face.idColor2, face.idColor3);
+ });
+ mesh.vertices(model->getTriMeshModel()->vertices)
+ .faces(viz_faces)
+ .pose(model->getGlobalPose());
+ arviz.commitLayerContaining("CollisionModel", mesh);
+ }
+
+} // namespace armarx::obstacle_avoidance
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..d56a2d5bda01b7492f6b73f5ce3f9416bf7b0db7
--- /dev/null
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
@@ -0,0 +1,62 @@
+/*
+ * 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 17.02.23
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+namespace armarx::obstacle_avoidance
+{
+
+ class CollisionModelHelper
+ {
+ public:
+ using ManipulationObjectSet = std::vector<VirtualRobot::ManipulationObject>;
+ using ManipulationObjectSetPtr = std::shared_ptr<ManipulationObjectSet>;
+
+ static VirtualRobot::ManipulationObjectPtr
+ asManipulationObject(const objpose::ObjectPose& objectPose);
+ static ManipulationObjectSetPtr
+ asManipulationObjects(const objpose::ObjectPoseSeq& objectPoses);
+ static VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
+ static VirtualRobot::SceneObjectSetPtr
+ asSceneObjects(const armem::vision::OccupancyGrid& occupancyGrid,
+ const OccupancyGridHelper::Params& params);
+ static void visualizeCollisionModel(const VirtualRobot::CollisionModelPtr& model,
+ viz::Client& arviz);
+
+
+ CollisionModelHelper(const objpose::ObjectPoseClient& client);
+ VirtualRobot::SceneObjectSetPtr fetchSceneObjects();
+ ManipulationObjectSetPtr fetchManipulationObjects();
+
+ private:
+ objpose::ObjectPoseClient objectPoseClient_;
+ };
+} // namespace armarx::obstacle_avoidance
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
index 23a708aceace48317d04884a8579fd21001b92c3..60a38b465ddb8bd9d6cbdac5ec4952606624880c 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.cpp
@@ -33,7 +33,9 @@ namespace armarx
}
else
{
- ARMARX_INFO << "Trying to add a provider with name '" << info.providerName << "' but the provider already exists.";
+ ARMARX_INFO << "Trying to add a provider with name '" << info.providerName << "' but the provider already exists. "
+ << "Overwriting the old provider info.";
+ skillProviderMap[info.providerName] = info.provider;
}
}
@@ -47,7 +49,7 @@ namespace armarx
}
else
{
- ARMARX_INFO << "Trying to remove a provider with name '" << providerName << "' but it couldn't found.";
+ ARMARX_INFO << "Trying to remove a provider with name '" << providerName << "' but it couldn't be found.";
}
}
@@ -79,31 +81,64 @@ namespace armarx
providerName = info.skillId.providerName;
}
+ bool remove = false;
if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
{
- skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
- getProxy(myPrx, -1);
+ const auto& n = it->first;
+ const auto& s = it->second;
- skills::provider::dto::SkillExecutionRequest exInfo;
- exInfo.skillName = info.skillId.skillName;
- exInfo.executorName = info.executorName;
- exInfo.callbackInterface = myPrx;
- exInfo.params = info.params;
+ if (s)
+ {
+ skills::callback::dti::SkillProviderCallbackInterfacePrx myPrx;
+ getProxy(myPrx, -1);
+
+ skills::provider::dto::SkillExecutionRequest exInfo;
+ exInfo.skillName = info.skillId.skillName;
+ exInfo.executorName = info.executorName;
+ exInfo.callbackInterface = myPrx;
+ exInfo.params = info.params;
+
+ return s->executeSkill(exInfo);
+ }
+ else
+ {
+ remove = true;
+ }
- return it->second->executeSkill(exInfo);
+ if (remove)
+ {
+ std::scoped_lock l(skillProviderMapMutex);
+ if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "' during execution. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ }
+ }
}
else
{
ARMARX_ERROR << "Could not execute a skill of provider '" + providerName + "' because the provider does not exist.";
throw skills::error::SkillException(__PRETTY_FUNCTION__, "Skill execution failed. Could not execute a skill of provider '" + providerName + "' because the provider does not exist.");
}
+ return {}; // Never happens
}
void SkillManagerComponentPluginUser::abortSkill(const std::string& providerName, const std::string& skillName, const Ice::Current ¤t)
{
+ std::scoped_lock l(skillProviderMapMutex);
if (auto it = skillProviderMap.find(providerName); it != skillProviderMap.end())
{
- it->second->abortSkill(skillName);
+ const auto& n = it->first;
+ const auto& s = it->second;
+ if (s)
+ {
+ s->abortSkill(skillName);
+ }
+ else
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ }
}
}
@@ -116,10 +151,23 @@ namespace armarx
skills::manager::dto::SkillDescriptionMapMap SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current ¤t)
{
skills::manager::dto::SkillDescriptionMapMap ret;
- for (const auto& [n, s] : skillProviderMap)
+
+ std::scoped_lock l(skillProviderMapMutex);
+ for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
{
- skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
- ret.insert({n, m});
+ const auto& n = it->first;
+ const auto& s = it->second;
+ if (s)
+ {
+ skills::provider::dto::SkillDescriptionMap m = s->getSkillDescriptions();
+ ret.insert({n, m});
+ ++it;
+ }
+ else
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ }
}
return ret;
}
@@ -127,10 +175,23 @@ namespace armarx
skills::manager::dto::SkillStatusUpdateMapMap SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current ¤t)
{
skills::manager::dto::SkillStatusUpdateMapMap ret;
- for (const auto& [n, s] : skillProviderMap)
+
+ std::scoped_lock l(skillProviderMapMutex);
+ for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
{
- skills::provider::dto::SkillStatusUpdateMap m = s->getSkillExecutionStatuses();
- ret.insert({n, m});
+ const auto& n = it->first;
+ const auto& s = it->second;
+ if (s)
+ {
+ skills::provider::dto::SkillStatusUpdateMap m = s->getSkillExecutionStatuses();
+ ret.insert({n, m});
+ it++;
+ }
+ else
+ {
+ ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '" << n << "'. Removing it from skills.";
+ it = skillProviderMap.erase(it);
+ }
}
return ret;
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillID.cpp b/source/RobotAPI/libraries/skills/provider/SkillID.cpp
index 4619f717f4eeeedc4f7e61dc8ca0698241966b00..07c6248b2f8c366ceeb7782b78315a6d16f74c70 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillID.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillID.cpp
@@ -37,4 +37,9 @@ namespace armarx
return (prefix.empty() ? std::string("") : (prefix + PREFIX_SEPARATOR)) + providerName + NAME_SEPARATOR + skillName;
}
}
+
+ std::ostream& skills::operator<<(std::ostream& os, const SkillID& id)
+ {
+ return os << "'" << id.toString() << "'";
+ }
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillID.h b/source/RobotAPI/libraries/skills/provider/SkillID.h
index 4c44ba82db38ce760a333adfa8518b6e86b62159..320a6e946e04694c6d703d0f78b18552d756cf7d 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillID.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillID.h
@@ -32,5 +32,7 @@ namespace armarx
provider::dto::SkillID toIce() const;
std::string toString(const std::string& prefix = "") const;
};
+
+ std::ostream& operator<<(std::ostream& os, const SkillID& id);
}
}
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index c018ecd9a2610c823241f3b482d799598e37aa8b..d6d8a49a944aa190c47152aced4065bd15a19885 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -4,6 +4,7 @@
#include <queue>
#include <thread>
#include <functional>
+#include <type_traits>
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -66,6 +67,18 @@ namespace armarx
void addSkill(const skills::LambdaSkill::FunT&, const skills::SkillDescription&);
void addSkill(std::unique_ptr<skills::Skill>&&);
+ template<typename T>
+ T* addSkill()
+ {
+ static_assert(std::is_base_of<skills::Skill, T>::value, "T must be derived from skills::Skill!");
+ static_assert(std::is_default_constructible<T>::value, "T must be default constructible!");
+
+ auto skill = std::make_unique<T>();
+ auto* skillPtr = skill.get();
+ addSkill(std::move(skill));
+ return static_cast<T*>(skillPtr);
+ }
+
private:
armarx::plugins::SkillProviderComponentPlugin* plugin = nullptr;
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
index 9f554e11e5c878bdd77cd7c866bef6871b3c6584..3f2b9371e2b087b4a5da77fb06e898bca525c320 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.h
@@ -76,6 +76,7 @@ namespace armarx
}
private:
+ /// Override this method if you want to disable a skill based on certain conditions
virtual bool isAvailable(const SpecializedInitInput&) const
{
return true;
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 1fb8c3ac52010943e01b044d0e99ce144739a2b6..81e91b131294d542c286659e0691056ea0bc0894 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -86,7 +86,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 101e: An error occured during the check whether skill '" + skillName + "' is available. The error was: " + ex.what();
+ std::string message = "SkillError 101e: An error occured during the check whether skill '" + skillName + "' is available. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
resetExecParam();
return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
@@ -102,7 +102,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 201e: An error occured during the reset of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 201e: An error occured during the reset of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
updateStatus(SkillStatus::Failed);
@@ -116,7 +116,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 301e: An error occured during waiting for skill dependencies of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 301e: An error occured during waiting for skill dependencies of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
updateStatus(SkillStatus::Failed);
@@ -143,7 +143,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 401e: An error occured during the initialization of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 401e: An error occured during the initialization of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
@@ -174,7 +174,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 501e: An error occured during the main method of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 501e: An error occured during the main method of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
@@ -199,7 +199,7 @@ namespace armarx
}
catch (const std::exception& ex)
{
- std::string message = "SkillError 601e: An error occured during the exit method of skill '" + skillName + "'. The error was: " + ex.what();
+ std::string message = "SkillError 601e: An error occured during the exit method of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
updateStatus(SkillStatus::Failed);
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/RobotWritingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/RobotWritingSkillMixin.h
new file mode 100644
index 0000000000000000000000000000000000000000..34856452c4b8bd3b4e61dd69ee03eb122d409566
--- /dev/null
+++ b/source/RobotAPI/libraries/skills/provider/mixins/RobotWritingSkillMixin.h
@@ -0,0 +1,28 @@
+#pragma once
+
+
+// Others
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h>
+
+namespace armarx::skills::mixin
+{
+ struct RobotWritingSkillMixin
+ {
+ armem::robot_state::VirtualRobotWriter robotWriter;
+
+ RobotWritingSkillMixin(armem::client::MemoryNameSystem& mns) :
+ robotWriter(mns)
+ {}
+ };
+
+ struct SpecificRobotWritingSkillMixin
+ {
+ std::string robotName;
+ armem::robot_state::VirtualRobotWriter robotWriter;
+
+ SpecificRobotWritingSkillMixin(const std::string& rn, armem::client::MemoryNameSystem& mns) :
+ robotName(rn),
+ robotWriter(mns)
+ {}
+ };
+}
diff --git a/source/RobotAPI/libraries/ukfm/CMakeLists.txt b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
index 96259ddebcdd577a2e049a2e9dda7546b76f46e4..798d6767bec35847f7f86e3cd64a5db5083ed230 100644
--- a/source/RobotAPI/libraries/ukfm/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
@@ -3,10 +3,7 @@ set(LIB_NAME ukfm)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
-find_package(manif QUIET)
armarx_build_if(manif_FOUND "manif not available")
-
-find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
set(LIBS
diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
index 318ed6eb747bc0c92f9935050e385854b5df29e2..0c3c84c01670eb3604d50bdda35e22560ef417ba 100644
--- a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
@@ -217,3 +217,187 @@ template class UnscentedKalmanFilter<SystemModelSE3<double>>;
template class UnscentedKalmanFilter<SystemModelSO3xR3<float>>;
template class UnscentedKalmanFilter<SystemModelSO3xR3<double>>;
+
+
+///////////////////
+
+
+
+template <typename SystemModelT>
+UnscentedKalmanFilterWithoutControl<SystemModelT>::UnscentedKalmanFilterWithoutControl(
+ ObsCovT R,
+ const AlphaT& alpha,
+ StateT state0,
+ StateCovT P0) :
+ R_(std::move(R)),
+ alpha_(alpha),
+ state_(std::move(state0)),
+ P_(std::move(P0)),
+ weights_(alpha)
+{
+}
+
+template <typename SystemModelT>
+void
+UnscentedKalmanFilterWithoutControl<SystemModelT>::propagation(FloatT dt)
+{
+ StateCovT P = P_ + eps * StateCovT::Identity();
+ ARMARX_CHECK(P.allFinite());
+ // ControlNoiseT w = ControlNoiseT::Zero();
+ // update mean
+ StateT state_after = SystemModelT::propagationFunction(state_, dt);
+
+ // set sigma points (Xi in paper when on Manifold, sigma in Tangent Space)
+ Eigen::LLT<StateCovT> llt = P.llt();
+ if (llt.info() != Eigen::ComputationInfo::Success)
+ {
+ P = calculateClosestPosSemidefMatrix(P);
+ P += eps * StateCovT::Identity();
+ llt = P.llt();
+ ARMARX_CHECK(llt.info() == Eigen::ComputationInfo::Success);
+ }
+ StateCovT xi_j_before =
+ weights_.state.sqrt_d_lambda * llt.matrixL().transpose().toDenseMatrix();
+ Eigen::Matrix<FloatT, 2 * dim::state, dim::state> xi_j_after;
+ Eigen::Matrix<FloatT, 2 * dim::control, dim::state> w_j_after;
+
+ auto propagate_and_retract = [&dt, &state_after](long row,
+ long offset,
+ const StateT& state,
+ auto& xi_j) -> void
+ {
+ const StateT sig_j_after = SystemModelT::propagationFunction(state, dt);
+ xi_j.row(offset + row) = SystemModelT::inverseRetraction(state_after, sig_j_after);
+ };
+
+ auto calculate_covariance = [](FloatT wj, FloatT w0, auto& xi_j) -> StateCovT
+ {
+ SigmaPointsT xi_0 = wj * xi_j.colwise().sum();
+ xi_j.rowwise() -= xi_0.transpose();
+ StateCovT cov_after = wj * xi_j.transpose() * xi_j + w0 * xi_0 * xi_0.transpose();
+ return cov_after;
+ };
+
+ for (long j = 0; j < dim::state; j++)
+ {
+ const SigmaPointsT sigma = xi_j_before.row(j);
+ const StateT sig_j_plus = SystemModelT::retraction(state_, sigma);
+ const StateT sig_j_minus = SystemModelT::retraction(state_, -sigma);
+ propagate_and_retract(j, 0, sig_j_plus, xi_j_after);
+ propagate_and_retract(j, dim::state, sig_j_minus, xi_j_after);
+ }
+
+ StateCovT P_after = calculate_covariance(weights_.state.wj, weights_.state.w0, xi_j_after);
+ ARMARX_CHECK(P_after.allFinite());
+ // for (long j = 0; j < dim::control; j++)
+ // {
+ // const ControlNoiseT w_plus = weights_.control.sqrt_d_lambda * chol_Q_.row(j);
+ // const ControlNoiseT w_minus = -weights_.control.sqrt_d_lambda * chol_Q_.row(j);
+ // propagate_and_retract(j, 0, state_, w_plus, w_j_after);
+ // propagate_and_retract(j, dim::control, state_, w_minus, w_j_after);
+ // }
+
+ // StateCovT Q = calculate_covariance(weights_.control.wj, weights_.control.w0, w_j_after);
+ // ARMARX_CHECK(Q.allFinite());
+ StateCovT new_P = P_after; // + Q;
+ P_ = std::move((new_P + new_P.transpose()) / 2.0f);
+ // P_ = new_P;
+ state_ = std::move(state_after);
+}
+
+template <typename SystemModelT>
+void
+UnscentedKalmanFilterWithoutControl<SystemModelT>::update(const ObsT& y)
+{
+ StateCovT P = P_ + eps * StateCovT::Identity();
+ ARMARX_CHECK(P.allFinite());
+ StateCovT xi_j = weights_.state.sqrt_d_lambda * P.llt().matrixL().transpose().toDenseMatrix();
+ ARMARX_CHECK(P.llt().info() == Eigen::ComputationInfo::Success);
+ ARMARX_CHECK(xi_j.allFinite());
+ Eigen::Matrix<FloatT, 2 * dim::state, dim::obs> y_j =
+ Eigen::Matrix<FloatT, 2 * dim::state, dim::obs>::Zero();
+ ObsT y_hat = SystemModelT::observationFunction(state_);
+
+ for (long j = 0; j < dim::state; j++)
+ {
+ const SigmaPointsT sigma = xi_j.row(j);
+ const StateT sig_j_plus = SystemModelT::retraction(state_, sigma);
+ const StateT sig_j_minus = SystemModelT::retraction(state_, -sigma);
+ y_j.row(j) = SystemModelT::observationFunction(sig_j_plus);
+ y_j.row(j + dim::state) = SystemModelT::observationFunction(sig_j_minus);
+ }
+ ARMARX_CHECK(y_j.allFinite());
+ const ObsT y_mean =
+ weights_.update.wm * y_hat + weights_.update.wj * y_j.colwise().sum().transpose();
+ y_j.rowwise() -= y_mean.transpose();
+ y_hat -= y_mean;
+
+ const ObsCovT P_yy = weights_.update.w0 * y_hat * y_hat.transpose() +
+ weights_.update.wj * y_j.transpose() * y_j + R_;
+ ARMARX_CHECK(P_yy.allFinite());
+ const Eigen::Matrix<FloatT, dim::state, dim::obs> P_xiy =
+ weights_.update.wj *
+ (Eigen::Matrix<FloatT, dim::state, 2 * dim::state>() << xi_j, -xi_j).finished() *
+ y_j /* + weights_.update.w0 * xi_0 * y_hat.transpose()*/;
+ ARMARX_CHECK(P_xiy.allFinite());
+ Eigen::Matrix<FloatT, dim::state, dim::obs> K =
+ P_yy.colPivHouseholderQr().solve(P_xiy.transpose()).transpose();
+ FloatT relative_error = (P_yy * K.transpose() - P_xiy.transpose()).norm() / P_xiy.norm();
+ ARMARX_CHECK(relative_error < 1e-4);
+ ARMARX_CHECK(K.allFinite());
+
+ const auto residual = y - y_mean;
+
+ SigmaPointsT xi_plus = K * residual;
+ ARMARX_CHECK(xi_plus.allFinite());
+ state_ = SystemModelT::retraction(state_, xi_plus);
+ StateCovT new_P = P - K * (P_yy)*K.transpose();
+
+ // make result symmetric
+ P_ = calculateClosestPosSemidefMatrix(new_P);
+}
+
+template <typename SystemModelT>
+const typename UnscentedKalmanFilterWithoutControl<SystemModelT>::StateT&
+UnscentedKalmanFilterWithoutControl<SystemModelT>::getCurrentState() const
+{
+ return state_;
+}
+
+template <typename SystemModelT>
+const typename UnscentedKalmanFilterWithoutControl<SystemModelT>::StateCovT&
+UnscentedKalmanFilterWithoutControl<SystemModelT>::getCurrentStateCovariance() const
+{
+ return P_;
+}
+
+template <typename SystemModelT>
+typename UnscentedKalmanFilterWithoutControl<SystemModelT>::StateCovT
+UnscentedKalmanFilterWithoutControl<SystemModelT>::calculateClosestPosSemidefMatrix(
+ const UnscentedKalmanFilterWithoutControl::StateCovT& cov)
+{
+ const StateCovT new_P = ((cov + cov.transpose()) / 2.0f);
+ Eigen::EigenSolver<StateCovT> solver(new_P);
+ Eigen::Matrix<FloatT, dim::state, dim::state> D = solver.eigenvalues().real().asDiagonal();
+ const Eigen::Matrix<FloatT, dim::state, dim::state> V = solver.eigenvectors().real();
+ D = D.cwiseMax(0);
+
+ return (V * D * V.inverse());
+}
+
+template <typename SystemModelT>
+UnscentedKalmanFilterWithoutControl<SystemModelT>::Weights::Weights(AlphaT alpha) :
+ state(dim::state, alpha(0)), update(dim::state, alpha(2))
+{
+}
+
+template <typename SystemModelT>
+UnscentedKalmanFilterWithoutControl<SystemModelT>::Weights::W::W(size_t l, FloatT alpha)
+{
+ ARMARX_CHECK_GREATER(alpha, 0);
+ FloatT m = (alpha * alpha - 1) * l;
+ sqrt_d_lambda = std::sqrt(l + m);
+ wj = 1 / (2 * (l + m));
+ wm = m / (m + l);
+ w0 = m / (m + l) + 3 - alpha * alpha;
+}
diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
index b7da12e5027bafc5786c01e13e6063a32bc215cc..343a89292cf2c743d508e257782aa193c4b9ea32 100644
--- a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
@@ -96,4 +96,63 @@ extern template class UnscentedKalmanFilter<SystemModelSE3<double>>;
extern template class UnscentedKalmanFilter<SystemModelSO3xR3<float>>;
extern template class UnscentedKalmanFilter<SystemModelSO3xR3<double>>;
+
+
+template <typename SystemModelT>
+class UnscentedKalmanFilterWithoutControl
+{
+public:
+ using FloatT = typename SystemModelT::FloatT;
+ static constexpr float eps = 10 * std::numeric_limits<float>::epsilon();
+ // variable dimensions:
+ using dim = typename SystemModelT::dim;
+ using StateT = typename SystemModelT::StateT;
+ using SigmaPointsT = typename SystemModelT::SigmaPointsT; // todo: rename
+
+ using StateCovT = Eigen::Matrix<FloatT, dim::state, dim::state>;
+ using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+ using ObsCovT = Eigen::Matrix<FloatT, dim::obs, dim::obs>;
+ using AlphaT = Eigen::Matrix<FloatT, 3, 1>;
+
+
+ UnscentedKalmanFilterWithoutControl(
+ ObsCovT R,
+ const AlphaT& alpha,
+ StateT state0,
+ StateCovT P0);
+ UnscentedKalmanFilterWithoutControl() = delete;
+
+private:
+ ObsCovT R_;
+ AlphaT alpha_;
+ StateT state_;
+ StateCovT P_;
+
+ struct Weights
+ {
+ struct W
+ {
+ W(size_t l, FloatT alpha);
+ float sqrt_d_lambda, wj, wm, w0;
+ };
+
+ explicit Weights(AlphaT alpha);
+ W state, update;
+ };
+
+ Weights weights_;
+ StateCovT calculateClosestPosSemidefMatrix(const StateCovT& cov);
+
+public:
+ void propagation(FloatT dt);
+ void update(const ObsT& y);
+ const StateT& getCurrentState() const;
+ const StateCovT& getCurrentStateCovariance() const;
+};
+
+extern template class UnscentedKalmanFilterWithoutControl<SystemModelSE3<float>>;
+extern template class UnscentedKalmanFilterWithoutControl<SystemModelSE3<double>>;
+extern template class UnscentedKalmanFilterWithoutControl<SystemModelSO3xR3<float>>;
+extern template class UnscentedKalmanFilterWithoutControl<SystemModelSO3xR3<double>>;
+
#endif //ROBDEKON_UNSCENTEDKALMANFILTER_H