32 #ifndef TF_TRANSFORM_DATATYPES_H 33 #define TF_TRANSFORM_DATATYPES_H 36 #include "geometry_msgs/PointStamped.h" 37 #include "geometry_msgs/Vector3Stamped.h" 38 #include "geometry_msgs/QuaternionStamped.h" 39 #include "geometry_msgs/TransformStamped.h" 40 #include "geometry_msgs/PoseStamped.h" 63 Stamped() :frame_id_ (
"NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){};
67 T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ;
70 void setData(
const T& input){*
static_cast<T*
>(
this) = input;};
88 tf::
Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };
108 if (fabs(bt.
length2() - 1 ) > QUATERNION_TOLERANCE)
110 ROS_WARN(
"MSG to TF: Quaternion Not Properly Normalized");
117 if (fabs(bt.
length2() - 1 ) > QUATERNION_TOLERANCE)
119 ROS_WARN(
"TF to MSG: Quaternion Not Properly Normalized");
122 msg.x = bt_temp.x(); msg.y = bt_temp.y(); msg.z = bt_temp.z(); msg.w = bt_temp.w();
126 msg.x = bt.x(); msg.y = bt.y(); msg.z = bt.z(); msg.w = bt.w();
132 tfScalar useless_pitch, useless_roll, yaw;
138 static inline double getYaw(
const geometry_msgs::Quaternion& msg_q){
152 q.
setRPY(roll, pitch, yaw);
173 geometry_msgs::Quaternion q_msg;
185 geometry_msgs::Quaternion q_msg;
235 {bt =
Transform(
Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w),
Vector3(msg.translation.x, msg.translation.y, msg.translation.z));};
249 {bt =
Transform(
Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w),
Vector3(msg.position.x, msg.position.y, msg.position.z));};
267 #endif //TF_TRANSFORM_DATATYPES_H static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
construct a Quaternion from Fixed angles
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
convert Pose msg to Pose
static void pointMsgToTF(const geometry_msgs::Point &msg_v, Point &bt_v)
convert Point msg to Point
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Stamped(const T &input, const ros::Time ×tamp, const std::string &frame_id)
void setData(const T &input)
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
tfScalar length2() const
Return the length squared of the quaternion.
TFSIMD_FORCE_INLINE bool operator==(const Matrix3x3 &m1, const Matrix3x3 &m2)
Equality operator between two matrices It will test all elements are equal.
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
convert Stamped<Pose> to PoseStamped msg
static double getYaw(const Quaternion &bt_q)
Helper function for getting yaw from a Quaternion.
static void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped &msg, Stamped< Quaternion > &bt)
convert QuaternionStamped msg to Stamped<Quaternion>
static const double QUATERNION_TOLERANCE
static void vector3StampedTFToMsg(const Stamped< Vector3 > &bt, geometry_msgs::Vector3Stamped &msg)
convert Stamped<Vector3> to Vector3Stamped msg
static void vector3TFToMsg(const Vector3 &bt_v, geometry_msgs::Vector3 &msg_v)
convert Vector3 to Vector3 msg
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
static tf::Quaternion createIdentityQuaternion()
construct an Identity Quaternion
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
static void pointStampedMsgToTF(const geometry_msgs::PointStamped &msg, Stamped< Point > &bt)
convert PointStamped msg to Stamped<Point>
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
static void vector3MsgToTF(const geometry_msgs::Vector3 &msg_v, Vector3 &bt_v)
convert Vector3 msg to Vector3
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
static Quaternion createQuaternionFromYaw(double yaw)
construct a Quaternion from yaw only
static void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped &msg, Stamped< Vector3 > &bt)
convert Vector3Stamped msg to Stamped<Vector3>
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Set the quaternion using fixed axis RPY.
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
convert Quaternion msg to Quaternion
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
Get the matrix represented as roll pitch and yaw about fixed axes XYZ.
static void pointStampedTFToMsg(const Stamped< Point > &bt, geometry_msgs::PointStamped &msg)
convert Stamped<Point> to PointStamped msg
TFSIMD_FORCE_INLINE Vector3()
No initialization constructor.
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
convert Quaternion to Quaternion msg
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
static void quaternionStampedTFToMsg(const Stamped< Quaternion > &bt, geometry_msgs::QuaternionStamped &msg)
convert Stamped<Quaternion> to QuaternionStamped msg
ros::Time stamp_
The timestamp associated with this data.
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
construct a Quaternion Message from yaw only
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
convert Pose to Pose msg
std::string frame_id_
The frame_id associated this data.
static void transformMsgToTF(const geometry_msgs::Transform &msg, Transform &bt)
convert Transform msg to Transform
static geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw)
construct a Quaternion Message from Fixed angles
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
convert PoseStamped msg to Stamped<Pose>
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
convert tf::StampedTransform to TransformStamped msg
static void transformStampedMsgToTF(const geometry_msgs::TransformStamped &msg, StampedTransform &bt)
convert TransformStamped msg to tf::StampedTransform
static void transformTFToMsg(const Transform &bt, geometry_msgs::Transform &msg)
convert Transform to Transform msg
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
static void pointTFToMsg(const Point &bt_v, geometry_msgs::Point &msg_v)
convert Point to Point msg