From 697ad84d97a3b567cd1233ea71584267c61284d4 Mon Sep 17 00:00:00 2001
From: "fabian.peller-konrad@kit.edu" <fabian.peller-konrad@kit.edu>
Date: Thu, 6 May 2021 15:26:10 +0200
Subject: [PATCH] first working version

---
 source/RobotAPI/libraries/aron/core/Debug.h   |  12 +-
 .../RobotAPI/libraries/aron/core/Randomizer.h |  35 +++-
 .../codeWriter/cpp/AronCppClass.h             |   2 +-
 .../codeWriter/cpp/serializer/Serializer.cpp  |  40 +++-
 .../codeWriter/cpp/serializer/Serializer.h    |   8 +-
 .../cpp/serializer/container/Dict.cpp         |  12 +-
 .../cpp/serializer/container/List.cpp         |  11 +-
 .../cpp/serializer/container/Object.cpp       |   8 +-
 .../cpp/serializer/container/Pair.cpp         |  13 +-
 .../cpp/serializer/container/Tuple.cpp        |  11 +-
 .../cpp/serializer/enum/IntEnum.cpp           |   2 +-
 .../cpp/serializer/ndarray/EigenMatrix.cpp    |   9 +-
 .../serializer/ndarray/EigenQuaternion.cpp    |   9 +-
 .../cpp/serializer/ndarray/IVTCByteImage.cpp  |  14 +-
 .../cpp/serializer/ndarray/IVTCByteImage.h    |   2 -
 .../cpp/serializer/ndarray/OpenCVMat.cpp      |  14 +-
 .../cpp/serializer/ndarray/OpenCVMat.h        |   1 -
 .../cpp/serializer/ndarray/Orientation.cpp    |  16 +-
 .../cpp/serializer/ndarray/PCLPointCloud.cpp  |  14 +-
 .../cpp/serializer/ndarray/PCLPointCloud.h    |   2 -
 .../cpp/serializer/ndarray/Pose.cpp           |  16 +-
 .../cpp/serializer/ndarray/Position.cpp       |  16 +-
 .../cpp/serializer/primitive/Primitive.cpp    |   8 +-
 .../cpp/serializer/toplevel/IntEnumClass.cpp  |   2 +-
 .../cpp/serializer/toplevel/ObjectClass.cpp   |  18 +-
 .../libraries/aron/core/io/dataIO/Reader.h    |  50 +++--
 .../core/io/dataIO/converter/Converter.cpp    |  32 +--
 .../aron/core/io/dataIO/reader/ReaderToken.h  |   9 +-
 .../reader/navigator/NavigatorReader.cpp      | 182 ++++++++++++------
 .../dataIO/reader/navigator/NavigatorReader.h |  34 ++--
 .../reader/navigator/NavigatorReaderToken.h   |   5 +
 .../nlohmannJSON/NlohmannJSONReader.cpp       | 159 +++++++++------
 .../reader/nlohmannJSON/NlohmannJSONReader.h  |  30 ++-
 .../nlohmannJSON/NlohmannJSONReaderToken.h    |   5 +
 .../aron/core/io/dataIO/visitor/Visitor.cpp   |  12 ++
 .../aron/core/navigator/NavigatorFactory.h    |   1 -
 .../core/navigator/data/NavigatorFactory.cpp  |  63 +-----
 .../core/navigator/data/NavigatorFactory.h    |   1 -
 .../core/navigator/type/NavigatorFactory.cpp  |   5 -
 .../core/navigator/type/NavigatorFactory.h    |   1 -
 .../libraries/aron/core/test/aronTest.cpp     |  76 ++++----
 .../aron/core/test/xmls/OptionalTest.xml      |  10 +
 42 files changed, 543 insertions(+), 427 deletions(-)

diff --git a/source/RobotAPI/libraries/aron/core/Debug.h b/source/RobotAPI/libraries/aron/core/Debug.h
index 28b395864..672b47dee 100644
--- a/source/RobotAPI/libraries/aron/core/Debug.h
+++ b/source/RobotAPI/libraries/aron/core/Debug.h
@@ -46,6 +46,11 @@ namespace armarx::aron
 
         static std::string AronDataPtrToString(const data::AronDataPtr& data)
         {
+            if (!data)
+            {
+                return "";
+            }
+
             dataIO::writer::NlohmannJSONWriter w;
             dataIO::Visitor::VisitAndSetup(w, data);
             return w.getResult().dump(2);
@@ -53,10 +58,11 @@ namespace armarx::aron
 
         static std::string NavigatorPtrToString(const datanavigator::NavigatorPtr& data)
         {
-            if (data == nullptr)
+            if (!data)
             {
                 return "";
             }
+
             dataIO::writer::NlohmannJSONWriter w;
             dataIO::Visitor::VisitAndSetup(w, data);
             return w.getResult().dump(2);
@@ -71,10 +77,6 @@ namespace armarx::aron
 
         static std::string NavigatorPtrToString(const typenavigator::NavigatorPtr& data)
         {
-            if (data == nullptr)
-            {
-                return "";
-            }
             typeIO::writer::NlohmannJSONWriter w;
             typeIO::Visitor::VisitAndSetup(w, data);
             return w.getResult().dump(2);
diff --git a/source/RobotAPI/libraries/aron/core/Randomizer.h b/source/RobotAPI/libraries/aron/core/Randomizer.h
index 315dbed65..b92bc4497 100644
--- a/source/RobotAPI/libraries/aron/core/Randomizer.h
+++ b/source/RobotAPI/libraries/aron/core/Randomizer.h
@@ -119,7 +119,7 @@ namespace armarx::aron
                     t->setSecondAcceptedType(b);
                     return t;
                 }
-                case type::Descriptor::eEigenMatrix:
+                case type::Descriptor::eEigenMatrix: // TODO
                 case type::Descriptor::eEigenQuaternion:
                 case type::Descriptor::eIVTCByteImage:
                 case type::Descriptor::eOpenCVMat:
@@ -170,8 +170,16 @@ namespace armarx::aron
             }
         }
 
-        datanavigator::NavigatorPtr generateAronDataFromType(const typenavigator::NavigatorPtr& type) const
+        datanavigator::NavigatorPtr generateAronDataFromType(const typenavigator::NavigatorPtr& type, bool ignore_optional = false) const
         {
+            if (type->isOptional() && !ignore_optional)
+            {
+                if (fiftyPercentChance())
+                {
+                    return nullptr;
+                }
+            }
+
             const type::Descriptor desc = type->getDescriptor();
             switch (desc)
             {
@@ -233,6 +241,23 @@ case type::Descriptor::e##upperType: \
 
         void initializeRandomly(datanavigator::NavigatorPtr& data, const typenavigator::NavigatorPtr& type) const
         {
+            if (!data)
+            {
+                if (!type->isOptional())
+                {
+                    throw error::AronException("Randomizer", "initializeRandomly", "The data is null but the type is not optional. This is not valid.");
+                }
+
+                if (fiftyPercentChance()) // 50% chance
+                {
+                    data = generateAronDataFromType(type, true);
+                }
+                else
+                {
+                    return;
+                }
+            }
+
             // Containers
             type::Descriptor desc = type->getDescriptor();
             switch (desc)
@@ -264,6 +289,12 @@ case type::Descriptor::e##upperType: \
             return random;
         }
 
+
+        bool fiftyPercentChance() const
+        {
+            return generateRandom(2, 1) == 1;
+        }
+
         std::string generateRandomWord(const std::set<std::string>& usedKeys = {}) const
         {
             std::vector<string> words =
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
index 390a0de8b..4616c362b 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h
@@ -49,7 +49,7 @@ namespace armarx::aron::cppcodegenerator
         AronCppClass() = default;
         virtual void reset() = 0;
         virtual void initialize() = 0;
-        virtual void read(armarx::aron::dataIO::ReaderInterface& r) = 0;
+        virtual void read(armarx::aron::dataIO::ReaderInterface& r, bool skip_first_readStartDict = false) = 0;
         virtual void write(armarx::aron::dataIO::WriterInterface& w) const = 0;
     };
 }
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
index 098952c5b..ed79c6825 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.cpp
@@ -35,6 +35,8 @@ namespace armarx::aron::cppcodegenerator
     const std::string Serializer::ARON_DATA_NAME = simox::meta::get_type_name(typeid(armarx::aron::data::AronData));
     const std::string Serializer::ARON_DATA_PTR_NAME = Serializer::ARON_DATA_NAME + "::PointerType";
 
+    const std::string Serializer::READ_START_RETURN_TYPE_ACCESSOR = "_return_type";
+
     const std::string Serializer::ARON_TYPE_NAME = simox::meta::get_type_name(typeid(armarx::aron::type::AronType));
     const std::string Serializer::ARON_TYPE_PTR_NAME = Serializer::ARON_TYPE_NAME + "::PointerType";
 
@@ -47,8 +49,10 @@ namespace armarx::aron::cppcodegenerator
     {
         {"->", "_ptr_"},
         {".", "_dot_"},
-        {"[", "_lbr_"},
-        {"]", "_rbr_"},
+        {"[", "_lbrE_"},
+        {"]", "_rbrE_"},
+        {"(", "_lbrR_"},
+        {")", "_rbrR_"}
     };
 
     const SerializerFactoryPtr Serializer::FACTORY = SerializerFactoryPtr(new SerializerFactory());
@@ -126,7 +130,7 @@ namespace armarx::aron::cppcodegenerator
         return s;
     }
 
-    CppBlockPtr Serializer::ResolveOptionalBlock(const std::string& accessor, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
+    CppBlockPtr Serializer::ResolveOptionalWriteBlock(const std::string& accessor, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
     {
         if (typenavigator->isOptional())
         {
@@ -145,6 +149,30 @@ namespace armarx::aron::cppcodegenerator
         }
     }
 
+    CppBlockPtr Serializer::ResolveOptionalReadBlock(const std::string& accessor, const std::string& readElement, const CppBlockPtr& block_if_data, const typenavigator::NavigatorPtr& typenavigator)
+    {
+        std::string escaped_accessor = EscapeAccessor(accessor);
+        std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
+
+        CppBlockPtr b = std::make_shared<CppBlock>();
+        b->addLine("auto " + read_start_result_accessor + " = " + readElement + ";");
+        if (typenavigator->isOptional())
+        {
+            b->addLine("if (" + read_start_result_accessor + ".success)");
+
+            CppBlockPtr b2 = std::make_shared<CppBlock>();
+            b2->addLine(accessor + ".emplace();");
+            b2->appendBlock(block_if_data);
+
+            b->addBlock(b2);
+        }
+        else
+        {
+            b->appendBlock(block_if_data);
+        }
+        return b;
+    }
+
     // constructors
     Serializer::Serializer(const std::string& cppName, const std::string& aronDataTypename, const std::string& aronTypeTypename, SerializerAccessType accessType) :
         accessType(accessType),
@@ -264,8 +292,10 @@ namespace armarx::aron::cppcodegenerator
         doc << "@param r - The reader implementation\n";
         doc << "@return - nothing";
 
-        CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void read(armarx::aron::dataIO::ReaderInterface& r) override", doc.str()));
-        CppBlockPtr b = this->getReadBlock("");
+        CppMethodPtr m = CppMethodPtr(new CppMethod("virtual void read(armarx::aron::dataIO::ReaderInterface& r, bool skip_first_readStartDict = false) override", doc.str()));
+        CppBlockPtr b = std::make_shared<CppBlock>();
+        b->addLine("this->reset();");
+        b->appendBlock(this->getReadBlock(""));
         m->setBlock(b);
         return m;
     }
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
index f631e9ecb..443d77098 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/Serializer.h
@@ -32,6 +32,7 @@
 // Simox
 #include <SimoxUtility/meta/type_name.h>
 #include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/algorithm/vector.hpp>
 
 // ArmarX
 #include <ArmarXCore/libraries/cppgen/CppBlock.h>
@@ -121,7 +122,8 @@ namespace armarx::aron::cppcodegenerator
         static SerializerPtr FromAronTypeNaviagtorPtr(const typenavigator::NavigatorPtr&);
 
         static std::string ResolveOptionalAccessor(const std::string&, const typenavigator::NavigatorPtr&);
-        static CppBlockPtr ResolveOptionalBlock(const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
+        static CppBlockPtr ResolveOptionalWriteBlock(const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
+        static CppBlockPtr ResolveOptionalReadBlock(const std::string&, const std::string&, const CppBlockPtr&, const typenavigator::NavigatorPtr&);
 
     protected:
         std::string nextEl() const
@@ -139,8 +141,10 @@ namespace armarx::aron::cppcodegenerator
             }
         }
 
+    protected:
+        static const std::string READ_START_RETURN_TYPE_ACCESSOR;
+
     private:
-        // members
         const static std::string ARON_DATA_NAME;
         const static std::string ARON_DATA_PTR_NAME;
 
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
index 821574260..38f19a14c 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Dict.cpp
@@ -105,13 +105,12 @@ namespace armarx::aron::cppcodegenerator::serializer
         block_if_data->addBlock(b2);
         block_if_data->addLine("w.writeEndDict();");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr DictSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
-        b->addLine("r.readStartDict();");
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
 
         std::string escaped_accessor = EscapeAccessor(accessor);
         std::string accessor_iterator = escaped_accessor + DICT_ITERATOR_ACCESSOR;
@@ -119,15 +118,16 @@ namespace armarx::aron::cppcodegenerator::serializer
 
         auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
 
-        b->addLine("while(!r.readEndDict())");
+        block_if_data->addLine("while(!r.readEndDict())");
         CppBlockPtr b2 = CppBlockPtr(new CppBlock());
+
         b2->addLine("std::string " + accessor_iterator_key + " = r.readKey();");
         b2->addLine(type_s->getFullCppTypename() + " " + accessor_iterator + ";");
         b2->appendBlock(type_s->getReadBlock(accessor_iterator));
         b2->addLine(accessor + nextEl() + "insert({" + accessor_iterator_key + ", " + accessor_iterator + "});");
 
-        b->addBlock(b2);
-        return b;
+        block_if_data->addBlock(b2);
+        return ResolveOptionalReadBlock(accessor, "r.readStartDict()", block_if_data, typenavigator);
     }
 
     CppBlockPtr DictSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
index ae2428570..5a3cb2da4 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/List.cpp
@@ -94,18 +94,17 @@ namespace armarx::aron::cppcodegenerator::serializer
         block_if_data->addBlock(type_s->getWriteBlock(accessor + nextEl() + "at(" + accessor_iterator + ")"));
         block_if_data->addLine("w.writeEndList();");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr ListSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
-        b->addLine("r.readStartList();");
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
 
         std::string escaped_accessor = EscapeAccessor(accessor);
         std::string accessor_iterator = escaped_accessor + LIST_ITERATOR_ACCESSOR;
 
-        b->addLine("while(!r.readEndList())");
+        block_if_data->addLine("while(!r.readEndList())");
         CppBlockPtr b2 = CppBlockPtr(new CppBlock());
 
         auto type_s = FromAronTypeNaviagtorPtr(typenavigator->getAcceptedType());
@@ -113,8 +112,8 @@ namespace armarx::aron::cppcodegenerator::serializer
         b2->appendBlock(type_s->getReadBlock(accessor_iterator));
         b2->addLine(accessor + nextEl() + "push_back(" + accessor_iterator + ");");
 
-        b->addBlock(b2);
-        return b;
+        block_if_data->addBlock(b2);
+        return ResolveOptionalReadBlock(accessor, "r.readStartList()", block_if_data, typenavigator);
     }
 
     CppBlockPtr ListSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
index 31d398f40..166e21417 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Object.cpp
@@ -94,14 +94,14 @@ namespace armarx::aron::cppcodegenerator::serializer
         CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
         block_if_data->addLine(accessor + nextEl() + "write(w);");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr ObjectSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
-        b->addLine(accessor + nextEl() + "read(r);");
-        return b;
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+        block_if_data->addLine(accessor + nextEl() + "read(r, true);");
+        return ResolveOptionalReadBlock(accessor, "r.readStartDict()", block_if_data, typenavigator);
     }
 
     CppBlockPtr ObjectSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
index 447f5b92c..bd93b2827 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Pair.cpp
@@ -132,24 +132,23 @@ namespace armarx::aron::cppcodegenerator::serializer
 
         block_if_data->addLine("w.writeEndList();");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr PairSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
-        b->addLine("r.readStartList();");
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
 
         auto child_s1 = FromAronTypeNaviagtorPtr(typenavigator->getFirstAcceptedType());
         CppBlockPtr b21 = child_s1->getReadBlock(accessor + nextEl() + "first");
-        b->appendBlock(b21);
+        block_if_data->appendBlock(b21);
 
         auto child_s2 = FromAronTypeNaviagtorPtr(typenavigator->getSecondAcceptedType());
         CppBlockPtr b22 = child_s2->getReadBlock(accessor + nextEl() + "second");
-        b->appendBlock(b22);
+        block_if_data->appendBlock(b22);
 
-        b->addLine("r.readEndList();");
-        return b;
+        block_if_data->addLine("r.readEndList();");
+        return ResolveOptionalReadBlock(accessor, "r.readStartList()", block_if_data, typenavigator);
     }
 
     CppBlockPtr PairSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
index 57d7e0419..41b9f2325 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/container/Tuple.cpp
@@ -129,13 +129,12 @@ namespace armarx::aron::cppcodegenerator::serializer
         }
         block_if_data->addLine("w.writeEndList();");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr TupleSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
-        b->addLine("r.readStartList();");
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
 
         std::string resolved_accessor = ResolveOptionalAccessor(accessor, typenavigator);
 
@@ -144,10 +143,10 @@ namespace armarx::aron::cppcodegenerator::serializer
         {
             auto type_s = FromAronTypeNaviagtorPtr(type);
             CppBlockPtr b2 = type_s->getReadBlock("std::get<" + std::to_string(i++) + ">(" + resolved_accessor + ")");
-            b->appendBlock(b2);
+            block_if_data->appendBlock(b2);
         }
-        b->addLine("r.readEndList();");
-        return b;
+        block_if_data->addLine("r.readEndList();");
+        return ResolveOptionalReadBlock(accessor, "r.readStartList()", block_if_data, typenavigator);
     }
 
     CppBlockPtr TupleSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
index 7cbd3deb4..3b320f482 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/enum/IntEnum.cpp
@@ -86,7 +86,7 @@ namespace armarx::aron::cppcodegenerator::serializer
         CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
         block_if_data->addLine(accessor + nextEl() + "write(w);");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr IntEnumSerializer::getReadBlock(const std::string& accessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
index 4184f0754..aac6ddc92 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenMatrix.cpp
@@ -91,15 +91,14 @@ namespace armarx::aron::cppcodegenerator::serializer
         CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
         block_if_data->addLine("w.writeNDArray({" + accessor + nextEl() + "rows(), " + accessor + nextEl() + "cols(), " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data()));");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr EigenMatrixSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
