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> 74 typedef btScalar Scalar;
76 typedef btQuaternion Quaternion;
78 typedef btVector3 Vector3;
80 typedef btVector3 Point;
82 typedef btTransform Transform;
84 typedef btTransform Pose;
86 typedef btMatrix3x3 Matrix3x3;
89 typedef uint32_t CompactFrameID;
110 const std::string &frame_id,
const std::string &child_frame_id)
111 : tf::Transform(input), stamp(timestamp),
112 frame_id(frame_id), child_frame_id(child_frame_id)
123 { *
static_cast<tf::Transform*
>(
this) = input; };
128 template <
typename T>
137 Stamped() :frame_id (
"NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){};
145 const std::string &frame_id)
146 : T(input), stamp(timestamp), frame_id(frame_id) {};
151 void set_data(
const T& input){*
static_cast<T*
>(
this) = input;};
169 static_cast<const Transform&
>(a) == static_cast<const Transform&>(b);
175 assert_quaternion_valid(
const Quaternion & q)
177 if (std::isnan(q.x()) || std::isnan(q.y()) ||
178 std::isnan(q.z()) || std::isnan(q.w()))
183 double magnitude = q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w();
184 if(std::fabs(magnitude - 1) > 0.01) {
186 "should be 1.0", magnitude);
196 static inline Quaternion
197 create_quaternion_from_rpy(
double roll,
double pitch,
double yaw)
200 q.setEulerZYX(yaw, pitch, roll);
208 static inline Quaternion
209 create_quaternion_from_yaw(
double yaw)
212 q.setEulerZYX(yaw, 0.0, 0.0);
221 static inline double get_yaw(
const Quaternion& bt_q){
222 Scalar useless_pitch, useless_roll, yaw;
223 Matrix3x3(bt_q).getEulerZYX(yaw, useless_pitch, useless_roll);
231 static inline double get_yaw(Pose& t)
233 double yaw, pitch, roll;
234 t.getBasis().getEulerZYX(yaw,pitch,roll);
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.