From 0305f9936b23a1469a9ad513bafb2c19efb10938 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 1 Mar 2023 17:12:03 +0100
Subject: [PATCH] exteroception core segment + converters etc
---
.../RobotStateMemory/RobotStateMemory.cpp | 3 +
.../RobotStateMemory/RobotStateMemory.h | 4 +
.../armem_robot_state/CMakeLists.txt | 1 +
.../armem_robot_state/aron/Exteroception.xml | 35 +++
.../armem_robot_state/aron/Proprioception.xml | 16 -
.../armem_robot_state/aron_conversions.cpp | 5 +-
.../armem_robot_state/aron_conversions.h | 9 +-
.../client/common/RobotReader.cpp | 11 +-
.../client/common/constants.h | 1 +
.../armem_robot_state/memory_ids.cpp | 1 +
.../libraries/armem_robot_state/memory_ids.h | 1 +
.../armem_robot_state/server/CMakeLists.txt | 16 +-
.../server/exteroception/Segment.cpp | 50 +++
.../server/exteroception/Segment.h | 55 ++++
.../converters/ArmarDEConverter.cpp | 130 ++++++++
.../converters/ArmarDEConverter.h | 51 +++
.../converters/ConverterInterface.cpp | 11 +
.../converters/ConverterInterface.h | 32 ++
.../converters/ConverterRegistry.cpp | 31 ++
.../converters/ConverterRegistry.h | 36 +++
.../converters/ConverterTools.cpp | 179 +++++++++++
.../exteroception/converters/ConverterTools.h | 106 +++++++
.../proprioception/RobotStateWriter.cpp | 27 +-
.../server/proprioception/RobotStateWriter.h | 1 +
.../server/proprioception/RobotUnitData.h | 1 +
.../server/proprioception/RobotUnitReader.cpp | 18 +-
.../server/proprioception/RobotUnitReader.h | 7 +-
.../converters/ArmarDEConverter.cpp | 296 ------------------
.../converters/ArmarDEConverter.h | 78 -----
.../converters/ConverterRegistry.cpp | 3 +-
30 files changed, 800 insertions(+), 415 deletions(-)
create mode 100644 source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
create mode 100644 source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
delete mode 100644 source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.cpp
delete mode 100644 source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.h
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index eab8bf5e6..f77e01607 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -47,6 +47,7 @@ namespace armarx::armem::server::robot_state
RobotStateMemory::RobotStateMemory() :
descriptionSegment(iceAdapter()),
proprioceptionSegment(iceAdapter()),
+ exteroceptionSegment(iceAdapter()),
localizationSegment(iceAdapter()),
commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
@@ -86,6 +87,7 @@ namespace armarx::armem::server::robot_state
setMemoryName(armem::robot_state::memoryID.memoryName);
descriptionSegment.defineProperties(defs, prefix + "desc.");
+ exteroceptionSegment.defineProperties(defs, prefix + "ext.");
proprioceptionSegment.defineProperties(defs, prefix + "prop.");
localizationSegment.defineProperties(defs, prefix + "loc.");
commonVisu.defineProperties(defs, prefix + "visu.");
@@ -104,6 +106,7 @@ namespace armarx::armem::server::robot_state
{
descriptionSegment.init();
proprioceptionSegment.init();
+ exteroceptionSegment.init();
localizationSegment.init();
commonVisu.init();
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index aed5fec80..70513a0cf 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -38,6 +38,7 @@
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h>
namespace armarx::plugins
@@ -105,6 +106,9 @@ namespace armarx::armem::server::robot_state
proprioception::Segment proprioceptionSegment;
armem::data::MemoryID robotUnitProviderID;
+ // - exteroception
+ exteroception::Segment exteroceptionSegment;
+
// - localization
localization::Segment localizationSegment;
diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index a9c0ceb1b..8dda90015 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -65,6 +65,7 @@ armarx_enable_aron_file_generation_for_target(
ARON_FILES
aron/JointState.xml
aron/Proprioception.xml
+ aron/Exteroception.xml
aron/TransformHeader.xml
aron/Transform.xml
)
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
new file mode 100644
index 000000000..80f72e060
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Exteroception.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <SystemInclude include="Eigen/Core" />
+ <SystemInclude include="Eigen/Geometry" />
+ </CodeIncludes>
+
+ <GenerateTypes>
+
+ <Object name="armarx::armem::exteroception::arondto::ToF">
+
+ <ObjectChild key="array">
+ <Matrix rows="-1" cols="-1" type="float32" />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name="armarx::armem::arondto::Exteroception">
+
+ <ObjectChild key="iterationID">
+ <long />
+ </ObjectChild>
+
+ <ObjectChild key="tof">
+ <Dict>
+ <armarx::armem::exteroception::arondto::ToF/>
+ </Dict>
+ </ObjectChild>
+
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
index e84ccf382..d5472cbde 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
+++ b/source/RobotAPI/libraries/armem_robot_state/aron/Proprioception.xml
@@ -234,15 +234,6 @@
</ObjectChild>
</Object>
-
- <Object name="armarx::armem::prop::arondto::ToF">
-
- <ObjectChild key="array">
- <Matrix rows="-1" cols="-1" type="float32" />
- </ObjectChild>
-
- </Object>
-
<Object name="armarx::armem::arondto::Proprioception">
@@ -264,13 +255,6 @@
</Dict>
</ObjectChild>
- <!-- TODO move to separate segment -->
- <ObjectChild key="tof">
- <Dict>
- <armarx::armem::prop::arondto::ToF/>
- </Dict>
- </ObjectChild>
-
<ObjectChild key="extraFloats">
<Dict>
<Float />
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index b079cec2e..3749c892d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -6,6 +6,7 @@
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
@@ -98,11 +99,11 @@ namespace armarx::armem
}
- void fromAron(const armarx::armem::prop::arondto::ToF& dto, robot::ToFArray& bo){
+ void fromAron(const armarx::armem::exteroception::arondto::ToF& dto, robot::ToFArray& bo){
bo = dto.array;
}
- void toAron(armarx::armem::prop::arondto::ToF& dto, const robot::ToFArray& bo){
+ void toAron(armarx::armem::exteroception::arondto::ToF& dto, const robot::ToFArray& bo){
dto.array = bo;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
index 1912e6c69..f78efc200 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.h
@@ -38,6 +38,10 @@ namespace armarx::armem
{
struct Platform;
struct ForceTorque;
+ }
+
+ namespace exteroception::arondto
+ {
struct ToF;
}
@@ -66,9 +70,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::prop::arondto::ToF& dto, robot::ToFArray& bo);
- void toAron(armarx::armem::prop::arondto::ToF& dto, const robot::ToFArray& 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);
} // 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 d5f4844b5..cb55df698 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -24,6 +24,7 @@
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
@@ -722,7 +723,7 @@ namespace armarx::armem::robot_state
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
- .getCoreSegment(constants::proprioceptionCoreSegment);
+ .getCoreSegment(constants::exteroceptionCoreSegment);
// clang-format on
coreSegment.forEachEntity(
@@ -736,12 +737,12 @@ namespace armarx::armem::robot_state
}
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto proprioception =
- tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
- ARMARX_CHECK(proprioception.has_value());
+ const auto exteroception =
+ tryCast<::armarx::armem::arondto::Exteroception>(entityInstance);
+ ARMARX_CHECK(exteroception.has_value());
- for (const auto& [handName, dtoFt] : proprioception->tof)
+ for (const auto& [handName, dtoFt] : exteroception->tof)
{
ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h b/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
index d9efcbcf7..5d093c195 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/constants.h
@@ -28,6 +28,7 @@ namespace armarx::armem::robot_state::constants
inline const std::string descriptionCoreSegment = "Description";
inline const std::string localizationCoreSegment = "Localization";
inline const std::string proprioceptionCoreSegment = "Proprioception";
+ inline const std::string exteroceptionCoreSegment = "Exteroception";
inline const std::string descriptionEntityName = "description";
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
index 926c2e843..2f219eff3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.cpp
@@ -30,6 +30,7 @@ namespace armarx::armem
const MemoryID robot_state::descriptionSegmentID { memoryID.withCoreSegmentName("Description") };
const MemoryID robot_state::proprioceptionSegmentID { memoryID.withCoreSegmentName("Proprioception") };
+ const MemoryID robot_state::exteroceptionSegmentID { memoryID.withCoreSegmentName("Exteroception") };
const MemoryID robot_state::localizationSegmentID { memoryID.withCoreSegmentName("Localization") };
diff --git a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
index 1d8008d4d..f653236b1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
+++ b/source/RobotAPI/libraries/armem_robot_state/memory_ids.h
@@ -32,6 +32,7 @@ namespace armarx::armem::robot_state
extern const MemoryID descriptionSegmentID;
extern const MemoryID proprioceptionSegmentID;
+ extern const MemoryID exteroceptionSegmentID;
extern const MemoryID localizationSegmentID;
} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
index 887cc2eec..3ad1d5068 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt
@@ -43,11 +43,16 @@ armarx_add_library(
proprioception/RobotUnitData.h
proprioception/RobotUnitReader.h
+ proprioception/converters/ConverterInterface.h
proprioception/converters/Armar6Converter.h
- proprioception/converters/ArmarDEConverter.h
proprioception/converters/ConverterTools.h
proprioception/converters/ConverterRegistry.h
- proprioception/converters/ConverterInterface.h
+
+ exteroception/converters/ConverterInterface.h
+ exteroception/converters/ConverterTools.h
+ exteroception/converters/ConverterRegistry.h
+ exteroception/converters/ArmarDEConverter.h
+ exteroception/Segment.h
description/Segment.h
@@ -64,11 +69,16 @@ armarx_add_library(
proprioception/RobotUnitReader.cpp
proprioception/converters/Armar6Converter.cpp
- proprioception/converters/ArmarDEConverter.cpp
proprioception/converters/ConverterTools.cpp
proprioception/converters/ConverterRegistry.cpp
proprioception/converters/ConverterInterface.cpp
+ exteroception/converters/ConverterInterface.cpp
+ exteroception/converters/ConverterTools.cpp
+ exteroception/converters/ConverterRegistry.cpp
+ exteroception/converters/ArmarDEConverter.cpp
+ exteroception/Segment.cpp
+
description/Segment.cpp
)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
new file mode 100644
index 000000000..63f24d0f4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
@@ -0,0 +1,50 @@
+#include "Segment.h"
+
+#include <filesystem>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ Base(memoryToIceAdapter,
+ armem::robot_state::exteroceptionSegmentID.coreSegmentName,
+ arondto::Exteroception::ToAronType())
+ {
+ }
+
+ Segment::~Segment() = default;
+
+
+ // void
+ // Segment::onConnect(const RobotUnitInterfacePrx& robotUnitPrx)
+ // {
+ // robotUnit = robotUnitPrx;
+
+ // // store the robot description linked to the robot unit in this segment
+ // }
+
+
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
new file mode 100644
index 000000000..e8471563d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
@@ -0,0 +1,55 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <optional>
+#include <string>
+#include <unordered_map>
+
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ class Segment : public segment::SpecializedCoreSegment
+ {
+ using Base = segment::SpecializedCoreSegment;
+
+ public:
+ Segment(server::MemoryToIceAdapter& iceMemory);
+ ~Segment() override;
+
+
+ // void onConnect(const RobotUnitInterfacePrx& robotUnitPrx);
+
+
+ private:
+ // RobotUnitInterfacePrx robotUnit;
+ };
+
+} // namespace armarx::armem::server::robot_state::exteroception
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
new file mode 100644
index 000000000..d71bb045b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
@@ -0,0 +1,130 @@
+#include "ArmarDEConverter.h"
+#include <cmath>
+#include <string>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.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 "ConverterTools.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ArmarDEConverter::ArmarDEConverter() = default;
+ ArmarDEConverter::~ArmarDEConverter() = default;
+
+
+ aron::data::DictPtr
+ ArmarDEConverter::convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description)
+ {
+ arondto::Exteroception dto;
+ dto.iterationID = data.iterationId;
+
+ 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());
+
+ 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)
+ {
+ 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 << "'";
+
+ const std::string& category = split.at(0);
+ // const std::string& name = split.at(1);
+ const std::string& field = split.at(2);
+ ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << entryName;
+
+ if (simox::alg::contains(field, "tofDepth"))
+ {
+ // 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)
+ {
+ // split e.g. "sens.LeftHand.tofDepth.element_15" (split at dot)
+
+ ARMARX_CHECK_EQUAL(split.size(), 4);
+ ARMARX_CHECK_EQUAL(split.at(2), "tofDepth");
+
+ 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;
+ processToFEntry(tofs[side], split, 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(), "_");
+ ARMARX_CHECK_EQUAL(elements.size(), 2);
+
+ const int idx = std::stoi(elements.at(1));
+
+ if(tof.array.size() < (idx +1))
+ {
+ tof.array.resize(idx+1, 1);
+ }
+
+ tof.array(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
new file mode 100644
index 000000000..bf6c482a0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.h
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <map>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ struct ConverterValue;
+ // class ConverterTools;
+
+
+ class ArmarDEConverter : public ConverterInterface
+ {
+ public:
+ ArmarDEConverter();
+ ~ArmarDEConverter() override;
+
+
+ aron::data::DictPtr
+ convert(const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) override;
+
+
+ protected:
+ void process(arondto::Exteroception& dtoExteroception,
+ const std::string& entryName,
+ const ConverterValue& value);
+
+
+ private:
+
+
+ void processToFEntry(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,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
+
+
+ // std::unique_ptr<ConverterTools> tools;
+ };
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
new file mode 100644
index 000000000..4ba33e6c5
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
@@ -0,0 +1,11 @@
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ConverterInterface::~ConverterInterface()
+ {
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
new file mode 100644
index 000000000..5ba0ecc3c
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <memory>
+
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+namespace armarx::RobotUnitDataStreaming
+{
+ struct TimeStep;
+ struct DataStreamingDescription;
+ struct DataEntry;
+}
+namespace armarx::armem::arondto
+{
+ class Proprioception;
+}
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ class ConverterInterface
+ {
+ public:
+
+ virtual ~ConverterInterface();
+
+ virtual
+ aron::data::DictPtr convert(
+ const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
+
+ };
+}
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
new file mode 100644
index 000000000..ef7296b66
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
@@ -0,0 +1,31 @@
+#include "ConverterRegistry.h"
+
+#include "ArmarDEConverter.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ ConverterRegistry::ConverterRegistry()
+ {
+ add<ArmarDEConverter>("ArmarDE");
+ // add<Armar7Converter>("Armar7");
+ }
+
+
+ ConverterInterface*
+ ConverterRegistry::get(const std::string& key) const
+ {
+ auto it = converters.find(key);
+ return it != converters.end() ? it->second.get() : nullptr;
+ }
+
+
+ std::vector<std::string> ConverterRegistry::getKeys() const
+ {
+ return simox::alg::get_keys(converters);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
new file mode 100644
index 000000000..1e1cac9a1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ class ConverterRegistry
+ {
+ public:
+
+ ConverterRegistry();
+
+
+ template <class ConverterT, class ...Args>
+ void add(const std::string& key, Args... args)
+ {
+ converters[key].reset(new ConverterT(args...));
+ }
+
+
+ ConverterInterface* get(const std::string& key) const;
+ std::vector<std::string> getKeys() const;
+
+
+ private:
+
+ std::map<std::string, std::unique_ptr<ConverterInterface>> converters;
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
new file mode 100644
index 000000000..6086a9aee
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
@@ -0,0 +1,179 @@
+#include "ConverterTools.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/advanced.h>
+
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
+
+namespace armarx::armem::server::robot_state
+{
+
+ std::optional<std::string>
+ exteroception::findByPrefix(const std::string& key, const std::set<std::string>& prefixes)
+ {
+ for (const auto& prefix : prefixes)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return prefix;
+ }
+ }
+ return std::nullopt;
+ }
+}
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+ ConverterTools::ConverterTools()
+ {
+ {
+ vector3fSetters["X"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.x() = f;
+ };
+ vector3fSetters["Y"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.y() = f;
+ };
+ vector3fSetters["Z"] = [](Eigen::Vector3f & v, float f)
+ {
+ v.z() = f;
+ };
+ vector3fSetters["x"] = vector3fSetters["X"];
+ vector3fSetters["y"] = vector3fSetters["Y"];
+ vector3fSetters["z"] = vector3fSetters["Z"];
+ vector3fSetters["Rotation"] = vector3fSetters["Z"];
+ }
+ {
+ platformPoseGetters["acceleration"] = [](prop::arondto::Platform & p)
+ {
+ return &p.acceleration;
+ };
+ platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
+ {
+ return &p.relativePosition;
+ };
+ platformPoseGetters["velocity"] = [](prop::arondto::Platform & p)
+ {
+ return &p.velocity;
+ };
+ platformIgnored.insert("absolutePosition");
+ }
+ {
+ ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationForce;
+ };
+ ftGetters["gravCompT"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.gravityCompensationTorque;
+ };
+ ftGetters["f"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.force;
+ };
+ ftGetters["t"] = [](prop::arondto::ForceTorque & ft)
+ {
+ return &ft.torque;
+ };
+ }
+ {
+ jointGetters["acceleration"] = [](prop::arondto::Joints & j)
+ {
+ return &j.acceleration;
+ };
+ jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.gravityTorque;
+ };
+ jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inertiaTorque;
+ };
+ jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.inverseDynamicsTorque;
+ };
+ jointGetters["position"] = [](prop::arondto::Joints & j)
+ {
+ return &j.position;
+ };
+ jointGetters["torque"] = [](prop::arondto::Joints & j)
+ {
+ return &j.torque;
+ };
+ jointGetters["velocity"] = [](prop::arondto::Joints & j)
+ {
+ return &j.velocity;
+ };
+ }
+ {
+
+#define ADD_SCALAR_SETTER(container, name, type) \
+ container [ #name ] = []( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue & value) \
+ { \
+ dto. name [split.at(1)] = getValueAs< type >(value); \
+ }
+
+ ADD_SCALAR_SETTER(jointSetters, position, float);
+ ADD_SCALAR_SETTER(jointSetters, velocity, float);
+ ADD_SCALAR_SETTER(jointSetters, acceleration, float);
+
+ ADD_SCALAR_SETTER(jointSetters, relativePosition, float);
+ ADD_SCALAR_SETTER(jointSetters, filteredVelocity, float);
+
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, velocityTarget, float);
+
+ ADD_SCALAR_SETTER(jointSetters, torque, float);
+ ADD_SCALAR_SETTER(jointSetters, inertiaTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, gravityCompensatedTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, inverseDynamicsTorque, float);
+ ADD_SCALAR_SETTER(jointSetters, torqueTicks, int);
+
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, currentTarget, float);
+ ADD_SCALAR_SETTER(jointSetters, positionTarget, float);
+
+ // "temperature" handled below
+ // ADD_SCALAR_SETTER(jointSetters, temperature, float);
+
+ ADD_SCALAR_SETTER(jointSetters, motorCurrent, float);
+ ADD_SCALAR_SETTER(jointSetters, maxTargetCurrent, float);
+
+ ADD_SCALAR_SETTER(jointSetters, sensorBoardUpdateRate, float);
+ ADD_SCALAR_SETTER(jointSetters, I2CUpdateRate, float);
+
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEmergencyStop, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusEnabled, bool);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusError, int);
+ ADD_SCALAR_SETTER(jointSetters, JointStatusOperation, int);
+
+
+#define ADD_VECTOR3_SETTER(container, name, type) \
+ container [ #name ] = [this]( \
+ prop::arondto::Joints & dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue & value) \
+ { \
+ auto& vec = dto. name [split.at(1)]; \
+ auto& setter = this->vector3fSetters.at(split.at(3)); \
+ setter(vec, getValueAs< type >(value)); \
+ }
+
+ ADD_VECTOR3_SETTER(jointSetters, angularVelocity, float);
+ ADD_VECTOR3_SETTER(jointSetters, linearAcceleration, float);
+
+ // ADD_GETTER(jointVectorGetters, orientation, float);
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
new file mode 100644
index 000000000..7460ebb13
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
@@ -0,0 +1,106 @@
+#pragma once
+
+#include <map>
+#include <set>
+#include <string>
+
+#include <Eigen/Core>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+
+#include "ConverterInterface.h"
+
+
+namespace armarx::armem::server::robot_state::exteroception
+{
+
+ struct ConverterValue
+ {
+ const RobotUnitDataStreaming::TimeStep& data;
+ const RobotUnitDataStreaming::DataEntry& entry;
+ };
+
+
+ template <class T>
+ T
+ getValueAs(const ConverterValue& value)
+ {
+ return RobotUnitDataStreamingReceiver::GetAs<T>(value.data, value.entry);
+ }
+
+
+ /**
+ * @brief Search
+ * @param key
+ * @param prefixes
+ * @return
+ */
+ std::optional<std::string>
+ findByPrefix(const std::string& key, const std::set<std::string>& prefixes);
+
+
+ template <class ValueT>
+ ValueT
+ findByPrefix(const std::string& key, const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [prefix, value] : map)
+ {
+ if (simox::alg::starts_with(key, prefix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+ template <class ValueT>
+ ValueT
+ findBySuffix(const std::string& key, const std::map<std::string, ValueT>& map)
+ {
+ for (const auto& [suffix, value] : map)
+ {
+ if (simox::alg::ends_with(key, suffix))
+ {
+ return value;
+ }
+ }
+ return nullptr;
+ }
+
+
+
+ class ConverterTools
+ {
+ public:
+
+ ConverterTools();
+
+
+ public:
+
+ std::map<std::string, std::function<void(Eigen::Vector3f&, float)> > vector3fSetters;
+
+ std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
+ std::map<std::string, std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)> > jointVectorGetters;
+
+ using JointSetter = std::function<void(prop::arondto::Joints& dto, const std::vector<std::string>& split, const ConverterValue& value)>;
+ std::map<std::string, JointSetter> jointSetters;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)> > platformPoseGetters;
+ std::set<std::string> platformIgnored;
+
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)> > ftGetters;
+
+
+ std::map<std::string, std::string> sidePrefixMap
+ {
+ { "R", "Right" },
+ { "L", "Left" },
+ };
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index 52de49d14..cf1a5a8ab 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -87,8 +87,9 @@ namespace armarx::armem::server::robot_state::proprioception
// Commits lock the core segments.
- // Proprioception
- armem::CommitResult result = memory.commitLocking(update.proprioception);
+ // Proprioception + Exteroception
+ armem::CommitResult resultProprioception = memory.commitLocking(update.proprioception);
+ armem::CommitResult resultExteroception = memory.commitLocking(update.exteroception);
endProprioception = std::chrono::high_resolution_clock::now();
// Localization
@@ -100,11 +101,18 @@ namespace armarx::armem::server::robot_state::proprioception
}
endLocalization = std::chrono::high_resolution_clock::now();
- if (not result.allSuccess())
+ if (not resultProprioception.allSuccess())
{
- ARMARX_WARNING << "Could not commit data to memory. Error message: "
- << result.allErrorMessages();
+ ARMARX_WARNING << "Could not commit data to proprioception segment in memory. Error message: "
+ << resultProprioception.allErrorMessages();
}
+
+ if (not resultExteroception.allSuccess())
+ {
+ ARMARX_WARNING << "Could not commit data to exteroception segment in memory. Error message: "
+ << resultExteroception.allErrorMessages();
+ }
+
if (debugObserver)
{
auto end = std::chrono::high_resolution_clock::now();
@@ -151,6 +159,15 @@ namespace armarx::armem::server::robot_state::proprioception
up.instancesData = {data.proprioception};
}
+ // Exteroception
+ {
+ armem::EntityUpdate& up = update.exteroception.add();
+ up.entityID = properties.robotUnitProviderID.withEntityName(
+ properties.robotUnitProviderID.providerSegmentName);
+ up.timeCreated = data.timestamp;
+ up.instancesData = {data.exteroception};
+ }
+
// Extract odometry data.
const std::string platformKey = "platform";
if (data.proprioception->hasElement(platformKey))
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index f911d43fc..78863697a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -71,6 +71,7 @@ namespace armarx::armem::server::robot_state::proprioception
struct Update
{
armem::Commit proprioception;
+ armem::Commit exteroception;
std::vector<armem::robot_state::Transform> localization;
};
Update buildUpdate(const RobotUnitData& dataQueue);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
index f62d0b0c9..8d904f249 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
@@ -14,6 +14,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
Time timestamp;
aron::data::DictPtr proprioception;
+ aron::data::DictPtr exteroception;
};
using Queue = boost::sync_queue<RobotUnitData>;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index c74f355c2..e2107e09a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -24,8 +24,8 @@ namespace armarx::armem::server::robot_state::proprioception
const std::string& robotTypeName)
{
{
- converter = converterRegistry.get(robotTypeName);
- ARMARX_CHECK_NOT_NULL(converter)
+ converterProprioception = converterRegistry.get(robotTypeName);
+ ARMARX_CHECK_NOT_NULL(converterProprioception)
<< "No converter for robot type '" << robotTypeName << "' available. \n"
<< "Known are: " << converterRegistry.getKeys();
@@ -67,7 +67,7 @@ namespace armarx::armem::server::robot_state::proprioception
std::optional<RobotUnitData>
RobotUnitReader::fetchAndConvertLatestRobotUnitData()
{
- ARMARX_CHECK_NOT_NULL(converter);
+ ARMARX_CHECK_NOT_NULL(converterProprioception);
const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
if (not data.has_value())
@@ -79,7 +79,17 @@ namespace armarx::armem::server::robot_state::proprioception
auto start = std::chrono::high_resolution_clock::now();
RobotUnitData result;
- result.proprioception = converter->convert(data.value(), description);
+
+ if(converterProprioception != nullptr)
+ {
+ result.proprioception = converterProprioception->convert(data.value(), description);
+ }
+
+ if(converterExteroception != nullptr)
+ {
+ result.exteroception = converterExteroception->convert(data.value(), description);
+ }
+
result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
auto stop = std::chrono::high_resolution_clock::now();
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
index cc3cd2758..7dd485b90 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
@@ -15,7 +15,9 @@
#include "RobotUnitData.h"
#include "converters/ConverterInterface.h"
+#include "../exteroception/converters/ConverterInterface.h"
#include "converters/ConverterRegistry.h"
+#include "../exteroception/converters/ConverterRegistry.h"
namespace armarx::plugins
@@ -28,6 +30,8 @@ namespace armarx
using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
}
+
+// TODO move this out of proprioception. it is now for proprioception and exteroception
namespace armarx::armem::server::robot_state::proprioception
{
class RobotUnitReader : public armarx::Logging
@@ -69,7 +73,8 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitDataStreaming::DataStreamingDescription description;
ConverterRegistry converterRegistry;
- ConverterInterface* converter = nullptr;
+ proprioception::ConverterInterface* converterProprioception = nullptr;
+ exteroception::ConverterInterface* converterExteroception = nullptr;
armarx::SimpleRunningTask<>::pointer_type task = nullptr;
};
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.cpp
deleted file mode 100644
index a4ac7a8c3..000000000
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.cpp
+++ /dev/null
@@ -1,296 +0,0 @@
-#include "ArmarDEConverter.h"
-#include <cmath>
-#include <string>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/algorithm/advanced.h>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-
-#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
-#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
-
-#include "ConverterTools.h"
-
-
-namespace armarx::armem::server::robot_state::proprioception
-{
-
- ArmarDEConverter::ArmarDEConverter() :
- tools(std::make_unique<ConverterTools>())
- {
- }
-
-
- ArmarDEConverter::~ArmarDEConverter()
- {
- }
-
-
- aron::data::DictPtr
- ArmarDEConverter::convert(
- const RobotUnitDataStreaming::TimeStep& data,
- const RobotUnitDataStreaming::DataStreamingDescription& description)
- {
- arondto::Proprioception dto;
- dto.iterationID = data.iterationId;
-
- 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());
-
- if(tof.array.size() > 0 and isSquare)
- {
- tof.array.resize(sr, sr);
- }
- }
-
-
- return dto.toAron();
- }
-
-
- void ArmarDEConverter::process(
- arondto::Proprioception& 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 << "'";
-
- const std::string& category = split.at(0);
- const std::string& name = split.at(1);
- const std::string& field = split.at(2);
- ARMARX_CHECK_EQUAL(category, "sens") << category << " | " << entryName;
-
- if (name == "Platform")
- {
- // Platform
- processPlatformEntry(dto.platform, field, value);
- }
- else if (simox::alg::starts_with(name, "FT"))
- {
- // Force Torque
- processForceTorqueEntry(dto.forceTorque, split, value);
- }
- else if (simox::alg::contains(field, "tofDepth"))
- {
- // ARMARX_DEBUG << "Processing ToF sensor data";
- processToFEntry(dto.tof, split, value);
- }
- else
- {
- // Joint
- bool processed = processJointEntry(dto.joints, split, value);
- if (not processed)
- {
- // Fallback: Put in extra.
- const std::vector<std::string> comps{simox::alg::advanced(split.begin(), 1), split.end()};
- const std::string key = simox::alg::join(comps, ".");
-
- switch (value.entry.type)
- {
- case RobotUnitDataStreaming::NodeTypeFloat:
- dto.extraFloats[key] = getValueAs<float>(value);
- break;
- case RobotUnitDataStreaming::NodeTypeLong:
- dto.extraLongs[key] = getValueAs<long>(value);
- break;
- default:
- ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
- << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
- break;
- }
- }
- }
- }
-
-
-
- void ArmarDEConverter::processPlatformEntry(
- prop::arondto::Platform& dto,
- const std::string& fieldName,
- const ConverterValue& value)
- {
- if (findByPrefix(fieldName, tools->platformIgnored))
- {
- return;
- }
- else if (auto getter = findByPrefix(fieldName, tools->platformPoseGetters))
- {
- if (Eigen::Vector3f* dst = getter(dto))
- {
- if (auto setter = findBySuffix(fieldName, tools->vector3fSetters))
- {
- setter(*dst, getValueAs<float>(value));
- }
- }
- }
- else
- {
- // No setter for this field. Put in extra.
- dto.extra[fieldName] = getValueAs<float>(value);
- }
- }
-
-
- void ArmarDEConverter::processForceTorqueEntry(
- std::map<std::string, prop::arondto::ForceTorque>& fts,
- const std::vector<std::string>& split,
- const ConverterValue& value)
- {
- const std::string& name = split.at(1);
- std::vector<std::string> splitName = simox::alg::split(name, " ", false, false);
- ARMARX_CHECK_EQUAL(splitName.size(), 2);
- ARMARX_CHECK_EQUAL(splitName.at(0), "FT");
-
- auto it = tools->sidePrefixMap.find(splitName.at(1));
- ARMARX_CHECK(it != tools->sidePrefixMap.end()) << splitName.at(1);
-
- const std::string& side = it->second;
- processForceTorqueEntry(fts[side], split, value);
- }
-
- void ArmarDEConverter::processToFEntry(
- std::map<std::string, prop::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");
-
- 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;
- processToFEntry(tofs[side], split, value);
- }
-
- void ArmarDEConverter::processToFEntry(prop::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));
-
- if(tof.array.size() < (idx +1))
- {
- tof.array.resize(idx+1, 1);
- }
-
- tof.array(idx) = getValueAs<float>(value);
- }
-
-
- void ArmarDEConverter::processForceTorqueEntry(
- prop::arondto::ForceTorque& dto,
- const std::vector<std::string>& split,
- const ConverterValue& value)
- {
- const std::string& fieldName = split.at(2);
- if (auto getter = findByPrefix(fieldName, tools->ftGetters))
- {
- if (Eigen::Vector3f* dst = getter(dto))
- {
- if (auto setter = findBySuffix(fieldName, tools->vector3fSetters))
- {
- setter(*dst, getValueAs<float>(value));
- }
- }
- }
- else
- {
- // No setter for this field. Put in extra.
- std::string key = split.size() == 4
- ? (fieldName + "." + split.at(3))
- : fieldName;
-
- switch (value.entry.type)
- {
- case RobotUnitDataStreaming::NodeTypeFloat:
- dto.extra[key] = getValueAs<float>(value);
- break;
- case RobotUnitDataStreaming::NodeTypeInt:
- dto.extra[key] = getValueAs<int>(value);
- break;
- case RobotUnitDataStreaming::NodeTypeLong:
- dto.extra[key] = getValueAs<long>(value);
- break;
- default:
- ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
- << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
- break;
- }
- }
- }
-
-
- bool ArmarDEConverter::processJointEntry(
- prop::arondto::Joints& dto,
- const std::vector<std::string>& split,
- const ConverterValue& value)
- {
- const std::string& jointName = split.at(1);
- const std::string& fieldName = split.at(2);
- if (false)
- {
- // Only in simulation.
- if (auto getter = findByPrefix(fieldName, tools->jointGetters))
- {
- if (std::map<std::string, float>* map = getter(dto))
- {
- (*map)[jointName] = getValueAs<float>(value);
- }
- }
- }
-
- const std::string tempSuffix = "Temperature";
- if (simox::alg::ends_with(split.at(2), tempSuffix))
- {
- // Handle "dieTemperature" etc
- const std::string name = split.at(2).substr(0, split.at(2).size() - tempSuffix.size());
- dto.temperature[split.at(1)][name] = getValueAs<float>(value);
- return true;
- }
- else if (auto it = tools->jointSetters.find(fieldName); it != tools->jointSetters.end())
- {
- const ConverterTools::JointSetter& setter = it->second;
- setter(dto, split, value);
- return true;
- }
- else
- {
- // ARMARX_DEBUG << "Ignoring unhandled field: '" << simox::alg::join(split, ".") << "'";
- return false;
- }
- }
-
-}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.h
deleted file mode 100644
index 6f5c28715..000000000
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ArmarDEConverter.h
+++ /dev/null
@@ -1,78 +0,0 @@
-#pragma once
-
-#include <map>
-#include <string>
-
-#include <Eigen/Core>
-
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
-
-#include "ConverterInterface.h"
-
-
-
-namespace armarx::armem::server::robot_state::proprioception
-{
- struct ConverterValue;
- class ConverterTools;
-
-
- class ArmarDEConverter : public ConverterInterface
- {
- public:
-
- ArmarDEConverter();
- virtual ~ArmarDEConverter() override;
-
-
- aron::data::DictPtr
- convert(
- const RobotUnitDataStreaming::TimeStep& data,
- const RobotUnitDataStreaming::DataStreamingDescription& description) override;
-
-
- protected:
-
- void process(arondto::Proprioception& dto, const std::string& entryName, const ConverterValue& value);
-
-
-
- private:
-
- void processPlatformEntry(
- prop::arondto::Platform& dto,
- const std::string& fieldName,
- const ConverterValue& value);
-
- void processForceTorqueEntry(
- std::map<std::string, prop::arondto::ForceTorque>& fts,
- const std::vector<std::string>& split,
- const ConverterValue& value);
-
- void processForceTorqueEntry(
- prop::arondto::ForceTorque& ft,
- const std::vector<std::string>& split,
- const ConverterValue& value);
-
- void processToFEntry(
- std::map<std::string, prop::arondto::ToF>& fts,
- const std::vector<std::string>& split,
- const ConverterValue& value);
-
- void processToFEntry(
- prop::arondto::ToF& tof,
- const std::vector<std::string>& split,
- const ConverterValue& value);
-
- bool processJointEntry(
- prop::arondto::Joints& dto,
- const std::vector<std::string>& split,
- const ConverterValue& value);
-
-
- private:
-
- std::unique_ptr<ConverterTools> tools;
-
- };
-}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
index 48ea15375..0c6baa3b7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
@@ -1,7 +1,6 @@
#include "ConverterRegistry.h"
#include "Armar6Converter.h"
-#include "ArmarDEConverter.h"
#include <SimoxUtility/algorithm/get_map_keys_values.h>
@@ -12,7 +11,7 @@ namespace armarx::armem::server::robot_state::proprioception
ConverterRegistry::ConverterRegistry()
{
add<Armar6Converter>("Armar6");
- add<ArmarDEConverter>("ArmarDE");
+ add<Armar6Converter>("ArmarDE");
add<Armar6Converter>("Armar7");
}
--
GitLab