MRPT  2.0.1
FrameTransformer.h
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 #pragma once
10 
11 #include <mrpt/poses/Lie/SE.h>
12 #include <mrpt/system/datetime.h>
13 #include <map>
14 #include <string>
15 
16 namespace mrpt::poses
17 {
19 {
20  LKUP_GOOD = 0,
24 };
25 
26 /** Virtual base class for interfaces to a [ROS
27  * tf2](http://wiki.ros.org/tf2)-like
28  * service capable of "publishing" and "looking-up" relative poses between two
29  * "coordinate frames".
30  * Use derived classes for:
31  * - wrapping real ROS tf (TO-DO in
32  * [mrpt-bridge](http://wiki.ros.org/mrpt_bridge)), or
33  * - using a pure MRPT standalone TF service with mrpt::poses::FrameTransformer
34  *
35  * Frame IDs are strings.
36  * MRPT modules use the standard ROS [REP
37  * 105](http://www.ros.org/reps/rep-0105.html#coordinate-frames)
38  * document regarding common names for frames:
39  * - `base_link`: "the robot"
40  * - `odom`: Origin for odometry
41  * - `map`: Origin for "the map"
42  *
43  * \tparam DIM Can be 2 for SE(2), 2D transformations; or 3 for SE(3), 3D
44  * transformations.
45  * \ingroup poses_grp
46  * \sa FrameTransformer, CPose3D
47  */
48 template <int DIM>
50 {
51  public:
52  /** This will be mapped to CPose2D (DIM=2) or CPose3D (DIM=3) */
53  using pose_t = typename Lie::SE<DIM>::type;
54  /** This will be mapped to mrpt::math::TPose2D (DIM=2) or
55  * mrpt::math::TPose3D (DIM=3) */
57 
60 
61  /** Publish a time-stampped transform between two frames */
62  virtual void sendTransform(
63  const std::string& parent_frame, const std::string& child_frame,
64  const pose_t& child_wrt_parent,
65  const mrpt::system::TTimeStamp& timestamp = mrpt::system::now()) = 0;
66 
67  /** Queries the current pose of `target_frame` wrt ("as seen from")
68  * `source_frame`.
69  * It tries to return the pose at the given timepoint, unless it is
70  * INVALID_TIMESTAMP (default),
71  * which means returning the latest know transformation.
72  */
74  const std::string& target_frame, const std::string& source_frame,
75  light_type& child_wrt_parent,
77  /** Timeout */
78  const double timeout_secs = .0) = 0;
79 
80 }; // End of class def.
81 
82 /** See docs in FrameTransformerInterface.
83  * This class is an implementation for standalone (non ROS) applications.
84  * \ingroup poses_grp
85  * \sa FrameTransformerInterface
86  */
87 template <int DIM>
89 {
90  public:
92 
94  ~FrameTransformer() override;
95 
96  // See base docs
97  void sendTransform(
98  const std::string& parent_frame, const std::string& child_frame,
99  const typename base_t::pose_t& child_wrt_parent,
100  const mrpt::system::TTimeStamp& timestamp =
101  mrpt::system::now()) override;
102  // See base docs
104  const std::string& target_frame, const std::string& source_frame,
105  typename base_t::light_type& child_wrt_parent,
106  const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP,
107  const double timeout_secs = .0) override;
108 
109  /** \overload */
111  const std::string& target_frame, const std::string& source_frame,
112  typename base_t::pose_t& child_wrt_parent,
113  const mrpt::system::TTimeStamp query_time = INVALID_TIMESTAMP,
114  /** Timeout */
115  const double timeout_secs = .0)
116  {
117  typename base_t::light_type p;
119  target_frame, source_frame, p, query_time, timeout_secs);
120  child_wrt_parent = typename base_t::pose_t(p);
121  return ret;
122  }
123 
124  protected:
125  // double m_max_extrapolation_time; //!< for extrapolation in the past or
126  // in the future [s]
127  // double m_max_age_pose_cache; //!< Max age of stored poses [s]
128 
129  struct TF_TreeEdge
130  {
131  // TODO: CPose{2,3}DInterpolator?
134 
136  const typename base_t::pose_t& pose_,
137  const mrpt::system::TTimeStamp& timestamp_)
138  : pose(pose_), timestamp(timestamp_)
139  {
140  }
142  };
143 
144  // map: [parent] -> { [child] -> relPoseChildWRTParent }
145  using pose_tree_t =
146  std::map<std::string, std::map<std::string, TF_TreeEdge>>;
148 };
149 
150 } // namespace mrpt::poses
typename Lie::SE< DIM >::light_type light_type
This will be mapped to mrpt::math::TPose2D (DIM=2) or mrpt::math::TPose3D (DIM=3) ...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
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)
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::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
std::map< std::string, std::map< std::string, TF_TreeEdge > > pose_tree_t
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
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)
virtual FrameLookUpStatus lookupTransform(const std::string &target_frame, const std::string &source_frame, light_type &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.
See docs in FrameTransformerInterface.
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
TF_TreeEdge(const typename base_t::pose_t &pose_, const mrpt::system::TTimeStamp &timestamp_)



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020