Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 
65 namespace fawkes {
66  namespace tf {
67 #if 0 /* just to make Emacs auto-indent happy */
68  }
69 }
70 #endif
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, const fawkes::Time &timestamp,
109  const std::string &frame_id, const std::string &child_frame_id)
110  : tf::Transform(input), stamp(timestamp),
111  frame_id(frame_id), child_frame_id(child_frame_id)
112  {};
113 
114 
115  /** Default constructor only to be used for preallocation */
117 
118  /** Set the inherited Transform data.
119  * @param input transform to set
120  */
121  void set_data(const tf::Transform &input)
122  { *static_cast<tf::Transform*>(this) = input; };
123 };
124 
125 
126 /** Wrapper class to add time stamp and frame ID to base types. */
127 template <typename T>
128 class Stamped : public T{
129  public:
130  fawkes::Time stamp; ///< The timestamp associated with this data
131  std::string frame_id; ///< The frame_id associated this data
132 
133  /** Default constructor.
134  * Default constructor used only for preallocation.
135  */
136  Stamped() :frame_id ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){};
137 
138  /** Constructor.
139  * @param input transform
140  * @param timestamp timestamp for this transform
141  * @param frame_id frame ID the transform is relative to
142  */
143  Stamped(const T &input, const fawkes::Time &timestamp,
144  const std::string &frame_id)
145  : T(input), stamp(timestamp), frame_id(frame_id) {};
146 
147  /** Set the data element.
148  * @param input data to set this instance to
149  */
150  void set_data(const T& input){*static_cast<T*>(this) = input;};
151 };
152 
153 
154 
155 /** Comparison operator for StampedTransform.
156  * @param a transform to compare
157  * @param b transform to compare
158  * @return true of the transforms are the same, i.e. the parent and
159  * child frame IDs between the transforms are the same, as well as the
160  * time stamps and transforms.
161  */
162 static inline bool operator==(const StampedTransform &a, const StampedTransform &b)
163 {
164  return
165  a.frame_id == b.frame_id &&
166  a.child_frame_id == b.child_frame_id &&
167  a.stamp == b.stamp &&
168  static_cast<const Transform&>(a) == static_cast<const Transform&>(b);
169 };
170 
171 
172 /** \brief Throw InvalidArgument if quaternion is malformed */
173 inline void
174 assert_quaternion_valid(const Quaternion & q)
175 {
176  if (std::isnan(q.x()) || std::isnan(q.y()) ||
177  std::isnan(q.z()) || std::isnan(q.w()))
178  {
179  throw InvalidArgumentException("Quaternion malformed, contains NaN value");
180  }
181 
182  double magnitude = q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w();
183  if(std::fabs(magnitude - 1) > 0.01) {
184  throw InvalidArgumentException("Quaternion malformed, magnitude: %f, "
185  "should be 1.0", magnitude);
186  }
187 };
188 
189 /** Construct a Quaternion from fixed angles.
190  * @param roll The roll about the X axis
191  * @param pitch The pitch about the Y axis
192  * @param yaw The yaw about the Z axis
193  * @return The quaternion constructed
194  */
195 static inline Quaternion
196 create_quaternion_from_rpy(double roll, double pitch, double yaw)
197 {
198  Quaternion q;
199  q.setEulerZYX(yaw, pitch, roll);
200  return q;
201 }
202 
203 /** Construct a Quaternion from yaw only.
204  * @param yaw The yaw about the Z axis
205  * @return The quaternion constructed
206  */
207 static inline Quaternion
208 create_quaternion_from_yaw(double yaw)
209 {
210  Quaternion q;
211  q.setEulerZYX(yaw, 0.0, 0.0);
212  return q;
213 }
214 
215 
216 /** Helper function for getting yaw from a Quaternion.
217  * @param bt_q quaternion to get yaw from
218  * @return yaw value
219  */
220 static inline double get_yaw(const Quaternion& bt_q){
221  Scalar useless_pitch, useless_roll, yaw;
222  Matrix3x3(bt_q).getEulerZYX(yaw, useless_pitch, useless_roll);
223  return yaw;
224 }
225 
226 /** Helper function for getting yaw from a pose
227  * @param t pose to get yaw from
228  * @return yaw value
229  */
230 static inline double get_yaw(Pose& t)
231 {
232  double yaw, pitch, roll;
233  t.getBasis().getEulerZYX(yaw,pitch,roll);
234  return yaw;
235 }
236 
237 
238 } // end namespace tf
239 } // end namespace fawkes
240 
241 #endif