Add tests for C++ code and refactor python tests

CMakeLists.txt

@@ -47,6 +47,17 @@ if(BUILD_TESTING)
       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})
+
 endif()
 
 ament_package()

README.md

@@ -3,12 +3,23 @@
 [![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)
 
-```
-TODO: Write a simple description / introduction to the repository
-```
+This repository contains general, often-used utility functions and structs for both C++ and Python.
+
 
-# Setup
+# Usage
+
+
+In Python, import your desired function/dataclass like for example:
+```python
+from vortex_utils.python_utils import ssa
+```
 
+In C++, include vortex_utils like
+```C++
+#include <vortex_utils/cpp_utils.hpp>
 ```
-TODO: Write a setup guide
+and in the code
+```C++
+double some_angle = 3.14;
+double angle_ssa = vortex_utils::ssa(some_angle);
 ```

include/vortex_utils/cpp_utils.hpp

@@ -9,13 +9,18 @@ typedef Eigen::Matrix<double, 3, 3> Matrix3d;
 typedef Eigen::Matrix<double, 6, 1> Vector6d;
 
 namespace vortex_utils {
-// @brief Function to calculate the smallest signed angle between two angles
+// @brief Function to calculate the smallest signed angle between two angles.
+// Maps the angle to the interval [-pi, pi].
 double ssa(const double angle);
 
+// @brief Calculates the skew-symmetric matrix from a 3D vector.
 Matrix3d skew_symmetric(const Eigen::Vector3d& vector);
 
+// @brief Converts a quaternion to Euler angles.
 Eigen::Vector3d quat_to_euler(const Eigen::Quaterniond& q);
 
+// @brief Struct to represent the state vector eta,
+// containing the position and orientation.
 struct Eta {
     double x = 0.0;
     double y = 0.0;

package.xml

@@ -13,8 +13,10 @@
   <depend>eigen</depend>
   <depend>python3-numpy</depend>
   <depend>python3-scipy</depend>
+  <depend>geometry_msgs</depend>
 
   <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/cpp_utils.cpp

@@ -2,7 +2,8 @@
 
 namespace vortex_utils {
 double ssa(const double angle) {
-    double angle_ssa = fmod(angle + M_PI, 2 * M_PI) - M_PI;
+    double result = fmod(angle + M_PI, 2 * M_PI);
+    double angle_ssa = result < 0 ? result + M_PI : result - M_PI;
     return angle_ssa;
 }
 

test/test_cpp_utils.cpp

@@ -0,0 +1,90 @@
+#include <gtest/gtest.h>
+#include <vortex_utils/cpp_utils.hpp>
+
+namespace vortex_utils {
+
+// 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(skew_symmetric, 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 = skew_symmetric(vector);
+    EXPECT_EQ(expected, result);
+}
+
+// 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 y in the quat only affects pitch
+TEST(quat_to_euler, test_quat_to_euler_3) {
+    Eigen::Quaterniond q3(0.707, 0.0, 0.707, 0.0);
+    Eigen::Vector3d expected3(0.0, 1.57, 0.0);
+    Eigen::Vector3d result3 = quat_to_euler(q3);
+    for (int i = 0; i < 3; ++i) {
+        EXPECT_NEAR(expected3[i], result3[i], 0.01);
+    }
+}
+
+// Test that only changing w and z in the quat only affects yaw
+TEST(quat_to_euler, test_quat_to_euler_4) {
+    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(quat_to_euler, test_quat_to_euler_5) {
+    Eigen::Quaterniond q5(0.770, 0.4207, -0.4207, -0.229);
+    Eigen::Vector3d expected5(1.0, -1.0, 0.0);
+    Eigen::Vector3d result5 = quat_to_euler(q5);
+    for (int i = 0; i < 3; ++i) {
+        EXPECT_NEAR(expected5[i], result5[i], 0.01);
+    }
+}
+
+}  // namespace vortex_utils
+
+int main(int argc, char** argv) {
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}