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