-        b->addLine("r.readStartNDArray(); // We do not need the dims and type since a EigenMat can not change (templated)");
-        b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data()));");
-        return b;
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+        block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data()));");
+        return ResolveOptionalReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
     }
 
     CppBlockPtr EigenMatrixSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
index c1f541675..9d440b662 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/EigenQuaternion.cpp
@@ -86,15 +86,14 @@ namespace armarx::aron::cppcodegenerator::serializer
         CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
         block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "coeffs().data()));");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr EigenQuaternionSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
-        b->addLine("r.readStartNDArray(); // We do not need the dims and type since a EigenMat can not change (templated)");
-        b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "coeffs().data()));");
-        return b;
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+        block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "coeffs().data()));");
+        return ResolveOptionalReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
     }
 
     CppBlockPtr EigenQuaternionSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
index 170b9a086..b0837995b 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.cpp
@@ -86,20 +86,18 @@ namespace armarx::aron::cppcodegenerator::serializer
         CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
         block_if_data->addLine("w.writeNDArray({" + accessor + nextEl() + "width, " + accessor + nextEl() + "height, " + accessor + nextEl() + "bytesPerPixel}, std::to_string(" + accessor + nextEl() + "type), reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "pixels));");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr IVTCByteImageSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
         std::string escaped_accessor = EscapeAccessor(accessor);
-        std::string dims_accessor = escaped_accessor + DIMENSION_ACCESSOR;
-        std::string type_accessor = escaped_accessor + TYPE_ACCESSOR;
+        std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
 
-        b->addLine("const auto [" + dims_accessor + ", " + type_accessor + "] = r.readStartNDArray();");
-        b->addLine(accessor + nextEl() + "Set(" + dims_accessor + "[0], " + dims_accessor + "[1], static_cast<CByteImage::ImageType>(std::stoi(" + type_accessor + ")));");
-        b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "pixels));");
-        return b;
+        block_if_data->addLine(accessor + nextEl() + "Set(" + read_start_result_accessor + ".dims[0], " + read_start_result_accessor + ".dims[1], static_cast<CByteImage::ImageType>(std::stoi(" + read_start_result_accessor + ".type)));");
+        block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "pixels));");
+        return ResolveOptionalReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
     }
 
     CppBlockPtr IVTCByteImageSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
index a801d7f3d..adc0bd711 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/IVTCByteImage.h
@@ -63,8 +63,6 @@ namespace armarx::aron::cppcodegenerator::serializer
     private:
         // members
         static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
-        static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
-        static constexpr const char* TYPE_ACCESSOR = "_type";
 
         typenavigator::IVTCByteImageNavigatorPtr typenavigator;
     };
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
index 4076b1dc7..a1e6c70af 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.cpp
@@ -119,20 +119,18 @@ namespace armarx::aron::cppcodegenerator::serializer
         block_if_data->addLine(accessor_dimensions + ".push_back(" + accessor + nextEl() + "elemSize());");
         block_if_data->addLine("w.writeNDArray(" + accessor_dimensions + ", std::to_string(" + accessor + nextEl() + "type()), reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data));");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr OpenCVMatSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
         std::string escaped_accessor = EscapeAccessor(accessor);
-        std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
-        std::string accessor_type = escaped_accessor + TYPE_ACCESSOR;
+        std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
 
-        b->addLine("const auto [" + accessor_dimensions + ", " + accessor_type + "] = r.readStartNDArray();");
-        b->addLine(accessor + " = " + getFullCppTypename() + "(std::vector<int>({" + accessor_dimensions + ".begin(), std::prev(" + accessor_dimensions + ".end())}), std::stoi(" + accessor_type + "));");
-        b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data));");
-        return b;
+        block_if_data->addLine(accessor + " = " + getCoreCppTypename() + "(std::vector<int>({" + read_start_result_accessor + ".dims.begin(), std::prev(" + read_start_result_accessor + ".dims.end())}), std::stoi(" + read_start_result_accessor + ".type));");
+        block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data));");
+        return ResolveOptionalReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
     }
 
     CppBlockPtr OpenCVMatSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
index 79c8b0c05..d09789b18 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/OpenCVMat.h
@@ -65,7 +65,6 @@ namespace armarx::aron::cppcodegenerator::serializer
         static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
         static constexpr const char* ITERATOR_ACCESSOR = "_iterator";
         static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
-        static constexpr const char* TYPE_ACCESSOR = "_type";
 
         typenavigator::OpenCVMatNavigatorPtr typenavigator;
     };
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
index 8a8afb3c7..e174d775d 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Orientation.cpp
@@ -77,22 +77,16 @@ namespace armarx::aron::cppcodegenerator::serializer
     CppBlockPtr OrientationSerializer::getWriteBlock(const std::string& accessor) const
     {
         CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-        block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + "}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "coeffs().data()));");
+        block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "coeffs().data()));");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr OrientationSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
-        std::string escaped_accessor = EscapeAccessor(accessor);
-        std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
-        std::string accessor_type = escaped_accessor + TYPE_ACCESSOR;
-
-        b->addLine("const auto [" + accessor_dimensions + ", " + accessor_type + "] = r.readStartNDArray();");
-        b->addLine(accessor + " = " + getFullCppTypename() + "();");
-        b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "coeffs().data()));");
-        return b;
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+        block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "coeffs().data()));");
+        return ResolveOptionalReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
     }
 
     CppBlockPtr OrientationSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
index 761e2b6f0..421712428 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.cpp
@@ -99,20 +99,18 @@ namespace armarx::aron::cppcodegenerator::serializer
         CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
         block_if_data->addLine("w.writeNDArray({" + accessor + nextEl() + "width, " + accessor + nextEl() + "height, " + std::to_string(ACCEPTED_TYPES.at(typenavigator->getTypename()).second) + "}, \"" + typenavigator->getTypename() + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "points.data()));");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr PCLPointCloudSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
         std::string escaped_accessor = EscapeAccessor(accessor);
-        std::string dims_accessor = escaped_accessor + DIMENSION_ACCESSOR;
-        std::string type_accessor = escaped_accessor + TYPE_ACCESSOR;
+        std::string read_start_result_accessor = escaped_accessor + READ_START_RETURN_TYPE_ACCESSOR;
 
-        b->addLine("const auto [" + dims_accessor + ", " + type_accessor + "] = r.readStartNDArray();");
-        b->addLine(accessor + " = " + getFullCppTypename() + "(" + dims_accessor + "[0], " + dims_accessor + "[1]);");
-        b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "points.data()));");
-        return b;
+        block_if_data->addLine(accessor + " = " + getCoreCppTypename() + "(" + read_start_result_accessor + ".dims[0], " + read_start_result_accessor + ".dims[1]);");
+        block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "points.data()));");
+        return ResolveOptionalReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
     }
 
     CppBlockPtr PCLPointCloudSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
index 2224fce91..737d7fedf 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/PCLPointCloud.h
@@ -62,8 +62,6 @@ namespace armarx::aron::cppcodegenerator::serializer
     private:
         // members
         static const std::map<std::string, std::pair<std::string, int>> ACCEPTED_TYPES;
-        static constexpr const char* DIMENSION_ACCESSOR = "_dimensions";
-        static constexpr const char* TYPE_ACCESSOR = "_type";
 
         typenavigator::PCLPointCloudNavigatorPtr typenavigator;
     };
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
index f13b1d44b..cfd519183 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Pose.cpp
@@ -77,22 +77,16 @@ namespace armarx::aron::cppcodegenerator::serializer
     CppBlockPtr PoseSerializer::getWriteBlock(const std::string& accessor) const
     {
         CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-        block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + "}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data()));");
+        block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data()));");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr PoseSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
-        std::string escaped_accessor = EscapeAccessor(accessor);
-        std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
-        std::string accessor_type = escaped_accessor + TYPE_ACCESSOR;
-
-        b->addLine("const auto [" + accessor_dimensions + ", " + accessor_type + "] = r.readStartNDArray();");
-        b->addLine(accessor + " = " + getFullCppTypename() + "();");
-        b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data()));");
-        return b;
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+        block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data()));");
+        return ResolveOptionalReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
     }
 
     CppBlockPtr PoseSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
index 4d1ee7b6e..2a10218ee 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/ndarray/Position.cpp
@@ -77,22 +77,16 @@ namespace armarx::aron::cppcodegenerator::serializer
     CppBlockPtr PositionSerializer::getWriteBlock(const std::string& accessor) const
     {
         CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
-        block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + "}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data()));");
+        block_if_data->addLine("w.writeNDArray({" + simox::alg::to_string(typenavigator->ACCEPTED_DIMENSION, ", ") + ", 4}, \"" + typenavigator->ACCEPTED_TYPE + "\", reinterpret_cast<const unsigned char*>(" + accessor + nextEl() + "data()));");
 
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator);
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator);
     }
 
     CppBlockPtr PositionSerializer::getReadBlock(const std::string& accessor) const
     {
-        CppBlockPtr b = CppBlockPtr(new CppBlock());
-        std::string escaped_accessor = EscapeAccessor(accessor);
-        std::string accessor_dimensions = escaped_accessor + DIMENSION_ACCESSOR;
-        std::string accessor_type = escaped_accessor + TYPE_ACCESSOR;
-
-        b->addLine("const auto [" + accessor_dimensions + ", " + accessor_type + "] = r.readStartNDArray();");
-        b->addLine(accessor + " = " + getFullCppTypename() + "();");
-        b->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data()));");
-        return b;
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock());
+        block_if_data->addLine("r.readEndNDArray(reinterpret_cast<unsigned char*>(" + accessor + nextEl() + "data()));");
+        return ResolveOptionalReadBlock(accessor, "r.readStartNDArray()", block_if_data, typenavigator);
     }
 
     CppBlockPtr PositionSerializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Primitive.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Primitive.cpp
index 8987f2d59..2b445e9a9 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Primitive.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/primitive/Primitive.cpp
@@ -100,15 +100,15 @@ namespace armarx::aron::cppcodegenerator::serializer
         std::string resolved_accessor = ResolveOptionalAccessor(accessor, typenavigator); \
         block_if_data->addLine("w.writePrimitive(" + resolved_accessor + ");"); \
         \
-        return ResolveOptionalBlock(accessor, block_if_data, typenavigator); \
+        return ResolveOptionalWriteBlock(accessor, block_if_data, typenavigator); \
     } \
     \
     CppBlockPtr upperType##Serializer::getReadBlock(const std::string& accessor) const \
     { \
-        CppBlockPtr b = CppBlockPtr(new CppBlock()); \
+        CppBlockPtr block_if_data = CppBlockPtr(new CppBlock()); \
         std::string resolved_accessor = ResolveOptionalAccessor(accessor, typenavigator); \
-        b->addLine("r.readPrimitive(" + resolved_accessor + ");"); \
-        return b; \
+        block_if_data->addLine("r.readEndPrimitive(" + resolved_accessor + ");"); \
+        return ResolveOptionalReadBlock(accessor, "r.readStartPrimitive()", block_if_data, typenavigator); \
     } \
     \
     CppBlockPtr upperType##Serializer::getEqualsBlock(const std::string& accessor, const std::string& otherInstanceAccessor) const \
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
index 68fc518a9..bba85ed45 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/IntEnumClass.cpp
@@ -120,7 +120,7 @@ namespace armarx::aron::cppcodegenerator::serializer
     {
         CppBlockPtr b = CppBlockPtr(new CppBlock());
         b->addLine("int temporary;");
-        b->addLine("r.readPrimitive(temporary);");
+        b->addLine("r.readEndPrimitive(temporary);");
         b->addLine("value = ValueToEnumMap.at(temporary);");
         return b;
     }
diff --git a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
index 4ad79753c..21c9a67e0 100644
--- a/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
+++ b/source/RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/serializer/toplevel/ObjectClass.cpp
@@ -150,7 +150,11 @@ namespace armarx::aron::cppcodegenerator::serializer
             b->addLine(extends_s->getFullCppTypename() + "::read(r);");
         }
 
-        b->addLine("r.readStartDict();");
+        b->addLine("if(!skip_first_readStartDict)");
+        auto readStartDict = std::make_shared<CppBlock>();
+        readStartDict->addLine("r.readStartDict();");
+        b->addBlock(readStartDict);
+
         for (const auto& [key, child] : navigator->getMemberTypes())
         {
             const auto child_s = FromAronTypeNaviagtorPtr(child);
@@ -158,17 +162,9 @@ namespace armarx::aron::cppcodegenerator::serializer
 
             CppBlockPtr child_b = child_s->getReadBlock(key);
             q->appendBlock(child_b);
+            b->addLine("r.loadMember(\"" + key + "\");");
+            b->appendBlock(q);
 
-            if (child->isOptional())
-            {
-                b->addLine("if (r.loadMember(\"" + key + "\"));");
-                b->addBlock(q);
-            }
-            else
-            {
-                b->addLine("r.loadMember(\"" + key + "\");");
-                b->appendBlock(q);
-            }
         }
         b->addLine("r.readEndDict();");
         return b;
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
index affc2342e..ac4ee93d1 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/Reader.h
@@ -38,31 +38,45 @@ namespace armarx::aron::dataIO
     class ReaderInterface
     {
     public:
-        virtual int readStartDict() = 0;
+        struct ReadStartDictReturnType
+        {
+            int elements;
+            bool success;
+        };
+        virtual ReadStartDictReturnType readStartDict() = 0;
         virtual bool readEndDict() = 0;
-        virtual int readStartList() = 0;
-        virtual bool readEndList() = 0;
 
-        virtual std::tuple<std::vector<int>, std::string> readStartNDArray() = 0;
-        virtual bool readEndNDArray(unsigned char*) = 0;
+        struct ReadStartListReturnType
+        {
+            int elements;
+            bool success;
+        };
+        virtual ReadStartListReturnType readStartList() = 0;
+        virtual bool readEndList() = 0;
 
-        virtual void readPrimitive(int&) = 0;
-        virtual void readPrimitive(long&) = 0;
-        virtual void readPrimitive(float&) = 0;
-        virtual void readPrimitive(double&) = 0;
-        virtual void readPrimitive(std::string&) = 0;
-        virtual void readPrimitive(bool&) = 0;
+        struct ReadStartNDArrayReturnType
+        {
+            std::vector<int> dims;
+            std::string type;
+            bool success;
+        };
+        virtual ReadStartNDArrayReturnType readStartNDArray() = 0;
+        virtual void readEndNDArray(unsigned char*) = 0;
 
-        template<class T>
-        T readPrimitive()
+        struct ReadPrimitiveReturnType
         {
-            T p;
-            readPrimitive(p);
-            return p;
-        }
+            bool success;
+        };
+        virtual ReadPrimitiveReturnType readStartPrimitive() = 0;
+        virtual void readEndPrimitive(int&) = 0;
+        virtual void readEndPrimitive(long&) = 0;
+        virtual void readEndPrimitive(float&) = 0;
+        virtual void readEndPrimitive(double&) = 0;
+        virtual void readEndPrimitive(std::string&) = 0;
+        virtual void readEndPrimitive(bool&) = 0;
 
         virtual std::string readKey() = 0;
-        virtual bool loadMember(const std::string&) = 0;
+        virtual void loadMember(const std::string&) = 0;
 
         // Helper functions
         virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const = 0;
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
index a0feee781..a7629cd5c 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/converter/Converter.cpp
@@ -40,7 +40,7 @@ namespace armarx::aron::dataIO
         {
             case data::Descriptor::eDict:
             {
-                int elements = reader.readStartDict();
+                auto readStartDict = reader.readStartDict();
                 writer.writeStartDict();
 
                 typenavigator::NavigatorPtr childType = nullptr;
@@ -50,7 +50,7 @@ namespace armarx::aron::dataIO
                     childType = t->getAcceptedType();
                 }
 
-                for (int i = 0; i < elements; ++i)
+                for (int i = 0; i < readStartDict.elements; ++i)
                 {
                     std::string key = reader.readKey();
                     writer.writeKey(key);
@@ -71,7 +71,7 @@ namespace armarx::aron::dataIO
             }
             case data::Descriptor::eList:
             {
-                int elements = reader.readStartList();
+                auto readStartList = reader.readStartList();
                 writer.writeStartList();
 
                 typenavigator::NavigatorPtr childType = nullptr;
@@ -81,7 +81,7 @@ namespace armarx::aron::dataIO
                     childType = t->getAcceptedType();
                 }
 
-                for (int i = 0; i < elements; ++i)
+                for (int i = 0; i < readStartList.elements; ++i)
                 {
                     if (t_desc == type::Descriptor::eObject)
                     {
@@ -99,48 +99,54 @@ namespace armarx::aron::dataIO
 
             case data::Descriptor::eNDArray:
             {
-                auto [dims, type] = reader.readStartNDArray();
-                int elements = std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>());
+                auto readStartNDArray = reader.readStartNDArray();
+                int elements = std::accumulate(std::begin(readStartNDArray.dims), std::end(readStartNDArray.dims), 1, std::multiplies<int>());
                 std::vector<unsigned char> data(elements);
                 reader.readEndNDArray(data.data());
 
-                writer.writeNDArray(dims, type, data.data());
+                writer.writeNDArray(readStartNDArray.dims, readStartNDArray.type, data.data());
                 break;
             }
 
             case data::Descriptor::eInt:
             {
-                int val = reader.readPrimitive<int>();
+                int val;
+                reader.readEndPrimitive(val);
                 writer.writePrimitive(val);
                 break;
             }
             case data::Descriptor::eLong:
             {
-                long val = reader.readPrimitive<long>();
+                long val;
+                reader.readEndPrimitive(val);
                 writer.writePrimitive(val);
                 break;
             }
             case data::Descriptor::eFloat:
             {
-                float val = reader.readPrimitive<float>();
+                float val;
+                reader.readEndPrimitive(val);
                 writer.writePrimitive(val);
                 break;
             }
             case data::Descriptor::eDouble:
             {
-                double val = reader.readPrimitive<double>();
+                double val;
+                reader.readEndPrimitive(val);
                 writer.writePrimitive(val);
                 break;
             }
             case data::Descriptor::eString:
             {
-                std::string val = reader.readPrimitive<std::string>();
+                std::string val;
+                reader.readEndPrimitive(val);
                 writer.writePrimitive(val);
                 break;
             }
             case data::Descriptor::eBool:
             {
-                bool val = reader.readPrimitive<bool>();
+                bool val;
+                reader.readEndPrimitive(val);
                 writer.writePrimitive(val);
                 break;
             }
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
index 5fff9e81c..9e22a2acd 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/ReaderToken.h
@@ -116,11 +116,14 @@ namespace armarx::aron::dataIO
             }
         }
 
