Skip to content
Snippets Groups Projects
Commit c9f3ff9c authored by Fabian Reister's avatar Fabian Reister
Browse files

new class TransformHelper to unify client and server queries

parent 86a8df33
No related branches found
No related tags found
2 merge requests!157armem/dev => master,!144robot state memory update
Hide whitespace changes
Inline Side-by-side
Showing
with 442 additions and 327 deletions
source/RobotAPI/libraries/armem_robot_state/CMakeLists.txt
+ 6
− 0
View file @ c9f3ff9c
......@@ -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
......
source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+ 6
− 278
View file @ c9f3ff9c
......@@ -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
source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+ 0
− 19
View file @ c9f3ff9c
......@@ -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;
......
source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
+ 12
− 30
View file @ c9f3ff9c
......@@ -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 &timestamp) 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
source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp 0 → 100644
+ 297
− 0
View file @ c9f3ff9c
#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 &timestamp)
{
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
source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h 0 → 100644
+ 65
− 0
View file @ c9f3ff9c
/*
* 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 &timestamp);
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
source/RobotAPI/libraries/armem_robot_state/common/localization/types.h 0 → 100644
+ 56
− 0
View file @ c9f3ff9c
/*
* 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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment