Fawkes API  Fawkes Development Version
transformer.h
1 /***************************************************************************
2  * transformer.h - Fawkes tf transformer (based on ROS tf)
3  *
4  * Created: Tue Oct 18 17:03:47 2011
5  * Copyright 2011 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version. A runtime exception applies to
12  * this software (see LICENSE.GPL_WRE file mentioned below for details).
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
20  */
21 
22 /* This code is based on ROS tf with the following copyright and license:
23  *
24  * Copyright (c) 2008, Willow Garage, Inc.
25  * All rights reserved.
26  *
27  * Redistribution and use in source and binary forms, with or without
28  * modification, are permitted provided that the following conditions are met:
29  *
30  * * Redistributions of source code must retain the above copyright
31  * notice, this list of conditions and the following disclaimer.
32  * * Redistributions in binary form must reproduce the above copyright
33  * notice, this list of conditions and the following disclaimer in the
34  * documentation and/or other materials provided with the distribution.
35  * * Neither the name of the Willow Garage, Inc. nor the names of its
36  * contributors may be used to endorse or promote products derived from
37  * this software without specific prior written permission.
38  *
39  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
40  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
41  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
42  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
43  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
44  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
45  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
46  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
47  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49  * POSSIBILITY OF SUCH DAMAGE.
50  */
51 
52 #ifndef _LIBS_TF_TRANSFORMER_H_
53 #define _LIBS_TF_TRANSFORMER_H_
54 
55 #include <tf/buffer_core.h>
56 #include <tf/types.h>
57 
58 namespace fawkes {
59 namespace tf {
60 
61 class TimeCacheInterface;
62 typedef std::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
63 
64 class Transformer : public BufferCore
65 {
66 public:
67  Transformer(float cache_time_sec = BufferCore::DEFAULT_CACHE_TIME);
68  virtual ~Transformer(void);
69 
70  void set_enabled(bool enabled);
71  bool is_enabled() const;
72 
73  float get_cache_time() const;
74  void lock();
75  bool try_lock();
76  void unlock();
77 
78  bool frame_exists(const std::string &frame_id_str) const;
79  TimeCacheInterfacePtr get_frame_cache(const std::string &frame_id) const;
80  std::vector<TimeCacheInterfacePtr> get_frame_caches() const;
81  std::vector<std::string> get_frame_id_mappings() const;
82 
83  void lookup_transform(const std::string & target_frame,
84  const std::string & source_frame,
85  const fawkes::Time &time,
86  StampedTransform & transform) const;
87 
88  void lookup_transform(const std::string & target_frame,
89  const fawkes::Time &target_time,
90  const std::string & source_frame,
91  const fawkes::Time &source_time,
92  const std::string & fixed_frame,
93  StampedTransform & transform) const;
94 
95  void lookup_transform(const std::string &target_frame,
96  const std::string &source_frame,
97  StampedTransform & transform) const;
98 
99  bool can_transform(const std::string & target_frame,
100  const std::string & source_frame,
101  const fawkes::Time &time,
102  std::string * error_msg = NULL) const;
103 
104  bool can_transform(const std::string & target_frame,
105  const fawkes::Time &target_time,
106  const std::string & source_frame,
107  const fawkes::Time &source_time,
108  const std::string & fixed_frame,
109  std::string * error_msg = NULL) const;
110 
111  void transform_quaternion(const std::string & target_frame,
112  const Stamped<Quaternion> &stamped_in,
113  Stamped<Quaternion> & stamped_out) const;
114  void transform_vector(const std::string & target_frame,
115  const Stamped<Vector3> &stamped_in,
116  Stamped<Vector3> & stamped_out) const;
117  void transform_point(const std::string & target_frame,
118  const Stamped<Point> &stamped_in,
119  Stamped<Point> & stamped_out) const;
120  void transform_pose(const std::string & target_frame,
121  const Stamped<Pose> &stamped_in,
122  Stamped<Pose> & stamped_out) const;
123 
124  bool transform_origin(const std::string &source_frame,
125  const std::string &target_frame,
126  Stamped<Pose> & stamped_out,
127  const fawkes::Time time = fawkes::Time(0, 0)) const;
128 
129  void transform_quaternion(const std::string & target_frame,
130  const fawkes::Time & target_time,
131  const Stamped<Quaternion> &stamped_in,
132  const std::string & fixed_frame,
133  Stamped<Quaternion> & stamped_out) const;
134  void transform_vector(const std::string & target_frame,
135  const fawkes::Time & target_time,
136  const Stamped<Vector3> &stamped_in,
137  const std::string & fixed_frame,
138  Stamped<Vector3> & stamped_out) const;
139  void transform_point(const std::string & target_frame,
140  const fawkes::Time & target_time,
141  const Stamped<Point> &stamped_in,
142  const std::string & fixed_frame,
143  Stamped<Point> & stamped_out) const;
144  void transform_pose(const std::string & target_frame,
145  const fawkes::Time & target_time,
146  const Stamped<Pose> &stamped_in,
147  const std::string & fixed_frame,
148  Stamped<Pose> & stamped_out) const;
149 
150  std::string all_frames_as_dot(bool print_time, fawkes::Time *time = 0) const;
151 
152 private:
153  bool enabled_;
154 };
155 
156 } // end namespace tf
157 } // end namespace fawkes
158 
159 #endif
fawkes::tf::StampedTransform
Transform that contains a timestamp and frame IDs.
Definition: types.h:98
fawkes::tf::Transformer::get_frame_id_mappings
std::vector< std::string > get_frame_id_mappings() const
Get mappings from frame ID to names.
Definition: transformer.cpp:203
fawkes::tf::Transformer::frame_exists
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
Definition: transformer.cpp:173
fawkes::tf::Transformer::is_enabled
bool is_enabled() const
Check if transformer is enabled.
Definition: transformer.cpp:126
fawkes::tf::Transformer::transform_pose
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
Definition: transformer.cpp:443
fawkes::tf::Transformer::get_frame_cache
TimeCacheInterfacePtr get_frame_cache(const std::string &frame_id) const
Get cache for specific frame.
Definition: transformer.cpp:185
fawkes::tf::Transformer::set_enabled
void set_enabled(bool enabled)
Set enabled status of transformer.
Definition: transformer.cpp:117
fawkes::tf::Stamped
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:136
fawkes::tf::Transformer::transform_quaternion
void transform_quaternion(const std::string &target_frame, const Stamped< Quaternion > &stamped_in, Stamped< Quaternion > &stamped_out) const
Transform a stamped Quaternion into the target frame.
Definition: transformer.cpp:358
fawkes
fawkes::tf::Transformer::unlock
void unlock()
Unlock.
Definition: transformer.cpp:163
fawkes::tf::Transformer::get_frame_caches
std::vector< TimeCacheInterfacePtr > get_frame_caches() const
Get all currently existing caches.
Definition: transformer.cpp:194
fawkes::tf::Transformer::Transformer
Transformer(float cache_time_sec=BufferCore::DEFAULT_CACHE_TIME)
Constructor.
Definition: transformer.cpp:106
fawkes::tf::Transformer::lock
void lock()
Lock transformer.
Definition: transformer.cpp:145
fawkes::tf::Transformer::try_lock
bool try_lock()
Try to acquire lock.
Definition: transformer.cpp:154
fawkes::tf::Transformer::can_transform
bool can_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
Definition: transformer.cpp:217
fawkes::tf::Transformer::~Transformer
virtual ~Transformer(void)
Destructor.
Definition: transformer.cpp:111
fawkes::tf::Transformer::lookup_transform
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
Definition: transformer.cpp:272
fawkes::tf::Transformer::transform_vector
void transform_vector(const std::string &target_frame, const Stamped< Vector3 > &stamped_in, Stamped< Vector3 > &stamped_out) const
Transform a stamped vector into the target frame.
Definition: transformer.cpp:386
fawkes::Time
Definition: time.h:96
fawkes::tf::Transformer::all_frames_as_dot
std::string all_frames_as_dot(bool print_time, fawkes::Time *time=0) const
Get DOT graph of all frames.
Definition: transformer.cpp:643
fawkes::tf::Transformer::get_cache_time
float get_cache_time() const
Get cache time.
Definition: transformer.cpp:135
fawkes::tf::Transformer::transform_point
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.
Definition: transformer.cpp:417
fawkes::tf::BufferCore::DEFAULT_CACHE_TIME
static const int DEFAULT_CACHE_TIME
The default amount of time to cache data in seconds.
Definition: buffer_core.h:116
fawkes::tf::Transformer::transform_origin
bool transform_origin(const std::string &source_frame, const std::string &target_frame, Stamped< Pose > &stamped_out, const fawkes::Time time=fawkes::Time(0, 0)) const
Transform ident pose from one frame to another.
Definition: transformer.cpp:475