diff --git a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
index 7f25517703685f36cfa7cf992a27252f1bd54135..e6a42e3a28b9db59d4dc24bf4dd34e5ee2074450 100644
--- a/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
@@ -22,6 +22,9 @@ armarx_add_library(
aroncommon
HEADERS
+ ./common/localization/types.h
+ ./common/localization/TransformHelper.h
+
./client/localization/interfaces.h
./client/localization/TransformReader.h
./client/localization/TransformWriter.h
@@ -39,6 +42,9 @@ armarx_add_library(
./aron_conversions.h
SOURCES
+ ./common/localization/TransformHelper.cpp
+
+
./client/localization/TransformReader.cpp
./client/localization/TransformWriter.cpp
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 727c954dbce9679de1ae81343ce9bba4a5c6bb8b..1c9289135d6a654f8fbf24b1bd401b242d855e2d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -56,6 +56,8 @@
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
+
namespace armarx::armem::client::robot_state::localization
{
@@ -128,254 +130,9 @@ namespace armarx::armem::client::robot_state::localization
// }
- std::vector<std::string> TransformReader::buildTransformChain(const armem::wm::Memory& memory,
- const TransformQuery& query) const
- {
- ARMARX_DEBUG << "Building transform chain";
-
- auto join = [](const std::string & parentFrame, const std::string & frame)
- {
- return parentFrame + "," + frame;
- };
-
- std::vector<std::string> chain;
-
- const auto& agentProviderSegment = memory.getCoreSegment(properties.localizationSegment)
- .getProviderSegment(query.header.agent);
-
- auto addToChain = [&](const std::string & parentFrame, const std::string & frame)
- {
- const auto entityName = join(parentFrame, frame);
-
- if (agentProviderSegment.hasEntity(entityName))
- {
- chain.push_back(entityName);
- }
- else
- {
- ARMARX_WARNING << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
- << "'";
- }
- };
-
- std::array<std::string, 3> knownChain
- {
- GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
-
- auto* frameBeginIt =
- std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
- auto* const frameEndIt =
- std::find(knownChain.begin(), knownChain.end(), query.header.frame);
-
- if (frameBeginIt == knownChain.end())
- {
- ARMARX_WARNING << "Parent frame '" << query.header.parentFrame << "' unknown";
- return {};
- }
-
- if (frameEndIt == knownChain.end())
- {
- ARMARX_DEBUG << "Frame '" << query.header.frame << "' must be robot frame";
- }
-
- const size_t nFrames = std::distance(frameBeginIt, frameEndIt);
- ARMARX_DEBUG << "Lookup '" << query.header.parentFrame << " -> " << query.header.frame
- << "' across " << nFrames << " frames";
-
- for (; frameBeginIt != knownChain.end() - 1; frameBeginIt++)
- {
- addToChain(*frameBeginIt, *(frameBeginIt + 1));
- }
-
- if (frameEndIt == knownChain.end())
- {
- addToChain(knownChain.back(), query.header.frame);
- }
-
- if (chain.empty())
- {
- ARMARX_WARNING << "Cannot create tf lookup chain '" << query.header.parentFrame
- << " -> " << query.header.frame << "'";
- return {};
- }
-
- return chain;
- }
-
- inline ::armarx::armem::robot_state::Transform convertEntityToTransform(const armem::wm::EntityInstance& item)
- {
- arondto::Transform aronTransform;
- aronTransform.fromAron(item.data());
-
- ::armarx::armem::robot_state::Transform transform;
- fromAron(aronTransform, transform);
-
- return transform;
- }
-
- auto findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
- const int64_t timestamp)
- {
- auto timestampBeyond = [timestamp](const ::armarx::armem::robot_state::Transform & transform)
- {
- return transform.header.timestamp > timestamp;
- };
-
- const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
- return poseNextIt;
- }
-
- Eigen::Affine3f interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform>& queue, int64_t timestamp)
- {
- ARMARX_TRACE;
-
- ARMARX_DEBUG << "Entering";
-
-
- ARMARX_CHECK(not queue.empty())
- << "The queue has to contain at least two items to perform a lookup";
-
-
- ARMARX_DEBUG << "Entering ... "
- << "Q front " << queue.front().header.timestamp << " "
- << "Q back " << queue.back().header.timestamp << " "
- << "query timestamp " << timestamp;
-
- // TODO(fabian.reister): sort queue.
-
-
- ARMARX_CHECK(queue.back().header.timestamp > timestamp)
- << "Cannot perform lookup into the future!";
-
- // ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
- ARMARX_CHECK(queue.front().header.timestamp < timestamp)
- << "Cannot perform lookup. Timestamp too old";
- // => now we know that there is an element right after and before the timestamp within our queue
-
- ARMARX_DEBUG << "Entering 2";
-
- const auto poseNextIt = findFirstElementAfter(queue, timestamp);
-
- ARMARX_DEBUG << "it ari";
-
- const auto posePreIt = poseNextIt - 1;
-
- ARMARX_DEBUG << "deref";
-
- // the time fraction [0..1] of the lookup wrt to posePre and poseNext
- const float t = static_cast<float>(timestamp - posePreIt->header.timestamp) /
- (poseNextIt->header.timestamp - posePreIt->header.timestamp);
-
- ARMARX_DEBUG << "interpolate";
-
- return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, t);
- }
-
- Eigen::Affine3f TransformReader::obtainTransform(const std::string& entityName, const armem::wm::ProviderSegment& agentProviderSegment, const int64_t timestamp) const
- {
-
- ARMARX_DEBUG << "getEntity:" + entityName;
- const auto& entity = agentProviderSegment.getEntity(entityName);
-
- ARMARX_DEBUG << "History (size: " << entity.history().size() << ")"
- << simox::alg::get_keys(entity.history());
-
- // if (entity.history.empty())
- // {
- // // TODO(fabian.reister): fixme boom
- // ARMARX_ERROR << "No snapshots received.";
- // return Eigen::Affine3f::Identity();
- // }
-
-
- std::vector<::armarx::armem::robot_state::Transform> transforms;
- transforms.reserve(entity.history().size());
-
- const auto entitySnapshots = simox::alg::get_values(entity.history());
- std::transform(entitySnapshots.begin(),
- entitySnapshots.end(),
- std::back_inserter(transforms),
- [](const auto & entity)
- {
- return convertEntityToTransform(entity.getInstance(0));
- });
-
- ARMARX_DEBUG << "obtaining transform";
-
- if (transforms.size() > 1)
- {
- // TODO(fabian.reister): remove
- return transforms.front().transform;
-
- ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
- const auto p = interpolateTransform(transforms, timestamp);
- ARMARX_DEBUG << "Done interpolating transform";
- return p;
- }
-
-
- // accept this to fail (will raise armem::error::MissingEntry)
- if (transforms.empty())
- {
- ARMARX_DEBUG << "empty transform";
-
- throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
- }
-
- ARMARX_DEBUG << "single transform";
-
-
- return transforms.front().transform;
- }
-
- std::vector<Eigen::Affine3f>
- TransformReader::obtainTransforms(const armem::wm::Memory& memory,
- const std::vector<std::string>& tfChain,
- const std::string& agent,
- const std::int64_t& timestamp) const
- {
-
- ARMARX_DEBUG << "Core segments" << simox::alg::get_keys(memory.coreSegments());
-
- const auto& agentProviderSegment =
- memory.getCoreSegment(properties.localizationSegment).getProviderSegment(agent);
-
- ARMARX_DEBUG << "Provider segments"
- << simox::alg::get_keys(
- memory.getCoreSegment(properties.localizationSegment)
- .providerSegments());
-
- ARMARX_DEBUG << "Entities: " << simox::alg::get_keys(agentProviderSegment.entities());
-
- try
- {
- std::vector<Eigen::Affine3f> transforms;
- transforms.reserve(tfChain.size());
- std::transform(tfChain.begin(),
- tfChain.end(),
- std::back_inserter(transforms),
- [&](const std::string & entityName)
- {
- return obtainTransform(entityName, agentProviderSegment, timestamp);
- });
- return transforms;
- }
- catch (const armem::error::MissingEntry& missingEntryError)
- {
- ARMARX_WARNING << missingEntryError.what();
- }
- catch (const ::armarx::exceptions::local::ExpressionException& ex)
- {
- ARMARX_WARNING << "local exception: " << ex.what();
- }
-
- return {};
-
- }
TransformResult TransformReader::lookupTransform(const TransformQuery& query) const
{
-
const auto timestamp = IceUtil::Time::microSeconds(query.header.timestamp);
const auto durationEpsilon = IceUtil::Time::milliSeconds(100);
@@ -390,7 +147,7 @@ namespace armarx::armem::client::robot_state::localization
.coreSegments().withName(properties.localizationSegment)
.providerSegments().withName(query.header.agent) // agent
.entities().all() // parentFrame,frame
- .snapshots().timeRange(timestamp - durationEpsilon, timestamp + durationEpsilon);
+ .snapshots().latest(); //timeRange(timestamp - durationEpsilon, timestamp + durationEpsilon);
// clang-format on
// TODO(fabian.reister): remove latest() and add atTime
@@ -411,39 +168,10 @@ namespace armarx::armem::client::robot_state::localization
query.header.frame + "' : " + qResult.errorMessage};
}
- const std::vector<std::string> tfChain = buildTransformChain(qResult.memory, query);
-
- if (tfChain.empty())
- {
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Cannot create tf lookup chain '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "'"};
- }
-
- const std::vector<Eigen::Affine3f> transforms = obtainTransforms(
- qResult.memory, tfChain, query.header.agent, timestamp.toMicroSeconds());
-
- if (transforms.empty())
- {
- ARMARX_WARNING << "No transform available.";
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF loookup: '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "'. No memory data in time range."};
- }
-
- const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
- transforms.end(),
- Eigen::Affine3f::Identity(),
- std::multiplies<>());
-
- ARMARX_DEBUG << "Found valid transform";
+ const auto& localizationCoreSegment = qResult.memory.getCoreSegment(properties.localizationSegment);
- return {.transform = {.header = query.header, .transform = transform},
- .status = TransformResult::Status::Success};
+ using armarx::armem::common::robot_state::localization::TransformHelper;
+ return TransformHelper::lookupTransform(localizationCoreSegment, query);
}
} // namespace armarx::armem::client::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index cd577b6f07e205f3e97d609aea0066fe4ab3d2b1..53d3ad522db60c0cb6150e7bc90b6bb3174f0cbe 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -43,7 +43,6 @@ namespace armarx::armem::client::robot_state::localization
*/
class TransformReader :
virtual public TransformReaderInterface
- // virtual public ::armarx::armem::MemoryConnector
{
public:
TransformReader(armem::ClientReaderComponentPluginUser& memoryClient);
@@ -60,25 +59,7 @@ namespace armarx::armem::client::robot_state::localization
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
- // const std::string& getPropertyPrefix() const override
- // {
- // return propertyPrefix;
- // }
-
private:
- std::vector<std::string> buildTransformChain(const armem::wm::Memory& memory,
- const TransformQuery& query) const;
-
- std::vector<Eigen::Affine3f> obtainTransforms(const armem::wm::Memory& memory,
- const std::vector<std::string>& tfChain,
- const std::string& agent, const std::int64_t& timestamp) const;
-
- Eigen::Affine3f obtainTransform(
- const std::string& entityName,
- const armem::wm::ProviderSegment& agentProviderSegment,
- int64_t timestamp)
- const;
-
armem::client::Reader memoryReader;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
index a1989d6f6c3289db0f731cebcfede34556153821..e8afaf3e4d853da674ade877200a4de1b672bf84 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
@@ -26,61 +26,43 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::client::robot_state::localization
{
- struct TransformResult
- {
- ::armarx::armem::robot_state::Transform transform;
-
- enum class Status
- {
- Success,
- ErrorLookupIntoFuture,
- ErrorFrameNotAvailable
- } status;
-
- explicit operator bool() const { return status == Status::Success; }
-
- std::string errorMessage = "";
- };
-
- struct TransformQuery
- {
- ::armarx::armem::robot_state::TransformHeader header;
- // bool exact;
- };
+ using armarx::armem::common::robot_state::localization::TransformQuery;
+ using armarx::armem::common::robot_state::localization::TransformResult;
class TransformInterface
{
- public:
+ public:
virtual ~TransformInterface() = default;
- virtual void registerPropertyDefinitions(PropertyDefinitionsPtr &def) = 0;
+ virtual void registerPropertyDefinitions(PropertyDefinitionsPtr& def) = 0;
virtual void connect() = 0;
};
class TransformReaderInterface : virtual public TransformInterface
{
- public:
+ public:
virtual ~TransformReaderInterface() = default;
- virtual TransformResult getGlobalPose(const std::string &agentName,
- const std::string &robotRootFrame,
- const std::int64_t ×tamp) const = 0;
+ virtual TransformResult getGlobalPose(const std::string& agentName,
+ const std::string& robotRootFrame,
+ const std::int64_t& timestamp) const = 0;
- virtual TransformResult lookupTransform(const TransformQuery &query) const = 0;
+ virtual TransformResult lookupTransform(const TransformQuery& query) const = 0;
// waitForTransform()
};
class TransformWriterInterface : virtual public TransformInterface
{
- public:
+ public:
~TransformWriterInterface() override = default;
- virtual bool commitTransform(const ::armarx::armem::robot_state::Transform &transform) = 0;
+ virtual bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) = 0;
};
} // namespace armarx::armem::client::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bc4f7e897e05ec7d945158de62c3bcba4f11d603
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -0,0 +1,297 @@
+#include "TransformHelper.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <SimoxUtility/color/cmaps.h>
+#include <SimoxUtility/math/pose/interpolate.h>
+
+#include "RobotAPI/libraries/core/FramedPose.h"
+
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include "RobotAPI/libraries/armem/core/error/ArMemError.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/Memory.h"
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+
+namespace armarx::armem::common::robot_state::localization
+{
+
+ TransformResult TransformHelper::lookupTransform(const armem::wm::CoreSegment &localizationCoreSegment,
+ const TransformQuery &query)
+ {
+ const std::vector<std::string> tfChain =
+ buildTransformChain(localizationCoreSegment, query);
+
+ if (tfChain.empty())
+ {
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" +
+ query.header.parentFrame + " -> " + query.header.frame +
+ "'"};
+ }
+
+ const std::vector<Eigen::Affine3f> transforms = obtainTransforms(
+ localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+
+ if (transforms.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
+ " -> " + query.header.frame +
+ "'. No memory data in time range."};
+ }
+
+ const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
+ transforms.end(),
+ Eigen::Affine3f::Identity(),
+ std::multiplies<>());
+
+ ARMARX_DEBUG << "Found valid transform";
+
+ return {.transform = {.header = query.header, .transform = transform},
+ .status = TransformResult::Status::Success};
+ }
+
+ std::vector<std::string>
+ TransformHelper::buildTransformChain(const armem::wm::CoreSegment &localizationCoreSegment,
+ const TransformQuery &query)
+ {
+ ARMARX_DEBUG << "Building transform chain";
+
+ auto join = [](const std::string &parentFrame, const std::string &frame) {
+ return parentFrame + "," + frame;
+ };
+
+ std::vector<std::string> chain;
+
+ const auto &agentProviderSegment =
+ localizationCoreSegment.getProviderSegment(query.header.agent);
+
+ auto addToChain = [&](const std::string &parentFrame, const std::string &frame) {
+ const auto entityName = join(parentFrame, frame);
+
+ if (agentProviderSegment.hasEntity(entityName))
+ {
+ chain.push_back(entityName);
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
+ << "'";
+ }
+ };
+
+ std::array<std::string, 3> knownChain{
+ GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
+
+ auto *frameBeginIt =
+ std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
+ auto *const frameEndIt =
+ std::find(knownChain.begin(), knownChain.end(), query.header.frame);
+
+ if (frameBeginIt == knownChain.end())
+ {
+ ARMARX_WARNING << "Parent frame '" << query.header.parentFrame << "' unknown";
+ return {};
+ }
+
+ if (frameEndIt == knownChain.end())
+ {
+ ARMARX_DEBUG << "Frame '" << query.header.frame << "' must be robot frame";
+ }
+
+ const size_t nFrames = std::distance(frameBeginIt, frameEndIt);
+ ARMARX_DEBUG << "Lookup '" << query.header.parentFrame << " -> " << query.header.frame
+ << "' across " << nFrames << " frames";
+
+ for (; frameBeginIt != knownChain.end() - 1; frameBeginIt++)
+ {
+ addToChain(*frameBeginIt, *(frameBeginIt + 1));
+ }
+
+ if (frameEndIt == knownChain.end())
+ {
+ addToChain(knownChain.back(), query.header.frame);
+ }
+
+ if (chain.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Cannot create tf lookup chain '" << query.header.parentFrame
+ << " -> " << query.header.frame << "'";
+ return {};
+ }
+
+ return chain;
+ }
+
+ inline ::armarx::armem::robot_state::Transform
+ convertEntityToTransform(const armem::wm::EntityInstance &item)
+ {
+ arondto::Transform aronTransform;
+ aronTransform.fromAron(item.data());
+
+ ::armarx::armem::robot_state::Transform transform;
+ fromAron(aronTransform, transform);
+
+ return transform;
+ }
+
+ auto
+ findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform> &transforms,
+ const int64_t timestamp)
+ {
+ auto timestampBeyond =
+ [timestamp](const ::armarx::armem::robot_state::Transform &transform) {
+ return transform.header.timestamp > timestamp;
+ };
+
+ const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
+ return poseNextIt;
+ }
+
+ Eigen::Affine3f
+ interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform> &queue,
+ int64_t timestamp)
+ {
+ ARMARX_TRACE;
+
+ ARMARX_DEBUG << "Entering";
+
+ ARMARX_CHECK(not queue.empty())
+ << "The queue has to contain at least two items to perform a lookup";
+
+ ARMARX_DEBUG << "Entering ... "
+ << "Q front " << queue.front().header.timestamp << " "
+ << "Q back " << queue.back().header.timestamp << " "
+ << "query timestamp " << timestamp;
+
+ // TODO(fabian.reister): sort queue.
+
+ ARMARX_CHECK(queue.back().header.timestamp > timestamp)
+ << "Cannot perform lookup into the future!";
+
+ // ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
+ ARMARX_CHECK(queue.front().header.timestamp < timestamp)
+ << "Cannot perform lookup. Timestamp too old";
+ // => now we know that there is an element right after and before the timestamp within our queue
+
+ ARMARX_DEBUG << "Entering 2";
+
+ const auto poseNextIt = findFirstElementAfter(queue, timestamp);
+
+ ARMARX_DEBUG << "it ari";
+
+ const auto posePreIt = poseNextIt - 1;
+
+ ARMARX_DEBUG << "deref";
+
+ // the time fraction [0..1] of the lookup wrt to posePre and poseNext
+ const float t = static_cast<float>(timestamp - posePreIt->header.timestamp) /
+ (poseNextIt->header.timestamp - posePreIt->header.timestamp);
+
+ ARMARX_DEBUG << "interpolate";
+
+ return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, t);
+ }
+
+ std::vector<Eigen::Affine3f>
+ TransformHelper::obtainTransforms(const armem::wm::CoreSegment &localizationCoreSegment,
+ const std::vector<std::string> &tfChain,
+ const std::string &agent,
+ const std::int64_t ×tamp)
+ {
+ const auto &agentProviderSegment = localizationCoreSegment.getProviderSegment(agent);
+
+ ARMARX_DEBUG << "Provider segments"
+ << simox::alg::get_keys(localizationCoreSegment.providerSegments());
+
+ ARMARX_DEBUG << "Entities: " << simox::alg::get_keys(agentProviderSegment.entities());
+
+ try
+ {
+ std::vector<Eigen::Affine3f> transforms;
+ transforms.reserve(tfChain.size());
+ std::transform(tfChain.begin(),
+ tfChain.end(),
+ std::back_inserter(transforms),
+ [&](const std::string &entityName) {
+ return obtainTransform(
+ entityName, agentProviderSegment, timestamp);
+ });
+ return transforms;
+ }
+ catch (const armem::error::MissingEntry &missingEntryError)
+ {
+ ARMARX_WARNING << missingEntryError.what();
+ }
+ catch (const ::armarx::exceptions::local::ExpressionException &ex)
+ {
+ ARMARX_WARNING << "local exception: " << ex.what();
+ }
+
+ ARMARX_WARNING << "Unkown error.";
+
+ return {};
+ }
+
+ Eigen::Affine3f
+ TransformHelper::obtainTransform(const std::string &entityName,
+ const armem::wm::ProviderSegment &agentProviderSegment,
+ const int64_t timestamp)
+ {
+
+ ARMARX_DEBUG << "getEntity:" + entityName;
+ const auto &entity = agentProviderSegment.getEntity(entityName);
+
+ ARMARX_DEBUG << "History (size: " << entity.history().size() << ")"
+ << simox::alg::get_keys(entity.history());
+
+ // if (entity.history.empty())
+ // {
+ // // TODO(fabian.reister): fixme boom
+ // ARMARX_ERROR << "No snapshots received.";
+ // return Eigen::Affine3f::Identity();
+ // }
+
+ std::vector<::armarx::armem::robot_state::Transform> transforms;
+ transforms.reserve(entity.history().size());
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ std::transform(
+ entitySnapshots.begin(),
+ entitySnapshots.end(),
+ std::back_inserter(transforms),
+ [](const auto &entity) { return convertEntityToTransform(entity.getInstance(0)); });
+
+ ARMARX_DEBUG << "obtaining transform";
+
+ if (transforms.size() > 1)
+ {
+ // TODO(fabian.reister): remove
+ return transforms.front().transform;
+
+ ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
+ const auto p = interpolateTransform(transforms, timestamp);
+ ARMARX_DEBUG << "Done interpolating transform";
+ return p;
+ }
+
+ // accept this to fail (will raise armem::error::MissingEntry)
+ if (transforms.empty())
+ {
+ ARMARX_DEBUG << "empty transform";
+
+ throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
+ }
+
+ ARMARX_DEBUG << "single transform";
+
+ return transforms.front().transform;
+ }
+} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..7505dd3859be919000e5bb80b1c9b6aaddb7ddd4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -0,0 +1,65 @@
+/*
+ * 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 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
+
+namespace armarx::armem::wm
+{
+ class CoreSegment;
+ class ProviderSegment;
+} // namespace armarx::armem::wm
+
+namespace armarx::armem::common::robot_state::localization
+{
+ using armarx::armem::common::robot_state::localization::TransformQuery;
+ using armarx::armem::common::robot_state::localization::TransformResult;
+
+ class TransformHelper
+ {
+ public:
+ static TransformResult
+ lookupTransform(const armem::wm::CoreSegment &localizationCoreSegment,
+ const TransformQuery &query);
+
+ private:
+ static std::vector<std::string>
+ buildTransformChain(const armem::wm::CoreSegment &localizationCoreSegment,
+ const TransformQuery &query);
+
+ static std::vector<Eigen::Affine3f>
+ obtainTransforms(const armem::wm::CoreSegment &localizationCoreSegment,
+ const std::vector<std::string> &tfChain,
+ const std::string &agent,
+ const std::int64_t ×tamp);
+
+ static Eigen::Affine3f
+ obtainTransform(const std::string &entityName,
+ const armem::wm::ProviderSegment &agentProviderSegment,
+ int64_t timestamp);
+ };
+} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..207660c68325fee922a7497ba771c014d597ef71
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
@@ -0,0 +1,56 @@
+/*
+ * 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/>.
+ *
+ * @package RobotAPI::ArmarXObjects::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Geometry>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
+namespace armarx::armem::common::robot_state::localization
+{
+ struct TransformResult
+ {
+ ::armarx::armem::robot_state::Transform transform;
+
+ enum class Status
+ {
+ Success,
+ ErrorLookupIntoFuture,
+ ErrorFrameNotAvailable
+ } status;
+
+ explicit operator bool() const { return status == Status::Success; }
+
+ std::string errorMessage = "";
+ };
+
+ struct TransformQuery
+ {
+ ::armarx::armem::robot_state::TransformHeader header;
+
+ // bool exact;
+ };
+
+} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file