Feature/kitchen open articulated objects

source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp

 #include "ArmarDEConverter.h"
+
 #include <cmath>
 #include <string>
 
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
 #include <SimoxUtility/algorithm/advanced.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 
 #include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
 #include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
 #include "ConverterTools.h"
 
-
 namespace armarx::armem::server::robot_state::exteroception
 {
 
-    ArmarDEConverter::ArmarDEConverter() = default;
-    ArmarDEConverter::~ArmarDEConverter() = default;
+    ArmarDEConverter::ArmarDEConverter(const Properties& properties) : properties_(properties)
+    {
+    }
 
+    ArmarDEConverter::~ArmarDEConverter() = default;
 
     aron::data::DictPtr
-    ArmarDEConverter::convert(
-        const RobotUnitDataStreaming::TimeStep& data,
-        const RobotUnitDataStreaming::DataStreamingDescription& description)
+    ArmarDEConverter::convert(const RobotUnitDataStreaming::TimeStep& data,
+                              const RobotUnitDataStreaming::DataStreamingDescription& description)
     {
         arondto::Exteroception dto;
         dto.iterationID = data.iterationId;
 
+        // initialize fields
+        const std::size_t singleDimSize = static_cast<std::size_t>(std::sqrt(properties_.tofSize));
+        ARMARX_CHECK_EQUAL(singleDimSize * singleDimSize, properties_.tofSize);
+
+        dto.tof["Left"].array.resize(singleDimSize, singleDimSize);
+        dto.tof["Left"].array.setConstant(-1); // in order to unset fields faults
+
+        dto.tof["Right"].array.resize(singleDimSize, singleDimSize);
+        dto.tof["Right"].array.setConstant(-1); // in order to unset fields faults
+
         for (const auto& [dataEntryName, dataEntry] : description.entries)
         {
             process(dto, dataEntryName, {data, dataEntry});
         }
 
-        // resize to square 
-        for(auto& [_, tof] : dto.tof)
-        {
-            const int sr = std::sqrt(tof.array.size());
-            const bool isSquare = (sr * sr == tof.array.size());
+        // resize to square
+        // for (auto& [_, tof] : dto.tof)
+        // {
+        //     const int sr = std::sqrt(tof.array.size());
+        //     const bool isSquare = (sr * sr == tof.array.size());
+
+        //     if (tof.array.size() > 0 and isSquare)
+        //     {
+        //         tof.array.resize(sr, sr);
+        //     }
+        // }
 
-            if(tof.array.size() > 0 and isSquare)
-            {
-                tof.array.resize(sr, sr);
-            }
-        }
-        
 
         return dto.toAron();
     }
 
-
-    void ArmarDEConverter::process(
-        arondto::Exteroception& dto,
-        const std::string& entryName,
-        const ConverterValue& value)
+    void
+    ArmarDEConverter::process(arondto::Exteroception& dto,
+                              const std::string& entryName,
+                              const ConverterValue& value)
     {
         const std::vector<std::string> split = simox::alg::split(entryName, ".", false, false);
         ARMARX_CHECK_GREATER_EQUAL(split.size(), 3);
         const std::set<size_t> acceptedSizes{3, 4, 5};
         ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
-                << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
-                << "\n- split: '" << split << "'";
+            << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
+            << "\n- split: '" << split << "'";
 
         const std::string& category = split.at(0);
         // const std::string& name = split.at(1);
@@ -74,19 +84,16 @@ namespace armarx::armem::server::robot_state::exteroception
             // ARMARX_DEBUG << "Processing ToF sensor data";
             processToFEntry(dto.tof, split, value);
         }
-        
     }
 
-
-
-
-    void ArmarDEConverter::processToFEntry(
-            std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
-            const std::vector<std::string>& split,
-            const ConverterValue& value)
+    void
+    ArmarDEConverter::processToFEntry(
+        std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
+        const std::vector<std::string>& split,
+        const ConverterValue& value)
     {
         // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
-        
+
         ARMARX_CHECK_EQUAL(split.size(), 4);
         ARMARX_CHECK_EQUAL(split.at(2), "tofDepth");
 
@@ -95,11 +102,8 @@ namespace armarx::armem::server::robot_state::exteroception
         // element 0 sens
         // element 1 is either LeftHand or RightHand
 
-        const std::map<std::string, std::string> sidePrefixMap
-        {
-            {"LeftHand", "Left"},
-            {"RightHand", "Right"}
-        };
+        const std::map<std::string, std::string> sidePrefixMap{{"LeftHand", "Left"},
+                                                               {"RightHand", "Right"}};
 
         auto it = sidePrefixMap.find(name);
         ARMARX_CHECK(it != sidePrefixMap.end()) << name;
@@ -107,10 +111,11 @@ namespace armarx::armem::server::robot_state::exteroception
         const std::string& side = it->second;
         processToFEntry(tofs[side], split, value);
     }
-    
-    void ArmarDEConverter::processToFEntry(armarx::armem::exteroception::arondto::ToF& tof,
-                const std::vector<std::string>& split,
-                const ConverterValue& value)
+
+    void
+    ArmarDEConverter::processToFEntry(armarx::armem::exteroception::arondto::ToF& tof,
+                                      const std::vector<std::string>& split,
+                                      const ConverterValue& value)
     {
         // split, e.g., element_12
         const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
@@ -118,13 +123,16 @@ namespace armarx::armem::server::robot_state::exteroception
 
         const int idx = std::stoi(elements.at(1));
 
-        if(tof.array.size() < (idx +1))
-        {
-            tof.array.resize(idx+1, 1);
-        }
+        ARMARX_CHECK_LESS(idx, tof.array.size());
+
+        // we don't allow dynamic ToF size atm
+        // if (tof.array.size() < (idx + 1))
+        // {
+        //     tof.array.resize(idx + 1, 1);
+        // }
 
         tof.array(idx) = getValueAs<float>(value);
     }
 
 
-}
+} // namespace armarx::armem::server::robot_state::exteroception