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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019