FrameTransformer.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"  // Precompiled headers

#include <mrpt/core/exceptions.h>  // for ASSERTMSG_
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/FrameTransformer.h>  // for FrameTransformer, FrameTran...
#include <mrpt/system/datetime.h>  // for TTimeStamp, INVALID_TIMESTAMP
#include <string>  // for string

using namespace mrpt::poses;

// ------- FrameTransformerInterface --------
template <int DIM>
FrameTransformerInterface<DIM>::FrameTransformerInterface() = default;
template <int DIM>
FrameTransformerInterface<DIM>::~FrameTransformerInterface() = default;

namespace mrpt
{
namespace poses
{
// Explicit instantations:
template class FrameTransformerInterface<2>;
template class FrameTransformerInterface<3>;
}  // namespace poses
}  // namespace mrpt

// ------- FrameTransformer --------
template <int DIM>
FrameTransformer<DIM>::FrameTransformer() = default;
template <int DIM>
FrameTransformer<DIM>::~FrameTransformer() = default;

template <int DIM>
void FrameTransformer<DIM>::sendTransform(
    const std::string& parent_frame, const std::string& child_frame,
    const typename base_t::pose_t& child_wrt_parent,
    const mrpt::system::TTimeStamp& timestamp)
{
    m_pose_edges_buffer[parent_frame][child_frame] =
        TF_TreeEdge(child_wrt_parent, timestamp);
}

// future work: allow graph traversal and do pose chain composition?
// We dont need to replicate the full functionality of ROS tf, though... just
// what we really need.

template <int DIM>
FrameLookUpStatus FrameTransformer<DIM>::lookupTransform(
    const std::string& target_frame, const std::string& source_frame,
    typename base_t::light_type& child_wrt_parent,
    const mrpt::system::TTimeStamp query_time, const double timeout_secs)
{
    ASSERTMSG_(
        timeout_secs == .0,
        "timeout_secs!=0: Blocking calls not supported yet!");
    ASSERTMSG_(
        query_time == INVALID_TIMESTAMP,
        "`query_time` different than 'latest' not supported yet!");

    const auto& it_src = m_pose_edges_buffer.find(source_frame);
    if (it_src == m_pose_edges_buffer.end())
    {
        return LKUP_UNKNOWN_FRAME;
    }

    const auto& it_dst = it_src->second.find(target_frame);
    if (it_dst == it_src->second.end())
    {
        return LKUP_NO_CONNECTIVITY;
    }

    const TF_TreeEdge& te = it_dst->second;
    child_wrt_parent = te.pose.asTPose();

    return LKUP_GOOD;
}

namespace mrpt
{
namespace poses
{
// Explicit instantations:
template class FrameTransformer<2>;
template class FrameTransformer<3>;
}  // namespace poses
}  // namespace mrpt