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