Fawkes API  Fawkes Development Version
types.h
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 &timestamp,
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 &timestamp,
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