diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
index 5c3db21fbe8604c9842cb834565274a4294e6233..ddd6ee11312de00f2b7451905ffc404ff5fd6322 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
@@ -257,9 +257,15 @@ namespace armarx
int dataSize = 0;
switch (i->getElementType())
{
+ case armarx::aron::type::matrix::INT8:
+ case armarx::aron::type::matrix::UINT8:
+ dataSize = 1;
+ break;
case armarx::aron::type::matrix::INT16:
+ case armarx::aron::type::matrix::UINT16:
dataSize = 2;
break;
+ case armarx::aron::type::matrix::UINT32:
case armarx::aron::type::matrix::INT32:
case armarx::aron::type::matrix::FLOAT32:
dataSize = 4;
@@ -273,6 +279,18 @@ namespace armarx
// UGLY HACK: FIX ME!!!
switch (i->getElementType())
{
+ case armarx::aron::type::matrix::UINT8:
+ createdMatrix->setType("unsigned char");
+ break;
+ case armarx::aron::type::matrix::UINT16:
+ createdMatrix->setType("unsigned short");
+ break;
+ case armarx::aron::type::matrix::UINT32:
+ createdMatrix->setType("unsigned int");
+ break;
+ case armarx::aron::type::matrix::INT8:
+ createdMatrix->setType("char");
+ break;
case armarx::aron::type::matrix::INT16:
createdMatrix->setType("short");
break;
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
index 5bc5a40bbd407c104ce96905850e9d0beabecb48..6f83e52c44b785fa1e277138a3e0d47f9220a89e 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
@@ -222,6 +222,18 @@ namespace armarx
QString type = "";
switch (i->getElementType())
{
+ case armarx::aron::type::matrix::UINT8:
+ type = "<uint8>";
+ break;
+ case armarx::aron::type::matrix::UINT16:
+ type = "<uint16>";
+ break;
+ case armarx::aron::type::matrix::UINT32:
+ type = "<uint32>";
+ break;
+ case armarx::aron::type::matrix::INT8:
+ type = "<int8>";
+ break;
case armarx::aron::type::matrix::INT16:
type = "<int16>";
break;
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
index cafc9b499b45a587efcc7a52ca531e028a8391df..5678fb7cf543371697ec01fdd0245fd92dc32b42 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.cpp
@@ -189,6 +189,30 @@ namespace armarx
float laundered = std::launder(interpreted)[elementNr];
return usString<double>(laundered);
}
+ case aron::type::matrix::ElementType::UINT8:
+ {
+ auto* interpreted = reinterpret_cast<uint8_t*>(rawData);
+ auto laudered = std::launder(interpreted)[elementNr];
+ return usString<uint8_t>(laudered);
+ }
+ case aron::type::matrix::ElementType::UINT16:
+ {
+ auto* interpreted = reinterpret_cast<uint16_t*>(rawData);
+ auto laudered = std::launder(interpreted)[elementNr];
+ return usString<uint16_t>(laudered);
+ }
+ case aron::type::matrix::ElementType::UINT32:
+ {
+ auto* interpreted = reinterpret_cast<uint32_t*>(rawData);
+ auto laudered = std::launder(interpreted)[elementNr];
+ return usString<uint32_t>(laudered);
+ }
+ case aron::type::matrix::ElementType::INT8:
+ {
+ auto* interpreted = reinterpret_cast<int8_t*>(rawData);
+ auto laudered = std::launder(interpreted)[elementNr];
+ return usString<int8_t>(laudered);
+ }
case aron::type::matrix::ElementType::INT16:
{
int16_t* interpreted = reinterpret_cast<int16_t*>(rawData);
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp
index 8e01710f03cf9fabe1f9f50b060bc058016099c8..2caddc1846edfb80005225c963c2bd93f7dfb73c 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp
@@ -206,6 +206,18 @@ namespace armarx
{
switch (elemType)
{
+ case armarx::aron::type::matrix::UINT8:
+ return NDArrayHelper::toByteVector<uint8_t>(str);
+
+ case armarx::aron::type::matrix::UINT16:
+ return NDArrayHelper::toByteVector<uint16_t>(str);
+
+ case armarx::aron::type::matrix::UINT32:
+ return NDArrayHelper::toByteVector<uint32_t>(str);
+
+ case armarx::aron::type::matrix::INT8:
+ return NDArrayHelper::toByteVector<int8_t>(str);
+
case armarx::aron::type::matrix::INT16:
return NDArrayHelper::toByteVector<int16_t>(str);
diff --git a/source/RobotAPI/interface/aron/Aron.ice b/source/RobotAPI/interface/aron/Aron.ice
index aae664fa5f4d79e7a59f94bebc1e74823d06dfc5..f06217c34e149cdaaea8ec1b54240dd14c6eb6b6 100644
--- a/source/RobotAPI/interface/aron/Aron.ice
+++ b/source/RobotAPI/interface/aron/Aron.ice
@@ -111,6 +111,10 @@ module armarx
{
enum ElementType
{
+ UINT8,
+ UINT16,
+ UINT32,
+ INT8,
INT16,
INT32,
INT64,
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index f497c3e2ddcbec47b26585202f3f97d0dedbf957..d94903c36c3df2678eddc4317751a65bc0ce506f 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -42,11 +42,16 @@ namespace armarx::armem::robot
Eigen::Vector3f gravityCompensatedTorque;
};
- using ToFArray = Eigen::MatrixXf;
-
struct ToF
{
- ToFArray array;
+ using DepthMatrixType = Eigen::MatrixXf; // FIXME uint16
+ using DepthSigmaMatrixType = Eigen::MatrixXf; // FIXME uint8
+ using TargetDetectedMatrixType = Eigen::MatrixXf; // FIXME uint8
+
+ DepthMatrixType depth;
+ DepthSigmaMatrixType sigma;
+ TargetDetectedMatrixType targetDetected;
+
std::string frame;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
index 6f0f045ac7f1b10df70776093aa0541e5eca723f..b8f7a936392a401a94a8b4b11571983de0d24d21 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
@@ -9,7 +9,18 @@
<Object name="armarx::armem::exteroception::arondto::ToF">
- <ObjectChild key="array">
+ <ObjectChild key="depth">
+ <!-- <Matrix rows="-1" cols="-1" type="uint16" /> -->
+ <Matrix rows="-1" cols="-1" type="float32" />
+ </ObjectChild>
+
+ <ObjectChild key="sigma">
+ <!-- <Matrix rows="-1" cols="-1" type="uint8" /> -->
+ <Matrix rows="-1" cols="-1" type="float32" />
+ </ObjectChild>
+
+ <ObjectChild key="targetDetected">
+ <!-- <Matrix rows="-1" cols="-1" type="uint8" /> -->
<Matrix rows="-1" cols="-1" type="float32" />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 78014ea76bf070f08f01212719da1c1be7483fa3..714ee296537470b7119d0ed62bb9039bb6a38553 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -85,12 +85,16 @@ namespace armarx::armem
}
- void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToFArray& bo){
- bo = dto.array;
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToF& bo){
+ bo.depth = dto.depth;
+ bo.sigma = dto.sigma;
+ bo.targetDetected = dto.targetDetected;
}
- void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToFArray& bo){
- dto.array = bo;
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToF& bo){
+ dto.depth = bo.depth;
+ dto.sigma = bo.sigma;
+ dto.targetDetected = bo.targetDetected;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index a3e821caac1379d7413160583822b792f8f7c453..3df14cb39abfd1231b8222fff7e3d7b3a15905a3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -67,8 +67,8 @@ namespace armarx::armem
void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
- void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToFArray& bo);
- void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToFArray& bo);
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToF& bo);
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToF& bo);
} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index f3b4731b0ab07e0fb93ce563ae6729163fd2ac05..8a3b67e984d51a40d85e44113bc54a872233d910 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -604,7 +604,7 @@ namespace armarx::armem::robot_state
}
}
- std::optional<std::map<RobotReader::Hand, robot::ToFArray>>
+ std::optional<std::map<RobotReader::Hand, robot::ToF>>
RobotReader::queryToF(const robot::RobotDescription& description,
const armem::Time& timestamp) const
{
@@ -762,10 +762,10 @@ namespace armarx::armem::robot_state
return forceTorques;
}
- std::map<RobotReader::Hand, robot::ToFArray>
+ std::map<RobotReader::Hand, robot::ToF>
RobotReader::getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const
{
- std::map<RobotReader::Hand, robot::ToFArray> tofs;
+ std::map<RobotReader::Hand, robot::ToF> tofs;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -792,7 +792,7 @@ namespace armarx::armem::robot_state
{
ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
- robot::ToFArray tof;
+ robot::ToF tof;
fromAron(dtoFt, tof);
const auto hand = fromHandName(handName);
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index e3ff0308939a3a6e6b7858c7e8b1cdfb198230b8..650227dee1666bb124d4941a13c16088f62d88a5 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -107,7 +107,7 @@ namespace armarx::armem::robot_state
const armem::Time& end) const;
- std::optional<std::map<Hand, robot::ToFArray>>
+ std::optional<std::map<Hand, robot::ToF>>
queryToF(const robot::RobotDescription& description, const armem::Time& timestamp) const;
/**
@@ -156,7 +156,7 @@ namespace armarx::armem::robot_state
std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
- std::map<RobotReader::Hand, robot::ToFArray> getToF(const armarx::armem::wm::Memory& memory,
+ std::map<RobotReader::Hand, robot::ToF> getToF(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
struct Properties
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 cd810a93503d39f01ceeed7ccee297e7a3d08824..0f69c92e67bca58037d8b2fe7709048521092c7f 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
@@ -35,11 +35,31 @@ namespace armarx::armem::server::robot_state::exteroception
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
+ // Left
+ // FIXME change if unsigned
+ dto.tof["Left"].depth.resize(singleDimSize, singleDimSize);
+ dto.tof["Left"].depth.setConstant(-1); // in order to unset fields faults
+
+ // FIXME change if unsigned
+ dto.tof["Left"].sigma.resize(singleDimSize, singleDimSize);
+ dto.tof["Left"].sigma.setConstant(-1); // in order to unset fields faults
+
+ // FIXME change if unsigned
+ dto.tof["Left"].targetDetected.resize(singleDimSize, singleDimSize);
+ dto.tof["Left"].targetDetected.setConstant(-1); // in order to unset fields faults
+
+ // Right
+ // FIXME change if unsigned
+ dto.tof["Right"].depth.resize(singleDimSize, singleDimSize);
+ dto.tof["Right"].depth.setConstant(-1); // in order to unset fields faults
+
+ // FIXME change if unsigned
+ dto.tof["Right"].sigma.resize(singleDimSize, singleDimSize);
+ dto.tof["Right"].sigma.setConstant(-1); // in order to unset fields faults
+
+ // FIXME change if unsigned
+ dto.tof["Right"].targetDetected.resize(singleDimSize, singleDimSize);
+ dto.tof["Right"].targetDetected.setConstant(-1); // in order to unset fields faults
for (const auto& [dataEntryName, dataEntry] : description.entries)
{
@@ -82,12 +102,24 @@ namespace armarx::armem::server::robot_state::exteroception
if (simox::alg::contains(field, "tofDepth"))
{
// ARMARX_DEBUG << "Processing ToF sensor data";
- processToFEntry(dto.tof, split, value);
+ processToFDepthEntry(dto.tof, split, value);
+ }
+
+ if (simox::alg::contains(field, "tofSigma"))
+ {
+ // ARMARX_DEBUG << "Processing ToF sensor data";
+ processToFSigmaEntry(dto.tof, split, value);
+ }
+
+ if (simox::alg::contains(field, "tofTargetDetected"))
+ {
+ // ARMARX_DEBUG << "Processing ToF sensor data";
+ processToFTargetDetectedEntry(dto.tof, split, value);
}
}
void
- ArmarDEConverter::processToFEntry(
+ ArmarDEConverter::processToFDepthEntry(
std::map<std::string, armarx::armem::exteroception::arondto::ToF>& tofs,
const std::vector<std::string>& split,
const ConverterValue& value)
@@ -109,11 +141,11 @@ namespace armarx::armem::server::robot_state::exteroception
ARMARX_CHECK(it != sidePrefixMap.end()) << name;
const std::string& side = it->second;
- processToFEntry(tofs[side], split, value);
+ processToFDepthEntry(tofs[side], split, value);
}
void
- ArmarDEConverter::processToFEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ ArmarDEConverter::processToFDepthEntry(armarx::armem::exteroception::arondto::ToF& tof,
const std::vector<std::string>& split,
const ConverterValue& value)
{
@@ -123,7 +155,103 @@ namespace armarx::armem::server::robot_state::exteroception
const int idx = std::stoi(elements.at(1));
- ARMARX_CHECK_LESS(idx, tof.array.size());
+ ARMARX_CHECK_LESS(idx, tof.depth.size());
+
+ // we don't allow dynamic ToF size atm
+ // if (tof.array.size() < (idx + 1))
+ // {
+ // tof.array.resize(idx + 1, 1);
+ // }
+
+ tof.depth(idx) = getValueAs<float>(value);
+ }
+
+ void
+ ArmarDEConverter::processToFSigmaEntry(
+ 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), "tofSigma");
+
+ const std::string& name = split.at(1);
+
+ // element 0 sens
+ // element 1 is either LeftHand or RightHand
+
+ const std::map<std::string, std::string> sidePrefixMap{{"LeftHand", "Left"},
+ {"RightHand", "Right"}};
+
+ auto it = sidePrefixMap.find(name);
+ ARMARX_CHECK(it != sidePrefixMap.end()) << name;
+
+ const std::string& side = it->second;
+ processToFDepthEntry(tofs[side], split, value);
+ }
+
+ void
+ ArmarDEConverter::processToFSigmaEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ // split, e.g., element_12
+ const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
+ ARMARX_CHECK_EQUAL(elements.size(), 2);
+
+ const int idx = std::stoi(elements.at(1));
+
+ ARMARX_CHECK_LESS(idx, tof.sigma.size());
+
+ // we don't allow dynamic ToF size atm
+ // if (tof.array.size() < (idx + 1))
+ // {
+ // tof.array.resize(idx + 1, 1);
+ // }
+
+ tof.sigma(idx) = getValueAs<float>(value);
+ }
+
+ void
+ ArmarDEConverter::processToFTargetDetectedEntry(
+ 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), "tofTargetDetected");
+
+ const std::string& name = split.at(1);
+
+ // element 0 sens
+ // element 1 is either LeftHand or RightHand
+
+ const std::map<std::string, std::string> sidePrefixMap{{"LeftHand", "Left"},
+ {"RightHand", "Right"}};
+
+ auto it = sidePrefixMap.find(name);
+ ARMARX_CHECK(it != sidePrefixMap.end()) << name;
+
+ const std::string& side = it->second;
+ processToFTargetDetectedEntry(tofs[side], split, value);
+ }
+
+ void
+ ArmarDEConverter::processToFTargetDetectedEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
+ {
+ // split, e.g., element_12
+ const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
+ ARMARX_CHECK_EQUAL(elements.size(), 2);
+
+ const int idx = std::stoi(elements.at(1));
+
+ ARMARX_CHECK_LESS(idx, tof.targetDetected.size());
// we don't allow dynamic ToF size atm
// if (tof.array.size() < (idx + 1))
@@ -131,7 +259,7 @@ namespace armarx::armem::server::robot_state::exteroception
// tof.array.resize(idx + 1, 1);
// }
- tof.array(idx) = getValueAs<float>(value);
+ tof.targetDetected(idx) = getValueAs<float>(value);
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
index 2f77e88a8c1583308cb448a2189fe2341fd6ee46..2d8368c1eb21daa8ba119a4b60cdae439fc27f37 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
@@ -19,7 +19,7 @@ namespace armarx::armem::server::robot_state::exteroception
{
struct Properties
{
- std::size_t tofSize = 16; // 4x4 grid
+ std::size_t tofSize = 64; // 4x4 grid
};
} // namespace detail
@@ -44,14 +44,34 @@ namespace armarx::armem::server::robot_state::exteroception
private:
- void processToFEntry(std::map<std::string, armarx::armem::exteroception::arondto::ToF>& fts,
+ void
+ processToFDepthEntry(std::map<std::string, armarx::armem::exteroception::arondto::ToF>& fts,
const std::vector<std::string>& split,
const ConverterValue& value);
- void processToFEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ void processToFDepthEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ void
+ processToFSigmaEntry(std::map<std::string, armarx::armem::exteroception::arondto::ToF>& fts,
const std::vector<std::string>& split,
const ConverterValue& value);
+ void processToFSigmaEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+
+ void processToFTargetDetectedEntry(
+ std::map<std::string, armarx::armem::exteroception::arondto::ToF>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+ void processToFTargetDetectedEntry(armarx::armem::exteroception::arondto::ToF& tof,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
// std::unique_ptr<ConverterTools> tools;
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
index 404373a0a4b1c64778d597c15304ba8f30c62d49..818ff92d715ea10294b1d0c9c60d01417952112c 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
@@ -32,6 +32,12 @@ namespace armarx::aron::codegenerator::cpp::generator
{
const std::map<type::matrix::ElementType, std::tuple<std::string, int, std::string>>
ElementType2Cpp = {
+ {type::matrix::UINT8,
+ {TypeName<unsigned char>::Get(), sizeof(unsigned char), "::armarx::aron::type::matrix::UINT8"}},
+ {type::matrix::UINT16,
+ {TypeName<unsigned short>::Get(), sizeof(unsigned short), "::armarx::aron::type::matrix::UINT16"}},
+ {type::matrix::INT8,
+ {TypeName<char>::Get(), sizeof(char), "::armarx::aron::type::matrix::INT8"}},
{type::matrix::INT16,
{TypeName<short>::Get(), sizeof(short), "::armarx::aron::type::matrix::INT16"}},
{type::matrix::INT32,
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp
index 4f77481df505023025c4315481b98e78b47fb49e..1719491ccb0a771d3d9b1703a12f72a366af2a2c 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.cpp
@@ -445,6 +445,10 @@ namespace armarx::aron::typereader::xml
ReaderFactory::createMatrix(const RapidXmlReaderNode& node, const Path& path) const
{
static const std::map<std::string, type::matrix::ElementType> StringSlug2MatrixType = {
+ {constantes::UCHAR_SLUG, type::matrix::ElementType::UINT8},
+ {constantes::USHORT_SLUG, type::matrix::ElementType::UINT16},
+ {constantes::UINT_SLUG, type::matrix::ElementType::UINT32},
+ {constantes::CHAR_SLUG, type::matrix::ElementType::INT8},
{constantes::SHORT_SLUG, type::matrix::ElementType::INT16},
{constantes::INT_SLUG, type::matrix::ElementType::INT32},
{constantes::LONG_SLUG, type::matrix::ElementType::INT64},
diff --git a/source/RobotAPI/libraries/aron/codegeneration_util/TypeName.h b/source/RobotAPI/libraries/aron/codegeneration_util/TypeName.h
index 8018b67a20f32b7185c0ed82f5ebe3c2bea15175..00920427d4f4ddfeac4fd5ceda9ef1e0b708684a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration_util/TypeName.h
+++ b/source/RobotAPI/libraries/aron/codegeneration_util/TypeName.h
@@ -17,6 +17,36 @@ namespace armarx::aron
}
};
+ template <>
+ struct TypeName<unsigned char>
+ {
+ static const char*
+ Get()
+ {
+ return "unsigned char";
+ }
+ };
+
+ template <>
+ struct TypeName<unsigned short>
+ {
+ static const char*
+ Get()
+ {
+ return "unsigned short";
+ }
+ };
+
+ template <>
+ struct TypeName<unsigned int>
+ {
+ static const char*
+ Get()
+ {
+ return "unsigned int";
+ }
+ };
+
template <>
struct TypeName<short>
{
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h b/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h
index c9348cf61aec6ec5ddf17db09245af5c1a4330b6..83357fe1c7e0e9d128d7bd499ac3b39a63b8384e 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h
@@ -97,6 +97,10 @@ namespace armarx::aron::data::rw::json
const std::map<type::matrix::ElementType, std::string> MatrixType2String =
{
+ {type::matrix::ElementType::UINT8, "type::matrix::UINT8"},
+ {type::matrix::ElementType::UINT16, "type::matrix::UINT16"},
+ {type::matrix::ElementType::UINT32, "type::matrix::UINT32"},
+ {type::matrix::ElementType::INT8, "type::matrix::INT8"},
{type::matrix::ElementType::INT16, "type::matrix::INT16"},
{type::matrix::ElementType::INT32, "type::matrix::INT32"},
{type::matrix::ElementType::INT64, "type::matrix::INT64"},
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
index 985f914d16d8c501992aeab2b44a0c7a00d9b8e6..2e864ad7a5fc3189d79e769bb5edad4dfd4693de 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
@@ -66,6 +66,10 @@ namespace armarx::aron::data::reader
const std::map<std::string, type::matrix::ElementType> ElementTypeAsString = {
+ {"unsigned char", ::armarx::aron::type::matrix::UINT8},
+ {"unsigned short", ::armarx::aron::type::matrix::UINT16},
+ {"unsigned int", ::armarx::aron::type::matrix::UINT32},
+ {"char", ::armarx::aron::type::matrix::INT8},
{"short", ::armarx::aron::type::matrix::INT16},
{"int", ::armarx::aron::type::matrix::INT32},
{"long", ::armarx::aron::type::matrix::INT64},
@@ -105,6 +109,18 @@ namespace armarx::aron::data::reader
// and then convert it to the vector<uchar>
switch (elementType)
{
+ case type::matrix::UINT8:
+ readTo<std::uint8_t>(input, data);
+ break;
+ case type::matrix::UINT16:
+ readTo<std::uint16_t>(input, data);
+ break;
+ case type::matrix::UINT32:
+ readTo<std::uint32_t>(input, data);
+ break;
+ case type::matrix::INT8:
+ readTo<std::int8_t>(input, data);
+ break;
case type::matrix::INT16:
readTo<std::int16_t>(input, data);
break;
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h b/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h
index 834b0b462f7f6de3e1bb6d1d4a1212773995f4ab..f14aba418e60c0d2758149265b9c610bf65c173b 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h
+++ b/source/RobotAPI/libraries/aron/core/type/rw/json/Data.h
@@ -118,6 +118,10 @@ namespace armarx::aron::type::rw::json
const auto String2NDArrayType = aron::conversion::util::InvertMap(NDArrayType2String);
const std::map<type::matrix::ElementType, std::string> MatrixType2String = {
+ {type::matrix::ElementType::UINT8, "type::matrix::UINT8"},
+ {type::matrix::ElementType::UINT16, "type::matrix::UINT16"},
+ {type::matrix::ElementType::UINT32, "type::matrix::UINT32"},
+ {type::matrix::ElementType::INT8, "type::matrix::INT8"},
{type::matrix::ElementType::INT16, "type::matrix::INT16"},
{type::matrix::ElementType::INT32, "type::matrix::INT32"},
{type::matrix::ElementType::INT64, "type::matrix::INT64"},
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
index e3bcbf57344c50fe915a6177c1f60df0ead2d239..bd188938f1f6c2c6127dd4ca73313202c789144a 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.cpp
@@ -30,6 +30,10 @@
namespace armarx::aron::type
{
const std::map<matrix::ElementType, std::string> Matrix::Elementtype2String{
+ {matrix::ElementType::UINT8, "UINT8"},
+ {matrix::ElementType::UINT16, "UINT16"},
+ {matrix::ElementType::UINT32, "UINT32"},
+ {matrix::ElementType::INT8, "INT8"},
{matrix::ElementType::INT16, "INT16"},
{matrix::ElementType::INT32, "INT32"},
{matrix::ElementType::INT64, "INT64"},
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetConverter.cpp
index 63fc8b0de060b4f79de3a06cb168e8ede1e4efc2..832258ea2c1083742ae9c0f13192e9f6903169ef 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetConverter.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetConverter.cpp
@@ -257,9 +257,15 @@ namespace armarx::skills::gui
int dataSize = 0;
switch (i->getElementType())
{
+ case armarx::aron::type::matrix::UINT8:
+ case armarx::aron::type::matrix::INT8:
+ dataSize = 1;
+ break;
+ case armarx::aron::type::matrix::UINT16:
case armarx::aron::type::matrix::INT16:
dataSize = 2;
break;
+ case armarx::aron::type::matrix::UINT32:
case armarx::aron::type::matrix::INT32:
case armarx::aron::type::matrix::FLOAT32:
dataSize = 4;
@@ -273,6 +279,18 @@ namespace armarx::skills::gui
// UGLY HACK: FIX ME!!!
switch (i->getElementType())
{
+ case armarx::aron::type::matrix::UINT8:
+ createdMatrix->setType("unsigned char");
+ break;
+ case armarx::aron::type::matrix::UINT16:
+ createdMatrix->setType("unsigned short");
+ break;
+ case armarx::aron::type::matrix::UINT32:
+ createdMatrix->setType("unsigned int");
+ break;
+ case armarx::aron::type::matrix::INT8:
+ createdMatrix->setType("char");
+ break;
case armarx::aron::type::matrix::INT16:
createdMatrix->setType("short");
break;
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetCreator.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetCreator.cpp
index 037c8dab99d890443224e1491d25337dd901c085..f36713e02b6d63258a028b35193d92f4aa7bb191 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetCreator.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetCreator.cpp
@@ -222,6 +222,18 @@ namespace armarx::skills::gui
QString type = "";
switch (i->getElementType())
{
+ case armarx::aron::type::matrix::UINT8:
+ type = "<uint8>";
+ break;
+ case armarx::aron::type::matrix::UINT16:
+ type = "<uint16>";
+ break;
+ case armarx::aron::type::matrix::UINT32:
+ type = "<uint32>";
+ break;
+ case armarx::aron::type::matrix::INT8:
+ type = "<int8>";
+ break;
case armarx::aron::type::matrix::INT16:
type = "<int16>";
break;
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetSetter.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetSetter.cpp
index 1ae9dbc9d8aa99ae0e0eabfb489a29dfdf0a8d78..4951555344ee891df2919466a43453cfbfb6d278 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetSetter.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetSetter.cpp
@@ -189,6 +189,30 @@ namespace armarx::skills::gui
float laundered = std::launder(interpreted)[elementNr];
return usString<double>(laundered);
}
+ case aron::type::matrix::ElementType::UINT8:
+ {
+ auto* interpreted = reinterpret_cast<uint8_t*>(rawData);
+ auto laudered = std::launder(interpreted)[elementNr];
+ return usString<>(laudered);
+ }
+ case aron::type::matrix::ElementType::UINT16:
+ {
+ auto* interpreted = reinterpret_cast<uint16_t*>(rawData);
+ auto laudered = std::launder(interpreted)[elementNr];
+ return usString<>(laudered);
+ }
+ case aron::type::matrix::ElementType::UINT32:
+ {
+ auto* interpreted = reinterpret_cast<uint32_t*>(rawData);
+ auto laudered = std::launder(interpreted)[elementNr];
+ return usString<>(laudered);
+ }
+ case aron::type::matrix::ElementType::INT8:
+ {
+ auto* interpreted = reinterpret_cast<int8_t*>(rawData);
+ auto laudered = std::launder(interpreted)[elementNr];
+ return usString<>(laudered);
+ }
case aron::type::matrix::ElementType::INT16:
{
int16_t* interpreted = reinterpret_cast<int16_t*>(rawData);
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.cpp
index c40c0821533ad3ce24d23dcc76611f4a5e1cb3f3..cac9ef75f6c5cb9553a955a0440c801603edb5ee 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.cpp
@@ -206,6 +206,18 @@ namespace armarx::skills::gui
{
switch (elemType)
{
+ case armarx::aron::type::matrix::UINT8:
+ return NDArrayHelper::toByteVector<uint8_t>(str);
+
+ case armarx::aron::type::matrix::UINT16:
+ return NDArrayHelper::toByteVector<uint16_t>(str);
+
+ case armarx::aron::type::matrix::UINT32:
+ return NDArrayHelper::toByteVector<uint32_t>(str);
+
+ case armarx::aron::type::matrix::INT8:
+ return NDArrayHelper::toByteVector<int8_t>(str);
+
case armarx::aron::type::matrix::INT16:
return NDArrayHelper::toByteVector<int16_t>(str);