52 #ifndef _LIBS_TF_TYPES_H_
53 #define _LIBS_TF_TYPES_H_
56 # error HAVE_TF not defined, forgot CFLAGS_TF in Makefile or bullet no installed?
59 #include <LinearMath/btQuaternion.h>
60 #include <LinearMath/btTransform.h>
61 #include <LinearMath/btVector3.h>
62 #include <tf/exceptions.h>
63 #include <utils/time/time.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;
123 *
static_cast<tf::Transform *
>(
this) = input;
128 template <
typename T>
154 *
static_cast<T *
>(
this) = input;
166 operator==(
const StampedTransform &a,
const StampedTransform &b)
168 return a.frame_id == b.frame_id && a.child_frame_id == b.child_frame_id && a.stamp == b.stamp
169 &&
static_cast<const Transform &
>(a) ==
static_cast<const Transform &
>(b);
179 template <
typename T>
181 operator==(
const Stamped<T> &a,
const Stamped<T> &b)
183 return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_
184 &&
static_cast<const T &
>(a) ==
static_cast<const T &
>(b);
189 assert_quaternion_valid(
const Quaternion &q)
191 if (std::isnan(q.x()) || std::isnan(q.y()) || std::isnan(q.z()) || std::isnan(q.w())) {
192 throw InvalidArgumentException(
"Quaternion malformed, contains NaN value");
195 double magnitude = q.x() * q.x() + q.y() * q.y() + q.z() * q.z() + q.w() * q.w();
196 if (std::fabs(magnitude - 1) > 0.01) {
197 throw InvalidArgumentException(
"Quaternion malformed, magnitude: %f, "
209 static inline Quaternion
210 create_quaternion_from_rpy(
double roll,
double pitch,
double yaw)
213 q.setEulerZYX(yaw, pitch, roll);
221 static inline Quaternion
222 create_quaternion_from_yaw(
double yaw)
225 q.setEulerZYX(yaw, 0.0, 0.0);
233 static inline Quaternion
234 create_quaternion_from_array(
double *q)
236 return Quaternion(q[0], q[1], q[2], q[3]);
244 get_yaw(
const Quaternion &bt_q)
246 Scalar useless_pitch, useless_roll, yaw;
247 Matrix3x3(bt_q).getEulerZYX(yaw, useless_pitch, useless_roll);
258 double yaw, pitch, roll;
259 t.getBasis().getEulerZYX(yaw, pitch, roll);
268 get_yaw(
const double *q)
270 return get_yaw(Quaternion(q[0], q[1], q[2], q[3]));
278 get_yaw(
const float *q)
280 return get_yaw(Quaternion(q[0], q[1], q[2], q[3]));