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 
49 typedef tf::Vector3 Point;
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:30
Stamped(const T &input, const ros::Time &timestamp, const std::string &frame_id)
void setData(const T &input)
Quaternion getRotation() const
Return a quaternion representing the rotation.
Definition: Transform.h:120
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: Scalar.h:160
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:642
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:176
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:33
static tf::Quaternion createIdentityQuaternion()
construct an Identity Quaternion
StampedTransform()
Default constructor only to be used for preallocation.
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
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:96
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
convert Quaternion msg to Quaternion
static void pointStampedTFToMsg(const Stamped< Point > &bt, geometry_msgs::PointStamped &msg)
convert Stamped<Point> to PointStamped msg
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
tfScalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:163
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
StampedTransform(const tf::Transform &input, const ros::Time &timestamp, const std::string &frame_id, const std::string &child_frame_id)
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:367
static geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw)
construct a Quaternion Message from Fixed angles
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
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.
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 Fri Feb 22 2019 03:34:50