00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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 "tf/LinearMath/Transform.h"
00042 #include "ros/time.h"
00043
00044 #include "ros/console.h"
00045
00046 namespace tf
00047 {
00048
00049 typedef tf::Vector3 Point;
00050 typedef tf::Transform Pose;
00051
00052 static const double QUATERNION_TOLERANCE = 0.1f;
00053
00056 template <typename T>
00057 class Stamped : public T{
00058 public:
00059 ros::Time stamp_;
00060 std::string frame_id_;
00061
00063 Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){};
00064
00066 Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id) :
00067 T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ;
00068
00070 void setData(const T& input){*static_cast<T*>(this) = input;};
00071 };
00072
00074 template <typename T>
00075 bool operator==(const Stamped<T> &a, const Stamped<T> &b) {
00076 return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast<const T&>(a) == static_cast<const T&>(b);
00077 };
00078
00079
00081 class StampedTransform : public tf::Transform
00082 {
00083 public:
00084 ros::Time stamp_;
00085 std::string frame_id_;
00086 std::string child_frame_id_;
00087 StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):
00088 tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };
00089
00091 StampedTransform() { };
00092
00094 void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;};
00095
00096 };
00097
00099 static inline bool operator==(const StampedTransform &a, const StampedTransform &b) {
00100 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);
00101 };
00102
00103
00105 static inline void quaternionMsgToTF(const geometry_msgs::Quaternion& msg, Quaternion& bt)
00106 {
00107 bt = Quaternion(msg.x, msg.y, msg.z, msg.w);
00108 if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE)
00109 {
00110 ROS_WARN("MSG to TF: Quaternion Not Properly Normalized");
00111 bt.normalize();
00112 }
00113 };
00115 static inline void quaternionTFToMsg(const Quaternion& bt, geometry_msgs::Quaternion& msg)
00116 {
00117 if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE)
00118 {
00119 ROS_WARN("TF to MSG: Quaternion Not Properly Normalized");
00120 Quaternion bt_temp = bt;
00121 bt_temp.normalize();
00122 msg.x = bt_temp.x(); msg.y = bt_temp.y(); msg.z = bt_temp.z(); msg.w = bt_temp.w();
00123 }
00124 else
00125 {
00126 msg.x = bt.x(); msg.y = bt.y(); msg.z = bt.z(); msg.w = bt.w();
00127 }
00128 };
00129
00131 static inline double getYaw(const Quaternion& bt_q){
00132 tfScalar useless_pitch, useless_roll, yaw;
00133 tf::Matrix3x3(bt_q).getRPY( useless_roll, useless_pitch,yaw);
00134 return yaw;
00135 }
00136
00138 static inline double getYaw(const geometry_msgs::Quaternion& msg_q){
00139 Quaternion bt_q;
00140 quaternionMsgToTF(msg_q, bt_q);
00141 return getYaw(bt_q);
00142 }
00143
00150 static inline tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw){
00151 Quaternion q;
00152 q.setRPY(roll, pitch, yaw);
00153 return q;
00154 }
00155
00160 static inline Quaternion createQuaternionFromYaw(double yaw){
00161 Quaternion q;
00162 q.setRPY(0.0, 0.0, yaw);
00163 return q;
00164 }
00165
00170 static inline geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw){
00171 Quaternion q;
00172 q.setRPY(0.0, 0.0, yaw);
00173 geometry_msgs::Quaternion q_msg;
00174 quaternionTFToMsg(q, q_msg);
00175 return q_msg;
00176 }
00177
00184 static inline geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw){
00185 geometry_msgs::Quaternion q_msg;
00186 quaternionTFToMsg(createQuaternionFromRPY(roll, pitch, yaw), q_msg);
00187 return q_msg;
00188 }
00189
00193 static inline tf::Quaternion createIdentityQuaternion()
00194 {
00195 Quaternion q;
00196 q.setRPY(0,0,0);
00197 return q;
00198 };
00199
00201 static inline void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped & msg, Stamped<Quaternion>& bt)
00202 {quaternionMsgToTF(msg.quaternion, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;};
00204 static inline void quaternionStampedTFToMsg(const Stamped<Quaternion>& bt, geometry_msgs::QuaternionStamped & msg)
00205 {quaternionTFToMsg(bt, msg.quaternion); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
00206
00208 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);};
00210 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();};
00211
00213 static inline void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped & msg, Stamped<Vector3>& bt)
00214 {vector3MsgToTF(msg.vector, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;};
00216 static inline void vector3StampedTFToMsg(const Stamped<Vector3>& bt, geometry_msgs::Vector3Stamped & msg)
00217 {vector3TFToMsg(bt, msg.vector); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
00218
00219
00221 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);};
00223 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();};
00224
00226 static inline void pointStampedMsgToTF(const geometry_msgs::PointStamped & msg, Stamped<Point>& bt)
00227 {pointMsgToTF(msg.point, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;};
00229 static inline void pointStampedTFToMsg(const Stamped<Point>& bt, geometry_msgs::PointStamped & msg)
00230 {pointTFToMsg(bt, msg.point); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
00231
00232
00234 static inline void transformMsgToTF(const geometry_msgs::Transform& msg, Transform& bt)
00235 {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));};
00237 static inline void transformTFToMsg(const Transform& bt, geometry_msgs::Transform& msg)
00238 {vector3TFToMsg(bt.getOrigin(), msg.translation); quaternionTFToMsg(bt.getRotation(), msg.rotation);};
00239
00241 static inline void transformStampedMsgToTF(const geometry_msgs::TransformStamped & msg, StampedTransform& bt)
00242 {transformMsgToTF(msg.transform, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id; bt.child_frame_id_ = msg.child_frame_id;};
00244 static inline void transformStampedTFToMsg(const StampedTransform& bt, geometry_msgs::TransformStamped & msg)
00245 {transformTFToMsg(bt, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_; msg.child_frame_id = bt.child_frame_id_;};
00246
00248 static inline void poseMsgToTF(const geometry_msgs::Pose& msg, Pose& bt)
00249 {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));};
00251 static inline void poseTFToMsg(const Pose& bt, geometry_msgs::Pose& msg)
00252 {pointTFToMsg(bt.getOrigin(), msg.position); quaternionTFToMsg(bt.getRotation(), msg.orientation);};
00253
00255 static inline void poseStampedMsgToTF(const geometry_msgs::PoseStamped & msg, Stamped<Pose>& bt)
00256 {poseMsgToTF(msg.pose, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;};
00258 static inline void poseStampedTFToMsg(const Stamped<Pose>& bt, geometry_msgs::PoseStamped & msg)
00259 {poseTFToMsg(bt, msg.pose); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
00260
00261
00262
00263
00264
00265
00266 }
00267 #endif //TF_TRANSFORM_DATATYPES_H