TransformHelper.cpp

Go to the documentation of this file.

#include "TransformHelper.h"
 
#include <optional>
#include <string>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/math/pose/interpolate.h>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/DateTime.h"
#include "ArmarXCore/core/time/Duration.h"
#include <ArmarXCore/core/exceptions/LocalException.h>
 
#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx::armem::robot_state::localization
{
 
    template <class... Args>
    TransformResult
    TransformHelper::_lookupTransform(
        const armem::base::CoreSegmentBase<Args...>& 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 + "' for robot `" +
                                    query.header.agent + "`."};
        }
 
        const std::vector<Eigen::Isometry3f> transforms = _obtainTransforms(
            localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
 
        const std::optional<armem::Time> sanitizedTimestamp =
            _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
 
        if (not sanitizedTimestamp.has_value())
        {
            return {.transform = {.header = query.header},
                    .status = TransformResult::Status::Error,
                    .errorMessage = "Error: Issue with timestamp"};
        }
 
 
        auto header = query.header;
 
        ARMARX_CHECK(sanitizedTimestamp.has_value());
 
        // ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
 
        header.timestamp = sanitizedTimestamp.value();
 
        if (transforms.empty())
        {
            ARMARX_INFO << deactivateSpam(1) << "No transform available.";
            return {.transform = {.header = query.header},
                    .status = TransformResult::Status::ErrorFrameNotAvailable,
                    .errorMessage = "Error in TF lookup:  '" + query.header.parentFrame +
                                    " -> " + query.header.frame +
                                    "'. No memory data in time range. Reference time " + sanitizedTimestamp.value().toTimeString()};
        }
 
        const Eigen::Isometry3f transform = std::accumulate(
            transforms.begin(), transforms.end(), Eigen::Isometry3f::Identity(), std::multiplies<>());
 
        ARMARX_DEBUG << "Found valid transform";
 
        return {.transform = {.header = header, .transform = transform},
                .status = TransformResult::Status::Success};
    }
 
    template <class... Args>
    TransformChainResult
    TransformHelper::_lookupTransformChain(
        const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
        const TransformQuery& query)
    {
        const std::vector<std::string> tfChain =
            _buildTransformChain(localizationCoreSegment, query);
        if (tfChain.empty())
        {
            ARMARX_DEBUG << "TF chain is empty";
            return {.header = query.header,
                    .transforms = std::vector<Eigen::Isometry3f>{},
                    .status = TransformChainResult::Status::ErrorFrameNotAvailable,
                    .errorMessage = "Cannot create tf lookup chain '" + query.header.parentFrame +
                                    " -> " + query.header.frame + "' for robot `" +
                                    query.header.agent + "`."};
        }
 
        const std::vector<Eigen::Isometry3f> transforms = _obtainTransforms(
            localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
        if (transforms.empty())
        {
            ARMARX_INFO << deactivateSpam(1) << "No transform available.";
            return
            {
                .header = query.header,
                .transforms = {},
                .status = TransformChainResult::Status::ErrorFrameNotAvailable,
                .errorMessage = "Error in TF lookup:  '" + query.header.parentFrame + " -> " +
                                 query.header.frame + "'. No memory data in time range. Reference time " + query.header.timestamp.toTimeString()
            };
        }
 
 
        ARMARX_DEBUG << "Found valid transform";
 
        return {.header = query.header,
                .transforms = transforms,
                .status = TransformChainResult::Status::Success};
    }
 
    template <class... Args>
    std::vector<std::string>
    TransformHelper::_buildTransformChain(
        const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
        const TransformQuery& query)
    {
        ARMARX_DEBUG << "Building transform chain for robot `" << query.header.agent << "`.";
 
        std::vector<std::string> chain;
 
        const auto& agentProviderSegment =
            localizationCoreSegment.getProviderSegment(query.header.agent);
 
        const std::vector<std::string> tfs = agentProviderSegment.getEntityNames();
 
        // lookup from robot root to global
        std::map<std::string, std::string> tfLookup;
 
        for (const std::string& tf : tfs)
        {
            const auto frames = simox::alg::split(tf, ",");
            ARMARX_CHECK_EQUAL(frames.size(), 2);
 
            tfLookup[frames.front()] = frames.back();
        }
 
        std::string currentFrame = query.header.parentFrame;
        chain.push_back(currentFrame);
        while (tfLookup.count(currentFrame) > 0 and currentFrame != query.header.frame)
        {
            currentFrame = tfLookup.at(currentFrame);
            chain.push_back(currentFrame);
        }
 
        ARMARX_DEBUG << VAROUT(chain);
 
        if (chain.empty() or chain.back() != query.header.frame)
        {
            ARMARX_INFO << deactivateSpam(60) << "Cannot create tf lookup chain '"
                        << query.header.parentFrame << " -> " << query.header.frame
                        << "' for robot `" + query.header.agent + "`.";
            return {};
        }
 
        std::vector<std::string> frameChain;
        for (size_t i = 0; i < (chain.size() - 1); i++)
        {
            frameChain.push_back(chain.at(i) + "," + chain.at(i + 1));
        }
 
        return frameChain;
    }
 
    template <class... Args>
    std::optional<armarx::core::time::DateTime>
    TransformHelper::_obtainTimestamp(
        const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
        const armem::Time& timestamp)
    {
 
        // first we check which the newest timestamp is
        std::optional<int64_t> timeSinceEpochUs = std::nullopt;
 
        localizationCoreSegment.forEachEntity(
            [&timeSinceEpochUs, &timestamp](const auto& entity)
            {
                auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
 
                if (snapshot == nullptr)
                {
                    return;
                }
 
                if (not snapshot->hasInstance(0))
                {
                    return;
                }
 
                const armem::wm::EntityInstance& item = snapshot->getInstance(0);
                const auto tf = _convertEntityToTransform(item);
 
                const auto& dataTs = tf.header.timestamp;
 
                timeSinceEpochUs =
                    std::max(timeSinceEpochUs.value_or(0), dataTs.toMicroSecondsSinceEpoch());
            });
 
        if (not timeSinceEpochUs.has_value())
        {
            return std::nullopt;
        }
 
        // then we ensure that the timestamp is not more recent than the query timestamp
        timeSinceEpochUs = std::min(timeSinceEpochUs.value(), timestamp.toMicroSecondsSinceEpoch());
 
        return armarx::core::time::DateTime(
            armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
    }
 
    template <class... Args>
    std::vector<Eigen::Isometry3f>
    TransformHelper::_obtainTransforms(
        const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
        const std::vector<std::string>& tfChain,
        const std::string& agent,
        const armem::Time& timestamp)
    {
        const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(agent);
 
        ARMARX_DEBUG << "Provider segments" << localizationCoreSegment.getProviderSegmentNames();
        ARMARX_DEBUG << "Entities: " << agentProviderSegment.getEntityNames();
 
        try
        {
            std::vector<Eigen::Isometry3f> 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_VERBOSE << missingEntryError.what();
        }
        catch (const ::armarx::exceptions::local::ExpressionException& ex)
        {
            ARMARX_VERBOSE << "Local expression exception: " << ex.what();
        }
        catch (const ::armarx::LocalException& ex)
        {
            ARMARX_VERBOSE << "Local exception: " << ex.what();
        }
        catch (...)
        {
            ARMARX_VERBOSE << "Unexpected error: " << GetHandledExceptionString();
        }
 
        return {};
    }
 
    template <class... Args>
    Eigen::Isometry3f
    TransformHelper::_obtainTransform(
        const std::string& entityName,
        const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
        const armem::Time& timestamp)
    {
        // ARMARX_DEBUG << "getEntity:" + entityName;
        const auto& entity = agentProviderSegment.getEntity(entityName);
 
        // ARMARX_DEBUG << "History (size: " << entity.size() << "): " << entity.getTimestamps();
 
        // if (entity.history.empty())
        // {
        //     // TODO(fabian.reister): fixme boom
        //     ARMARX_ERROR << "No snapshots received.";
        //     return Eigen::Isometry3f::Identity();
        // }
 
        std::vector<::armarx::armem::robot_state::localization::Transform> transforms;
 
        auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
        ARMARX_CHECK(snapshot) << "No snapshot found before or at time " << timestamp;
        transforms.push_back(_convertEntityToTransform(snapshot->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", 0);
        }
 
        // ARMARX_DEBUG << "single transform";
 
        return transforms.front().transform;
    }
 
    ::armarx::armem::robot_state::localization::Transform
    TransformHelper::_convertEntityToTransform(const armem::wm::EntityInstance& item)
    {
        arondto::Transform aronTransform;
        aronTransform.fromAron(item.data());
 
        ::armarx::armem::robot_state::localization::Transform transform;
        fromAron(aronTransform, transform);
 
        return transform;
    }
 
    auto
    findFirstElementAfter(const std::vector<::armarx::armem::robot_state::localization::Transform>& transforms,
                          const armem::Time& timestamp)
    {
        const auto comp = [](const armem::Time& 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 localization::Transform& transform)
        { return transform.header.timestamp > timestamp; };
 
        const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
 
        ARMARX_CHECK(it == poseNextIt);
 
        return poseNextIt;
    }
 
    Eigen::Isometry3f
    TransformHelper::_interpolateTransform(
        const std::vector<::armarx::armem::robot_state::localization::Transform>& queue,
        armem::Time 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 double t =
            (timestamp - posePreIt->header.timestamp).toMicroSecondsDouble() /
            (poseNextIt->header.timestamp - posePreIt->header.timestamp).toMicroSecondsDouble();
 
        ARMARX_DEBUG << "interpolate";
 
        return simox::math::interpolatePose(
            posePreIt->transform, poseNextIt->transform, static_cast<float>(t));
    }
 
    TransformResult
    TransformHelper::lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
                                     const TransformQuery& query)
    {
        return _lookupTransform(localizationCoreSegment, query);
    }
 
    TransformResult
    TransformHelper::lookupTransform(const armem::server::wm::CoreSegment& localizationCoreSegment,
                                     const TransformQuery& query)
    {
        return _lookupTransform(localizationCoreSegment, query);
    }
 
    TransformChainResult
    TransformHelper::lookupTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
                                          const TransformQuery& query)
    {
        return _lookupTransformChain(localizationCoreSegment, query);
    }
 
    TransformChainResult
    TransformHelper::lookupTransformChain(
        const armem::server::wm::CoreSegment& localizationCoreSegment,
        const TransformQuery& query)
    {
        return _lookupTransformChain(localizationCoreSegment, query);
    }
 
 
} // namespace armarx::armem::common::robot_state::localization