diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
index d71bb045b32028df15bb37150892175e1ec8ec72..cd810a93503d39f01ceeed7ccee297e7a3d08824 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
@@ -1,68 +1,78 @@
#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
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
index bf6c482a06bfd4be24fca5ad78a320b5b6834042..2f77e88a8c1583308cb448a2189fe2341fd6ee46 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
@@ -9,17 +9,26 @@
#include "ConverterInterface.h"
-
namespace armarx::armem::server::robot_state::exteroception
{
struct ConverterValue;
+
// class ConverterTools;
+ namespace detail
+ {
+ struct Properties
+ {
+ std::size_t tofSize = 16; // 4x4 grid
+ };
+ } // namespace detail
class ArmarDEConverter : public ConverterInterface
{
public:
- ArmarDEConverter();
+ using Properties = detail::Properties;
+
+ ArmarDEConverter(const Properties& properties = Properties());
~ArmarDEConverter() override;
@@ -35,8 +44,6 @@ namespace armarx::armem::server::robot_state::exteroception
private:
-
-
void processToFEntry(std::map<std::string, armarx::armem::exteroception::arondto::ToF>& fts,
const std::vector<std::string>& split,
const ConverterValue& value);
@@ -47,5 +54,7 @@ namespace armarx::armem::server::robot_state::exteroception
// std::unique_ptr<ConverterTools> tools;
+
+ Properties properties_;
};
} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
index ef7296b669504e0f8d0aff8b4cd3991fe6ec10b3..235c5efd988e40d89c5d10b051c773abd41eaf5b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
@@ -11,7 +11,7 @@ namespace armarx::armem::server::robot_state::exteroception
ConverterRegistry::ConverterRegistry()
{
add<ArmarDEConverter>("ArmarDE");
- // add<Armar7Converter>("Armar7");
+ add<ArmarDEConverter>("Armar7");
}