diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index e00518697c1b1c0dcc8d2d7c767df17d593e1e9e..bc8179ff695156ab4691f9a5feae5885699c2c4a 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -103,15 +103,15 @@ namespace armarx::armem::server::obj
}
catch (const LocalException& e)
{
- ARMARX_ERROR << "Failed to init " << segmentName << " segment. Reason: \n" << e.what();
+ ARMARX_ERROR << "Failed to init `" << segmentName << "` segment. Reason: \n" << e.what();
}
catch (const std::exception& e)
{
- ARMARX_ERROR << "Failed to init " << segmentName << " segment. Reason: \n" << e.what();
+ ARMARX_ERROR << "Failed to init `" << segmentName << "` segment. Reason: \n" << e.what();
}
catch (...)
{
- ARMARX_ERROR << "Failed to init " << segmentName << " segment for unknown reason.";
+ ARMARX_ERROR << "Failed to init `" << segmentName << "` segment for unknown reason.";
}
};
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/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 6d20564f660aee832698ca9d1eda297b56de9f3c..3b97c8040f4687bf58e48b409aaa4c2669f9fe35 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -1,67 +1,84 @@
#include "ObjectInfo.h"
+#include <filesystem>
+#include <optional>
+
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
+#include "ArmarXCore/core/exceptions/LocalException.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-
namespace armarx
{
namespace fs = std::filesystem;
-
- ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
- const path& relObjectsPath, const ObjectID& id) :
- _packageName(packageName), _absPackageDataDir(packageDataDir),
- _relObjectsPath(relObjectsPath), _id(id)
+ ObjectInfo::ObjectInfo(const std::string& packageName,
+ const ObjectInfo::path& packageDataDir,
+ const path& relObjectsPath,
+ const ObjectID& id) :
+ _packageName(packageName),
+ _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath),
+ _id(id)
{
}
- ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
+ ObjectInfo::ObjectInfo(const std::string& packageName,
+ const ObjectInfo::path& packageDataDir,
const path& relObjectsPath,
- const std::string& dataset, const std::string& className) :
- _packageName(packageName), _absPackageDataDir(packageDataDir),
- _relObjectsPath(relObjectsPath), _id(dataset, className)
+ const std::string& dataset,
+ const std::string& className) :
+ _packageName(packageName),
+ _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath),
+ _id(dataset, className)
{
}
- void ObjectInfo::setLogError(bool enabled)
+ void
+ ObjectInfo::setLogError(bool enabled)
{
this->_logError = enabled;
}
- std::string ObjectInfo::package() const
+ std::string
+ ObjectInfo::package() const
{
return _packageName;
}
- std::string ObjectInfo::dataset() const
+ std::string
+ ObjectInfo::dataset() const
{
return _id.dataset();
}
- std::string ObjectInfo::className() const
+ std::string
+ ObjectInfo::className() const
{
return _id.className();
}
- ObjectID ObjectInfo::id() const
+ ObjectID
+ ObjectInfo::id() const
{
return _id;
}
- std::string ObjectInfo::idStr() const
+ std::string
+ ObjectInfo::idStr() const
{
return _id.str();
}
- ObjectInfo::path ObjectInfo::objectDirectory(const bool fixDataPath) const
+ ObjectInfo::path
+ ObjectInfo::objectDirectory(const bool fixDataPath) const
{
- if(fixDataPath)
+ if (fixDataPath)
{
return _relObjectsPath / _id.dataset() / _id.className();
}
@@ -69,7 +86,10 @@ namespace armarx
return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
}
- PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix, const bool fixDataPath) const
+ PackageFileLocation
+ ObjectInfo::file(const std::string& _extension,
+ const std::string& suffix,
+ const bool fixDataPath) const
{
std::string extension = _extension;
if (extension.at(0) != '.')
@@ -85,23 +105,26 @@ namespace armarx
return loc;
}
-
- PackageFileLocation ObjectInfo::simoxXML() const
+ PackageFileLocation
+ ObjectInfo::simoxXML() const
{
return file(".xml");
}
- PackageFileLocation ObjectInfo::urdf() const
+ PackageFileLocation
+ ObjectInfo::urdf() const
{
return file(".urdf");
}
- PackageFileLocation ObjectInfo::sdf() const
+ PackageFileLocation
+ ObjectInfo::sdf() const
{
return file(".sdf");
}
- std::optional<PackageFileLocation> ObjectInfo::getModel() const
+ std::optional<PackageFileLocation>
+ ObjectInfo::getModel() const
{
if (fs::is_regular_file(simoxXML().absolutePath))
{
@@ -121,22 +144,26 @@ namespace armarx
}
}
- PackageFileLocation ObjectInfo::articulatedSimoxXML() const
+ PackageFileLocation
+ ObjectInfo::articulatedSimoxXML() const
{
return file(".xml", "_articulated", true);
}
- PackageFileLocation ObjectInfo::articulatedUrdf() const
+ PackageFileLocation
+ ObjectInfo::articulatedUrdf() const
{
return file(".urdf", "_articulated", true);
}
- PackageFileLocation ObjectInfo::articulatedSdf() const
+ PackageFileLocation
+ ObjectInfo::articulatedSdf() const
{
return file(".sdf", "_articulated");
}
- std::optional<PackageFileLocation> ObjectInfo::getArticulatedModel() const
+ std::optional<PackageFileLocation>
+ ObjectInfo::getArticulatedModel() const
{
if (fs::is_regular_file(articulatedSimoxXML().absolutePath))
{
@@ -156,33 +183,37 @@ namespace armarx
}
}
-
-
- PackageFileLocation ObjectInfo::meshWrl() const
+ PackageFileLocation
+ ObjectInfo::meshWrl() const
{
return file(".wrl");
}
- PackageFileLocation ObjectInfo::wavefrontObj() const
+ PackageFileLocation
+ ObjectInfo::wavefrontObj() const
{
return file(".obj");
}
- PackageFileLocation ObjectInfo::boundingBoxJson() const
+ PackageFileLocation
+ ObjectInfo::boundingBoxJson() const
{
return file(".json", "_bb");
}
- PackageFileLocation ObjectInfo::namesJson() const
+ PackageFileLocation
+ ObjectInfo::namesJson() const
{
return file(".json", "_names");
}
- std::optional<simox::AxisAlignedBoundingBox> ObjectInfo::loadAABB() const
+ std::optional<simox::AxisAlignedBoundingBox>
+ ObjectInfo::loadAABB() const
{
nlohmann::json j;
try
{
+ ARMARX_CHECK(std::filesystem::exists(boundingBoxJson().absolutePath));
j = nlohmann::read_json(boundingBoxJson().absolutePath);
}
catch (const std::exception& e)
@@ -194,28 +225,52 @@ namespace armarx
return std::nullopt;
}
- nlohmann::json jaabb = j.at("aabb");
- auto center = jaabb.at("center").get<Eigen::Vector3f>();
- auto extents = jaabb.at("extents").get<Eigen::Vector3f>();
- auto min = jaabb.at("min").get<Eigen::Vector3f>();
- auto max = jaabb.at("max").get<Eigen::Vector3f>();
-
- simox::AxisAlignedBoundingBox aabb(min, max);
+ try
+ {
- static const float prec = 1e-3f;
- ARMARX_CHECK_LESS_EQUAL((aabb.center() - center).norm(), prec) << aabb.center().transpose() << "\n" << center.transpose() << "\n" << id();
- ARMARX_CHECK_LESS_EQUAL((aabb.extents() - extents).norm(), prec) << aabb.extents().transpose() << "\n" << extents.transpose() << "\n" << id();
- ARMARX_CHECK_LESS_EQUAL((aabb.min() - min).norm(), prec) << aabb.min().transpose() << "\n" << min.transpose() << "\n" << id();
- ARMARX_CHECK_LESS_EQUAL((aabb.max() - max).norm(), prec) << aabb.max().transpose() << "\n" << max.transpose() << "\n" << id();
+ nlohmann::json jaabb = j.at("aabb");
+ auto center = jaabb.at("center").get<Eigen::Vector3f>();
+ auto extents = jaabb.at("extents").get<Eigen::Vector3f>();
+ auto min = jaabb.at("min").get<Eigen::Vector3f>();
+ auto max = jaabb.at("max").get<Eigen::Vector3f>();
+
+ simox::AxisAlignedBoundingBox aabb(min, max);
+
+ static const float prec = 1e-3f;
+ ARMARX_CHECK_LESS_EQUAL((aabb.center() - center).norm(), prec)
+ << aabb.center().transpose() << "\n"
+ << center.transpose() << "\n"
+ << id();
+ ARMARX_CHECK_LESS_EQUAL((aabb.extents() - extents).norm(), prec)
+ << aabb.extents().transpose() << "\n"
+ << extents.transpose() << "\n"
+ << id();
+ ARMARX_CHECK_LESS_EQUAL((aabb.min() - min).norm(), prec)
+ << aabb.min().transpose() << "\n"
+ << min.transpose() << "\n"
+ << id();
+ ARMARX_CHECK_LESS_EQUAL((aabb.max() - max).norm(), prec)
+ << aabb.max().transpose() << "\n"
+ << max.transpose() << "\n"
+ << id();
+
+ return aabb;
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << GetHandledExceptionString();
+ }
- return aabb;
+ return std::nullopt;
}
- std::optional<simox::OrientedBox<float>> ObjectInfo::loadOOBB() const
+ std::optional<simox::OrientedBox<float>>
+ ObjectInfo::loadOOBB() const
{
nlohmann::json j;
try
{
+ ARMARX_CHECK(std::filesystem::exists(boundingBoxJson().absolutePath));
j = nlohmann::read_json(boundingBoxJson().absolutePath);
}
catch (const std::exception& e)
@@ -227,48 +282,60 @@ namespace armarx
return std::nullopt;
}
- nlohmann::json joobb = j.at("oobb");
- auto pos = joobb.at("pos").get<Eigen::Vector3f>();
- auto ori = joobb.at("ori").get<Eigen::Quaternionf>().toRotationMatrix();
- auto extents = joobb.at("extents").get<Eigen::Vector3f>();
-
- Eigen::Vector3f corner = pos - ori * extents / 2;
-
- simox::OrientedBox<float> oobb(corner,
- ori.col(0) * extents(0),
- ori.col(1) * extents(1),
- ori.col(2) * extents(2));
-
- static const float prec = 1e-3f;
- ARMARX_CHECK(oobb.rotation().isApprox(ori, prec)) << oobb.rotation() << "\n" << ori << "\n" << id();
- // If the object is too large, the above precision will trigger a false positive.
- if (extents.squaredNorm() < 1e5f * 1e5f)
+ try
+ {
+ nlohmann::json joobb = j.at("oobb");
+ auto pos = joobb.at("pos").get<Eigen::Vector3f>();
+ auto ori = joobb.at("ori").get<Eigen::Quaternionf>().toRotationMatrix();
+ auto extents = joobb.at("extents").get<Eigen::Vector3f>();
+
+ Eigen::Vector3f corner = pos - ori * extents / 2;
+
+ simox::OrientedBox<float> oobb(
+ corner, ori.col(0) * extents(0), ori.col(1) * extents(1), ori.col(2) * extents(2));
+
+ static const float prec = 1e-3f;
+ ARMARX_CHECK(oobb.rotation().isApprox(ori, prec)) << oobb.rotation() << "\n"
+ << ori << "\n"
+ << id();
+ // If the object is too large, the above precision will trigger a false positive.
+ if (extents.squaredNorm() < 1e5f * 1e5f)
+ {
+ ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec)
+ << VAROUT(oobb.center().transpose()) << "\n"
+ << VAROUT(pos.transpose()) << "\n"
+ << VAROUT(extents.norm()) << "\n"
+ << VAROUT(id());
+ ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec)
+ << VAROUT(oobb.dimensions().transpose()) << "\n"
+ << VAROUT(extents.transpose()) << "\n"
+ << VAROUT(extents.norm()) << "\n"
+ << VAROUT(id());
+ }
+ return oobb;
+ }
+ catch (...)
{
- ARMARX_CHECK_LESS_EQUAL((oobb.center() - pos).norm(), prec)
- << VAROUT(oobb.center().transpose())
- << "\n" << VAROUT(pos.transpose())
- << "\n" << VAROUT(extents.norm())
- << "\n" << VAROUT(id());
- ARMARX_CHECK_LESS_EQUAL((oobb.dimensions() - extents).norm(), prec)
- << VAROUT(oobb.dimensions().transpose())
- << "\n" << VAROUT(extents.transpose())
- << "\n" << VAROUT(extents.norm())
- << "\n" << VAROUT(id());
+ ARMARX_WARNING << GetHandledExceptionString();
}
- return oobb;
+
+ return std::nullopt;
}
- std::optional<std::vector<std::string>> ObjectInfo::loadRecognizedNames() const
+ std::optional<std::vector<std::string>>
+ ObjectInfo::loadRecognizedNames() const
{
return loadNames("recognized_name");
}
- std::optional<std::vector<std::string>> ObjectInfo::loadSpokenNames() const
+ std::optional<std::vector<std::string>>
+ ObjectInfo::loadSpokenNames() const
{
return loadNames("spoken_name");
}
- std::optional<std::vector<std::string> > ObjectInfo::loadNames(const std::string& jsonKey) const
+ std::optional<std::vector<std::string>>
+ ObjectInfo::loadNames(const std::string& jsonKey) const
{
const PackageFileLocation file = namesJson();
if (fs::is_regular_file(file.absolutePath))
@@ -283,7 +350,8 @@ namespace armarx
{
if (_logError)
{
- ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n" << e.what();
+ ARMARX_WARNING << "Failed to parse JSON file " << file.absolutePath << ": \n"
+ << e.what();
}
return std::nullopt;
}
@@ -291,7 +359,8 @@ namespace armarx
{
if (_logError)
{
- ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n" << e.what();
+ ARMARX_WARNING << "Failed to read file " << file.absolutePath << ": \n"
+ << e.what();
}
return std::nullopt;
}
@@ -304,8 +373,8 @@ namespace armarx
}
}
-
- bool ObjectInfo::checkPaths() const
+ bool
+ ObjectInfo::checkPaths() const
{
namespace fs = std::filesystem;
bool result = true;
@@ -314,7 +383,8 @@ namespace armarx
{
if (_logError)
{
- ARMARX_WARNING << "Expected simox object file for object " << *this << ": " << simoxXML().absolutePath;
+ ARMARX_WARNING << "Expected simox object file for object " << *this << ": "
+ << simoxXML().absolutePath;
}
result = false;
}
@@ -322,7 +392,8 @@ namespace armarx
{
if (_logError)
{
- ARMARX_WARNING << "Expected wavefront object file (.obj) for object " << *this << ": " << wavefrontObj().absolutePath;
+ ARMARX_WARNING << "Expected wavefront object file (.obj) for object " << *this
+ << ": " << wavefrontObj().absolutePath;
}
result = false;
}
@@ -330,10 +401,10 @@ namespace armarx
return result;
}
-}
-
+} // namespace armarx
-std::ostream& armarx::operator<<(std::ostream& os, const ObjectInfo& rhs)
+std::ostream&
+armarx::operator<<(std::ostream& os, const ObjectInfo& rhs)
{
return os << rhs.id();
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
index bc94ec838a5afda6a2d26740f068592bbf49a607..103811f410578b39acf50ab7575b0aa8208f5e6e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
@@ -8,7 +8,9 @@
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
namespace armarx::armem::server::ltm::detail::mixin
{
@@ -35,6 +37,15 @@ namespace armarx::armem::server::ltm::detail::mixin
TIMING_END_STREAM(LTM_Memory_DirectlyStore, ARMARX_DEBUG);
}
+ void
+ directlyStore(const armem::server::wm::Memory& serverMemory)
+ {
+ wm::Memory memory;
+ memory.setName(serverMemory.name());
+ memory.update(armem::toCommit(serverMemory));
+ this->directlyStore(memory);
+ }
+
void
bufferFinished()
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 5506ddbf598ed92ac11522769b9e7b0734d20162..a9b406a13c1fa22384705afa88777a5a9c504a9e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -7,6 +7,7 @@
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
+#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -55,6 +56,7 @@ namespace armarx::armem::server::obj::clazz
if (p.loadFromObjectsPackage)
{
+ ARMARX_VERBOSE << "Loading objects from package";
loadByObjectFinder(p.objectsPackage);
}
}
@@ -71,6 +73,7 @@ namespace armarx::armem::server::obj::clazz
void
Segment::loadByObjectFinder(const std::string& objectsPackage)
{
+ ARMARX_TRACE;
loadByObjectFinder(ObjectFinder(objectsPackage));
}
@@ -84,6 +87,8 @@ namespace armarx::armem::server::obj::clazz
void
Segment::loadByObjectFinder()
{
+ ARMARX_TRACE;
+
const Time now = Time::Now();
const bool checkPaths = false;
@@ -96,15 +101,23 @@ namespace armarx::armem::server::obj::clazz
Commit commit;
for (ObjectInfo& info : infos)
{
+ ARMARX_TRACE;
+
+ ARMARX_VERBOSE << info.idStr();
info.setLogError(false);
EntityUpdate& update = commit.add();
update.entityID = providerID.withEntityName(info.id().str());
update.arrivedTime = update.referencedTime = update.sentTime = now;
+ ARMARX_TRACE;
+ ARMARX_VERBOSE << "object class from info";
arondto::ObjectClass objectClass = objectClassFromInfo(info);
update.instancesData = {objectClass.toAron()};
}
+
+ ARMARX_TRACE;
+
ARMARX_INFO << "Loaded " << commit.updates.size() << " object classes from '"
<< objectFinder.getPackageName() << "'.";
iceMemory.commitLocking(commit);
@@ -179,6 +192,8 @@ namespace armarx::armem::server::obj::clazz
{
namespace fs = std::filesystem;
+ // ARMARX_VERBOSE << info.idStr();
+
arondto::ObjectClass data;
toAron(data.id, info.id());
@@ -203,9 +218,14 @@ namespace armarx::armem::server::obj::clazz
setPathIfExists(data.meshObjPath, info.wavefrontObj());
setPathIfExists(data.meshWrlPath, info.meshWrl());
+ ARMARX_TRACE;
+ ARMARX_VERBOSE << "AABB";
auto aabb = info.loadAABB();
toAron(data.aabb, aabb ? aabb.value() : simox::AxisAlignedBoundingBox());
+
+ ARMARX_TRACE;
auto oobb = info.loadOOBB();
+ ARMARX_VERBOSE << "OOBB";
toAron(data.oobb, oobb ? oobb.value() : simox::OrientedBoxf());
if (auto recogNames = info.loadRecognizedNames())
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index 89c13ce4c4a6baf346f977ce0a1a66cee0eb1630..ba42e0072097beb531b1fedbef3f6344eb208cc4 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -41,7 +41,7 @@ namespace armarx::armem::server::obj::clazz
void visualizeClass(const MemoryID& entityID, bool showAABB = true, bool showOOBB = true);
- static arondto::ObjectClass objectClassFromInfo(const ObjectInfo& info);
+ arondto::ObjectClass objectClassFromInfo(const ObjectInfo& info);
private:
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 1bf0200459eb987e6ad842c38d9138c40401dcb3..865fa2ae35b1ce975b3be920214035674c3c9050 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,5 +1,6 @@
#include "Segment.h"
+#include <filesystem>
#include <sstream>
#include <Eigen/Dense>
@@ -1013,6 +1014,7 @@ namespace armarx::armem::server::obj::instance
{
try
{
+ ARMARX_CHECK(std::filesystem::exists(path)) << path;
return simox::json::read<armarx::objects::Scene>(path);
}
catch (const simox::json::error::JsonError& e)
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 8f7c057e4dbd935d5574e6ee4914280611c7808a..8a3b67e984d51a40d85e44113bc54a872233d910 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -185,6 +185,26 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState>
RobotReader::queryState(const robot::RobotDescription& description,
const armem::Time& timestamp) const
+ {
+ std::optional<robot::RobotState> robotState = queryJointState(description, timestamp);
+
+ if (robotState)
+ {
+ const auto globalPose = queryGlobalPose(description, timestamp);
+ if (not globalPose)
+ {
+ ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
+ return std::nullopt;
+ }
+ robotState->globalPose = *globalPose;
+ }
+
+ return robotState;
+ }
+
+ std::optional<robot::RobotState>
+ RobotReader::queryJointState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const
{
const auto proprioception = queryProprioception(description, timestamp);
@@ -196,15 +216,8 @@ namespace armarx::armem::robot_state
}
const auto jointMap = proprioception->joints.position;
- const auto globalPose = queryGlobalPose(description, timestamp);
- if (not globalPose)
- {
- ARMARX_VERBOSE << "Failed to query global pose for robot " << description.name;
- return std::nullopt;
- }
-
return robot::RobotState{.timestamp = timestamp,
- .globalPose = *globalPose,
+ .globalPose = robot::RobotState::Pose::Identity(),
.jointMap = jointMap,
.proprioception = proprioception};
}
@@ -591,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
{
@@ -749,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
@@ -779,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 8397ea9a8d8b8de46a7c2b1ec1167396d8383d06..650227dee1666bb124d4941a13c16088f62d88a5 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -67,6 +67,9 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+ std::optional<robot::RobotState> queryJointState(const robot::RobotDescription& description,
+ const armem::Time& timestamp) const;
+
std::optional<armarx::armem::arondto::Proprioception>
queryProprioception(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
@@ -104,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;
/**
@@ -153,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/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 0effa5c23d6763ff984610100b1770024e452bee..e23178dfa4a884654b8ba924a71006dbd88e8e8b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -47,6 +47,30 @@ namespace armarx::armem::robot_state
return true;
}
+ bool
+ VirtualRobotReader::synchronizeRobotJoints(VirtualRobot::Robot& robot,
+ const armem::Time& timestamp) const
+ {
+ // const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ // const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
+
+ const robot::RobotDescription robotDescription{.name = robot.getName(),
+ .xml = PackagePath{"", ""}};
+
+ const auto robotState = queryJointState(robotDescription, timestamp);
+ if (not robotState)
+ {
+ ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
+ << robot.getName() << "` "
+ << "(type `" << robot.getType() << "`)!";
+ return false;
+ }
+
+ robot.setJointValues(robotState->jointMap);
+
+ return true;
+ }
+
VirtualRobot::RobotPtr
VirtualRobotReader::getRobot(const std::string& name,
const armem::Time& timestamp,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index 1d91f05829b1c49fc5a07782ecbead486e075a5c..d0a672c21ba5ae953d3bc3b8bed2333a9f6efa97 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -49,6 +49,9 @@ namespace armarx::armem::robot_state
[[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
const armem::Time& timestamp) const;
+ [[nodiscard]] bool synchronizeRobotJoints(VirtualRobot::Robot& robot,
+ const armem::Time& timestamp) const;
+
[[nodiscard]] VirtualRobot::RobotPtr
getRobot(const std::string& name,
const armem::Time& timestamp = armem::Time::Invalid(),
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/common/rw/eigen.h b/source/RobotAPI/libraries/aron/common/rw/eigen.h
index 69fd857063b85792e46f7e197e933a71b93c46e9..7cf56de94f1ecf81937bc2729f3b664d597e4866 100644
--- a/source/RobotAPI/libraries/aron/common/rw/eigen.h
+++ b/source/RobotAPI/libraries/aron/common/rw/eigen.h
@@ -26,16 +26,20 @@ namespace armarx
std::vector<unsigned char> data;
aron_r.readNDArray(input, shape, typeAsString, data);
- ARMARX_CHECK_AND_THROW(
- ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
- ::armarx::aron::error::AronException(
- __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
+ std::stringstream ss;
+ ss << "Received wrong dimensions for matrix member. Dimensions are " << shape.at(0)
+ << "," << shape.at(1) << " but should be " << ret.rows() << "/" << ret.cols();
+
ARMARX_CHECK_AND_THROW(
typeAsString == TypeName<EigenT>::Get(),
::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
"Received wrong typename",
typeAsString,
TypeName<EigenT>::Get()));
+ ARMARX_CHECK_AND_THROW(
+ ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
+ ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, ss.str()));
+
std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
}
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);