Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00032 #ifndef TF2_GEOMETRY_MSGS_H
00033 #define TF2_GEOMETRY_MSGS_H
00034
00035 #include <tf2_ros/buffer.h>
00036 #include <geometry_msgs/PointStamped.h>
00037 #include <geometry_msgs/Vector3Stamped.h>
00038 #include <geometry_msgs/PoseStamped.h>
00039 #include <kdl/frames.hpp>
00040
00041 namespace tf2
00042 {
00043
00044 KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t)
00045 {
00046 return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y,
00047 t.transform.rotation.z, t.transform.rotation.w),
00048 KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
00049 }
00050
00051
00052
00054
00055
00056
00057 template <>
00058 const ros::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;}
00059
00060
00061 template <>
00062 const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;}
00063
00064
00065 template <>
00066 void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const geometry_msgs::TransformStamped& transform)
00067 {
00068 tf2::Stamped<KDL::Vector> v_out = tf2::Stamped<KDL::Vector>(gmTransformToKDL(transform).M * KDL::Vector(t_in.vector.x, t_in.vector.y, t_in.vector.z),
00069 transform.header.stamp, transform.header.frame_id);
00070 t_out.vector.x = v_out[0];
00071 t_out.vector.y = v_out[1];
00072 t_out.vector.z = v_out[2];
00073 t_out.header.stamp = v_out.stamp_;
00074 t_out.header.frame_id = v_out.frame_id_;
00075 }
00076 geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in)
00077 {
00078 return in;
00079 }
00080 void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out)
00081 {
00082 out = msg;
00083 }
00084
00085
00086
00087
00089
00090
00091
00092 template <>
00093 const ros::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return t.header.stamp;}
00094
00095
00096 template <>
00097 const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;}
00098
00099
00100 template <>
00101 void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const geometry_msgs::TransformStamped& transform)
00102 {
00103 tf2::Stamped<KDL::Vector> v_out = tf2::Stamped<KDL::Vector>(gmTransformToKDL(transform) * KDL::Vector(t_in.point.x, t_in.point.y, t_in.point.z),
00104 transform.header.stamp, transform.header.frame_id);
00105 t_out.point.x = v_out[0];
00106 t_out.point.y = v_out[1];
00107 t_out.point.z = v_out[2];
00108 t_out.header.stamp = v_out.stamp_;
00109 t_out.header.frame_id = v_out.frame_id_;
00110 }
00111 geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in)
00112 {
00113 return in;
00114 }
00115 void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out)
00116 {
00117 out = msg;
00118 }
00119
00120
00121
00123
00124
00125
00126 template <>
00127 const ros::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return t.header.stamp;}
00128
00129
00130 template <>
00131 const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;}
00132
00133
00134 template <>
00135 void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const geometry_msgs::TransformStamped& transform)
00136 {
00137 KDL::Vector v(t_in.pose.position.x, t_in.pose.position.y, t_in.pose.position.z);
00138 KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.orientation.x, t_in.pose.orientation.y, t_in.pose.orientation.z, t_in.pose.orientation.w);
00139
00140 tf2::Stamped<KDL::Frame> v_out = tf2::Stamped<KDL::Frame>(gmTransformToKDL(transform) * KDL::Frame(r, v),
00141 transform.header.stamp, transform.header.frame_id);
00142 t_out.pose.position.x = v_out.p[0];
00143 t_out.pose.position.y = v_out.p[1];
00144 t_out.pose.position.z = v_out.p[2];
00145 v_out.M.GetQuaternion(t_out.pose.orientation.x, t_out.pose.orientation.y, t_out.pose.orientation.z, t_out.pose.orientation.w);
00146 t_out.header.stamp = v_out.stamp_;
00147 t_out.header.frame_id = v_out.frame_id_;
00148 }
00149 geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in)
00150 {
00151 return in;
00152 }
00153 void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out)
00154 {
00155 out = msg;
00156 }
00157
00158
00159
00160
00161 }
00162
00163 #endif // TF2_GEOMETRY_MSGS_H