Main MRPT website > C++ reference for MRPT 1.9.9
FrameTransformer.h
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 #pragma once
10 
11 #include <mrpt/system/datetime.h>
13 #include <mrpt/poses/SE_traits.h>
14 #include <string>
15 #include <map>
16 
17 namespace mrpt
18 {
19 namespace poses
20 {
22 {
23  LKUP_GOOD = 0,
27 };
28 
29 /** Virtual base class for interfaces to a [ROS
30 * tf2](http://wiki.ros.org/tf2)-like
31 * service capable of "publishing" and "looking-up" relative poses between two
32 * "coordinate frames".
33 * Use derived classes for:
34 * - wrapping real ROS tf (TO-DO in
35 * [mrpt-bridge](http://wiki.ros.org/mrpt_bridge)), or
36 * - using a pure MRPT standalone TF service with mrpt::poses::FrameTransformer
37 *
38 * Frame IDs are strings.
39 * MRPT modules use the standard ROS [REP
40 * 105](http://www.ros.org/reps/rep-0105.html#coordinate-frames)
41 * document regarding common names for frames:
42 * - `base_link`: "the robot"
43 * - `odom`: Origin for odometry
44 * - `map`: Origin for "the map"
45 *
46 * \tparam DIM Can be 2 for SE(2), 2D transformations; or 3 for SE(3), 3D
47 * transformations.
48 * \ingroup poses_grp
49 * \sa FrameTransformer, CPose3D
50 */
51 template <int DIM>
53 {
54  public:
55  /** This will be mapped to CPose2D (DIM=2) or CPose3D (DIM=3) */
56  using pose_t = typename SE_traits<DIM>::pose_t;
57  /** This will be mapped to mrpt::math::TPose2D (DIM=2) or
58  * mrpt::math::TPose3D (DIM=3) */
60 
63 
64  /** Publish a time-stampped transform between two frames */
65  virtual void sendTransform(
66  const std::string& parent_frame, const std::string& child_frame,
67  const pose_t& child_wrt_parent,
68  const mrpt::system::TTimeStamp& timestamp = mrpt::system::now()) = 0;
69 
70  /** Queries the current pose of `target_frame` wrt ("as seen from")
71  * `source_frame`.
72  * It tries to return the pose at the given timepoint, unless it is
73  * INVALID_TIMESTAMP (default),
74  * which means returning the latest know transformation.
75  */
77  const std::string& target_frame, const std::string& source_frame,
78  lightweight_pose_t& child_wrt_parent,
80  /** Timeout */
81  const double timeout_secs = .0) = 0;
82 
83 }; // End of class def.
84 
85 /** See docs in FrameTransformerInterface.
86 * This class is an implementation for standalone (non ROS) applications.
87 * \ingroup poses_grp
88 * \sa FrameTransformerInterface
89 */
90 template <int DIM>
92 {
93  public:
95 
98 
99  // See base docs
100  virtual void sendTransform(
101  const std::string& parent_frame, const std::string& child_frame,
102  const typename base_t::pose_t& child_wrt_parent,
103  const mrpt::system::TTimeStamp& timestamp =
104  mrpt::system::now()) override;
105  // See base docs
107  const std::string& target_frame, const std::string& source_frame,
108  typename base_t::lightweight_pose_t& child_wrt_parent,
109  const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP,
110  const double timeout_secs = .0) override;
111 
112  /** \overload */
114  const std::string& target_frame, const std::string& source_frame,
115  typename base_t::pose_t& child_wrt_parent,
116  const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP,
117  /** Timeout */
118  const double timeout_secs = .0)
119  {
120  typename base_t::lightweight_pose_t p;
122  target_frame, source_frame, p, query_time, timeout_secs);
123  child_wrt_parent = typename base_t::pose_t(p);
124  return ret;
125  }
126 
127  protected:
128  // double m_max_extrapolation_time; //!< for extrapolation in the past or
129  // in the future [s]
130  // double m_max_age_pose_cache; //!< Max age of stored poses [s]
131 
132  struct TF_TreeEdge
133  {
134  // TODO: CPose{2,3}DInterpolator?
137 
139  const typename base_t::pose_t& pose_,
140  const mrpt::system::TTimeStamp& timestamp_)
141  : pose(pose_), timestamp(timestamp_)
142  {
143  }
145  };
146 
147  // map: [parent] -> { [child] -> relPoseChildWRTParent }
148  using pose_tree_t =
152 };
153 
154 } // ns
155 } // ns
mrpt::poses::LKUP_GOOD
@ LKUP_GOOD
Definition: FrameTransformer.h:23
mrpt::poses::FrameTransformerInterface::pose_t
typename SE_traits< DIM >::pose_t pose_t
This will be mapped to CPose2D (DIM=2) or CPose3D (DIM=3)
Definition: FrameTransformer.h:56
mrpt::poses::FrameTransformer::lookupTransform
FrameLookUpStatus lookupTransform(const std::string &target_frame, const std::string &source_frame, typename base_t::pose_t &child_wrt_parent, const mrpt::system::TTimeStamp query_time=INVALID_TIMESTAMP, const double timeout_secs=.0)
This is an overloaded member function, provided for convenience. It differs from the above function o...
Definition: FrameTransformer.h:113
mrpt::poses::FrameLookUpStatus
FrameLookUpStatus
Definition: FrameTransformer.h:21
mrpt::poses::FrameTransformer::TF_TreeEdge::TF_TreeEdge
TF_TreeEdge()
Definition: FrameTransformer.h:144
mrpt::poses::FrameTransformer::m_pose_edges_buffer
pose_tree_t m_pose_edges_buffer
Definition: FrameTransformer.h:151
INVALID_TIMESTAMP
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
mrpt::poses::FrameTransformer::pose_tree_t
mrpt::aligned_std_map< std::string, mrpt::aligned_std_map< std::string, TF_TreeEdge > > pose_tree_t
Definition: FrameTransformer.h:150
mrpt::system::now
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
mrpt::aligned_std_map
std::map< KEY, VALUE, std::less< KEY >, mrpt::aligned_allocator_cpp11< std::pair< const KEY, VALUE > >> aligned_std_map
Definition: aligned_std_map.h:17
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
aligned_std_map.h
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::poses::FrameTransformerInterface::lightweight_pose_t
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)
Definition: FrameTransformer.h:59
mrpt::poses::SE_traits
A helper class for SE(2) and SE(3) geometry-related transformations, on-manifold optimization Jacobia...
Definition: SE_traits.h:29
mrpt::system::TTimeStamp
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
Definition: datetime.h:31
mrpt::poses::FrameTransformer::sendTransform
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
Definition: FrameTransformer.cpp:50
mrpt::poses::FrameTransformer::FrameTransformer
FrameTransformer()
Definition: FrameTransformer.cpp:41
mrpt::poses::FrameTransformer::TF_TreeEdge::TF_TreeEdge
TF_TreeEdge(const typename base_t::pose_t &pose_, const mrpt::system::TTimeStamp &timestamp_)
Definition: FrameTransformer.h:138
mrpt::poses::LKUP_EXTRAPOLATION_ERROR
@ LKUP_EXTRAPOLATION_ERROR
Definition: FrameTransformer.h:26
SE_traits.h
mrpt::poses::FrameTransformerInterface::FrameTransformerInterface
FrameTransformerInterface()
Definition: FrameTransformer.cpp:21
mrpt::poses::FrameTransformer
See docs in FrameTransformerInterface.
Definition: FrameTransformer.h:91
mrpt::poses::FrameTransformer::TF_TreeEdge::timestamp
mrpt::system::TTimeStamp timestamp
Definition: FrameTransformer.h:136
mrpt::poses::FrameTransformerInterface::~FrameTransformerInterface
virtual ~FrameTransformerInterface()
Definition: FrameTransformer.cpp:25
mrpt::poses::FrameTransformerInterface::lookupTransform
virtual FrameLookUpStatus lookupTransform(const std::string &target_frame, const std::string &source_frame, lightweight_pose_t &child_wrt_parent, const mrpt::system::TTimeStamp query_time=INVALID_TIMESTAMP, const double timeout_secs=.0)=0
Queries the current pose of target_frame wrt ("as seen from") source_frame.
mrpt::poses::FrameTransformer::TF_TreeEdge
Definition: FrameTransformer.h:132
mrpt::poses::FrameTransformer::TF_TreeEdge::pose
base_t::pose_t pose
Definition: FrameTransformer.h:135
mrpt::poses::FrameTransformerInterface::sendTransform
virtual void sendTransform(const std::string &parent_frame, const std::string &child_frame, const pose_t &child_wrt_parent, const mrpt::system::TTimeStamp &timestamp=mrpt::system::now())=0
Publish a time-stampped transform between two frames.
mrpt::poses::FrameTransformerInterface
Virtual base class for interfaces to a ROS tf2-like service capable of "publishing" and "looking-up" ...
Definition: FrameTransformer.h:52
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::poses::FrameTransformer::~FrameTransformer
~FrameTransformer()
Definition: FrameTransformer.cpp:45
mrpt::poses::FrameTransformer::lookupTransform
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.
Definition: FrameTransformer.cpp:64
mrpt::poses::LKUP_NO_CONNECTIVITY
@ LKUP_NO_CONNECTIVITY
Definition: FrameTransformer.h:25
datetime.h
mrpt::poses::LKUP_UNKNOWN_FRAME
@ LKUP_UNKNOWN_FRAME
Definition: FrameTransformer.h:24



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST