FrameTransformer.cpp

Go to the documentation of this file.

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

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

#include <mrpt/poses/FrameTransformer.h>  // for FrameTransformer, FrameTran...
#include <string>  // for string
#include <mrpt/system/datetime.h>  // for TTimeStamp, INVALID_TIMESTAMP
#include <mrpt/utils/mrpt_macros.h>  // for ASSERTMSG_

using namespace mrpt::poses;

// ------- FrameTransformerInterface --------
template <int DIM>
FrameTransformerInterface<DIM>::FrameTransformerInterface()
{
}
template <int DIM>
FrameTransformerInterface<DIM>::~FrameTransformerInterface()
{
}

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

// ------- FrameTransformer --------
template <int DIM>
FrameTransformer<DIM>::FrameTransformer()
{
}
template <int DIM>
FrameTransformer<DIM>::~FrameTransformer()
{
}

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::lightweight_pose_t& 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;

        return LKUP_GOOD;
}

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