MRPT  1.9.9
FrameTransformer.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
12 #include <mrpt/core/exceptions.h> // for ASSERTMSG_
13 #include <mrpt/poses/CPose2D.h>
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/poses/FrameTransformer.h> // for FrameTransformer, FrameTran...
16 #include <mrpt/system/datetime.h> // for TTimeStamp, INVALID_TIMESTAMP
17 #include <string> // for string
18 
19 using namespace mrpt::poses;
20 
21 // ------- FrameTransformerInterface --------
22 template <int DIM>
24 template <int DIM>
26 
27 namespace mrpt
28 {
29 namespace poses
30 {
31 // Explicit instantations:
32 template class FrameTransformerInterface<2>;
33 template class FrameTransformerInterface<3>;
34 } // namespace poses
35 } // namespace mrpt
36 
37 // ------- FrameTransformer --------
38 template <int DIM>
40 template <int DIM>
42 
43 template <int DIM>
45  const std::string& parent_frame, const std::string& child_frame,
46  const typename base_t::pose_t& child_wrt_parent,
47  const mrpt::system::TTimeStamp& timestamp)
48 {
49  m_pose_edges_buffer[parent_frame][child_frame] =
50  TF_TreeEdge(child_wrt_parent, timestamp);
51 }
52 
53 // future work: allow graph traversal and do pose chain composition?
54 // We dont need to replicate the full functionality of ROS tf, though... just
55 // what we really need.
56 
57 template <int DIM>
59  const std::string& target_frame, const std::string& source_frame,
60  typename base_t::light_type& child_wrt_parent,
61  const mrpt::system::TTimeStamp query_time, const double timeout_secs)
62 {
63  ASSERTMSG_(
64  timeout_secs == .0,
65  "timeout_secs!=0: Blocking calls not supported yet!");
66  ASSERTMSG_(
67  query_time == INVALID_TIMESTAMP,
68  "`query_time` different than 'latest' not supported yet!");
69 
70  const auto& it_src = m_pose_edges_buffer.find(source_frame);
71  if (it_src == m_pose_edges_buffer.end())
72  {
73  return LKUP_UNKNOWN_FRAME;
74  }
75 
76  const auto& it_dst = it_src->second.find(target_frame);
77  if (it_dst == it_src->second.end())
78  {
79  return LKUP_NO_CONNECTIVITY;
80  }
81 
82  const TF_TreeEdge& te = it_dst->second;
83  child_wrt_parent = te.pose.asTPose();
84 
85  return LKUP_GOOD;
86 }
87 
88 namespace mrpt
89 {
90 namespace poses
91 {
92 // Explicit instantations:
93 template class FrameTransformer<2>;
94 template class FrameTransformer<3>;
95 } // namespace poses
96 } // namespace mrpt
typename Lie::SE< DIM >::light_type light_type
This will be mapped to mrpt::math::TPose2D (DIM=2) or mrpt::math::TPose3D (DIM=3) ...
Virtual base class for interfaces to a ROS tf2-like service capable of "publishing" and "looking-up" ...
typename Lie::SE< DIM >::type pose_t
This will be mapped to CPose2D (DIM=2) or CPose3D (DIM=3)
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void 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=mrpt::system::now()) override
FrameLookUpStatus 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=INVALID_TIMESTAMP, const double timeout_secs=.0) override
Queries the current pose of target_frame wrt ("as seen from") source_frame.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020