diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index 00c789ba7..c84fef8c7 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -36,6 +36,11 @@ #include #include +#define TF2_EIGEN_IGNORE_DEPRECATED_CALL_BEGIN \ +_Pragma("GCC diagnostic push")\ +_Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") +#define TF2_EIGEN_IGNORE_DEPRECATED_CALL_END \ +_Pragma("GCC diagnostic pop") namespace tf2 { @@ -62,9 +67,10 @@ Eigen::Isometry3d transformToEigen(const geometry_msgs::TransformStamped& t) { /** \brief Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type. * \param T The transform to convert, as an Eigen Affine3d transform. * \return The transform converted to a TransformStamped message. + * \deprecated Use Isometry3d instead. */ inline -geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d& T) +ROS_DEPRECATED geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d& T) { geometry_msgs::TransformStamped t; t.transform.translation.x = T.translation().x(); @@ -223,12 +229,14 @@ void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped inline -void doTransform(const Eigen::Affine3d& t_in, - Eigen::Affine3d& t_out, - const geometry_msgs::TransformStamped& transform) { +ROS_DEPRECATED void doTransform( + const Eigen::Affine3d& t_in, + Eigen::Affine3d& t_out, + const geometry_msgs::TransformStamped& transform) { t_out = Eigen::Affine3d(transformToEigen(transform) * t_in); } @@ -330,9 +338,10 @@ void doTransform(const tf2::Stamped& t_in, * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The Eigen Affine3d to convert. * \return The Eigen transform converted to a Pose message. + * \deprecated Use Isometry3d instead. */ inline -geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { +ROS_DEPRECATED geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { geometry_msgs::Pose msg; msg.position.x = in.translation().x(); msg.position.y = in.translation().y(); @@ -380,9 +389,10 @@ geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) { * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param msg The Pose message to convert. * \param out The pose converted to a Eigen Affine3d. + * \deprecated USe Isometry3d instead. */ inline -void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { +ROS_DEPRECATED void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { out = Eigen::Affine3d( Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) * Eigen::Quaterniond(msg.orientation.w, @@ -446,10 +456,11 @@ void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { * \param t_in The frame to transform, as a timestamped Eigen Affine3d transform. * \param t_out The transformed frame, as a timestamped Eigen Affine3d transform. * \param transform The timestamped transform to apply, as a TransformStamped message. + * \deprecated Use Isometry3d instead. */ template <> inline -void doTransform(const tf2::Stamped& t_in, +ROS_DEPRECATED void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out = tf2::Stamped(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id); @@ -476,14 +487,17 @@ void doTransform(const tf2::Stamped& t_in, * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped Eigen Affine3d to convert. * \return The Eigen transform converted to a PoseStamped message. + * \deprecated Use Isometry3d instead. */ inline -geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) +ROS_DEPRECATED geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) { geometry_msgs::PoseStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; + TF2_EIGEN_IGNORE_DEPRECATED_CALL_BEGIN msg.pose = toMsg(static_cast(in)); + TF2_EIGEN_IGNORE_DEPRECATED_CALL_END return msg; } @@ -501,13 +515,17 @@ geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param msg The PoseStamped message to convert. * \param out The pose converted to a timestamped Eigen Affine3d. + * \deprecated Use Isometry3d instead. */ inline -void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) +ROS_DEPRECATED void fromMsg(const geometry_msgs::PoseStamped& msg, + tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; + TF2_EIGEN_IGNORE_DEPRECATED_CALL_BEGIN fromMsg(msg.pose, static_cast(out)); + TF2_EIGEN_IGNORE_DEPRECATED_CALL_END } inline @@ -531,8 +549,10 @@ namespace Eigen { // tf2::convert(). inline -geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { +ROS_DEPRECATED geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { + TF2_EIGEN_IGNORE_DEPRECATED_CALL_BEGIN return tf2::toMsg(in); + TF2_EIGEN_IGNORE_DEPRECATED_CALL_END } inline @@ -551,8 +571,10 @@ geometry_msgs::Point toMsg(const Eigen::Vector3d& in) { } inline -void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { +ROS_DEPRECATED void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { + TF2_EIGEN_IGNORE_DEPRECATED_CALL_BEGIN tf2::fromMsg(msg, out); + TF2_EIGEN_IGNORE_DEPRECATED_CALL_END } inline @@ -582,4 +604,7 @@ void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { } // namespace +#undef TF2_EIGEN_IGNORE_DEPRECATED_CALL_BEGIN +#undef TF2_EIGEN_IGNORE_DEPRECATED_CALL_END + #endif // TF2_EIGEN_H diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index f175e6c0a..03f23726c 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -26,10 +26,17 @@ /** \author Wim Meeussen */ +#define TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_BEGIN \ +_Pragma("GCC diagnostic push")\ +_Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") +#define TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_END \ +_Pragma("GCC diagnostic pop") +TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_BEGIN #include -#include #include +TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_END +#include TEST(TfEigen, ConvertVector3dStamped) { @@ -93,7 +100,9 @@ TEST(TfEigen, TransformQuaterion) { const Eigen::Affine3d affine(iso); const tf2::Stamped expected(Eigen::Quaterniond(Eigen::AngleAxisd(1, Eigen::Vector3d::UnitZ())), ros::Time(10), "expected"); + TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_BEGIN geometry_msgs::TransformStamped trafo = tf2::eigenToTransform(affine); + TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_END trafo.header.stamp = ros::Time(10); trafo.header.frame_id = "expected"; @@ -122,8 +131,10 @@ TEST(TfEigen, ConvertAffine3dStamped) tf2::Stamped v1; geometry_msgs::PoseStamped p1; + TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_BEGIN tf2::convert(v, p1); tf2::convert(p1, v1); + TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_END EXPECT_EQ(v.translation(), v1.translation()); EXPECT_EQ(v.rotation(), v1.rotation()); @@ -153,8 +164,10 @@ TEST(TfEigen, ConvertAffine3d) Eigen::Affine3d v1; geometry_msgs::Pose p1; + TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_BEGIN tf2::convert(v, p1); tf2::convert(p1, v1); + TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_END EXPECT_EQ(v.translation(), v1.translation()); EXPECT_EQ(v.rotation(), v1.rotation()); @@ -188,7 +201,9 @@ TEST(TfEigen, ConvertTransform) Eigen::Affine3d T(tm); + TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_BEGIN geometry_msgs::TransformStamped msg = tf2::eigenToTransform(T); + TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_END Eigen::Affine3d Tback = tf2::transformToEigen(msg); EXPECT_TRUE(T.isApprox(Tback)); @@ -197,7 +212,9 @@ TEST(TfEigen, ConvertTransform) // same for Isometry Eigen::Isometry3d I(tm); + TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_BEGIN msg = tf2::eigenToTransform(T); + TF2_EIGEN_TEST_IGNORE_DEPRECATED_CALL_END Eigen::Isometry3d Iback = tf2::transformToEigen(msg); EXPECT_TRUE(I.isApprox(Iback));