diff --git a/.github/workflows/humble-ci.yml b/.github/workflows/humble-ci.yml index 96f1b9b..5545d9e 100644 --- a/.github/workflows/humble-ci.yml +++ b/.github/workflows/humble-ci.yml @@ -19,6 +19,6 @@ jobs: run: | docker run --rm -v $(pwd):/workspace -w /workspace \ glider-ros:humble \ - bash -c "cd glider && cmake -S . -B build -DBUILD_ROS=OFF -DBUILD_TESTS=ON \ + bash -c "cd glider && cmake -S . -B build -DBUILD_TESTS=ON \ && cd build && cmake --build . && ctest --output-on-failure" diff --git a/.github/workflows/jazzy-ci.yml b/.github/workflows/jazzy-ci.yml index 83f5256..05572f7 100644 --- a/.github/workflows/jazzy-ci.yml +++ b/.github/workflows/jazzy-ci.yml @@ -19,5 +19,5 @@ jobs: run: | docker run --rm -v $(pwd):/workspace -w /workspace \ glider-ros:jazzy \ - bash -c "cd glider && cmake -S . -B build -DBUILD_ROS=OFF -DBUILD_TESTS=ON \ + bash -c "cd glider && cmake -S . -B build -DBUILD_TESTS=ON \ && cd build && cmake --build . && ctest --output-on-failure" diff --git a/glider/CMakeLists.txt b/glider/CMakeLists.txt index 028967b..2e355d2 100644 --- a/glider/CMakeLists.txt +++ b/glider/CMakeLists.txt @@ -199,7 +199,14 @@ if (BUILD_TESTS) GTest::Main ${PROJECT_NAME} ) - + + add_executable(ros_conversion_test test/test_ros_conversions.cpp) + target_link_libraries(ros_conversion_test + GTest::GTest + GTest::Main + ${PROJECT_NAME} + ${PROJECT_NAME}_ros + ) include(GoogleTest) gtest_discover_tests(utm_test) @@ -209,4 +216,5 @@ if (BUILD_TESTS) gtest_discover_tests(glider_test) gtest_discover_tests(odometry_test) gtest_discover_tests(odometry_with_cov_test) + gtest_discover_tests(ros_conversion_test) endif() diff --git a/glider/test/test_ros_conversions.cpp b/glider/test/test_ros_conversions.cpp new file mode 100644 index 0000000..22c93a2 --- /dev/null +++ b/glider/test/test_ros_conversions.cpp @@ -0,0 +1,203 @@ +/*! +* Jason Hughes +* October 2025 +* +* Unit tests for ros to eigen and +* eigen to ros convertions +*/ + +#include + +#include "ros/conversions.hpp" + +TEST(RosToEigenTestSuite, Vector3Test) +{ + geometry_msgs::msg::Vector3 msg; + msg.x = 1.0; + msg.y = 1.0; + msg.z = 1.0; + + Eigen::Vector3d gt(msg.x, msg.y, msg.z); + + ASSERT_EQ(GliderROS::Conversions::rosToEigen(msg), gt); +} + +TEST(RosToEigenTestSuite, OrientTest) +{ + geometry_msgs::msg::Quaternion msg; + msg.w = 1.0; + msg.x = 0.0; + msg.y = 0.0; + msg.z = 0.0; + + Eigen::Vector4d gt(msg.w, msg.x, msg.y, msg.z); + + ASSERT_EQ(GliderROS::Conversions::rosToEigen(msg), gt); +} + +TEST(RosToEigenTestSuite, NavSatFixTest) +{ + sensor_msgs::msg::NavSatFix msg; + msg.latitude = 32.925; + msg.longitude = -75.119; + msg.altitude = 10.0; + + Eigen::Vector3d gt(msg.latitude, msg.longitude, msg.altitude); + + ASSERT_EQ(GliderROS::Conversions::rosToEigen(msg), gt); +} + +TEST(RosToEigenTestSuite, PoseTest) +{ + geometry_msgs::msg::PoseStamped msg; + msg.pose.position.x = 1.0; + msg.pose.position.y = 1.0; + msg.pose.position.z = 1.0; + msg.pose.orientation.w = 1.0; + msg.pose.orientation.x = 0.0; + msg.pose.orientation.y = 0.0; + msg.pose.orientation.z = 0.0; + + Eigen::Quaterniond quat(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z); + Eigen::Vector3d trans(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z); + + Eigen::Isometry3d gt = Eigen::Isometry3d::Identity(); + gt.linear() = quat.toRotationMatrix(); + gt.translation() = trans; + + ASSERT_EQ(GliderROS::Conversions::rosToEigen(msg).linear(), gt.linear()); + ASSERT_EQ(GliderROS::Conversions::rosToEigen(msg).translation(), gt.translation()); +} + +TEST(RosToEigenTestSuite, OdometryTest) +{ + nav_msgs::msg::Odometry msg; + msg.pose.pose.position.x = 1.0; + msg.pose.pose.position.y = 1.0; + msg.pose.pose.position.z = 1.0; + msg.pose.pose.orientation.w = 1.0; + msg.pose.pose.orientation.x = 0.0; + msg.pose.pose.orientation.y = 0.0; + msg.pose.pose.orientation.z = 0.0; + msg.twist.twist.linear.x = 1.0; + msg.twist.twist.linear.y = 1.0; + msg.twist.twist.linear.z = 1.0; + + Eigen::Isometry3d odom = GliderROS::Conversions::rosToEigen(msg); + + Eigen::Quaterniond quat(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z); + Eigen::Vector3d trans(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z); + + Eigen::Isometry3d gt = Eigen::Isometry3d::Identity(); + gt.linear() = quat.toRotationMatrix(); + gt.translation() = trans; + + ASSERT_EQ(odom.linear(), gt.linear()); + ASSERT_EQ(odom.translation(), gt.translation()); +} + +TEST(EigenToRosTestSuite, Vector3Test) +{ + Eigen::Vector3d in(1.0, 1.0, 1.0); + + geometry_msgs::msg::Vector3 msg = GliderROS::Conversions::eigenToRos(in); + + ASSERT_EQ(msg.x, in(0)); + ASSERT_EQ(msg.y, in(1)); + ASSERT_EQ(msg.z, in(2)); +} + +TEST(EigenToRosTestSuite, NavSatFixTest) +{ + Eigen::Vector3d in(32.925, -75.119, 10.0); + + sensor_msgs::msg::NavSatFix msg = GliderROS::Conversions::eigenToRos(in); + + ASSERT_EQ(msg.latitude, in(0)); + ASSERT_EQ(msg.longitude, in(1)); + ASSERT_EQ(msg.altitude, in(2)); +} + +TEST(EigenToRosTestSuite, OrientTest) +{ + Eigen::Vector4d in(1.0, 0.0, 0.0, 0.0); + + geometry_msgs::msg::Quaternion msg = GliderROS::Conversions::eigenToRos(in); + + ASSERT_EQ(msg.w, in(0)); + ASSERT_EQ(msg.x, in(1)); + ASSERT_EQ(msg.y, in(2)); + ASSERT_EQ(msg.z, in(3)); +} + +TEST(EigenToRosTestSuite, PoseTest) +{ + Eigen::Isometry3d in = Eigen::Isometry3d::Identity(); + Eigen::Quaterniond quat(1.0, 0.0, 0.0, 0.0); + Eigen::Vector3d t(1.0, 1.0, 1.0); + + in.linear() = quat.toRotationMatrix(); + in.translation() = t; + + geometry_msgs::msg::PoseStamped msg = GliderROS::Conversions::eigenToRos(in); + + ASSERT_EQ(msg.pose.position.x, t(0)); + ASSERT_EQ(msg.pose.position.y, t(1)); + ASSERT_EQ(msg.pose.position.z, t(2)); + + ASSERT_EQ(msg.pose.orientation.w, quat.w()); + ASSERT_EQ(msg.pose.orientation.x, quat.x()); + ASSERT_EQ(msg.pose.orientation.y, quat.y()); + ASSERT_EQ(msg.pose.orientation.z, quat.z()); +} + +TEST(OdometryToRosTestSuite, OdometryTest) +{ + int64_t timestamp = 1; + gtsam::Rot3 rot = gtsam::Rot3::Quaternion(1.0, 0.0, 0.0, 0.0); + gtsam::Point3 t(482981.6098624719, 4421256.568684888, 10.0); + gtsam::Point3 v(0.0, 0.0, 0.0); + + gtsam::NavState ns(rot, t, v); + + Glider::Odometry odom(ns, timestamp, true); + + nav_msgs::msg::Odometry msg = GliderROS::Conversions::odomToRos(odom); + + ASSERT_EQ(msg.pose.pose.position.x, odom.getPosition()(0)); + ASSERT_EQ(msg.pose.pose.position.y, odom.getPosition()(1)); + ASSERT_EQ(msg.pose.pose.position.z, odom.getPosition()(2)); + + ASSERT_EQ(msg.pose.pose.orientation.w, odom.getOrientation()(0)); + ASSERT_EQ(msg.pose.pose.orientation.x, odom.getOrientation()(1)); + ASSERT_EQ(msg.pose.pose.orientation.y, odom.getOrientation()(2)); + ASSERT_EQ(msg.pose.pose.orientation.z, odom.getOrientation()(3)); + + ASSERT_EQ(msg.twist.twist.linear.x, odom.getVelocity()(0)); + ASSERT_EQ(msg.twist.twist.linear.y, odom.getVelocity()(1)); + ASSERT_EQ(msg.twist.twist.linear.z, odom.getVelocity()(2)); + + ASSERT_STREQ(msg.header.frame_id.c_str(), "enu"); + ASSERT_GT(msg.header.stamp.nanosec, 0); +} + +TEST(OdometryToRosTestSuite, NavSatFixTest) +{ + int64_t timestamp = 1; + gtsam::Rot3 rot = gtsam::Rot3::Quaternion(1.0, 0.0, 0.0, 0.0); + gtsam::Point3 t(482981.6098624719, 4421256.568684888, 10.0); + gtsam::Point3 v(0.0, 0.0, 0.0); + + gtsam::NavState ns(rot, t, v); + + Glider::Odometry odom(ns, timestamp, true); + + sensor_msgs::msg::NavSatFix msg = GliderROS::Conversions::odomToRos(odom, "18S"); + + ASSERT_NEAR(msg.latitude, 39.941259, 0.001); + ASSERT_NEAR(msg.longitude, -75.199202, 0.001); + ASSERT_EQ(msg.altitude, 10.0); + ASSERT_STREQ(msg.header.frame_id.c_str(), "enu"); + ASSERT_GT(msg.header.stamp.nanosec, 0); +} +