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
58namespace fawkes {
59namespace tf {
60
61class TimeCacheInterface;
62typedef std::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
63
64class Transformer : public BufferCore
65{
66public:
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
152private:
153 bool enabled_;
154};
155
156} // end namespace tf
157} // end namespace fawkes
158
159#endif
A class for handling time.
Definition: time.h:93
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:107
static const int DEFAULT_CACHE_TIME
The default amount of time to cache data in seconds.
Definition: buffer_core.h:109
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
bool is_enabled() const
Check if transformer is enabled.
float get_cache_time() const
Get cache time.
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 lock()
Lock transformer.
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.
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.
void set_enabled(bool enabled)
Set enabled status of transformer.
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
TimeCacheInterfacePtr get_frame_cache(const std::string &frame_id) const
Get cache for specific frame.
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.
Transformer(float cache_time_sec=BufferCore::DEFAULT_CACHE_TIME)
Constructor.
Definition: transformer.cpp:99
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.
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.
bool try_lock()
Try to acquire lock.
std::vector< TimeCacheInterfacePtr > get_frame_caches() const
Get all currently existing caches.
std::string all_frames_as_dot(bool print_time, fawkes::Time *time=0) const
Get DOT graph of all frames.
virtual ~Transformer(void)
Destructor.
std::vector< std::string > get_frame_id_mappings() const
Get mappings from frame ID to names.
Fawkes library namespace.