Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * types.h - Fawkes tf types (based on ROS tf) 00003 * 00004 * Created: Tue Oct 18 17:03:47 2011 00005 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00006 ****************************************************************************/ 00007 00008 /* This program is free software; you can redistribute it and/or modify 00009 * it under the terms of the GNU General Public License as published by 00010 * the Free Software Foundation; either version 2 of the License, or 00011 * (at your option) any later version. A runtime exception applies to 00012 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00020 */ 00021 00022 /* This code is based on ROS tf with the following copyright and license: 00023 * 00024 * Copyright (c) 2008, Willow Garage, Inc. 00025 * All rights reserved. 00026 * 00027 * Redistribution and use in source and binary forms, with or without 00028 * modification, are permitted provided that the following conditions are met: 00029 * 00030 * * Redistributions of source code must retain the above copyright 00031 * notice, this list of conditions and the following disclaimer. 00032 * * Redistributions in binary form must reproduce the above copyright 00033 * notice, this list of conditions and the following disclaimer in the 00034 * documentation and/or other materials provided with the distribution. 00035 * * Neither the name of the Willow Garage, Inc. nor the names of its 00036 * contributors may be used to endorse or promote products derived from 00037 * this software without specific prior written permission. 00038 * 00039 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00040 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00041 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00042 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00043 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00044 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00045 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00046 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00047 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00048 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00049 * POSSIBILITY OF SUCH DAMAGE. 00050 */ 00051 00052 #ifndef __LIBS_TF_TYPES_H_ 00053 #define __LIBS_TF_TYPES_H_ 00054 00055 #include <tf/exceptions.h> 00056 #include <utils/time/time.h> 00057 00058 #include <LinearMath/btQuaternion.h> 00059 #include <LinearMath/btVector3.h> 00060 #include <LinearMath/btTransform.h> 00061 00062 #include <string> 00063 #include <cmath> 00064 00065 namespace fawkes { 00066 namespace tf { 00067 #if 0 /* just to make Emacs auto-indent happy */ 00068 } 00069 } 00070 #endif 00071 00072 /** Scalar datatype. */ 00073 typedef btScalar Scalar; 00074 /** Representaton of orientation or rotation depending on context. */ 00075 typedef btQuaternion Quaternion; 00076 /** Representation of a translation. */ 00077 typedef btVector3 Vector3; 00078 /** Representation of a point (position). */ 00079 typedef btVector3 Point; 00080 /** Representation of a translation and rotation. */ 00081 typedef btTransform Transform; 00082 /** Representation of pose (position and orientation). */ 00083 typedef btTransform Pose; 00084 /** Representation of 3x3 matrix. */ 00085 typedef btMatrix3x3 Matrix3x3; 00086 00087 /// Internally used to reference frames efficiently 00088 typedef uint32_t CompactFrameID; 00089 00090 /** Transform that contains a timestamp and frame IDs. */ 00091 class StampedTransform : public Transform 00092 { 00093 public: 00094 /// Timestamp of this transform. 00095 fawkes::Time stamp; 00096 /// Parent/reference frame ID. 00097 std::string frame_id; 00098 /// Frame ID of child frame, e.g. the transform denotes the 00099 /// transform from the parent frame to this child. 00100 std::string child_frame_id; 00101 00102 /** Constructor. 00103 * @param input transform 00104 * @param timestamp timestamp for this transform 00105 * @param frame_id parent frame ID 00106 * @param child_frame_id child frame ID 00107 */ 00108 StampedTransform(const tf::Transform &input, const fawkes::Time ×tamp, 00109 const std::string &frame_id, const std::string &child_frame_id) 00110 : tf::Transform(input), stamp(timestamp), 00111 frame_id(frame_id), child_frame_id(child_frame_id) 00112 {}; 00113 00114 00115 /** Default constructor only to be used for preallocation */ 00116 StampedTransform() {}; 00117 00118 /** Set the inherited Transform data. 00119 * @param input transform to set 00120 */ 00121 void set_data(const tf::Transform &input) 00122 { *static_cast<tf::Transform*>(this) = input; }; 00123 }; 00124 00125 00126 /** Wrapper class to add time stamp and frame ID to base types. */ 00127 template <typename T> 00128 class Stamped : public T{ 00129 public: 00130 fawkes::Time stamp; ///< The timestamp associated with this data 00131 std::string frame_id; ///< The frame_id associated this data 00132 00133 /** Default constructor. 00134 * Default constructor used only for preallocation. 00135 */ 00136 Stamped() :frame_id ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; 00137 00138 /** Constructor. 00139 * @param input transform 00140 * @param timestamp timestamp for this transform 00141 * @param frame_id frame ID the transform is relative to 00142 */ 00143 Stamped(const T &input, const fawkes::Time ×tamp, 00144 const std::string &frame_id) 00145 : T(input), stamp(timestamp), frame_id(frame_id) {}; 00146 00147 /** Set the data element. 00148 * @param input data to set this instance to 00149 */ 00150 void set_data(const T& input){*static_cast<T*>(this) = input;}; 00151 }; 00152 00153 00154 00155 /** Comparison operator for StampedTransform. 00156 * @param a transform to compare 00157 * @param b transform to compare 00158 * @return true of the transforms are the same, i.e. the parent and 00159 * child frame IDs between the transforms are the same, as well as the 00160 * time stamps and transforms. 00161 */ 00162 static inline bool operator==(const StampedTransform &a, const StampedTransform &b) 00163 { 00164 return 00165 a.frame_id == b.frame_id && 00166 a.child_frame_id == b.child_frame_id && 00167 a.stamp == b.stamp && 00168 static_cast<const Transform&>(a) == static_cast<const Transform&>(b); 00169 }; 00170 00171 00172 /** \brief Throw InvalidArgument if quaternion is malformed */ 00173 inline void 00174 assert_quaternion_valid(const Quaternion & q) 00175 { 00176 if (std::isnan(q.x()) || std::isnan(q.y()) || 00177 std::isnan(q.z()) || std::isnan(q.w())) 00178 { 00179 throw InvalidArgumentException("Quaternion malformed, contains NaN value"); 00180 } 00181 00182 double magnitude = q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w(); 00183 if(std::fabs(magnitude - 1) > 0.01) { 00184 throw InvalidArgumentException("Quaternion malformed, magnitude: %f, " 00185 "should be 1.0", magnitude); 00186 } 00187 }; 00188 00189 /** Construct a Quaternion from fixed angles. 00190 * @param roll The roll about the X axis 00191 * @param pitch The pitch about the Y axis 00192 * @param yaw The yaw about the Z axis 00193 * @return The quaternion constructed 00194 */ 00195 static inline Quaternion 00196 create_quaternion_from_rpy(double roll, double pitch, double yaw) 00197 { 00198 Quaternion q; 00199 q.setEulerZYX(yaw, pitch, roll); 00200 return q; 00201 } 00202 00203 /** Construct a Quaternion from yaw only. 00204 * @param yaw The yaw about the Z axis 00205 * @return The quaternion constructed 00206 */ 00207 static inline Quaternion 00208 create_quaternion_from_yaw(double yaw) 00209 { 00210 Quaternion q; 00211 q.setEulerZYX(yaw, 0.0, 0.0); 00212 return q; 00213 } 00214 00215 00216 /** Helper function for getting yaw from a Quaternion. 00217 * @param bt_q quaternion to get yaw from 00218 * @return yaw value 00219 */ 00220 static inline double get_yaw(const Quaternion& bt_q){ 00221 Scalar useless_pitch, useless_roll, yaw; 00222 Matrix3x3(bt_q).getEulerZYX(yaw, useless_pitch, useless_roll); 00223 return yaw; 00224 } 00225 00226 /** Helper function for getting yaw from a pose 00227 * @param t pose to get yaw from 00228 * @return yaw value 00229 */ 00230 static inline double get_yaw(Pose& t) 00231 { 00232 double yaw, pitch, roll; 00233 t.getBasis().getEulerZYX(yaw,pitch,roll); 00234 return yaw; 00235 } 00236 00237 00238 } // end namespace tf 00239 } // end namespace fawkes 00240 00241 #endif