Fawkes API  Fawkes Development Version
types.h
1 /***************************************************************************
2  * types.h - Fawkes tf types (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_TYPES_H_
53 #define __LIBS_TF_TYPES_H_
54 
55 #ifndef HAVE_TF
56 # error HAVE_TF not defined, forgot CFLAGS_TF in Makefile or bullet no installed?
57 #endif
58 
59 #include <utils/time/time.h>
60 #include <tf/exceptions.h>
61 
62 #include <LinearMath/btQuaternion.h>
63 #include <LinearMath/btVector3.h>
64 #include <LinearMath/btTransform.h>
65 
66 #include <string>
67 #include <cmath>
68 #include <cstdint>
69 
70 namespace fawkes {
71  namespace tf {
72 #if 0 /* just to make Emacs auto-indent happy */
73  }
74 }
75 #endif
76 
77 /** Scalar datatype. */
78 typedef btScalar Scalar;
79 /** Representaton of orientation or rotation depending on context. */
80 typedef btQuaternion Quaternion;
81 /** Representation of a translation. */
82 typedef btVector3 Vector3;
83 /** Representation of a point (position). */
84 typedef btVector3 Point;
85 /** Representation of a translation and rotation. */
86 typedef btTransform Transform;
87 /** Representation of pose (position and orientation). */
88 typedef btTransform Pose;
89 /** Representation of 3x3 matrix. */
90 typedef btMatrix3x3 Matrix3x3;
91 
92 /// Internally used to reference frames efficiently
93 typedef uint32_t CompactFrameID;
94 
95 /** Transform that contains a timestamp and frame IDs. */
96 class StampedTransform : public Transform
97 {
98  public:
99  /// Timestamp of this transform.
101  /// Parent/reference frame ID.
102  std::string frame_id;
103  /// Frame ID of child frame, e.g. the transform denotes the
104  /// transform from the parent frame to this child.
105  std::string child_frame_id;
106 
107  /** Constructor.
108  * @param input transform
109  * @param timestamp timestamp for this transform
110  * @param frame_id parent frame ID
111  * @param child_frame_id child frame ID
112  */
113  StampedTransform(const tf::Transform &input, const fawkes::Time &timestamp,
114  const std::string &frame_id, const std::string &child_frame_id)
115  : tf::Transform(input), stamp(timestamp),
116  frame_id(frame_id), child_frame_id(child_frame_id)
117  {};
118 
119 
120  /** Default constructor only to be used for preallocation */
122 
123  /** Set the inherited Transform data.
124  * @param input transform to set
125  */
126  void set_data(const tf::Transform &input)
127  { *static_cast<tf::Transform*>(this) = input; };
128 };
129 
130 
131 /** Wrapper class to add time stamp and frame ID to base types. */
132 template <typename T>
133 class Stamped : public T{
134  public:
135  fawkes::Time stamp; ///< The timestamp associated with this data
136  std::string frame_id; ///< The frame_id associated this data
137 
138  /** Default constructor.
139  * Default constructor used only for preallocation.
140  */
142  : stamp(0,0), frame_id("NO_ID_STAMPED_DEFAULT_CONSTRUCTION")
143  {};
144 
145  /** Constructor.
146  * @param input transform
147  * @param timestamp timestamp for this transform
148  * @param frame_id frame ID the transform is relative to
149  */
150  Stamped(const T &input, const fawkes::Time &timestamp,
151  const std::string &frame_id)
152  : T(input), stamp(timestamp), frame_id(frame_id) {};
153 
154  /** Set the data element.
155  * @param input data to set this instance to
156  */
157  void set_data(const T& input){*static_cast<T*>(this) = input;};
158 };
159 
160 
161 
162 /** Comparison operator for StampedTransform.
163  * @param a transform to compare
164  * @param b transform to compare
165  * @return true of the transforms are the same, i.e. the parent and
166  * child frame IDs between the transforms are the same, as well as the
167  * time stamps and transforms.
168  */
169 static inline bool
170 operator==(const StampedTransform &a, const StampedTransform &b)
171 {
172  return
173  a.frame_id == b.frame_id &&
175  a.stamp == b.stamp &&
176  static_cast<const Transform&>(a) == static_cast<const Transform&>(b);
177 }
178 
179 /** Comparison operator for StampedTransform.
180  * @param a transform to compare
181  * @param b transform to compare
182  * @return true of the transforms are the same, i.e. the parent and
183  * child frame IDs between the transforms are the same, as well as the
184  * time stamps and transforms.
185  */
186 template <typename T>
187 bool
188 operator==(const Stamped<T> &a, const Stamped<T> &b) {
189  return
190  a.frame_id_ == b.frame_id_ &&
191  a.stamp_ == b.stamp_ &&
192  static_cast<const T&>(a) == static_cast<const T&>(b);
193 }
194 
195 
196 
197 /** \brief Throw InvalidArgument if quaternion is malformed */
198 inline void
199 assert_quaternion_valid(const Quaternion & q)
200 {
201  if (std::isnan(q.x()) || std::isnan(q.y()) ||
202  std::isnan(q.z()) || std::isnan(q.w()))
203  {
204  throw InvalidArgumentException("Quaternion malformed, contains NaN value");
205  }
206 
207  double magnitude = q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w();
208  if(std::fabs(magnitude - 1) > 0.01) {
209  throw InvalidArgumentException("Quaternion malformed, magnitude: %f, "
210  "should be 1.0", magnitude);
211  }
212 }
213 
214 /** Construct a Quaternion from fixed angles.
215  * @param roll The roll about the X axis
216  * @param pitch The pitch about the Y axis
217  * @param yaw The yaw about the Z axis
218  * @return The quaternion constructed
219  */
220 static inline Quaternion
221 create_quaternion_from_rpy(double roll, double pitch, double yaw)
222 {
223  Quaternion q;
224  q.setEulerZYX(yaw, pitch, roll);
225  return q;
226 }
227 
228 /** Construct a Quaternion from yaw only.
229  * @param yaw The yaw about the Z axis
230  * @return The quaternion constructed
231  */
232 static inline Quaternion
233 create_quaternion_from_yaw(double yaw)
234 {
235  Quaternion q;
236  q.setEulerZYX(yaw, 0.0, 0.0);
237  return q;
238 }
239 
240 
241 /** Construct a Quaternion from an array of quaternion values.
242  * @param q quaternion as array of four values ordered as x, y, z, w
243  * @return The quaternion constructed
244  */
245 static inline Quaternion
246 create_quaternion_from_array(double *q)
247 {
248  return Quaternion(q[0], q[1], q[2], q[3]);
249 }
250 
251 
252 /** Helper function for getting yaw from a Quaternion.
253  * @param bt_q quaternion to get yaw from
254  * @return yaw value
255  */
256 static inline double get_yaw(const Quaternion& bt_q){
257  Scalar useless_pitch, useless_roll, yaw;
258  Matrix3x3(bt_q).getEulerZYX(yaw, useless_pitch, useless_roll);
259  return yaw;
260 }
261 
262 /** Helper function for getting yaw from a pose
263  * @param t pose to get yaw from
264  * @return yaw value
265  */
266 static inline double get_yaw(Pose& t)
267 {
268  double yaw, pitch, roll;
269  t.getBasis().getEulerZYX(yaw,pitch,roll);
270  return yaw;
271 }
272 
273 
274 /** Helper function for getting yaw from a Quaternion.
275  * @param q quaternion as array of four values ordered as x, y, z, w
276  * @return yaw value
277  */
278 static inline double get_yaw(const double *q)
279 {
280  return get_yaw(Quaternion(q[0], q[1], q[2], q[3]));
281 }
282 
283 /** Helper function for getting yaw from a Quaternion.
284  * @param q quaternion as array of four values ordered as x, y, z, w
285  * @return yaw value
286  */
287 static inline double get_yaw(const float *q)
288 {
289  return get_yaw(Quaternion(q[0], q[1], q[2], q[3]));
290 }
291 
292 } // end namespace tf
293 } // end namespace fawkes
294 
295 #endif
std::string frame_id
The frame_id associated this data.
Definition: types.h:136
void set_data(const tf::Transform &input)
Set the inherited Transform data.
Definition: types.h:126
Fawkes library namespace.
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:100
A class for handling time.
Definition: time.h:91
Passed argument was invalid.
Definition: exceptions.h:58
StampedTransform()
Default constructor only to be used for preallocation.
Definition: types.h:121
fawkes::Time stamp
The timestamp associated with this data.
Definition: types.h:135
Stamped()
Default constructor.
Definition: types.h:141
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
StampedTransform(const tf::Transform &input, const fawkes::Time &timestamp, const std::string &frame_id, const std::string &child_frame_id)
Constructor.
Definition: types.h:113
std::string child_frame_id
Frame ID of child frame, e.g.
Definition: types.h:105
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:133
Stamped(const T &input, const fawkes::Time &timestamp, const std::string &frame_id)
Constructor.
Definition: types.h:150
std::string frame_id
Parent/reference frame ID.
Definition: types.h:102
void set_data(const T &input)
Set the data element.
Definition: types.h:157