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 #if 0 /* just to make Emacs auto-indent happy */
61  }
62 }
63 #endif
64 
65 class TimeCacheInterface;
66 typedef std::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
67 
68 class Transformer : public BufferCore
69 {
70  public:
71  Transformer(float cache_time_sec = BufferCore::DEFAULT_CACHE_TIME);
72  virtual ~Transformer(void);
73 
74  void set_enabled(bool enabled);
75  bool is_enabled() const;
76 
77  float get_cache_time() const;
78  void lock();
79  bool try_lock();
80  void unlock();
81 
82  bool frame_exists(const std::string& frame_id_str) const;
83  TimeCacheInterfacePtr get_frame_cache(const std::string& frame_id) const;
84  std::vector<TimeCacheInterfacePtr> get_frame_caches() const;
85  std::vector<std::string> get_frame_id_mappings() const;
86 
87  void lookup_transform(const std::string& target_frame,
88  const std::string& source_frame,
89  const fawkes::Time& time,
90  StampedTransform& transform) const;
91 
92  void lookup_transform(const std::string& target_frame,
93  const fawkes::Time& target_time,
94  const std::string& source_frame,
95  const fawkes::Time& source_time,
96  const std::string& fixed_frame,
97  StampedTransform& transform) const;
98 
99  void lookup_transform(const std::string& target_frame,
100  const std::string& source_frame,
101  StampedTransform& transform) const;
102 
103  bool can_transform(const std::string& target_frame, const std::string& source_frame,
104  const fawkes::Time& time, std::string* error_msg = NULL) const;
105 
106  bool can_transform(const std::string& target_frame, const fawkes::Time& target_time,
107  const std::string& source_frame, const fawkes::Time& source_time,
108  const std::string& fixed_frame, std::string* error_msg = NULL) const;
109 
110  void transform_quaternion(const std::string& target_frame,
111  const Stamped<Quaternion>& stamped_in,
112  Stamped<Quaternion>& stamped_out) const;
113  void transform_vector(const std::string& target_frame,
114  const Stamped<Vector3>& stamped_in,
115  Stamped<Vector3>& stamped_out) const;
116  void transform_point(const std::string& target_frame,
117  const Stamped<Point>& stamped_in, Stamped<Point>& stamped_out) const;
118  void transform_pose(const std::string& target_frame,
119  const Stamped<Pose>& stamped_in, Stamped<Pose>& stamped_out) const;
120 
121  bool transform_origin(const std::string& source_frame,
122  const std::string& target_frame,
123  Stamped<Pose>& stamped_out,
124  const fawkes::Time time = fawkes::Time(0,0)) const;
125 
126  void transform_quaternion(const std::string& target_frame, const fawkes::Time& target_time,
127  const Stamped<Quaternion>& stamped_in,
128  const std::string& fixed_frame,
129  Stamped<Quaternion>& stamped_out) const;
130  void transform_vector(const std::string& target_frame, const fawkes::Time& target_time,
131  const Stamped<Vector3>& stamped_in,
132  const std::string& fixed_frame,
133  Stamped<Vector3>& stamped_out) const;
134  void transform_point(const std::string& target_frame, const fawkes::Time& target_time,
135  const Stamped<Point>& stamped_in,
136  const std::string& fixed_frame,
137  Stamped<Point>& stamped_out) const;
138  void transform_pose(const std::string& target_frame, const fawkes::Time& target_time,
139  const Stamped<Pose>& stamped_in,
140  const std::string& fixed_frame,
141  Stamped<Pose>& stamped_out) const;
142 
143  std::string all_frames_as_dot(bool print_time, fawkes::Time *time = 0) const;
144 
145  private:
146  bool enabled_;
147 };
148 
149 
150 } // end namespace tf
151 } // end namespace fawkes
152 
153 #endif
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.
std::vector< std::string > get_frame_id_mappings() const
Get mappings from frame ID to names.
Fawkes library namespace.
bool is_enabled() const
Check if transformer is enabled.
void set_enabled(bool enabled)
Set enabled status of transformer.
A class for handling time.
Definition: time.h:91
void lock()
Lock transformer.
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.
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.
TimeCacheInterfacePtr get_frame_cache(const std::string &frame_id) const
Get cache for specific frame.
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.
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:112
float get_cache_time() const
Get cache time.
bool try_lock()
Try to acquire lock.
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.
virtual ~Transformer(void)
Destructor.
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
std::string all_frames_as_dot(bool print_time, fawkes::Time *time=0) const
Get DOT graph of all frames.
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.
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:133
Transformer(float cache_time_sec=BufferCore::DEFAULT_CACHE_TIME)
Constructor.
static const int DEFAULT_CACHE_TIME
The default amount of time to cache data in seconds.
Definition: buffer_core.h:115
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
std::vector< TimeCacheInterfacePtr > get_frame_caches() const
Get all currently existing caches.
Coordinate transforms between any two frames in a system.
Definition: transformer.h:68
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.