-        ElementTypename getNextElementAndIncreaseCnt()
+        void increaseCounter()
         {
-            ElementTypename next = getNextElement();
             this->currentIndex++;
-            return next;
+        }
+
+        unsigned int getCurrentIndex() const
+        {
+            return currentIndex;
         }
 
     protected:
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
index 22eabe145..7c1a4bd02 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.cpp
@@ -34,47 +34,46 @@
 
 namespace armarx::aron::dataIO::reader
 {
-    NavigatorReader::NavigatorReader(const datanavigator::NavigatorPtr& n) :
-        input(n)
+    NavigatorReader::NavigatorReader(const data::AronDictPtr& n) :
+        NavigatorReader(datanavigator::DictNavigator::DynamicCastAndCheck(datanavigator::Navigator::FromAronData(n)))
     {
     }
 
-    NavigatorReader::NavigatorReader(const data::AronDataPtr& n) :
-        input(datanavigator::Navigator::FromAronData(n))
+    NavigatorReader::NavigatorReader(const datanavigator::DictNavigatorPtr& n) :
+        input(n)
     {
+        auto current_nav_casted = datanavigator::DictNavigator::DynamicCastAndCheck(input);
+        auto newToken = std::make_shared<NavigatorReaderToken>(data::Descriptor::eDict, current_nav_casted);
+        stack.push(newToken);
     }
 
-    datanavigator::NavigatorPtr NavigatorReader::getNextAndIncrease()
+    // Containers
+    ReaderInterface::ReadStartDictReturnType NavigatorReader::readStartDict()
     {
-        if (!readInitialStart)
+        if (!readFirstStartDict)
         {
-            readInitialStart = true;
-            return input;
+            // we already added in constructor
+            readFirstStartDict = true;
+            auto lastToken = stack.top();
+            auto current_nav = lastToken->getElement();
+            int c = current_nav->childrenSize();
+            return {c, true};
         }
+
         auto lastToken = stack.top();
-        return lastToken->getNextElementAndIncreaseCnt();
-    }
 
-    datanavigator::NavigatorPtr NavigatorReader::getNext()
-    {
-        if (!readInitialStart)
+        auto current_nav = lastToken->getNextElement();
+        if (!current_nav)
         {
-            readInitialStart = true;
-            return input;
+            lastToken->increaseCounter();
+            return {0, false};
         }
-        auto lastToken = stack.top();
-        return lastToken->getNextElement();
-    }
 
-    // Containers
-    int NavigatorReader::readStartDict()
-    {
-        auto current_nav = getNextAndIncrease();
         auto current_nav_casted = datanavigator::DictNavigator::DynamicCastAndCheck(current_nav);
         int c = current_nav->childrenSize();
         auto newToken = std::make_shared<NavigatorReaderToken>(data::Descriptor::eDict, current_nav_casted);
         stack.push(newToken);
-        return c;
+        return {c, true};
     }
 
     bool NavigatorReader::readEndDict()
@@ -82,22 +81,39 @@ namespace armarx::aron::dataIO::reader
         auto token = stack.top();
         datanavigator::DictNavigator::DynamicCastAndCheck(token->getElement());
 
+        std::cout << "We are at: " << token->getCurrentIndex() << "/" << token->getElementChildrenSize() << std::endl;
+
         if (token->finishedElement())
         {
             stack.pop();
+
+            if (!stack.empty())
+            {
+                auto lastToken = stack.top();
+                lastToken->increaseCounter();
+            }
+
             return true;
         }
         return false;
     }
 
-    int NavigatorReader::readStartList()
+    ReaderInterface::ReadStartListReturnType NavigatorReader::readStartList()
     {
-        auto current_nav = getNextAndIncrease();
+        auto lastToken = stack.top();
+
+        auto current_nav = lastToken->getNextElement();
+        if (!current_nav)
+        {
+            lastToken->increaseCounter();
+            return {0, false};
+        }
+
         auto current_nav_casted = datanavigator::ListNavigator::DynamicCastAndCheck(current_nav);
         int c = current_nav->childrenSize();
         auto newToken = std::make_shared<NavigatorReaderToken>(data::Descriptor::eList, current_nav_casted);
         stack.push(newToken);
-        return c;
+        return {c, true};
     }
 
     bool NavigatorReader::readEndList()
@@ -105,96 +121,136 @@ namespace armarx::aron::dataIO::reader
         auto token = stack.top();
         datanavigator::ListNavigator::DynamicCastAndCheck(token->getElement());
 
+        std::cout << "We are at: " << token->getCurrentIndex() << "/" << token->getElementChildrenSize() << std::endl;
         if (token->finishedElement())
         {
             stack.pop();
+
+            auto lastToken = stack.top();
+            lastToken->increaseCounter();
+
             return true;
         }
         return false;
     }
 
     // Complex Data
-    std::tuple<std::vector<int>, std::string> NavigatorReader::readStartNDArray()
+    ReaderInterface::ReadStartNDArrayReturnType NavigatorReader::readStartNDArray()
     {
-        datanavigator::NavigatorPtr nav = getNext();
-        datanavigator::NDArrayNavigatorPtr casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(nav);
-        return {casted->getDimensions(), casted->getType()};
+        auto lastToken = stack.top();
+
+        auto current_nav = lastToken->getNextElement();
+        if (!current_nav)
+        {
+            lastToken->increaseCounter();
+            return {{}, "", false};
+        }
+        datanavigator::NDArrayNavigatorPtr casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(current_nav);
+        return {casted->getDimensions(), casted->getType(), true};
     }
 
-    bool NavigatorReader::readEndNDArray(unsigned char* data)
+    void NavigatorReader::readEndNDArray(unsigned char* data)
     {
         if (data == NULL)
         {
             throw error::AronException("NavigatorReader", "readEndNDArray", "The corresponding data pointer of an complex aron ptr is NULL.");
         }
-        auto nav = getNextAndIncrease();
-        auto casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(nav);
+
+        auto lastToken = stack.top();
+        auto current_nav = lastToken->getNextElement();
+        lastToken->increaseCounter();
+
+        auto casted = datanavigator::NDArrayNavigator::DynamicCastAndCheck(current_nav);
         std::vector<int> dims = casted->getDimensions();
         memcpy(data, casted->getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
-        return true;
     }
 
     // Read primitives
-    void NavigatorReader::readPrimitive(int& t)
+    ReaderInterface::ReadPrimitiveReturnType NavigatorReader::readStartPrimitive()
+    {
+        auto lastToken = stack.top();
+        auto current_nav = lastToken->getNextElement();
+        if (!current_nav)
+        {
+            lastToken->increaseCounter();
+            return {false};
+        }
+        return {true};
+    }
+
+    void NavigatorReader::readEndPrimitive(int& t)
     {
-        auto nav = getNextAndIncrease();
-        auto casted = datanavigator::IntNavigator::DynamicCastAndCheck(nav);
+        auto lastToken = stack.top();
+        auto current_nav = lastToken->getNextElement();
+        lastToken->increaseCounter();
+
+        auto casted = datanavigator::IntNavigator::DynamicCastAndCheck(current_nav);
         t = casted->getValue();
     }
 
-    void NavigatorReader::readPrimitive(long& t)
+    void NavigatorReader::readEndPrimitive(long& t)
     {
-        auto nav = getNextAndIncrease();
-        auto casted = datanavigator::LongNavigator::DynamicCastAndCheck(nav);
+        auto lastToken = stack.top();
+        auto current_nav = lastToken->getNextElement();
+        lastToken->increaseCounter();
+
+        auto casted = datanavigator::LongNavigator::DynamicCastAndCheck(current_nav);
         t = casted->getValue();
     }
 
-    void NavigatorReader::readPrimitive(float& t)
+    void NavigatorReader::readEndPrimitive(float& t)
     {
-        auto nav = getNextAndIncrease();
-        auto casted = datanavigator::FloatNavigator::DynamicCastAndCheck(nav);
+        auto lastToken = stack.top();
+        auto current_nav = lastToken->getNextElement();
+        lastToken->increaseCounter();
+
+        auto casted = datanavigator::FloatNavigator::DynamicCastAndCheck(current_nav);
         t = casted->getValue();
     }
 
-    void NavigatorReader::readPrimitive(double& t)
+    void NavigatorReader::readEndPrimitive(double& t)
     {
-        auto nav = getNextAndIncrease();
-        auto casted = datanavigator::DoubleNavigator::DynamicCastAndCheck(nav);
+        auto lastToken = stack.top();
+        auto current_nav = lastToken->getNextElement();
+        lastToken->increaseCounter();
+
+        auto casted = datanavigator::DoubleNavigator::DynamicCastAndCheck(current_nav);
         t = casted->getValue();
     }
 
-    void NavigatorReader::readPrimitive(std::string& t)
+    void NavigatorReader::readEndPrimitive(std::string& t)
     {
-        auto nav = getNextAndIncrease();
-        auto casted = datanavigator::StringNavigator::DynamicCastAndCheck(nav);
+        auto lastToken = stack.top();
+        auto current_nav = lastToken->getNextElement();
+        lastToken->increaseCounter();
+
+        auto casted = datanavigator::StringNavigator::DynamicCastAndCheck(current_nav);
         t = casted->getValue();
     }
 
-    void NavigatorReader::readPrimitive(bool& t)
+    void NavigatorReader::readEndPrimitive(bool& t)
     {
-        auto nav = getNextAndIncrease();
-        auto casted = datanavigator::BoolNavigator::DynamicCastAndCheck(nav);
+        auto lastToken = stack.top();
+        auto current_nav = lastToken->getNextElement();
+        lastToken->increaseCounter();
+
+        auto casted = datanavigator::BoolNavigator::DynamicCastAndCheck(current_nav);
         t = casted->getValue();
     }
 
     std::string NavigatorReader::readKey()
     {
         auto token = stack.top();
-        return token->getCurrentKey();
+        auto s = token->getCurrentKey();
+        std::cout << "Read key: " << s << std::endl;
+        return s;
     }
 
-    bool NavigatorReader::loadMember(const std::string& k)
+    void NavigatorReader::loadMember(const std::string& k)
     {
+        std::cout << "Loading member " << k << std::endl;
         auto token = stack.top();
-        if (token->hasKey(k))
-        {
-            if (token->hasNextElement())
-            {
-                token->setCurrentKey(k);
-                return true;
-            }
-        }
-        return false;
+        token->setCurrentKey(k);
     }
 
     // Helper functions
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
index d5365a888..547f6be8f 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReader.h
@@ -45,36 +45,34 @@ namespace armarx::aron::dataIO::reader
 
         // constructors
         NavigatorReader() = delete;
-        NavigatorReader(const datanavigator::NavigatorPtr& n);
-        NavigatorReader(const data::AronDataPtr& n);
+        NavigatorReader(const datanavigator::DictNavigatorPtr& n);
+        NavigatorReader(const data::AronDictPtr& n);
 
-        virtual int readStartDict() override;
+        virtual ReadStartDictReturnType readStartDict() override;
         virtual bool readEndDict() override;
-        virtual int readStartList() override;
+
+        virtual ReadStartListReturnType readStartList() override;
         virtual bool readEndList() override;
 
-        virtual std::tuple<std::vector<int>, std::string> readStartNDArray() override;
-        virtual bool readEndNDArray(unsigned char*) override;
+        virtual ReadStartNDArrayReturnType readStartNDArray() override;
+        virtual void readEndNDArray(unsigned char*) override;
 
-        virtual void readPrimitive(int&) override;
-        virtual void readPrimitive(long&) override;
-        virtual void readPrimitive(float&) override;
-        virtual void readPrimitive(double&) override;
-        virtual void readPrimitive(std::string&) override;
-        virtual void readPrimitive(bool&) override;
+        virtual ReadPrimitiveReturnType readStartPrimitive() override;
+        virtual void readEndPrimitive(int&) override;
+        virtual void readEndPrimitive(long&) override;
+        virtual void readEndPrimitive(float&) override;
+        virtual void readEndPrimitive(double&) override;
+        virtual void readEndPrimitive(std::string&) override;
+        virtual void readEndPrimitive(bool&) override;
 
         virtual std::string readKey() override;
-        virtual bool loadMember(const std::string&) override;
+        virtual void loadMember(const std::string&) override;
 
         virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const override;
 
     private:
-        datanavigator::NavigatorPtr getNext();
-        datanavigator::NavigatorPtr getNextAndIncrease();
-
-    private:
+        bool readFirstStartDict = false;
         datanavigator::NavigatorPtr input;
-        bool readInitialStart = false;
         std::stack<NavigatorReaderTokenPtr> stack;
     };
 }
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
index 7f4bf59d7..a11b61613 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/navigator/NavigatorReaderToken.h
@@ -77,6 +77,11 @@ namespace armarx::aron::dataIO::reader
 
         datanavigator::NavigatorPtr getNextElement() const override
         {
+            if (!hasNextElement())
+            {
+                return nullptr;
+            }
+
             switch (descriptor)
             {
                 case data::Descriptor::eDict:
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
index 43a308298..95d3ecad6 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -34,45 +34,43 @@
 
 namespace armarx::aron::dataIO::reader
 {
-    NlohmannJSONReader::NlohmannJSONReader(const nlohmann::json& n) :
-        input(n)
+    NlohmannJSONReader::NlohmannJSONReader(const std::string& n) :
+        NlohmannJSONReader(nlohmann::json::parse(n))
     {
     }
 
-    NlohmannJSONReader::NlohmannJSONReader(const std::string& n) :
-        input(nlohmann::json::parse(n))
+    NlohmannJSONReader::NlohmannJSONReader(const nlohmann::json& n) :
+        input(n)
     {
+        auto newToken = std::make_shared<NlohmannJSONReaderToken>(data::Descriptor::eDict, n);
+        stack.push(newToken);
     }
 
-    nlohmann::json NlohmannJSONReader::getNextAndIncrease()
+    ReaderInterface::ReadStartDictReturnType NlohmannJSONReader::readStartDict()
     {
-        if (!readInitialStart)
+        if (!readFirstStartDict)
         {
-            readInitialStart = true;
-            return input;
+            // we already added in constructor
+            readFirstStartDict = true;
+            auto lastToken = stack.top();
+            auto current_json = lastToken->getElement();
+            int c = current_json.size();
+            return {c, true};
         }
-        NlohmannJSONReaderTokenPtr lastToken = stack.top();
-        return lastToken->getNextElementAndIncreaseCnt();
-    }
 
-    nlohmann::json NlohmannJSONReader::getNext()
-    {
-        if (!readInitialStart)
+        auto lastToken = stack.top();
+
+        auto current_json = lastToken->getNextElement();
+        if (current_json.is_null())
         {
-            readInitialStart = true;
-            return input;
+            lastToken->increaseCounter();
+            return {0, false};
         }
-        NlohmannJSONReaderTokenPtr lastToken = stack.top();
-        return lastToken->getNextElement();
-    }
 
-    int NlohmannJSONReader::readStartDict()
-    {
-        nlohmann::json current_json = getNextAndIncrease();
         int c = current_json.size();
         auto newToken = std::make_shared<NlohmannJSONReaderToken>(data::Descriptor::eDict, current_json);
         stack.push(newToken);
-        return c;
+        return {c, true};
     }
 
     bool NlohmannJSONReader::readEndDict()
@@ -83,18 +81,33 @@ namespace armarx::aron::dataIO::reader
         if (token->finishedElement())
         {
             stack.pop();
+
+            if (!stack.empty())
+            {
+                auto lastToken = stack.top();
+                lastToken->increaseCounter();
+            }
+
             return true;
         }
         return false;
     }
 
-    int NlohmannJSONReader::readStartList()
+    ReaderInterface::ReadStartListReturnType NlohmannJSONReader::readStartList()
     {
-        nlohmann::json current_json = getNextAndIncrease();
+        auto lastToken = stack.top();
+
+        auto current_json = lastToken->getNextElement();
+        if (current_json.is_null())
+        {
+            lastToken->increaseCounter();
+            return {0, false};
+        }
+
         int c = current_json.size();
         auto newToken = std::make_shared<NlohmannJSONReaderToken>(data::Descriptor::eList, current_json);
         stack.push(newToken);
-        return c;
+        return {c, true};
     }
 
     bool NlohmannJSONReader::readEndList()
@@ -105,67 +118,105 @@ namespace armarx::aron::dataIO::reader
         if (token->finishedElement())
         {
             stack.pop();
+
+            auto lastToken = stack.top();
+            lastToken->increaseCounter();
+
             return true;
         }
         return false;
     }
 
     // Complex Data
-    std::tuple<std::vector<int>, std::string> NlohmannJSONReader::readStartNDArray()
+    ReaderInterface::ReadStartNDArrayReturnType NlohmannJSONReader::readStartNDArray()
     {
-        nlohmann::json j = getNext();
-        std::vector<int> dims = j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
-        std::string type = j[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
-        return {dims, type};
+        auto lastToken = stack.top();
+
+        auto current_json = lastToken->getNextElement();
+        if (current_json.is_null())
+        {
+            lastToken->increaseCounter();
+            return {{}, "", false};
+        }
+        std::vector<int> dims = current_json[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
+        std::string type = current_json[io::Data::READER_WRITER_NDARRAY_TYPE_SLUG];
+        return {dims, type, true};
     }
 
-    bool NlohmannJSONReader::readEndNDArray(unsigned char* data)
+    void NlohmannJSONReader::readEndNDArray(unsigned char* data)
     {
         if (data == NULL)
         {
             throw error::AronException("NlohmannJSONReader", "readEndNDArray", "The corresponding data pointer of an complex aron ptr is NULL.");
         }
-        nlohmann::json j = getNextAndIncrease();
-        std::vector<int> dims = j[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
-        std::vector<unsigned char> d = j[io::Data::READER_WRITER_NDARRAY_DATA_SLUG];
+
+        auto lastToken = stack.top();
+
+        auto current_json = lastToken->getNextElement();
+        std::vector<int> dims = current_json[io::Data::READER_WRITER_NDARRAY_DIMENSIONS_SLUG];
+        std::vector<unsigned char> d = current_json[io::Data::READER_WRITER_NDARRAY_DATA_SLUG];
         memcpy(data, d.data(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
-        return true;
     }
 
     // Read primitives
-    void NlohmannJSONReader::readPrimitive(int& t)
+    ReaderInterface::ReadPrimitiveReturnType NlohmannJSONReader::readStartPrimitive()
     {
-        nlohmann::json j = getNextAndIncrease();
+        auto lastToken = stack.top();
+
+        auto current_json = lastToken->getNextElement();
+        if (current_json.is_null())
+        {
+            lastToken->increaseCounter();
+            return {false};
+        }
+        return {true};
+    }
+
+    void NlohmannJSONReader::readEndPrimitive(int& t)
+    {
+        auto lastToken = stack.top();
+        auto j = lastToken->getNextElement();
+        lastToken->increaseCounter();
         t = j;
     }
 
-    void NlohmannJSONReader::readPrimitive(long& t)
+    void NlohmannJSONReader::readEndPrimitive(long& t)
     {
-        nlohmann::json j = getNextAndIncrease();
+        auto lastToken = stack.top();
+        auto j = lastToken->getNextElement();
+        lastToken->increaseCounter();
         t = j;
     }
 
-    void NlohmannJSONReader::readPrimitive(float& t)
+    void NlohmannJSONReader::readEndPrimitive(float& t)
     {
-        nlohmann::json j = getNextAndIncrease();
+        auto lastToken = stack.top();
+        auto j = lastToken->getNextElement();
+        lastToken->increaseCounter();
         t = j;
     }
 
-    void NlohmannJSONReader::readPrimitive(double& t)
+    void NlohmannJSONReader::readEndPrimitive(double& t)
     {
-        nlohmann::json j = getNextAndIncrease();
+        auto lastToken = stack.top();
+        auto j = lastToken->getNextElement();
+        lastToken->increaseCounter();
         t = j;
     }
 
-    void NlohmannJSONReader::readPrimitive(std::string& t)
+    void NlohmannJSONReader::readEndPrimitive(std::string& t)
     {
-        nlohmann::json j = getNextAndIncrease();
+        auto lastToken = stack.top();
+        auto j = lastToken->getNextElement();
+        lastToken->increaseCounter();
         t = j;
     }
 
-    void NlohmannJSONReader::readPrimitive(bool& t)
+    void NlohmannJSONReader::readEndPrimitive(bool& t)
     {
-        nlohmann::json j = getNextAndIncrease();
+        auto lastToken = stack.top();
+        auto j = lastToken->getNextElement();
+        lastToken->increaseCounter();
         t = j;
     }
 
@@ -175,18 +226,10 @@ namespace armarx::aron::dataIO::reader
         return token->getCurrentKey();
     }
 
-    bool NlohmannJSONReader::loadMember(const std::string& k)
+    void NlohmannJSONReader::loadMember(const std::string& k)
     {
         auto token = stack.top();
-        if (token->hasKey(k))
-        {
-            if (token->hasNextElement())
-            {
-                token->setCurrentKey(k);
-                return true;
-            }
-        }
-        return false;
+        token->setCurrentKey(k);
     }
 
     // Helper functions
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
index 03202f7be..0285398f2 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReader.h
@@ -46,33 +46,31 @@ namespace armarx::aron::dataIO::reader
         NlohmannJSONReader(const nlohmann::json& n);
         NlohmannJSONReader(const std::string& n);
 
-        virtual int readStartDict() override;
+        virtual ReadStartDictReturnType readStartDict() override;
         virtual bool readEndDict() override;
-        virtual int readStartList() override;
+
+        virtual ReadStartListReturnType readStartList() override;
         virtual bool readEndList() override;
 
-        virtual std::tuple<std::vector<int>, std::string> readStartNDArray() override;
-        virtual bool readEndNDArray(unsigned char*) override;
+        virtual ReadStartNDArrayReturnType readStartNDArray() override;
+        virtual void readEndNDArray(unsigned char*) override;
 
-        virtual void readPrimitive(int&) override;
-        virtual void readPrimitive(long&) override;
-        virtual void readPrimitive(float&) override;
-        virtual void readPrimitive(double&) override;
-        virtual void readPrimitive(std::string&) override;
-        virtual void readPrimitive(bool&) override;
+        virtual ReadPrimitiveReturnType readStartPrimitive() override;
+        virtual void readEndPrimitive(int&) override;
+        virtual void readEndPrimitive(long&) override;
+        virtual void readEndPrimitive(float&) override;
+        virtual void readEndPrimitive(double&) override;
+        virtual void readEndPrimitive(std::string&) override;
+        virtual void readEndPrimitive(bool&) override;
 
         virtual std::string readKey() override;
-        virtual bool loadMember(const std::string&) override;
+        virtual void loadMember(const std::string&) override;
 
         virtual data::Descriptor getTypeOfNext(const type::Descriptor hint = type::Descriptor::eUnknown) const override;
 
     private:
-        nlohmann::json getNext();
-        nlohmann::json getNextAndIncrease();
-
-    private:
+        bool readFirstStartDict = false;
         nlohmann::json input;
-        bool readInitialStart = false;
         std::stack<NlohmannJSONReaderTokenPtr> stack;
     };
 }
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
index 29dccb820..7711670dd 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/reader/nlohmannJSON/NlohmannJSONReaderToken.h
@@ -120,6 +120,11 @@ namespace armarx::aron::dataIO::reader
 
         nlohmann::json getNextElement() const override
         {
+            if (!hasNextElement())
+            {
+                return nlohmann::json();
+            }
+
             switch (descriptor)
             {
                 case data::Descriptor::eDict:
diff --git a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp b/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
index b5be280a8..d93a54ff7 100644
--- a/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/io/dataIO/visitor/Visitor.cpp
@@ -35,11 +35,23 @@ namespace armarx::aron::dataIO
 
     void Visitor::VisitAndSetup(WriterInterface& writer, const datanavigator::NavigatorPtr& aron)
     {
+        if (!aron)
+        {
+            writer.writeNull();
+            return;
+        }
+
         VisitAndSetup(writer, aron->getResult());
     }
 
     void Visitor::VisitAndSetup(WriterInterface& writer, const data::AronDataPtr& aron)
     {
+        if (!aron)
+        {
+            writer.writeNull();
+            return;
+        }
+
         data::Descriptor desc = Resolver::GetDescriptor(aron);
         switch (desc)
         {
diff --git a/source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h b/source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h
index 4463de455..f66f163ee 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/NavigatorFactory.h
@@ -47,6 +47,5 @@ namespace armarx::aron
 
         virtual Output create(const Input&, const Path&) const = 0;
         virtual Output createSpecific(const Input&, const Path&) const = 0;
-        virtual Output createNull(const EnumT) const = 0;
     };
 }
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
index 9215638ff..6a4a62854 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.cpp
@@ -43,67 +43,18 @@ namespace armarx::aron::datanavigator
         };
 #undef RUN_ARON_MACRO
 
-        auto typeDescriptor = Resolver::GetDescriptor(aron);
-        auto factory_iterator = Factories.find(typeDescriptor);
-        if (factory_iterator == Factories.end())
+        if (!aron)
         {
-            throw error::AronException("NavigatorFactory", "create", "Cannot find the desired factory for input: " + data::DESCRIPTOR_TO_STRING(typeDescriptor) + ". Cannot create navigator", path);
+            return nullptr;
         }
-        return factory_iterator->second->createSpecific(aron, path);
-    }
 
-    NavigatorPtr NavigatorFactory::createNull(const data::Descriptor desc) const
-    {
-        switch (desc)
+        auto dataDescriptor = Resolver::GetDescriptor(aron);
+        auto factory_iterator = Factories.find(dataDescriptor);
+        if (factory_iterator == Factories.end())
         {
-            case data::Descriptor::eDict:
-            {
-                datanavigator::DictNavigatorPtr aron = nullptr;
-                return aron;
-            }
-            case data::Descriptor::eList:
-            {
-                datanavigator::ListNavigatorPtr aron = nullptr;
-                return aron;
-            }
-            case data::Descriptor::eNDArray:
-            {
-                datanavigator::NDArrayNavigatorPtr aron = nullptr;
-                return aron;
-            }
-            case data::Descriptor::eInt:
-            {
-                datanavigator::IntNavigatorPtr aron = nullptr;
-                return aron;
-            }
-            case data::Descriptor::eLong:
-            {
-                datanavigator::LongNavigatorPtr aron = nullptr;
-                return aron;
-            }
-            case data::Descriptor::eFloat:
-            {
-                datanavigator::FloatNavigatorPtr aron = nullptr;
-                return aron;
-            }
-            case data::Descriptor::eDouble:
-            {
-                datanavigator::DoubleNavigatorPtr aron = nullptr;
-                return aron;
-            }
-            case data::Descriptor::eString:
-            {
-                datanavigator::StringNavigatorPtr aron = nullptr;
-                return aron;
-            }
-            case data::Descriptor::eBool:
-            {
-                datanavigator::BoolNavigatorPtr aron = nullptr;
-                return aron;
-            }
-            default:
-                throw error::DescriptorNotValidException("NavigatorFactory", "createNull", "Received invalid descriptor to create NULL.", desc);
+            throw error::AronException("NavigatorFactory", "create", "Cannot find the desired factory for input: " + data::DESCRIPTOR_TO_STRING(dataDescriptor) + ". Cannot create navigator", path);
         }
+        return factory_iterator->second->createSpecific(aron, path);
     }
 
     NavigatorPtr NavigatorFactory::createSpecific(const data::AronDataPtr&, const Path& p) const
diff --git a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
index 924db3652..5e92b4092 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/data/NavigatorFactory.h
@@ -46,7 +46,6 @@ namespace armarx::aron::datanavigator
         NavigatorFactory() = default;
         virtual NavigatorPtr create(const data::AronDataPtr&, const Path&) const override;
         virtual NavigatorPtr createSpecific(const data::AronDataPtr&, const Path&) const override;
-        virtual NavigatorPtr createNull(const data::Descriptor) const override;
     };
 
     // Factories
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp
index b1dd72b21..f0375363b 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.cpp
@@ -51,11 +51,6 @@ namespace armarx::aron::typenavigator
         return factory_iterator->second->createSpecific(aron, path);
     }
 
-    typenavigator::NavigatorPtr NavigatorFactory::createNull(const type::Descriptor) const
-    {
-        return nullptr;
-    }
-
     NavigatorPtr NavigatorFactory::createSpecific(const type::AronTypePtr&, const Path& p) const
     {
         throw error::AronException("NavigatorFactory", "createSpecific", "Called disallowed method of an NavigatorFactory. Use child class instead!", p);
diff --git a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h
index 88783e884..e470407db 100644
--- a/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h
+++ b/source/RobotAPI/libraries/aron/core/navigator/type/NavigatorFactory.h
@@ -46,7 +46,6 @@ namespace armarx::aron::typenavigator
         NavigatorFactory() = default;
         virtual typenavigator::NavigatorPtr create(const type::AronTypePtr&, const Path&) const override;
         virtual typenavigator::NavigatorPtr createSpecific(const type::AronTypePtr&, const Path&) const override;
-        virtual typenavigator::NavigatorPtr createNull(const type::Descriptor) const override;
     };
 
     // Factories
diff --git a/source/RobotAPI/libraries/aron/core/test/aronTest.cpp b/source/RobotAPI/libraries/aron/core/test/aronTest.cpp
index f9e33b547..a2d07c93f 100644
--- a/source/RobotAPI/libraries/aron/core/test/aronTest.cpp
+++ b/source/RobotAPI/libraries/aron/core/test/aronTest.cpp
@@ -95,23 +95,24 @@ void runTestWithInstances(T& k1, T& k2)
     std::cout << "\t getting type 1" << std::endl;
     typenavigator::ObjectNavigatorPtr k1_type_nav = k1.toAronType();
     type::AronObjectPtr k1_type = k1_type_nav->toAronObjectPtr();
-    BOOST_CHECK_NE(k1_type.get(), nullptr);
+    BOOST_CHECK_EQUAL((bool) k1_type, true);
 
     std::cout << "\t getting type 2" << std::endl;
     typenavigator::ObjectNavigatorPtr k2_type_nav = k2.toAronType();
     type::AronObjectPtr k2_type = k2_type_nav->toAronObjectPtr();
-    BOOST_CHECK_NE(k2_type.get(), nullptr);
+    BOOST_CHECK_EQUAL((bool) k2_type, true);
 
-    std::cout << "\t getting aron 1" << std::endl;
     datanavigator::DictNavigatorPtr k1_aron_nav = k1.toAron();
-    data::AronDictPtr k1_aron = k1_aron_nav->toAronDictPtr();
-    BOOST_CHECK_NE(k1_aron.get(), nullptr);
 
     std::cout << "\t initialize aron 1 randomly" << std::endl;
     r.initializeRandomly(k1_aron_nav, k1_type_nav);
 
-    //std::cout << "K Aron:" << std::endl;
-    //std::cout << AronDebug::AronDataPtrToString(k_aron) << std::endl;
+    std::cout << "\t getting aron 1" << std::endl;
+    data::AronDictPtr k1_aron = k1_aron_nav->toAronDictPtr();
+    BOOST_CHECK_EQUAL((bool) k1_aron, true);
+
+    std::cout << "K1 Aron:" << std::endl;
+    std::cout << armarx::aron::Debug::AronDataPtrToString(k1_aron) << std::endl;
 
     std::cout << "\t setting aron 2 from aron 1" << std::endl;
     k2.fromAron(k1_aron);
@@ -119,7 +120,7 @@ void runTestWithInstances(T& k1, T& k2)
     std::cout << "\t getting aron 2" << std::endl;
     datanavigator::DictNavigatorPtr k2_aron_nav = k2.toAron();
     data::AronDictPtr k2_aron = k2_aron_nav->toAronDictPtr();
-    BOOST_CHECK_NE(k2_aron.get(), nullptr);
+    BOOST_CHECK_EQUAL((bool) k2_aron, true);
 
     //std::cout << "K2 Aron:" << std::endl;
     //std::cout << AronDebug::AronDataPtrToString(k2_aron) << std::endl;
@@ -128,6 +129,12 @@ void runTestWithInstances(T& k1, T& k2)
 
     k1.fromAron(k2_aron_nav);
 
+    //std::cout << "K1 Aron:" << std::endl;
+    //std::cout << armarx::aron::Debug::AronDataPtrToString(k1_aron) << std::endl;
+
+    //std::cout << "K2 Aron:" << std::endl;
+    //std::cout << armarx::aron::Debug::AronDataPtrToString(k2_aron) << std::endl;
+
     BOOST_CHECK_EQUAL((k1 == k2), true);
 
 
@@ -145,7 +152,6 @@ void runTestWithInstances(T& k1, T& k2)
 
     k1.fromAron(k1_aron);
 
-
     std::cout << "\t check JSON export of k and k2 for equality" << std::endl;
     armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_k1;
     armarx::aron::dataIO::writer::NlohmannJSONWriter json_writer_for_k2;
@@ -260,20 +266,16 @@ BOOST_AUTO_TEST_CASE(AronObjectTest)
 BOOST_AUTO_TEST_CASE(AronImageTest)
 {
     std::cout << "Running Image test" << std::endl;
-    IVTCByteImageTest i;
-    IVTCByteImageTest i2;
-    runTestWithInstances<IVTCByteImageTest>(i, i2);
-}
-
-/*BOOST_AUTO_TEST_CASE(AronImageChangeSizeTest)
-{
-    std::cout << "Running Image test with changed sizes" << std::endl;
     IVTCByteImageTest ii;
     IVTCByteImageTest ii2;
-    ii.the_ivt_image.Set(100, 100, ii.the_ivt_image.type);
-    ii2.the_ivt_image.Set(10, 10, ii.the_ivt_image.type);
+    ii.the_grayscale_image->Set(100, 100, CByteImage::eGrayScale);
+    ii2.the_grayscale_image->Set(10, 10, CByteImage::eGrayScale);
+
+    ii.the_rgb_image->Set(50, 50, CByteImage::eRGB24);
+    ii2.the_rgb_image->Set(10, 10, CByteImage::eRGB24);
+
     runTestWithInstances<IVTCByteImageTest>(ii, ii2);
-}*/
+}
 
 BOOST_AUTO_TEST_CASE(AronEigenMatrixTest)
 {
@@ -288,6 +290,12 @@ BOOST_AUTO_TEST_CASE(AronOpenCVTest)
     std::cout << "Running OpenCVMat test" << std::endl;
     OpenCVMatTest ocv;
     OpenCVMatTest ocv2;
+
+    ocv.the_2d_opencv_matrix = cv::Mat(2, 2, CV_64FC4);
+    ocv.the_3d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2}), CV_64FC4);
+    ocv.the_4d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2}), CV_64FC4);
+    ocv.the_5d_opencv_matrix = cv::Mat(std::vector<int>({2, 2, 2, 2, 2}), CV_64FC4);
+
     runTestWithInstances<OpenCVMatTest>(ocv, ocv2);
 }
 
