32 #ifndef TF2_GEOMETRY_MSGS_H 33 #define TF2_GEOMETRY_MSGS_H 35 #include <boost/array.hpp> 40 #include <geometry_msgs/PointStamped.h> 41 #include <geometry_msgs/QuaternionStamped.h> 42 #include <geometry_msgs/TransformStamped.h> 43 #include <geometry_msgs/Vector3Stamped.h> 44 #include <geometry_msgs/Pose.h> 45 #include <geometry_msgs/PoseStamped.h> 46 #include <geometry_msgs/PoseWithCovarianceStamped.h> 47 #include <geometry_msgs/Wrench.h> 48 #include <geometry_msgs/WrenchStamped.h> 49 #include <kdl/frames.hpp> 65 t.transform.rotation.z, t.transform.rotation.w),
66 KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
80 geometry_msgs::Vector3
toMsg(
const tf2::Vector3& in)
82 geometry_msgs::Vector3 out;
95 void fromMsg(
const geometry_msgs::Vector3& in, tf2::Vector3& out)
97 out = tf2::Vector3(in.x, in.y, in.z);
123 const std::string&
getFrameId(
const geometry_msgs::Vector3Stamped&
t) {
return t.header.frame_id;}
132 geometry_msgs::Vector3Stamped
toMsg(
const geometry_msgs::Vector3Stamped& in)
143 void fromMsg(
const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out)
156 geometry_msgs::Vector3Stamped out;
157 out.header.stamp = in.
stamp_;
159 out.vector.x = in.getX();
160 out.vector.y = in.getY();
161 out.vector.z = in.getZ();
173 out.
stamp_ = msg.header.stamp;
175 out.
setData(tf2::Vector3(msg.vector.x, msg.vector.y, msg.vector.z));
189 geometry_msgs::Point&
toMsg(
const tf2::Vector3& in, geometry_msgs::Point& out)
203 void fromMsg(
const geometry_msgs::Point& in, tf2::Vector3& out)
205 out = tf2::Vector3(in.x, in.y, in.z);
231 const std::string&
getFrameId(
const geometry_msgs::PointStamped&
t) {
return t.header.frame_id;}
239 geometry_msgs::PointStamped
toMsg(
const geometry_msgs::PointStamped& in)
250 void fromMsg(
const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out)
263 out.header.stamp = in.
stamp_;
265 out.point.x = in.getX();
266 out.point.y = in.getY();
267 out.point.z = in.getZ();
279 out.
stamp_ = msg.header.stamp;
281 out.
setData(tf2::Vector3(msg.point.x, msg.point.y, msg.point.z));
297 geometry_msgs::Quaternion out;
340 const std::string&
getFrameId(
const geometry_msgs::QuaternionStamped&
t) {
return t.header.frame_id;}
348 geometry_msgs::QuaternionStamped
toMsg(
const geometry_msgs::QuaternionStamped& in)
359 void fromMsg(
const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out)
372 geometry_msgs::QuaternionStamped out;
373 out.header.stamp = in.
stamp_;
375 out.quaternion.w = in.getW();
376 out.quaternion.x = in.getX();
377 out.quaternion.y = in.getY();
378 out.quaternion.z = in.getZ();
403 out.
stamp_ = in.header.stamp;
445 out.
setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z));
472 const std::string&
getFrameId(
const geometry_msgs::PoseStamped&
t) {
return t.header.frame_id;}
480 geometry_msgs::PoseStamped
toMsg(
const geometry_msgs::PoseStamped& in)
491 void fromMsg(
const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out)
504 out.header.stamp = in.
stamp_;
506 toMsg(in.getOrigin(), out.pose.position);
507 out.pose.orientation =
toMsg(in.getRotation());
519 out.
stamp_ = msg.header.stamp;
546 const std::string&
getFrameId(
const geometry_msgs::PoseWithCovarianceStamped&
t) {
return t.header.frame_id;}
554 geometry_msgs::PoseWithCovarianceStamped
toMsg(
const geometry_msgs::PoseWithCovarianceStamped& in)
565 void fromMsg(
const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs::PoseWithCovarianceStamped& out)
578 out.header.stamp = in.
stamp_;
580 toMsg(in.getOrigin(), out.pose.pose.position);
581 out.pose.pose.orientation =
toMsg(in.getRotation());
593 out.
stamp_ = msg.header.stamp;
612 geometry_msgs::Transform out;
656 const std::string&
getFrameId(
const geometry_msgs::TransformStamped&
t) {
return t.header.frame_id;}
664 geometry_msgs::TransformStamped
toMsg(
const geometry_msgs::TransformStamped& in)
675 void fromMsg(
const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out)
688 geometry_msgs::TransformStamped out;
689 out.header.stamp = in.
stamp_;
691 out.transform.translation =
toMsg(in.getOrigin());
692 out.transform.rotation =
toMsg(in.getRotation());
705 out.
stamp_ = msg.header.stamp;
720 void doTransform(
const geometry_msgs::Point& t_in, geometry_msgs::Point& t_out,
const geometry_msgs::TransformStamped& transform)
723 fromMsg(transform.transform, t);
726 tf2::Vector3 v_out = t * v_in;
738 void doTransform(
const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out,
const geometry_msgs::TransformStamped& transform)
741 t_out.header.stamp = transform.header.stamp;
742 t_out.header.frame_id = transform.header.frame_id;
753 void doTransform(
const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternion& t_out,
const geometry_msgs::TransformStamped& transform)
756 fromMsg(transform.transform.rotation, t);
760 t_out =
toMsg(q_out);
771 void doTransform(
const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out,
const geometry_msgs::TransformStamped& transform)
773 doTransform(t_in.quaternion, t_out.quaternion, transform);
774 t_out.header.stamp = transform.header.stamp;
775 t_out.header.frame_id = transform.header.frame_id;
787 void doTransform(
const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out,
const geometry_msgs::TransformStamped& transform)
795 fromMsg(transform.transform, t);
808 void doTransform(
const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out,
const geometry_msgs::TransformStamped& transform)
811 t_out.header.stamp = transform.header.stamp;
812 t_out.header.frame_id = transform.header.frame_id;
840 cov_in[6], cov_in[7], cov_in[8],
841 cov_in[12], cov_in[13], cov_in[14]);
843 cov_in[9], cov_in[10], cov_in[11],
844 cov_in[15], cov_in[16], cov_in[17]);
846 cov_in[24], cov_in[25], cov_in[26],
847 cov_in[30], cov_in[31], cov_in[32]);
849 cov_in[27], cov_in[28], cov_in[29],
850 cov_in[33], cov_in[34], cov_in[35]);
859 geometry_msgs::PoseWithCovariance::_covariance_type output;
860 output[0] = result_11[0][0];
861 output[1] = result_11[0][1];
862 output[2] = result_11[0][2];
863 output[6] = result_11[1][0];
864 output[7] = result_11[1][1];
865 output[8] = result_11[1][2];
866 output[12] = result_11[2][0];
867 output[13] = result_11[2][1];
868 output[14] = result_11[2][2];
870 output[3] = result_12[0][0];
871 output[4] = result_12[0][1];
872 output[5] = result_12[0][2];
873 output[9] = result_12[1][0];
874 output[10] = result_12[1][1];
875 output[11] = result_12[1][2];
876 output[15] = result_12[2][0];
877 output[16] = result_12[2][1];
878 output[17] = result_12[2][2];
880 output[18] = result_21[0][0];
881 output[19] = result_21[0][1];
882 output[20] = result_21[0][2];
883 output[24] = result_21[1][0];
884 output[25] = result_21[1][1];
885 output[26] = result_21[1][2];
886 output[30] = result_21[2][0];
887 output[31] = result_21[2][1];
888 output[32] = result_21[2][2];
890 output[21] = result_22[0][0];
891 output[22] = result_22[0][1];
892 output[23] = result_22[0][2];
893 output[27] = result_22[1][0];
894 output[28] = result_22[1][1];
895 output[29] = result_22[1][2];
896 output[33] = result_22[2][0];
897 output[34] = result_22[2][1];
898 output[35] = result_22[2][2];
911 void doTransform(
const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_msgs::PoseWithCovarianceStamped& t_out,
const geometry_msgs::TransformStamped& transform)
914 fromMsg(t_in.pose.pose.position, v);
916 fromMsg(t_in.pose.pose.orientation, r);
919 fromMsg(transform.transform, t);
921 toMsg(v_out, t_out.pose.pose);
922 t_out.header.stamp = transform.header.stamp;
923 t_out.header.frame_id = transform.header.frame_id;
936 void doTransform(
const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out,
const geometry_msgs::TransformStamped& transform)
939 fromMsg(t_in.transform, input);
942 fromMsg(transform.transform, t);
945 t_out.transform =
toMsg(v_out);
946 t_out.header.stamp = transform.header.stamp;
947 t_out.header.frame_id = transform.header.frame_id;
958 void doTransform(
const geometry_msgs::Vector3& t_in, geometry_msgs::Vector3& t_out,
const geometry_msgs::TransformStamped& transform)
961 fromMsg(transform.transform, t);
962 tf2::Vector3 v_out = t.
getBasis() * tf2::Vector3(t_in.x, t_in.y, t_in.z);
976 void doTransform(
const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out,
const geometry_msgs::TransformStamped& transform)
979 t_out.header.stamp = transform.header.stamp;
980 t_out.header.frame_id = transform.header.frame_id;
994 const std::string&
getFrameId(
const geometry_msgs::WrenchStamped&
t) {
return t.header.frame_id;}
998 geometry_msgs::WrenchStamped
toMsg(
const geometry_msgs::WrenchStamped& in)
1004 void fromMsg(
const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamped& out)
1011 geometry_msgs::WrenchStamped
toMsg(
const tf2::Stamped< boost::array<tf2::Vector3, 2> >& in, geometry_msgs::WrenchStamped& out)
1013 out.header.stamp = in.stamp_;
1014 out.header.frame_id = in.frame_id_;
1015 out.wrench.force =
toMsg(in[0]);
1016 out.wrench.torque =
toMsg(in[1]);
1024 out.
stamp_ = msg.header.stamp;
1027 fromMsg(msg.wrench.force, tmp);
1029 fromMsg(msg.wrench.torque, tmp1);
1030 boost::array<tf2::Vector3, 2> tmp_array;
1032 tmp_array[1] = tmp1;
1038 void doTransform(
const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out,
const geometry_msgs::TransformStamped& transform)
1041 doTransform(t_in.torque, t_out.torque, transform);
1047 void doTransform(
const geometry_msgs::WrenchStamped& t_in, geometry_msgs::WrenchStamped& t_out,
const geometry_msgs::TransformStamped& transform)
1049 doTransform(t_in.wrench, t_out.wrench, transform);
1050 t_out.header.stamp = transform.header.stamp;
1051 t_out.header.frame_id = transform.header.frame_id;
1056 #endif // TF2_GEOMETRY_MSGS_H
const std::string & getFrameId(const T &t)
TF2SIMD_FORCE_INLINE const tf2Scalar & getW() const
KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped &t) __attribute__((deprecated))
Convert a TransformStamped message to a KDL frame.
static Rotation Quaternion(double x, double y, double z, double w)
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
geometry_msgs::TransformStamped t
Matrix3x3 transpose() const
geometry_msgs::PoseWithCovariance::_covariance_type transformCovariance(const geometry_msgs::PoseWithCovariance::_covariance_type &cov_in, const tf2::Transform &transform)
Transform the covariance matrix of a PoseWithCovarianceStamped message to a new frame.
void fromMsg(const A &, B &b)
const ros::Time & getTimestamp(const T &t)
void setData(const T &input)