refactor: restructure vortex_utils and add more C++ utils

CMakeLists.txt

@@ -1,17 +1,24 @@
 cmake_minimum_required(VERSION 3.8)
 project(vortex_utils)
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
+set(CMAKE_CXX_STANDARD 17)
+add_compile_options(-Wall -Wextra -Wpedantic)
 
 find_package(ament_cmake REQUIRED)
+find_package(rclcpp)
 find_package(ament_cmake_python REQUIRED)
 find_package(Eigen3 REQUIRED)
 
 include_directories(include)
 
-add_library(${PROJECT_NAME} src/cpp_utils.cpp)
+add_library(${PROJECT_NAME} SHARED
+  src/math.cpp)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp Eigen3)
 
 ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
 
@@ -36,28 +43,8 @@ ament_export_libraries(${PROJECT_NAME})
 ament_python_install_package(${PROJECT_NAME})
 
 if(BUILD_TESTING)
-  find_package(ament_cmake_pytest REQUIRED)
-  set(_pytest_tests
-    tests/test_utils.py
-  )
-  foreach(_test_path ${_pytest_tests})
-    get_filename_component(_test_name ${_test_path} NAME_WE)
-    ament_add_pytest_test(${_test_name} ${_test_path}
-      APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
-      WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
-    )
-  endforeach()
-
-  find_package(ament_cmake_gtest REQUIRED)
-  ament_add_gtest(${PROJECT_NAME}_test
-    test/test_cpp_utils.cpp
-  )
-  target_include_directories(${PROJECT_NAME}_test PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>
-  )
-  target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME})
-
+  add_subdirectory(py_test)
+  add_subdirectory(cpp_test)
 endif()
 
 ament_package()

README.md

