diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index f2a1e4210..fe25dbfad 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -285,7 +285,7 @@ void doTransform(const Eigen::Quaterniond& t_in, const geometry_msgs::msg::TransformStamped& transform) { Eigen::Quaterniond t; fromMsg(transform.transform.rotation, t); - t_out = t.inverse() * t_in * t; + t_out = t * t_in; } /** \brief Convert a stamped Eigen Quaterniond type to a QuaternionStamped message. diff --git a/tf2_eigen/test/tf2_eigen-test.cpp b/tf2_eigen/test/tf2_eigen-test.cpp index ea9dc35b8..45d077c72 100644 --- a/tf2_eigen/test/tf2_eigen-test.cpp +++ b/tf2_eigen/test/tf2_eigen-test.cpp @@ -26,18 +26,26 @@ /** \author Wim Meeussen */ +// To get M_PI, especially on Windows. + #ifdef _MSC_VER #ifndef _USE_MATH_DEFINES #define _USE_MATH_DEFINES #endif #endif -// To get M_PI, especially on Windows. #include -#include + #include #include +#include +#include +#include + + +#include +#include // TODO(clalancette) Re-enable these tests once we have tf2/convert.h:convert(A, B) implemented // TEST(TfEigen, ConvertVector3dStamped) @@ -124,7 +132,151 @@ TEST(TfEigen, ConvertTransform) EXPECT_TRUE(tm.isApprox(Iback.matrix())); } -int main(int argc, char **argv){ +struct EigenBufferTransform : public ::testing::Test +{ + static void SetUpTestSuite() + { + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf_buffer = std::make_unique(clock); + tf_buffer->setUsingDedicatedThread(true); + + // populate buffer + geometry_msgs::msg::TransformStamped t; + t.transform.translation.x = 10; + t.transform.translation.y = 20; + t.transform.translation.z = 30; + t.transform.rotation.w = 0; + t.transform.rotation.x = 1; + t.transform.rotation.y = 0; + t.transform.rotation.z = 0; + t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + t.header.frame_id = "A"; + t.child_frame_id = "B"; + tf_buffer->setTransform(t, "test"); + } + + template + void testEigenTransform(); + + ::testing::AssertionResult doTestEigenQuaternion( + const Eigen::Quaterniond & parameter, const Eigen::Quaterniond & expected); + + static std::unique_ptr tf_buffer; + static constexpr double EPS = 1e-3; +}; + +std::unique_ptr EigenBufferTransform::tf_buffer; + +template +void EigenBufferTransform::testEigenTransform() +{ + using T = Eigen::Transform; + using stampedT = tf2::Stamped; + + const stampedT i1{ + T{Eigen::Translation3d{1, 2, 3} *Eigen::Quaterniond{0, 1, 0, 0}}, tf2::timeFromSec(2), "A"}; + + // simple api + const stampedT i_simple = tf_buffer->transform(i1, "B", tf2::durationFromSec(2.0)); + EXPECT_NEAR(i_simple.translation().x(), -9, EPS); + EXPECT_NEAR(i_simple.translation().y(), 18, EPS); + EXPECT_NEAR(i_simple.translation().z(), 27, EPS); + const auto q1 = Eigen::Quaterniond(i_simple.linear()); + EXPECT_NEAR(q1.x(), 0.0, EPS); + EXPECT_NEAR(q1.y(), 0.0, EPS); + EXPECT_NEAR(q1.z(), 0.0, EPS); + EXPECT_NEAR(q1.w(), 1.0, EPS); + + // advanced api + const stampedT v_advanced = + tf_buffer->transform(i1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); + EXPECT_NEAR(v_advanced.translation().x(), -9, EPS); + EXPECT_NEAR(v_advanced.translation().y(), 18, EPS); + EXPECT_NEAR(v_advanced.translation().z(), 27, EPS); + const auto q2 = Eigen::Quaterniond(v_advanced.linear()); + EXPECT_NEAR(q2.x(), 0.0, EPS); + EXPECT_NEAR(q2.y(), 0.0, EPS); + EXPECT_NEAR(q2.z(), 0.0, EPS); + EXPECT_NEAR(q2.w(), 1.0, EPS); +} + +TEST_F(EigenBufferTransform, Affine3d) { + testEigenTransform(); +} + +TEST_F(EigenBufferTransform, Isometry3d) { + testEigenTransform(); +} + +TEST_F(EigenBufferTransform, Vector) +{ + const tf2::Stamped v1{{1, 2, 3}, tf2::timeFromSec(2), "A"}; + + // simple api + const tf2::Stamped v_simple = + tf_buffer->transform(v1, "B", tf2::durationFromSec(2.0)); + EXPECT_NEAR(v_simple.x(), -9, EPS); + EXPECT_NEAR(v_simple.y(), 18, EPS); + EXPECT_NEAR(v_simple.z(), 27, EPS); + + // advanced api + const tf2::Stamped v_advanced = + tf_buffer->transform(v1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); + EXPECT_NEAR(v_advanced.x(), -9, EPS); + EXPECT_NEAR(v_advanced.y(), 18, EPS); + EXPECT_NEAR(v_advanced.z(), 27, EPS); +} + +// helper method for Quaternion tests +::testing::AssertionResult EigenBufferTransform::doTestEigenQuaternion( + const Eigen::Quaterniond & parameter, const Eigen::Quaterniond & expected) +{ + const tf2::Stamped q1{parameter, tf2::timeFromSec(2), "A"}; + // avoid linking error + const double eps = EPS; + + // simple api + const tf2::Stamped q_simple = + tf_buffer->transform(q1, "B", tf2::durationFromSec(2.0)); + // compare rotation matrices, as the quaternions can be ambigous + EXPECT_TRUE(q_simple.toRotationMatrix().isApprox(expected.toRotationMatrix(), eps)); + + // advanced api + const tf2::Stamped q_advanced = + tf_buffer->transform(q1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); + EXPECT_TRUE(q_advanced.toRotationMatrix().isApprox(expected.toRotationMatrix(), eps)); + return ::testing::AssertionSuccess(); +} + +TEST_F(EigenBufferTransform, QuaternionRotY) +{ + // rotated by -90° around y + // 0, 0, -1 + // 0, 1, 0, + // 1, 0, 0 + const Eigen::Quaterniond param{Eigen::AngleAxisd(-1 * M_PI_2, Eigen::Vector3d::UnitY())}; + const Eigen::Quaterniond expected{0, M_SQRT1_2, 0, -1 * M_SQRT1_2}; + EXPECT_TRUE(doTestEigenQuaternion(param, expected)); +} + +TEST_F(EigenBufferTransform, QuaternionRotX) +{ + // rotated by 90° around y + const Eigen::Quaterniond param{Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX())}; + const Eigen::Quaterniond expected{Eigen::AngleAxisd(-1 * M_PI_2, Eigen::Vector3d::UnitX())}; + EXPECT_TRUE(doTestEigenQuaternion(param, expected)); +} + +TEST_F(EigenBufferTransform, QuaternionRotZ) +{ + // rotated by 180° around z + const Eigen::Quaterniond param{Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())}; + const Eigen::Quaterniond expected{Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY())}; + EXPECT_TRUE(doTestEigenQuaternion(param, expected)); +} + +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 555ceba2c..2746d0b46 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -29,6 +29,14 @@ /** \author Wim Meeussen */ +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif + +// To get M_PI, especially on Windows. +#include #include #include @@ -230,6 +238,39 @@ TEST(TfGeometry, Point) EXPECT_NEAR(v_advanced.point.z, 27, EPS); } +TEST(TfGeometry, Quaternion) +{ + // rotated by -90° around y + // 0, 0, -1 + // 0, 1, 0, + // 1, 0, 0 + geometry_msgs::msg::QuaternionStamped q1, res; + q1.quaternion.x = 0; + q1.quaternion.y = -1 * M_SQRT1_2; + q1.quaternion.z = 0; + q1.quaternion.w = M_SQRT1_2; + q1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); + q1.header.frame_id = "A"; + + // simple api + const geometry_msgs::msg::QuaternionStamped q_simple = tf_buffer->transform( + q1, "B", tf2::durationFromSec( + 2.0)); + EXPECT_NEAR(q_simple.quaternion.x, M_SQRT1_2, EPS); + EXPECT_NEAR(q_simple.quaternion.y, 0, EPS); + EXPECT_NEAR(q_simple.quaternion.z, -1 * M_SQRT1_2, EPS); + EXPECT_NEAR(q_simple.quaternion.w, 0, EPS); + + // advanced api + const geometry_msgs::msg::QuaternionStamped q_advanced = tf_buffer->transform( + q1, "B", tf2::timeFromSec(2.0), + "A", tf2::durationFromSec(3.0)); + EXPECT_NEAR(q_advanced.quaternion.x, M_SQRT1_2, EPS); + EXPECT_NEAR(q_advanced.quaternion.y, 0, EPS); + EXPECT_NEAR(q_advanced.quaternion.z, -1 * M_SQRT1_2, EPS); + EXPECT_NEAR(q_advanced.quaternion.w, 0, EPS); +} + int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv);