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

workign draft: TransformHelper (need new queries ...)

parent 2c0aaac2
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+ 129
− 109
View file @ 86a6af0c
......@@ -19,38 +19,38 @@
namespace armarx::armem::common::robot_state::localization
{
TransformResult TransformHelper::lookupTransform(const armem::wm::CoreSegment &localizationCoreSegment,
const TransformQuery &query)
TransformResult TransformHelper::lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
const TransformQuery& query)
{
const std::vector<std::string> tfChain =
buildTransformChain(localizationCoreSegment, query);
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 +
"'"};
}
{
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);
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."};
}
{
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<>());
transforms.end(),
Eigen::Affine3f::Identity(),
std::multiplies<>());
ARMARX_DEBUG << "Found valid transform";
......@@ -59,79 +59,82 @@ namespace armarx::armem::common::robot_state::localization
}
std::vector<std::string>
TransformHelper::buildTransformChain(const armem::wm::CoreSegment &localizationCoreSegment,
const TransformQuery &query)
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) {
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);
const auto& agentProviderSegment =
localizationCoreSegment.getProviderSegment(query.header.agent);
auto addToChain = [&](const std::string &parentFrame, const std::string &frame) {
auto addToChain = [&](const std::string & parentFrame, const std::string & frame)
{
const auto entityName = join(parentFrame, frame);
if (agentProviderSegment.hasEntity(entityName))
{
chain.push_back(entityName);
}
{
chain.push_back(entityName);
}
else
{
ARMARX_WARNING << deactivateSpam(1) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
<< "'";
}
{
ARMARX_WARNING << deactivateSpam(1) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
<< "'";
}
};
std::array<std::string, 3> knownChain{
GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
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);
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 {};
}
{
ARMARX_WARNING << "Parent frame '" << query.header.parentFrame << "' unknown";
return {};
}
if (frameEndIt == knownChain.end())
{
ARMARX_DEBUG << "Frame '" << query.header.frame << "' must be robot frame";
}
{
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));
}
{
addToChain(*frameBeginIt, *(frameBeginIt + 1));
}
if (frameEndIt == knownChain.end())
{
addToChain(knownChain.back(), query.header.frame);
}
{
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 {};
}
{
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)
convertEntityToTransform(const armem::wm::EntityInstance& item)
{
arondto::Transform aronTransform;
aronTransform.fromAron(item.data());
......@@ -143,20 +146,33 @@ namespace armarx::armem::common::robot_state::localization
}
auto
findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform> &transforms,
findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
const int64_t timestamp)
{
const auto comp = [](const int64_t& timestamp, const auto & transform)
{
return transform.header.timestamp < timestamp;
};
const auto it = std::upper_bound(transforms.begin(), transforms.end(), timestamp, comp);
auto timestampBeyond =
[timestamp](const ::armarx::armem::robot_state::Transform &transform) {
return transform.header.timestamp > timestamp;
};
[timestamp](const ::armarx::armem::robot_state::Transform & transform)
{
return transform.header.timestamp > timestamp;
};
const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
ARMARX_WARNING << "Fooasdfasdjfh";
ARMARX_CHECK(it == poseNextIt);
return poseNextIt;
}
Eigen::Affine3f
interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform> &queue,
interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform>& queue,
int64_t timestamp)
{
ARMARX_TRACE;
......@@ -201,12 +217,12 @@ namespace armarx::armem::common::robot_state::localization
}
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)
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);
const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(agent);
ARMARX_DEBUG << "Provider segments"
<< simox::alg::get_keys(localizationCoreSegment.providerSegments());
......@@ -214,26 +230,27 @@ namespace armarx::armem::common::robot_state::localization
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)
{
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 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.";
......@@ -241,16 +258,16 @@ namespace armarx::armem::common::robot_state::localization
}
Eigen::Affine3f
TransformHelper::obtainTransform(const std::string &entityName,
const armem::wm::ProviderSegment &agentProviderSegment,
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);
const auto& entity = agentProviderSegment.getEntity(entityName);
ARMARX_DEBUG << "History (size: " << entity.history().size() << ")"
<< simox::alg::get_keys(entity.history());
<< simox::alg::get_keys(entity.history());
// if (entity.history.empty())
// {
......@@ -264,31 +281,34 @@ namespace armarx::armem::common::robot_state::localization
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)); });
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;
{
// 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;
}
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";
{
ARMARX_DEBUG << "empty transform";
throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
}
throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
}
ARMARX_DEBUG << "single transform";
......
source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+ 1
− 1
View file @ 86a6af0c
......@@ -182,7 +182,7 @@ namespace armarx::armem::server::robot_state
{
{
// std::scoped_lock lock(visuMutex);
ARMARX_INFO << "Update task";
ARMARX_DEBUG << "Update task";
if (p.enabled)
{
......
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