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
69namespace fawkes {
70namespace tf {
71
72/** Scalar datatype. */
73typedef btScalar Scalar;
74/** Representaton of orientation or rotation depending on context. */
75typedef btQuaternion Quaternion;
76/** Representation of a translation. */
77typedef btVector3 Vector3;
78/** Representation of a point (position). */
79typedef btVector3 Point;
80/** Representation of a translation and rotation. */
81typedef btTransform Transform;
82/** Representation of pose (position and orientation). */
83typedef btTransform Pose;
84/** Representation of 3x3 matrix. */
85typedef btMatrix3x3 Matrix3x3;
86
87/// Internally used to reference frames efficiently
88typedef uint32_t CompactFrameID;
89
90/** Transform that contains a timestamp and frame IDs. */
91class StampedTransform : public Transform
92{
93public:
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. */
128template <typename T>
129class Stamped : public T
130{
131public:
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 */
165static inline bool
166operator==(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 */
179template <typename T>
180bool
181operator==(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 */
188inline void
189assert_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 */
209static inline Quaternion
210create_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 */
221static inline Quaternion
222create_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 */
233static inline Quaternion
234create_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 */
243static inline double
244get_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 */
255static inline double
256get_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 */
267static inline double
268get_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 */
277static inline double
278get_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.
bool operator==(const Uuid &uuid, const Uuid &other) noexcept
Compare two Uuids.
Definition: uuid.cpp:131