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 <utils/time/time.h> 60 #include <tf/exceptions.h> 62 #include <LinearMath/btQuaternion.h> 63 #include <LinearMath/btVector3.h> 64 #include <LinearMath/btTransform.h> 78 typedef btScalar Scalar;
80 typedef btQuaternion Quaternion;
82 typedef btVector3 Vector3;
84 typedef btVector3 Point;
86 typedef btTransform Transform;
88 typedef btTransform Pose;
90 typedef btMatrix3x3 Matrix3x3;
93 typedef uint32_t CompactFrameID;
114 const std::string &frame_id,
const std::string &child_frame_id)
115 : tf::Transform(input), stamp(timestamp),
116 frame_id(frame_id), child_frame_id(child_frame_id)
127 { *
static_cast<tf::Transform*
>(
this) = input; };
132 template <
typename T>
142 : stamp(0,0), frame_id(
"NO_ID_STAMPED_DEFAULT_CONSTRUCTION")
151 const std::string &frame_id)
152 : T(input), stamp(timestamp), frame_id(frame_id) {};
157 void set_data(
const T& input){*
static_cast<T*
>(
this) = input;};
176 static_cast<const Transform&
>(a) == static_cast<const Transform&>(b);
186 template <
typename T>
190 a.frame_id_ == b.frame_id_ &&
191 a.stamp_ == b.stamp_ &&
192 static_cast<const T&
>(a) == static_cast<const T&>(b);
199 assert_quaternion_valid(
const Quaternion & q)
201 if (std::isnan(q.x()) || std::isnan(q.y()) ||
202 std::isnan(q.z()) || std::isnan(q.w()))
207 double magnitude = q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w();
208 if(std::fabs(magnitude - 1) > 0.01) {
210 "should be 1.0", magnitude);
220 static inline Quaternion
221 create_quaternion_from_rpy(
double roll,
double pitch,
double yaw)
224 q.setEulerZYX(yaw, pitch, roll);
232 static inline Quaternion
233 create_quaternion_from_yaw(
double yaw)
236 q.setEulerZYX(yaw, 0.0, 0.0);
245 static inline Quaternion
246 create_quaternion_from_array(
double *q)
248 return Quaternion(q[0], q[1], q[2], q[3]);
256 static inline double get_yaw(
const Quaternion& bt_q){
257 Scalar useless_pitch, useless_roll, yaw;
258 Matrix3x3(bt_q).getEulerZYX(yaw, useless_pitch, useless_roll);
266 static inline double get_yaw(Pose& t)
268 double yaw, pitch, roll;
269 t.getBasis().getEulerZYX(yaw,pitch,roll);
278 static inline double get_yaw(
const double *q)
280 return get_yaw(Quaternion(q[0], q[1], q[2], q[3]));
287 static inline double get_yaw(
const float *q)
289 return get_yaw(Quaternion(q[0], q[1], q[2], q[3]));
std::string frame_id
The frame_id associated this data.
Fawkes library namespace.
A class for handling time.
Passed argument was invalid.
fawkes::Time stamp
The timestamp associated with this data.
Stamped()
Default constructor.
Wrapper class to add time stamp and frame ID to base types.
Stamped(const T &input, const fawkes::Time ×tamp, const std::string &frame_id)
Constructor.
void set_data(const T &input)
Set the data element.