52 #ifndef __LIBS_TF_TYPES_H_
53 #define __LIBS_TF_TYPES_H_
55 #include <tf/exceptions.h>
56 #include <utils/time/time.h>
58 #include <LinearMath/btQuaternion.h>
59 #include <LinearMath/btVector3.h>
60 #include <LinearMath/btTransform.h>
73 typedef btScalar Scalar;
75 typedef btQuaternion Quaternion;
77 typedef btVector3 Vector3;
79 typedef btVector3 Point;
81 typedef btTransform Transform;
83 typedef btTransform Pose;
85 typedef btMatrix3x3 Matrix3x3;
88 typedef uint32_t CompactFrameID;
109 const std::string &frame_id,
const std::string &child_frame_id)
110 : tf::Transform(input), stamp(timestamp),
111 frame_id(frame_id), child_frame_id(child_frame_id)
121 void set_data(
const tf::Transform &input)
122 { *
static_cast<tf::Transform*
>(
this) = input; };
127 template <
typename T>
136 Stamped() :frame_id (
"NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){};
144 const std::string &frame_id)
145 : T(input), stamp(timestamp), frame_id(frame_id) {};
150 void set_data(
const T& input){*
static_cast<T*
>(
this) = input;};
162 static inline bool operator==(
const StampedTransform &a,
const StampedTransform &b)
165 a.frame_id == b.frame_id &&
166 a.child_frame_id == b.child_frame_id &&
167 a.stamp == b.stamp &&
168 static_cast<const Transform&
>(a) == static_cast<const Transform&>(b);
174 assert_quaternion_valid(
const Quaternion & q)
176 if (std::isnan(q.x()) || std::isnan(q.y()) ||
177 std::isnan(q.z()) || std::isnan(q.w()))
179 throw InvalidArgumentException(
"Quaternion malformed, contains NaN value");
182 double magnitude = q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w();
183 if(std::fabs(magnitude - 1) > 0.01) {
184 throw InvalidArgumentException(
"Quaternion malformed, magnitude: %f, "
185 "should be 1.0", magnitude);
195 static inline Quaternion
196 create_quaternion_from_rpy(
double roll,
double pitch,
double yaw)
199 q.setEulerZYX(yaw, pitch, roll);
207 static inline Quaternion
208 create_quaternion_from_yaw(
double yaw)
211 q.setEulerZYX(yaw, 0.0, 0.0);
220 static inline double get_yaw(
const Quaternion& bt_q){
221 Scalar useless_pitch, useless_roll, yaw;
222 Matrix3x3(bt_q).getEulerZYX(yaw, useless_pitch, useless_roll);
230 static inline double get_yaw(Pose& t)
232 double yaw, pitch, roll;
233 t.getBasis().getEulerZYX(yaw,pitch,roll);