@@ -3,23 +3,28 @@
 [![pre-commit.ci status](https://results.pre-commit.ci/badge/github/vortexntnu/vortex-utils/main.svg)](https://results.pre-commit.ci/latest/github/vortexntnu/vortex-utils/main)
 [![codecov](https://codecov.io/github/vortexntnu/vortex-utils/graph/badge.svg?token=d6D7d5xNdf)](https://codecov.io/github/vortexntnu/vortex-utils)
 
-This repository contains general, often-used utility functions and structs for both C++ and Python.
-
+This package contains common definitions and often-used utility functions in C++ and Python.
 
 # Usage
 
-
 In Python, import your desired function/dataclass like for example:
 ```python
 from vortex_utils.python_utils import ssa
 ```
+```python
+from vortex_utils.qos_profiles import sensor_data_profile, reliable_profile
+```
 
-In C++, include vortex_utils like
+In C++, include
+```C++
+#include <vortex/utils/math.hpp>
+```
+for mathematical functions,
 ```C++
-#include <vortex_utils/cpp_utils.hpp>
+#include <vortex/utils/qos_profiles.hpp>
 ```
-and in the code
+for common QoS profile definitions, and
 ```C++
-double some_angle = 3.14;
-double angle_ssa = vortex_utils::ssa(some_angle);
+#include <vortex/utils/types.hpp>
 ```
+for common structs like 6DOF `Eta` and `Nu`.

cpp_test/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.8)
+
+find_package(GTest REQUIRED)
+include(GoogleTest)
+
+set(TEST_BINARY_NAME ${PROJECT_NAME}_test)
+add_executable(
+    ${TEST_BINARY_NAME}
+    test_math.cpp
+    test_types.cpp
+)
+
+target_link_libraries(
+    ${TEST_BINARY_NAME}
+    PRIVATE
+    ${PROJECT_NAME}
+    GTest::GTest
+    GTest::gtest_main
+)
+
+ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3)
+
+gtest_discover_tests(${TEST_BINARY_NAME})

cpp_test/test_main.cpp

@@ -0,0 +1,6 @@
+#include <gtest/gtest.h>
+
+int main(int argc, char** argv) {
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}

cpp_test/test_math.cpp

@@ -0,0 +1,171 @@
+#include <gtest/gtest.h>
+#include "vortex/utils/math.hpp"
+
+namespace vortex::utils::math {
+
+// Test that the value does not change when already in the interval [-pi, pi]
+TEST(ssa, test_ssa_0) {
+    EXPECT_EQ(0, ssa(0));
+}
+
+// Test that 2 pi correctly maps to 0
+TEST(ssa, test_ssa_2pi) {
+    EXPECT_EQ(0, ssa(2 * M_PI));
+}
+
+// Test that values over pi gets mapped to the negative interval
+TEST(ssa, test_ssa_3_5) {
+    EXPECT_NEAR(-2.78, ssa(3.5), 0.01);
+}
+
+// Test that values under -pi gets mapped to the positive interval
+TEST(ssa, test_ssa_minus_3_5) {
+    EXPECT_NEAR(2.78, ssa(-3.5), 0.01);
+}
+
+// Test that the skew-symmetric matrix is correctly calculated
+TEST(get_skew_symmetric_matrix, test_skew_symmetric) {
+    Eigen::Vector3d vector(1, 2, 3);
+    Eigen::Matrix3d expected;
+    expected << 0, -3, 2, 3, 0, -1, -2, 1, 0;
+
+    Eigen::Matrix3d result = get_skew_symmetric_matrix(vector);
+    EXPECT_EQ(expected, result);
+}
+
+// Test that rotation matrix is correctly constructed
+TEST(get_rotation_matrix, test_rotation_matrix) {
+    double roll{1.0};
+    double pitch{2.0};
+    double yaw{3.0};
+    Eigen::Matrix3d expected;
+    expected << 0.41198225, -0.83373765, -0.36763046, -0.05872664, -0.42691762,
+        0.90238159, -0.90929743, -0.35017549, -0.2248451;
+    Eigen::Matrix3d result = get_rotation_matrix(roll, pitch, yaw);
+    EXPECT_NEAR(0.0, matrix_norm_diff(expected, result), 0.01);
+}
+
+TEST(get_transformation_matrix_attitude, test_transformation_matrix_zeros) {
+    Eigen::Matrix3d transformation_matrix{
+        get_transformation_matrix_attitude(0.0, 0.0)};
+    Eigen::Matrix3d expected{Eigen::Matrix3d::Identity()};
+    EXPECT_NEAR(0.0, matrix_norm_diff(expected, transformation_matrix), 0.01);
+}
+
+// Test that the identity quaternion correctly maps to 0, 0, 0
+TEST(quat_to_euler, test_quat_to_euler_1) {
+    Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0);
+    Eigen::Vector3d expected(0.0, 0.0, 0.0);
+    Eigen::Vector3d result = quat_to_euler(q);
+    for (int i = 0; i < 3; ++i) {
+        EXPECT_NEAR(expected[i], result[i], 0.01);
+    }
+}
+
+// Test that only changing w and x in the quat only affects roll
+TEST(quat_to_euler, test_quat_to_euler_2) {
+    Eigen::Quaterniond q2(0.707, 0.707, 0.0, 0.0);
+    Eigen::Vector3d expected2(1.57, 0.0, 0.0);
+    Eigen::Vector3d result2 = quat_to_euler(q2);
+    for (int i = 0; i < 3; ++i) {
+        EXPECT_NEAR(expected2[i], result2[i], 0.01);
+    }
+}
+
+// Test that only changing w and z in the quat only affects yaw
+TEST(quat_to_euler, test_quat_to_euler_3) {
+    Eigen::Quaterniond q4(0.707, 0.0, 0.0, 0.707);
+    Eigen::Vector3d expected4(0.0, 0.0, 1.57);
+    Eigen::Vector3d result4 = quat_to_euler(q4);
+    for (int i = 0; i < 3; ++i) {
+        EXPECT_NEAR(expected4[i], result4[i], 0.01);
+    }
+}
+
+// Test that a quaternion is correctly converted to euler angles
+TEST(quat_to_euler, test_quat_to_euler_4) {
+    Eigen::Quaterniond q5(0.770, 0.4207, -0.4207, -0.229);
+    Eigen::Vector3d expected5(1.237, -0.4729, -0.9179);
+    Eigen::Vector3d result5 = quat_to_euler(q5);
+    for (int i = 0; i < 3; ++i) {
+        EXPECT_NEAR(expected5[i], result5[i], 0.01);
+    }
+}
+
+// Test that a quaternion with flipped signs is correctly converted to euler
+// angles
+TEST(quat_to_euler, test_quat_to_euler_5) {
+    Eigen::Quaterniond q5(0.770, 0.4207, 0.4207, 0.229);
+    Eigen::Vector3d expected5(1.237, 0.4729, 0.9179);
+    Eigen::Vector3d result5 = quat_to_euler(q5);
+    for (int i = 0; i < 3; ++i) {
+        EXPECT_NEAR(expected5[i], result5[i], 0.01);
+    }
+}
+
+// Test that zero euler angles construct the correct quaternion
+TEST(euler_to_quat, test_euler_to_quat_1) {
+    double roll{};
+    double pitch{};
+    double yaw{};
+    Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
+    Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
+    Eigen::Vector4d expected{0.0, 0.0, 0.0, 1.0};
+    for (int i = 0; i < 4; ++i) {
+        EXPECT_NEAR(expected[i], result[i], 0.01);
+    }
+}
+
+// Test that non-zero roll constructs the correct quaternion
+TEST(euler_to_quat, test_euler_to_quat_2) {
+    double roll{1.0};
+    double pitch{};
+    double yaw{};
+    Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
+    Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
+    Eigen::Vector4d expected{0.479, 0.0, 0.0, 0.877};
+    for (int i = 0; i < 4; ++i) {
+        EXPECT_NEAR(expected[i], result[i], 0.01);
+    }
+}
+
+// Test that non-zero pitch constructs the correct quaternion
+TEST(euler_to_quat, test_euler_to_quat_3) {
+    double roll{};
+    double pitch{1.0};
+    double yaw{};
+    Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
+    Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
+    Eigen::Vector4d expected{0.0, 0.479, 0.0, 0.877};
+    for (int i = 0; i < 4; ++i) {
+        EXPECT_NEAR(expected[i], result[i], 0.01);
+    }
+}
+
+// Test that non-zero yaw constructs the correct quaternion
+TEST(euler_to_quat, test_euler_to_quat_4) {
+    double roll{};
+    double pitch{};
+    double yaw{1.0};
+    Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
+    Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
+    Eigen::Vector4d expected{0.0, 0.0, 0.479, 0.877};
+    for (int i = 0; i < 4; ++i) {
+        EXPECT_NEAR(expected[i], result[i], 0.01);
+    }
+}
+
+// Test that non-zero euler angles constructs the correct quaternion
+TEST(euler_to_quat, test_euler_to_quat_5) {
+    double roll{1.0};
+    double pitch{1.0};
+    double yaw{1.0};
+    Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
+    Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
+    Eigen::Vector4d expected{0.1675, 0.5709, 0.1675, 0.786};
+    for (int i = 0; i < 4; ++i) {
+        EXPECT_NEAR(expected[i], result[i], 0.01);
+    }
+}
+
+}  // namespace vortex::utils::math

cpp_test/test_types.cpp

@@ -0,0 +1,93 @@
+#include <gtest/gtest.h>
+
+#include "vortex/utils/math.hpp"
+#include "vortex/utils/types.hpp"
+
+class TypesTests : public ::testing::Test {
+   public:
+    TypesTests() = default;
+    void SetUp() override {}
+};
+
+TEST_F(TypesTests, test_eta) {
+    vortex::utils::types::Eta eta;
+    // Test correct zero initialization
+    EXPECT_EQ(eta.x, 0.0);
+    EXPECT_EQ(eta.y, 0.0);
+    EXPECT_EQ(eta.z, 0.0);
+    EXPECT_EQ(eta.roll, 0.0);
+    EXPECT_EQ(eta.pitch, 0.0);
+    EXPECT_EQ(eta.yaw, 0.0);
+
+    // Test rotation and transformation matrix
+    eta.roll = 1.0;
+    eta.pitch = 0.5;
+    eta.yaw = 1.7;
+    Eigen::Matrix3d expected_rm{
+        vortex::utils::math::get_rotation_matrix(1.0, 0.5, 1.7)};
+    Eigen::Matrix3d result_rm{eta.as_rotation_matrix()};
+    double diff_rm{
+        vortex::utils::math::matrix_norm_diff(expected_rm, result_rm)};
+    EXPECT_NEAR(diff_rm, 0.0, 1e-12);
+
+    Eigen::Matrix3d expected_tm{
+        vortex::utils::math::get_transformation_matrix_attitude(1.0, 0.5)};
+    Eigen::Matrix3d result_tm{eta.as_transformation_matrix()};
+    double diff_tm{
+        vortex::utils::math::matrix_norm_diff(expected_tm, result_tm)};
+    EXPECT_NEAR(diff_tm, 0.0, 1e-12);
+
+    // Test to_vector
+    eta.x = 5.0;
+    eta.y = -4.0;
+    eta.z = 2.1;
+    Eigen::Vector6d result_v{eta.to_vector()};
+    Eigen::Vector6d expected_v{5.0, -4.0, 2.1, 1.0, 0.5, 1.7};
+    for (int i = 0; i < 6; ++i) {
+        EXPECT_NEAR(expected_v[i], result_v[i], 1e-12);
+    }
+
+    // Test operator-
+    vortex::utils::types::Eta other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3};
+    vortex::utils::types::Eta diff{eta - other};
+    EXPECT_NEAR(diff.x, 4.0, 1e-12);
+    EXPECT_NEAR(diff.y, -6.0, 1e-12);
+    EXPECT_NEAR(diff.z, -0.9, 1e-12);
+    EXPECT_NEAR(diff.roll, 0.9, 1e-12);
+    EXPECT_NEAR(diff.pitch, 0.3, 1e-12);
+    EXPECT_NEAR(diff.yaw, 1.4, 1e-12);
+}
+
+TEST_F(TypesTests, test_nu) {
+    vortex::utils::types::Nu nu;
+    // Test correct zero initialization
+    EXPECT_EQ(nu.u, 0.0);
+    EXPECT_EQ(nu.v, 0.0);
+    EXPECT_EQ(nu.w, 0.0);
+    EXPECT_EQ(nu.p, 0.0);
+    EXPECT_EQ(nu.q, 0.0);
+    EXPECT_EQ(nu.r, 0.0);
+
+    // Test to_vector
+    nu.u = 5.0;
+    nu.v = -4.0;
+    nu.w = 2.1;
+    nu.p = 1.0;
+    nu.q = 0.5;
+    nu.r = 1.7;
+    Eigen::Vector6d result_v{nu.to_vector()};
+    Eigen::Vector6d expected_v{5.0, -4.0, 2.1, 1.0, 0.5, 1.7};
+    for (int i = 0; i < 6; ++i) {
+        EXPECT_NEAR(expected_v[i], result_v[i], 1e-12);
+    }
+
+    // Test operator-
+    vortex::utils::types::Nu other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3};
+    vortex::utils::types::Nu diff{nu - other};
+    EXPECT_NEAR(diff.u, 4.0, 1e-12);
+    EXPECT_NEAR(diff.v, -6.0, 1e-12);
+    EXPECT_NEAR(diff.w, -0.9, 1e-12);
+    EXPECT_NEAR(diff.p, 0.9, 1e-12);
+    EXPECT_NEAR(diff.q, 0.3, 1e-12);
+    EXPECT_NEAR(diff.r, 1.4, 1e-12);
+}