From 7f81d1ee8b046051f2b3da8124daaf8b250892e2 Mon Sep 17 00:00:00 2001 From: giacomo Date: Fri, 29 Nov 2024 11:47:28 +0100 Subject: [PATCH] Add pose and tf composition + composition tests --- CMakeLists.txt | 35 ++-- .../covariance_geometry_ros.hpp | 14 +- include/covariance_geometry_ros/utils.hpp | 7 - src/covariance_geometry_ros.cpp | 20 +++ test/composition_test.cpp | 159 ++++++++++-------- 5 files changed, 131 insertions(+), 104 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7744739..0ec9d24 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,8 @@ add_compile_options(-Wall -Wextra -Wpedantic -Wconversion -Wdouble-promotion -Wf find_package(ament_cmake REQUIRED) find_package(Eigen3 REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) include(ExternalProject) find_package(Git REQUIRED) @@ -57,7 +58,8 @@ ament_target_dependencies( ${PROJECT_NAME} Eigen3 geometry_msgs - tf2_ros + tf2 + tf2_geometry_msgs ) install( @@ -80,39 +82,30 @@ ament_export_dependencies( Eigen3 covariance_geometry_cpp geometry_msgs - tf2_ros + tf2 + tf2_geometry_msgs ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_find_gtest() + ament_find_gmock() + set(TEST_NAMES conversion_test - # composition_test + composition_test ) foreach(TEST_NAME ${TEST_NAMES}) ament_add_gtest(${TEST_NAME} test/${TEST_NAME}.cpp) - - add_dependencies( - ${TEST_NAME} - covariance_geometry_cpp - ) - target_link_libraries( ${TEST_NAME} - ${CMAKE_CURRENT_BINARY_DIR}/covariance_geometry_cpp/src/lib/libcovariance_geometry_cpp.so - ) - - ament_target_dependencies( - ${TEST_NAME} - Eigen3 - geometry_msgs - tf2_ros + ${PROJECT_NAME} ) - endforeach() endif() diff --git a/include/covariance_geometry_ros/covariance_geometry_ros.hpp b/include/covariance_geometry_ros/covariance_geometry_ros.hpp index 30f7e2e..a15ced4 100644 --- a/include/covariance_geometry_ros/covariance_geometry_ros.hpp +++ b/include/covariance_geometry_ros/covariance_geometry_ros.hpp @@ -14,9 +14,11 @@ #ifndef COVARIANCE_GEOMETRY_ROS__COVARIANCE_GEOMETRY_ROS_HPP_ #define COVARIANCE_GEOMETRY_ROS__COVARIANCE_GEOMETRY_ROS_HPP_ +#include + #include #include -// #include +#include using Pose = geometry_msgs::msg::Pose; using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; @@ -69,8 +71,14 @@ static inline PoseWithCovariance compose( } // Return-by-value versions using TF2 transforms: -// PoseWithCovariance compose(const PoseWithCovariance &a, -// const tf2::Transform &b); +Pose compose( + const Pose & a, + const tf2::Transform & b); + +PoseWithCovariance compose( + const PoseWithCovariance & a, + const tf2::Transform & b); + /** @} */ /** @name Pose inverse composition (a "as seen from" b): out = a (-) b @{ */ diff --git a/include/covariance_geometry_ros/utils.hpp b/include/covariance_geometry_ros/utils.hpp index c30488f..abc7835 100644 --- a/include/covariance_geometry_ros/utils.hpp +++ b/include/covariance_geometry_ros/utils.hpp @@ -18,7 +18,6 @@ #include #include -// #include using Pose = geometry_msgs::msg::Pose; using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; @@ -45,12 +44,6 @@ inline void fromROS(const PoseWithCovariance & in, PoseQuaternionCovarianceRPY & out.second = cov; } -// inline void fromROS(const tf2::Transform &in, PoseQuaternion &out) -// { -// out.first(in.getOrigin()); -// out.second(in.getRotation()); -// } - /** @name Convert from covariance geometry ROS type to ROS msgs @{ */ inline void toROS(const PoseQuaternion & in, Pose & out) diff --git a/src/covariance_geometry_ros.cpp b/src/covariance_geometry_ros.cpp index 7890c05..09bd887 100644 --- a/src/covariance_geometry_ros.cpp +++ b/src/covariance_geometry_ros.cpp @@ -66,6 +66,26 @@ void compose( toROS(pose_out, out); } +Pose compose( + const Pose & a, + const tf2::Transform & b) +{ + Pose b_msg, out; + tf2::toMsg(b, b_msg); + compose(a, b_msg, out); + return out; +} + +PoseWithCovariance compose( + const PoseWithCovariance & a, + const tf2::Transform & b) +{ + PoseWithCovariance b_msg, out; + tf2::toMsg(b, b_msg.pose); + compose(a, b_msg, out); + return out; +} + // Compose a with b^-1 void inverseCompose(const Pose & a, const Pose & b, Pose & out) { diff --git a/test/composition_test.cpp b/test/composition_test.cpp index 8618b42..fd937ee 100644 --- a/test/composition_test.cpp +++ b/test/composition_test.cpp @@ -11,96 +11,109 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "covariance_geometry/pose_covariance_composition.hpp" -#include "covariance_geometry_ros/utils.hpp" -#include "gtest/gtest.h" +#include + +#include +#include "covariance_geometry/pose_representation.hpp" +#include "covariance_geometry/pose_covariance_representation.hpp" +#include "covariance_geometry_ros/covariance_geometry_ros.hpp" -using Pose = geometry_msgs::msg::Pose; -using PoseWithCovariance = geometry_msgs::msg::PoseWithCovariance; +#include "gtest/gtest.h" namespace covariance_geometry { -const Eigen::Vector3d coord = {0.111, -1.24, 0.35}; // x, y, z -const Eigen::Vector3d rpy = {0.9067432, 0.4055079, 0.1055943}; // r, p, y -const Eigen::Quaterniond quat = {0.8746791, 0.4379822, 0.1581314, 0.1345454}; // w, x, y, z - -const Eigen::Vector3d rpy_gl = {0.12, M_PI_2, 0.34}; // r, p, y -const Eigen::Quaterniond quat_gl = {0.6884861, 0.1612045, 0.6884861, 0.1612045}; // w, x, y, z +const Eigen::Vector3d coord1 = {-0.333330, 0, 0.100000}; // x, y, z +const Eigen::Quaterniond quat1 = {0.707107, 0.0, 0.707107, -0.000005}; // w, x, y, z +const Eigen::Vector3d coord2 = {1.435644, 0, 0.0}; // x, y, z +const Eigen::Quaterniond quat2 = {1.0, 0.0, 0.0, 0.0}; // w, x, y, z TEST(Composition, ROSPoses) { - Pose pose_in; - PoseQuaternion pose_out; - pose_in.position.x = coord.x(); - pose_in.position.y = coord.y(); - pose_in.position.z = coord.z(); - pose_in.orientation.x = quat.x(); - pose_in.orientation.y = quat.y(); - pose_in.orientation.z = quat.z(); - pose_in.orientation.w = quat.w(); - fromROS(pose_in, pose_out); - EXPECT_TRUE(pose_out.first.isApprox(coord)); - EXPECT_TRUE(pose_out.second.isApprox(quat)); + geometry_msgs::msg::Pose pose1, pose2, pose_out; + + pose1.position.x = coord1.x(); + pose1.position.y = coord1.y(); + pose1.position.z = coord1.z(); + pose1.orientation.x = quat1.x(); + pose1.orientation.y = quat1.y(); + pose1.orientation.z = quat1.z(); + pose1.orientation.w = quat1.w(); + + pose2.position.x = coord2.x(); + pose2.position.y = coord2.y(); + pose2.position.z = coord2.z(); + pose2.orientation.x = quat2.x(); + pose2.orientation.y = quat2.y(); + pose2.orientation.z = quat2.z(); + pose2.orientation.w = quat2.w(); + + pose_out = compose(pose1, pose2); + EXPECT_NEAR(pose_out.position.x, coord1.x(), 1e-6); } -TEST(Conversion, ConvertPoseWithCovarianceFromROS) +TEST(Composition, ROSPoseAndTf) { - PoseWithCovariance pose_in; - PoseQuaternionCovarianceRPY pose_out; - pose_in.pose.position.x = coord.x(); - pose_in.pose.position.y = coord.y(); - pose_in.pose.position.z = coord.z(); - pose_in.pose.orientation.x = quat.x(); - pose_in.pose.orientation.y = quat.y(); - pose_in.pose.orientation.z = quat.z(); - pose_in.pose.orientation.w = quat.w(); - Eigen::Map> cov(pose_in.covariance.data()); - cov = Eigen::Matrix::Random(); - cov.selfadjointView(); - Eigen::Matrix cov_out; - cov_out = cov; - fromROS(pose_in, pose_out); - EXPECT_TRUE(pose_out.first.first.isApprox(coord)); - EXPECT_TRUE(pose_out.first.second.isApprox(quat)); - EXPECT_TRUE(pose_out.second.isApprox(cov_out)); + geometry_msgs::msg::Pose pose, pose_out; + tf2::Transform tf; + + pose.position.x = coord1.x(); + pose.position.y = coord1.y(); + pose.position.z = coord1.z(); + pose.orientation.x = quat1.x(); + pose.orientation.y = quat1.y(); + pose.orientation.z = quat1.z(); + pose.orientation.w = quat1.w(); + + tf.setOrigin(tf2::Vector3(coord2.x(), coord2.y(), coord2.z())); + tf.setRotation(tf2::Quaternion(quat2.x(), quat2.y(), quat2.z(), quat2.w())); + + pose_out = compose(pose, tf); + EXPECT_NEAR(pose_out.position.x, coord1.x(), 1e-6); } -TEST(Conversion, ConvertPoseToROS) +TEST(Composition, ROSPoseAndTfWithCovariance) { - PoseQuaternion pose_in; - Pose pose_out; - pose_in.first = coord; - pose_in.second = quat; - toROS(pose_in, pose_out); - EXPECT_DOUBLE_EQ(pose_out.position.x, coord.x()); - EXPECT_DOUBLE_EQ(pose_out.position.y, coord.y()); - EXPECT_DOUBLE_EQ(pose_out.position.z, coord.z()); - EXPECT_DOUBLE_EQ(pose_out.orientation.x, quat.x()); - EXPECT_DOUBLE_EQ(pose_out.orientation.y, quat.y()); - EXPECT_DOUBLE_EQ(pose_out.orientation.z, quat.z()); - EXPECT_DOUBLE_EQ(pose_out.orientation.w, quat.w()); + geometry_msgs::msg::PoseWithCovariance pose, pose_out; + tf2::Transform tf; + + pose.pose.position.x = coord1.x(); + pose.pose.position.y = coord1.y(); + pose.pose.position.z = coord1.z(); + pose.pose.orientation.x = quat1.x(); + pose.pose.orientation.y = quat1.y(); + pose.pose.orientation.z = quat1.z(); + pose.pose.orientation.w = quat1.w(); + + tf.setOrigin(tf2::Vector3(coord2.x(), coord2.y(), coord2.z())); + tf.setRotation(tf2::Quaternion(quat2.x(), quat2.y(), quat2.z(), quat2.w())); + + pose_out = compose(pose, tf); + EXPECT_NEAR(pose_out.pose.position.x, coord1.x(), 1e-6); } -TEST(Conversion, ConvertPoseWithCovarianceToROS) +TEST(Composition, ROSPosesWithCovariance) { - PoseQuaternionCovarianceRPY pose_in; - PoseWithCovariance pose_out; - pose_in.first.first = coord; - pose_in.first.second = quat; - pose_in.second = Eigen::Matrix::Random(); - pose_in.second.selfadjointView(); - - toROS(pose_in, pose_out); - EXPECT_DOUBLE_EQ(pose_out.pose.position.x, coord.x()); - EXPECT_DOUBLE_EQ(pose_out.pose.position.y, coord.y()); - EXPECT_DOUBLE_EQ(pose_out.pose.position.z, coord.z()); - EXPECT_DOUBLE_EQ(pose_out.pose.orientation.x, quat.x()); - EXPECT_DOUBLE_EQ(pose_out.pose.orientation.y, quat.y()); - EXPECT_DOUBLE_EQ(pose_out.pose.orientation.z, quat.z()); - EXPECT_DOUBLE_EQ(pose_out.pose.orientation.w, quat.w()); - Eigen::Map> cov(pose_out.covariance.data()); - EXPECT_TRUE(cov.isApprox(pose_in.second)); + geometry_msgs::msg::PoseWithCovariance pose1, pose2, pose_out; + + pose1.pose.position.x = coord1.x(); + pose1.pose.position.y = coord1.y(); + pose1.pose.position.z = coord1.z(); + pose1.pose.orientation.x = quat1.x(); + pose1.pose.orientation.y = quat1.y(); + pose1.pose.orientation.z = quat1.z(); + pose1.pose.orientation.w = quat1.w(); + + pose2.pose.position.x = coord2.x(); + pose2.pose.position.y = coord2.y(); + pose2.pose.position.z = coord2.z(); + pose2.pose.orientation.x = quat2.x(); + pose2.pose.orientation.y = quat2.y(); + pose2.pose.orientation.z = quat2.z(); + pose2.pose.orientation.w = quat2.w(); + + pose_out = compose(pose1, pose2); + EXPECT_NEAR(pose_out.pose.position.x, coord1.x(), 1e-6); } } // namespace covariance_geometry