transform_datatypes.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
32 #ifndef TF_TRANSFORM_DATATYPES_H
33 #define TF_TRANSFORM_DATATYPES_H
34 
35 #include <string>
36 #include "geometry_msgs/PointStamped.h"
37 #include "geometry_msgs/Vector3Stamped.h"
38 #include "geometry_msgs/QuaternionStamped.h"
39 #include "geometry_msgs/TransformStamped.h"
40 #include "geometry_msgs/PoseStamped.h"
42 #include "ros/time.h"
43 
44 #include "ros/console.h"
45 
46 namespace tf
47 {
48 
51 
52 static const double QUATERNION_TOLERANCE = 0.1f;
53 
56 template <typename T>
57 class Stamped : public T{
58  public:
60  std::string frame_id_;
61 
63  Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation
64 
66  Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id) :
67  T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ;
68 
70  void setData(const T& input){*static_cast<T*>(this) = input;};
71 };
72 
74 template <typename T>
75 bool operator==(const Stamped<T> &a, const Stamped<T> &b) {
76  return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast<const T&>(a) == static_cast<const T&>(b);
77 };
78 
79 
82 {
83 public:
85  std::string frame_id_;
86  std::string child_frame_id_;
87  StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):
88  tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };
89 
92 
94  void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;};
95 
96 };
97 
99 static inline bool operator==(const StampedTransform &a, const StampedTransform &b) {
100  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);
101 };
102 
103 
105 static inline void quaternionMsgToTF(const geometry_msgs::Quaternion& msg, Quaternion& bt)
106 {
107  bt = Quaternion(msg.x, msg.y, msg.z, msg.w);
108  if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE)
109  {
110  ROS_WARN("MSG to TF: Quaternion Not Properly Normalized");
111  bt.normalize();
112  }
113 };
115 static inline void quaternionTFToMsg(const Quaternion& bt, geometry_msgs::Quaternion& msg)
116 {
117  if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE)
118  {
119  ROS_WARN("TF to MSG: Quaternion Not Properly Normalized");
120  Quaternion bt_temp = bt;
121  bt_temp.normalize();
122  msg.x = bt_temp.x(); msg.y = bt_temp.y(); msg.z = bt_temp.z(); msg.w = bt_temp.w();
123  }
124  else
125  {
126  msg.x = bt.x(); msg.y = bt.y(); msg.z = bt.z(); msg.w = bt.w();
127  }
128 };
129 
131 static inline double getYaw(const Quaternion& bt_q){
132  tfScalar useless_pitch, useless_roll, yaw;
133  tf::Matrix3x3(bt_q).getRPY( useless_roll, useless_pitch,yaw);
134  return yaw;
135 }
136 
138 static inline double getYaw(const geometry_msgs::Quaternion& msg_q){
139  Quaternion bt_q;
140  quaternionMsgToTF(msg_q, bt_q);
141  return getYaw(bt_q);
142 }
143 
150 static inline tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw){
151  Quaternion q;
152  q.setRPY(roll, pitch, yaw);
153  return q;
154 }
155 
160 static inline Quaternion createQuaternionFromYaw(double yaw){
161  Quaternion q;
162  q.setRPY(0.0, 0.0, yaw);
163  return q;
164 }
165 
170 static inline geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw){
171  Quaternion q;
172  q.setRPY(0.0, 0.0, yaw);
173  geometry_msgs::Quaternion q_msg;
174  quaternionTFToMsg(q, q_msg);
175  return q_msg;
176 }
177 
184 static inline geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw){
185  geometry_msgs::Quaternion q_msg;
186  quaternionTFToMsg(createQuaternionFromRPY(roll, pitch, yaw), q_msg);
187  return q_msg;
188 }
189 
194 {
195  Quaternion q;
196  q.setRPY(0,0,0);
197  return q;
198 };
199 
201 static inline void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped & msg, Stamped<Quaternion>& bt)
202 {quaternionMsgToTF(msg.quaternion, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;};
204 static inline void quaternionStampedTFToMsg(const Stamped<Quaternion>& bt, geometry_msgs::QuaternionStamped & msg)
205 {quaternionTFToMsg(bt, msg.quaternion); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
206 
208 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);};
210 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();};
211 
213 static inline void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped & msg, Stamped<Vector3>& bt)
214 {vector3MsgToTF(msg.vector, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;};
216 static inline void vector3StampedTFToMsg(const Stamped<Vector3>& bt, geometry_msgs::Vector3Stamped & msg)
217 {vector3TFToMsg(bt, msg.vector); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
218 
219 
221 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);};
223 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();};
224 
226 static inline void pointStampedMsgToTF(const geometry_msgs::PointStamped & msg, Stamped<Point>& bt)
227 {pointMsgToTF(msg.point, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;};
229 static inline void pointStampedTFToMsg(const Stamped<Point>& bt, geometry_msgs::PointStamped & msg)
230 {pointTFToMsg(bt, msg.point); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
231 
232 
234 static inline void transformMsgToTF(const geometry_msgs::Transform& msg, Transform& bt)
235 {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));};
237 static inline void transformTFToMsg(const Transform& bt, geometry_msgs::Transform& msg)
238 {vector3TFToMsg(bt.getOrigin(), msg.translation); quaternionTFToMsg(bt.getRotation(), msg.rotation);};
239 
241 static inline void transformStampedMsgToTF(const geometry_msgs::TransformStamped & msg, StampedTransform& bt)
242 {transformMsgToTF(msg.transform, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id; bt.child_frame_id_ = msg.child_frame_id;};
244 static inline void transformStampedTFToMsg(const StampedTransform& bt, geometry_msgs::TransformStamped & msg)
245 {transformTFToMsg(bt, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_; msg.child_frame_id = bt.child_frame_id_;};
246 
248 static inline void poseMsgToTF(const geometry_msgs::Pose& msg, Pose& bt)
249 {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));};
251 static inline void poseTFToMsg(const Pose& bt, geometry_msgs::Pose& msg)
252 {pointTFToMsg(bt.getOrigin(), msg.position); quaternionTFToMsg(bt.getRotation(), msg.orientation);};
253 
255 static inline void poseStampedMsgToTF(const geometry_msgs::PoseStamped & msg, Stamped<Pose>& bt)
256 {poseMsgToTF(msg.pose, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;};
258 static inline void poseStampedTFToMsg(const Stamped<Pose>& bt, geometry_msgs::PoseStamped & msg)
259 {poseTFToMsg(bt, msg.pose); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
260 
261 
262 
263 
264 
265 
266 }
267 #endif //TF_TRANSFORM_DATATYPES_H
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
construct a Quaternion from Fixed angles
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
convert Pose msg to Pose
static void pointMsgToTF(const geometry_msgs::Point &msg_v, Point &bt_v)
convert Point msg to Point
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
Stamped(const T &input, const ros::Time &timestamp, const std::string &frame_id)
void setData(const T &input)
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: Scalar.h:160
tfScalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:161
TFSIMD_FORCE_INLINE bool operator==(const Matrix3x3 &m1, const Matrix3x3 &m2)
Equality operator between two matrices It will test all elements are equal.
Definition: Matrix3x3.h:640
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
convert Stamped<Pose> to PoseStamped msg
static double getYaw(const Quaternion &bt_q)
Helper function for getting yaw from a Quaternion.
static void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped &msg, Stamped< Quaternion > &bt)
convert QuaternionStamped msg to Stamped<Quaternion>
static const double QUATERNION_TOLERANCE
static void vector3StampedTFToMsg(const Stamped< Vector3 > &bt, geometry_msgs::Vector3Stamped &msg)
convert Stamped<Vector3> to Vector3Stamped msg
static void vector3TFToMsg(const Vector3 &bt_v, geometry_msgs::Vector3 &msg_v)
convert Vector3 to Vector3 msg
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: Quaternion.h:174
Definition: exceptions.h:38
ros::Time stamp_
The timestamp associated with this transform.
tf::Vector3 Point
#define ROS_WARN(...)
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:31
static tf::Quaternion createIdentityQuaternion()
construct an Identity Quaternion
StampedTransform()
Default constructor only to be used for preallocation.
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:263
tf::Transform Pose
std::string child_frame_id_
The frame_id of the coordinate frame this transform defines.
static void pointStampedMsgToTF(const geometry_msgs::PointStamped &msg, Stamped< Point > &bt)
convert PointStamped msg to Stamped<Point>
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
static void vector3MsgToTF(const geometry_msgs::Vector3 &msg_v, Vector3 &bt_v)
convert Vector3 msg to Vector3
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:267
static Quaternion createQuaternionFromYaw(double yaw)
construct a Quaternion from yaw only
static void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped &msg, Stamped< Vector3 > &bt)
convert Vector3Stamped msg to Stamped<Vector3>
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Set the quaternion using fixed axis RPY.
Definition: Quaternion.h:94
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
convert Quaternion msg to Quaternion
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
Get the matrix represented as roll pitch and yaw about fixed axes XYZ.
Definition: Matrix3x3.h:365
static void pointStampedTFToMsg(const Stamped< Point > &bt, geometry_msgs::PointStamped &msg)
convert Stamped<Point> to PointStamped msg
TFSIMD_FORCE_INLINE Vector3()
No initialization constructor.
Definition: Vector3.h:293
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
convert Quaternion to Quaternion msg
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:265
static void quaternionStampedTFToMsg(const Stamped< Quaternion > &bt, geometry_msgs::QuaternionStamped &msg)
convert Stamped<Quaternion> to QuaternionStamped msg
void setData(const tf::Transform &input)
Set the inherited Traonsform data.
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
Return the origin vector translation.
Definition: Transform.h:115
ros::Time stamp_
The timestamp associated with this data.
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
construct a Quaternion Message from yaw only
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
convert Pose to Pose msg
std::string frame_id_
The frame_id associated this data.
static void transformMsgToTF(const geometry_msgs::Transform &msg, Transform &bt)
convert Transform msg to Transform
Quaternion getRotation() const
Return a quaternion representing the rotation.
Definition: Transform.h:120
StampedTransform(const tf::Transform &input, const ros::Time &timestamp, const std::string &frame_id, const std::string &child_frame_id)
static geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw)
construct a Quaternion Message from Fixed angles
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
convert PoseStamped msg to Stamped<Pose>
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
convert tf::StampedTransform to TransformStamped msg
static void transformStampedMsgToTF(const geometry_msgs::TransformStamped &msg, StampedTransform &bt)
convert TransformStamped msg to tf::StampedTransform
static void transformTFToMsg(const Transform &bt, geometry_msgs::Transform &msg)
convert Transform to Transform msg
std::string frame_id_
The frame_id of the coordinate frame in which this transform is defined.
The Stamped Transform datatype used by tf.
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Definition: Vector3.h:38
static void pointTFToMsg(const Point &bt_v, geometry_msgs::Point &msg_v)
convert Point to Point msg


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Jun 10 2019 12:25:26