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>
73typedef btScalar Scalar;
75typedef btQuaternion Quaternion;
77typedef btVector3 Vector3;
79typedef btVector3 Point;
81typedef btTransform Transform;
83typedef btTransform Pose;
85typedef btMatrix3x3 Matrix3x3;
88typedef uint32_t CompactFrameID;
123 *
static_cast<tf::Transform *
>(
this) = input;
154 *
static_cast<T *
>(
this) = input;
166operator==(
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);
181operator==(
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);
189assert_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, "
209static inline Quaternion
210create_quaternion_from_rpy(
double roll,
double pitch,
double yaw)
213 q.setEulerZYX(yaw, pitch, roll);
221static inline Quaternion
222create_quaternion_from_yaw(
double yaw)
225 q.setEulerZYX(yaw, 0.0, 0.0);
233static inline Quaternion
234create_quaternion_from_array(
double *q)
236 return Quaternion(q[0], q[1], q[2], q[3]);
244get_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);
268get_yaw(
const double *q)
270 return get_yaw(Quaternion(q[0], q[1], q[2], q[3]));
278get_yaw(
const float *q)
280 return get_yaw(Quaternion(q[0], q[1], q[2], q[3]));
A class for handling time.
Wrapper class to add time stamp and frame ID to base types.
Stamped()
Default constructor.
Stamped(const T &input, const fawkes::Time ×tamp, const std::string &frame_id)
Constructor.
std::string frame_id
The frame_id associated this data.
fawkes::Time stamp
The timestamp associated with this data.
void set_data(const T &input)
Set the data element.
Fawkes library namespace.
bool operator==(const Uuid &uuid, const Uuid &other) noexcept
Compare two Uuids.