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 "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"){};
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