@@ -296,30 +304,16 @@ BOOST_AUTO_TEST_CASE(AronPCLPointCloudTest)
     std::cout << "Running PCLPointCloud test" << std::endl;
     PointCloudTest pc;
     PointCloudTest pc2;
+
+    pc.the_xyzrgb_pcl_pointcloud = pcl::PointCloud<pcl::PointXYZRGB>(20, 20);
+    pc.the_xyzrgba_pcl_pointcloud = pcl::PointCloud<pcl::PointXYZRGBA>(20, 20);
+
+    pc2.the_xyzrgb_pcl_pointcloud = pcl::PointCloud<pcl::PointXYZRGB>(50, 50);
+    pc2.the_xyzrgba_pcl_pointcloud = pcl::PointCloud<pcl::PointXYZRGBA>(50, 50);
+
     runTestWithInstances<PointCloudTest>(pc, pc2);
 }
 
-/*
-BOOST_AUTO_TEST_CASE(AronTests)
-{
-    initialize_random();
-
-    std::cout << "Running (Raphael) Trivial test" << std::endl;
-    AronTrivialDataWithoutCodeGeneration rt1;
-    AronTrivialDataWithoutCodeGeneration rt2;
-    runTestWithInstances<AronTrivialDataWithoutCodeGeneration>(rt1, rt2);
-
-    std::cout << "Running (Raphael) Simple test" << std::endl;
-    AronSimpleDataWithoutCodeGeneration rs1;
-    AronSimpleDataWithoutCodeGeneration rs2;
-    runTestWithInstances<AronSimpleDataWithoutCodeGeneration>(rs1, rs2);
-
-    std::cout << "Running (Raphael) Nested test" << std::endl;
-    AronNestedDataWithoutCodeGeneration rn1;
-    AronNestedDataWithoutCodeGeneration rn2;
-    runTestWithInstances<AronNestedDataWithoutCodeGeneration>(rn1, rn2);
-}*/
-
 BOOST_AUTO_TEST_CASE(AronEigenQuaternionTest)
 {
     std::cout << "Running EigenQuaternion test" << std::endl;
diff --git a/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml b/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
index 9c7462aa0..48cc19e61 100644
--- a/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
+++ b/source/RobotAPI/libraries/aron/core/test/xmls/OptionalTest.xml
@@ -51,6 +51,16 @@
                 <armarx::OptionalTestElement optional="ja"/>
             </ObjectChild>
 
+            <ObjectChild key='some_list_with_optional_list'>
+                <List optional="ja">
+                    <List optional="ja">
+                        <Float optional="ja" />
+                    </List>
+                </List>
+            </ObjectChild>
+
+
+
         </Object>
     </GenerateTypes>
 </AronTypeDefinition>
-- 
GitLab