$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00032 #ifndef TF_TRANSFORM_DATATYPES_H 00033 #define TF_TRANSFORM_DATATYPES_H 00034 00035 #include <string> 00036 #include "geometry_msgs/PointStamped.h" 00037 #include "geometry_msgs/Vector3Stamped.h" 00038 #include "geometry_msgs/QuaternionStamped.h" 00039 #include "geometry_msgs/TransformStamped.h" 00040 #include "geometry_msgs/PoseStamped.h" 00041 #include "LinearMath/btTransform.h" 00042 #include "ros/time.h" 00043 00044 #include "ros/console.h" 00045 00046 namespace tf 00047 { 00049 typedef btQuaternion Quaternion; 00050 00051 typedef btVector3 Vector3; 00053 typedef btVector3 Point; 00055 typedef btTransform Transform; 00057 typedef btTransform Pose; 00058 00059 static const double QUATERNION_TOLERANCE = 0.1f; 00060 00063 template <typename T> 00064 class Stamped : public T{ 00065 public: 00066 ros::Time stamp_; 00067 std::string frame_id_; 00068 00070 Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation 00071 00073 Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id) : 00074 T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ; 00075 00077 void setData(const T& input){*static_cast<T*>(this) = input;}; 00078 }; 00079 00081 template <typename T> 00082 bool operator==(const Stamped<T> &a, const Stamped<T> &b) { 00083 return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast<const T&>(a) == static_cast<const T&>(b); 00084 }; 00085 00086 00088 class StampedTransform : public tf::Transform 00089 { 00090 public: 00091 ros::Time stamp_; 00092 std::string frame_id_; 00093 std::string child_frame_id_; 00094 StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id): 00095 tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ }; 00096 00098 StampedTransform() { }; 00099 00101 void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;}; 00102 00103 }; 00104 00106 static inline bool operator==(const StampedTransform &a, const StampedTransform &b) { 00107 return a.frame_id_ == b.frame_id_ && a.child_frame_id_ == b.child_frame_id_ && a.stamp_ == b.stamp_ && static_cast<const tf::Transform&>(a) == static_cast<const tf::Transform&>(b); 00108 }; 00109 00110 00112 static inline void quaternionMsgToTF(const geometry_msgs::Quaternion& msg, Quaternion& bt) 00113 { 00114 bt = Quaternion(msg.x, msg.y, msg.z, msg.w); 00115 if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE) 00116 { 00117 ROS_WARN("MSG to TF: Quaternion Not Properly Normalized"); 00118 bt.normalize(); 00119 } 00120 }; 00122 static inline void quaternionTFToMsg(const Quaternion& bt, geometry_msgs::Quaternion& msg) 00123 { 00124 if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE) 00125 { 00126 ROS_WARN("TF to MSG: Quaternion Not Properly Normalized"); 00127 Quaternion bt_temp = bt; 00128 bt_temp.normalize(); 00129 msg.x = bt_temp.x(); msg.y = bt_temp.y(); msg.z = bt_temp.z(); msg.w = bt_temp.w(); 00130 } 00131 else 00132 { 00133 msg.x = bt.x(); msg.y = bt.y(); msg.z = bt.z(); msg.w = bt.w(); 00134 } 00135 }; 00136 00138 static inline double getYaw(const Quaternion& bt_q){ 00139 btScalar useless_pitch, useless_roll, yaw; 00140 btMatrix3x3(bt_q).getRPY( useless_roll, useless_pitch,yaw); 00141 return yaw; 00142 } 00143 00145 static inline double getYaw(const geometry_msgs::Quaternion& msg_q){ 00146 Quaternion bt_q; 00147 quaternionMsgToTF(msg_q, bt_q); 00148 return getYaw(bt_q); 00149 } 00150 00157 static inline tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw){ 00158 Quaternion q; 00159 q.setRPY(roll, pitch, yaw); 00160 return q; 00161 } 00162 00167 static inline Quaternion createQuaternionFromYaw(double yaw){ 00168 Quaternion q; 00169 q.setRPY(0.0, 0.0, yaw); 00170 return q; 00171 } 00172 00177 static inline geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw){ 00178 Quaternion q; 00179 q.setRPY(0.0, 0.0, yaw); 00180 geometry_msgs::Quaternion q_msg; 00181 quaternionTFToMsg(q, q_msg); 00182 return q_msg; 00183 } 00184 00191 static inline geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw){ 00192 geometry_msgs::Quaternion q_msg; 00193 quaternionTFToMsg(createQuaternionFromRPY(roll, pitch, yaw), q_msg); 00194 return q_msg; 00195 } 00196 00200 static inline tf::Quaternion createIdentityQuaternion() 00201 { 00202 Quaternion q; 00203 q.setRPY(0,0,0); 00204 return q; 00205 }; 00206 00208 static inline void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped & msg, Stamped<Quaternion>& bt) 00209 {quaternionMsgToTF(msg.quaternion, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; 00211 static inline void quaternionStampedTFToMsg(const Stamped<Quaternion>& bt, geometry_msgs::QuaternionStamped & msg) 00212 {quaternionTFToMsg(bt, msg.quaternion); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; 00213 00215 static inline void vector3MsgToTF(const geometry_msgs::Vector3& msg_v, Vector3& bt_v) {bt_v = Vector3(msg_v.x, msg_v.y, msg_v.z);}; 00217 static inline void vector3TFToMsg(const Vector3& bt_v, geometry_msgs::Vector3& msg_v) {msg_v.x = bt_v.x(); msg_v.y = bt_v.y(); msg_v.z = bt_v.z();}; 00218 00220 static inline void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped & msg, Stamped<Vector3>& bt) 00221 {vector3MsgToTF(msg.vector, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; 00223 static inline void vector3StampedTFToMsg(const Stamped<Vector3>& bt, geometry_msgs::Vector3Stamped & msg) 00224 {vector3TFToMsg(bt, msg.vector); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; 00225 00226 00228 static inline void pointMsgToTF(const geometry_msgs::Point& msg_v, Point& bt_v) {bt_v = Vector3(msg_v.x, msg_v.y, msg_v.z);}; 00230 static inline void pointTFToMsg(const Point& bt_v, geometry_msgs::Point& msg_v) {msg_v.x = bt_v.x(); msg_v.y = bt_v.y(); msg_v.z = bt_v.z();}; 00231 00233 static inline void pointStampedMsgToTF(const geometry_msgs::PointStamped & msg, Stamped<Point>& bt) 00234 {pointMsgToTF(msg.point, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; 00236 static inline void pointStampedTFToMsg(const Stamped<Point>& bt, geometry_msgs::PointStamped & msg) 00237 {pointTFToMsg(bt, msg.point); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; 00238 00239 00241 static inline void transformMsgToTF(const geometry_msgs::Transform& msg, Transform& bt) 00242 {bt = Transform(Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), Vector3(msg.translation.x, msg.translation.y, msg.translation.z));}; 00244 static inline void transformTFToMsg(const Transform& bt, geometry_msgs::Transform& msg) 00245 {vector3TFToMsg(bt.getOrigin(), msg.translation); quaternionTFToMsg(bt.getRotation(), msg.rotation);}; 00246 00248 static inline void transformStampedMsgToTF(const geometry_msgs::TransformStamped & msg, StampedTransform& bt) 00249 {transformMsgToTF(msg.transform, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id; bt.child_frame_id_ = msg.child_frame_id;}; 00251 static inline void transformStampedTFToMsg(const StampedTransform& bt, geometry_msgs::TransformStamped & msg) 00252 {transformTFToMsg(bt, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_; msg.child_frame_id = bt.child_frame_id_;}; 00253 00255 static inline void poseMsgToTF(const geometry_msgs::Pose& msg, Pose& bt) 00256 {bt = Transform(Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w), Vector3(msg.position.x, msg.position.y, msg.position.z));}; 00258 static inline void poseTFToMsg(const Pose& bt, geometry_msgs::Pose& msg) 00259 {pointTFToMsg(bt.getOrigin(), msg.position); quaternionTFToMsg(bt.getRotation(), msg.orientation);}; 00260 00262 static inline void poseStampedMsgToTF(const geometry_msgs::PoseStamped & msg, Stamped<Pose>& bt) 00263 {poseMsgToTF(msg.pose, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; 00265 static inline void poseStampedTFToMsg(const Stamped<Pose>& bt, geometry_msgs::PoseStamped & msg) 00266 {poseTFToMsg(bt, msg.pose); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; 00267 00268 00269 00270 00271 00272 00273 } 00274 #endif //TF_TRANSFORM_DATATYPES_H