diff --git a/CMakeLists.txt b/CMakeLists.txt index a6d161c..acd52ef 100644 --- a/CMakeLists.txt +++ b/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 + $ + $) + +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 - $ - $ - ) - target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) - + add_subdirectory(py_test) + add_subdirectory(cpp_test) endif() ament_package() diff --git a/README.md b/README.md index a588a77..e72daf0 100644 --- a/README.md +++ b/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 +``` +for mathematical functions, ```C++ -#include +#include ``` -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 ``` +for common structs like 6DOF `Eta` and `Nu`. diff --git a/cpp_test/CMakeLists.txt b/cpp_test/CMakeLists.txt new file mode 100644 index 0000000..b2fc415 --- /dev/null +++ b/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}) diff --git a/cpp_test/test_main.cpp b/cpp_test/test_main.cpp new file mode 100644 index 0000000..5ebbc76 --- /dev/null +++ b/cpp_test/test_main.cpp @@ -0,0 +1,6 @@ +#include + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/cpp_test/test_math.cpp b/cpp_test/test_math.cpp new file mode 100644 index 0000000..b18f8a9 --- /dev/null +++ b/cpp_test/test_math.cpp @@ -0,0 +1,171 @@ +#include +#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 diff --git a/cpp_test/test_types.cpp b/cpp_test/test_types.cpp new file mode 100644 index 0000000..ab700e1 --- /dev/null +++ b/cpp_test/test_types.cpp @@ -0,0 +1,93 @@ +#include + +#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); +} diff --git a/include/vortex/utils/math.hpp b/include/vortex/utils/math.hpp new file mode 100644 index 0000000..1e06366 --- /dev/null +++ b/include/vortex/utils/math.hpp @@ -0,0 +1,41 @@ +#ifndef VORTEX_UTILS_MATH_HPP +#define VORTEX_UTILS_MATH_HPP + +#include +#include +#include + +namespace vortex::utils::math { + +// @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 Helper to calculate error between two matrices +inline double matrix_norm_diff(Eigen::MatrixXd m1, Eigen::MatrixXd m2) { + return (m1 - m2).norm(); +} + +// @brief Calculates the skew-symmetric matrix from a 3D vector. +Eigen::Matrix3d get_skew_symmetric_matrix(const Eigen::Vector3d& vector); + +// @brief Rotation matrix from Eigen quat +Eigen::Matrix3d get_rotation_matrix(const double roll, + const double pitch, + const double yaw); + +// @brief Fossen, 2021 eq. 2.41 +Eigen::Matrix3d get_transformation_matrix_attitude(const double roll, + const double pitch); + +// @brief Converts a quaternion to Euler angles. +Eigen::Vector3d quat_to_euler(const Eigen::Quaterniond& q); + +// @brief Converts Euler angles to quaternion +Eigen::Quaterniond euler_to_quat(const double roll, + const double pitch, + const double yaw); + +} // namespace vortex::utils::math + +#endif // VORTEX_UTILS_MATH_HPP diff --git a/include/vortex/utils/qos_profiles.hpp b/include/vortex/utils/qos_profiles.hpp new file mode 100644 index 0000000..c1130d3 --- /dev/null +++ b/include/vortex/utils/qos_profiles.hpp @@ -0,0 +1,24 @@ +#ifndef VORTEX_UTILS_QOS_PROFILES_HPP +#define VORTEX_UTILS_QOS_PROFILES_HPP + +#include + +namespace vortex::utils::qos_profiles { + +inline rclcpp::QoS sensor_data_profile(const size_t depth = 5) { + auto profile{rclcpp::QoS(depth)}; // Keep last + profile.best_effort(); + profile.durability_volatile(); + return profile; +} + +inline rclcpp::QoS reliable_profile(const size_t depth = 10) { + auto profile{rclcpp::QoS(depth)}; // Keep last + profile.reliable(); + profile.durability_volatile(); + return profile; +} + +} // namespace vortex::utils::qos_profiles + +#endif // VORTEX_UTILS_QOS_PROFILES_HPP diff --git a/include/vortex_utils/cpp_utils.hpp b/include/vortex/utils/types.hpp similarity index 59% rename from include/vortex_utils/cpp_utils.hpp rename to include/vortex/utils/types.hpp index 85fb65d..559cad7 100644 --- a/include/vortex_utils/cpp_utils.hpp +++ b/include/vortex/utils/types.hpp @@ -1,33 +1,24 @@ -#ifndef VORTEX_UTILS_HPP -#define VORTEX_UTILS_HPP +#ifndef VORTEX_UTILS_TYPES_HPP +#define VORTEX_UTILS_TYPES_HPP -#include #include +namespace Eigen { typedef Eigen::Matrix Matrix6d; -typedef Eigen::Matrix Matrix3d; typedef Eigen::Matrix Vector6d; +} // namespace Eigen -namespace vortex_utils { -// @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); +namespace vortex::utils::types { // @brief Struct to represent the state vector eta, // containing the position and orientation. struct Eta { - double x = 0.0; - double y = 0.0; - double z = 0.0; - double roll = 0.0; - double pitch = 0.0; - double yaw = 0.0; + double x{}; + double y{}; + double z{}; + double roll{}; + double pitch{}; + double yaw{}; Eta operator-(const Eta& other) const { Eta eta; @@ -40,27 +31,25 @@ struct Eta { return eta; } - Vector6d to_vector() const { - Vector6d eta; - eta << x, y, z, roll, pitch, yaw; - return eta; + Eigen::Vector6d to_vector() const { + return Eigen::Vector6d{x, y, z, roll, pitch, yaw}; } // @brief Make the rotation matrix according to eq. 2.31 in Fossen, 2021 - Matrix3d as_rotation_matrix() const { + Eigen::Matrix3d as_rotation_matrix() const { Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle; - Matrix3d rotation_matrix = q.toRotationMatrix(); + Eigen::Matrix3d rotation_matrix = q.toRotationMatrix(); return rotation_matrix; } // @brief Make the transformation matrix according to eq. 2.41 in Fossen, // 2021 - Matrix3d as_transformation_matrix() const { + Eigen::Matrix3d as_transformation_matrix() const { double cphi = cos(roll); double sphi = sin(roll); double ctheta = cos(pitch); @@ -81,18 +70,18 @@ struct Eta { double t32 = sphi / ctheta; double t33 = cphi / ctheta; - Matrix3d transformation_matrix; + Eigen::Matrix3d transformation_matrix; transformation_matrix << t11, t12, t13, t21, t22, t23, t31, t32, t33; return transformation_matrix; } - Matrix6d as_j_matrix() const { - Matrix3d rotation_matrix = as_rotation_matrix(); - Matrix3d transformation_matrix = as_transformation_matrix(); + Eigen::Matrix6d as_j_matrix() const { + Eigen::Matrix3d rotation_matrix = as_rotation_matrix(); + Eigen::Matrix3d transformation_matrix = as_transformation_matrix(); - Matrix6d j_matrix = Matrix6d::Zero(); + Eigen::Matrix6d j_matrix = Eigen::Matrix6d::Zero(); j_matrix.block<3, 3>(0, 0) = rotation_matrix; j_matrix.block<3, 3>(3, 3) = transformation_matrix; @@ -101,12 +90,12 @@ struct Eta { }; struct Nu { - double u = 0.0; - double v = 0.0; - double w = 0.0; - double p = 0.0; - double q = 0.0; - double r = 0.0; + double u{}; + double v{}; + double w{}; + double p{}; + double q{}; + double r{}; Nu operator-(const Nu& other) const { Nu nu; @@ -119,12 +108,11 @@ struct Nu { return nu; } - Vector6d to_vector() const { - Vector6d nu; - nu << u, v, w, p, q, r; - return nu; + Eigen::Vector6d to_vector() const { + return Eigen::Vector6d{u, v, w, p, q, r}; } }; -} // namespace vortex_utils -#endif // VORTEX_UTILS_HPP +} // namespace vortex::utils::types + +#endif // VORTEX_UTILS_TYPES_HPP diff --git a/package.xml b/package.xml index 4697281..d0f1e3d 100644 --- a/package.xml +++ b/package.xml @@ -10,6 +10,7 @@ ament_cmake ament_cmake_python + rclcpp eigen python3-numpy python3-scipy diff --git a/py_test/CMakeLists.txt b/py_test/CMakeLists.txt new file mode 100644 index 0000000..32bea3b --- /dev/null +++ b/py_test/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.8) + +find_package(ament_cmake_pytest REQUIRED) +set(_pytest_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() diff --git a/tests/resources/test_video.h264 b/py_test/resources/test_video.h264 similarity index 100% rename from tests/resources/test_video.h264 rename to py_test/resources/test_video.h264 diff --git a/tests/test_utils.py b/py_test/test_utils.py similarity index 99% rename from tests/test_utils.py rename to py_test/test_utils.py index a7470c5..764a11d 100644 --- a/tests/test_utils.py +++ b/py_test/test_utils.py @@ -231,7 +231,7 @@ def test_h264_decoder_initialization(decoder): def test_h264_decoder_decodes_frames(decoder): """Test if the decoder correctly processes an H.264 stream.""" - test_file = "tests/resources/test_video.h264" + test_file = "py_test/resources/test_video.h264" # Read and push H.264 data with open(test_file, "rb") as f: @@ -254,7 +254,7 @@ def test_h264_decoder_decodes_frames(decoder): def test_h264_decoder_frame_properties(decoder): """Test if decoded frames have correct properties.""" - test_file = "tests/resources/test_video.h264" + test_file = "py_test/resources/test_video.h264" # Read and push H.264 data with open(test_file, "rb") as f: diff --git a/src/cpp_utils.cpp b/src/cpp_utils.cpp deleted file mode 100644 index 576446f..0000000 --- a/src/cpp_utils.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include "vortex_utils/cpp_utils.hpp" - -namespace vortex_utils { -double ssa(const double angle) { - double result = fmod(angle + M_PI, 2 * M_PI); - double angle_ssa = result < 0 ? result + M_PI : result - M_PI; - return angle_ssa; -} - -Matrix3d skew_symmetric(const Eigen::Vector3d& vector) { - Matrix3d skew_symmetric_matrix; - skew_symmetric_matrix << 0, -vector.z(), vector.y(), vector.z(), 0, - -vector.x(), -vector.y(), vector.x(), 0; - return skew_symmetric_matrix; -} - -Eigen::Vector3d quat_to_euler(const Eigen::Quaterniond& q) { - Eigen::Vector3d euler_angles = q.toRotationMatrix().eulerAngles(0, 1, 2); - return euler_angles; -} - -} // namespace vortex_utils diff --git a/src/math.cpp b/src/math.cpp new file mode 100644 index 0000000..96b8100 --- /dev/null +++ b/src/math.cpp @@ -0,0 +1,72 @@ +#include "vortex/utils/math.hpp" + +namespace vortex::utils::math { + +double ssa(const double angle) { + double angle_ssa{fmod(angle + M_PI, 2 * M_PI)}; + return angle_ssa < 0 ? angle_ssa + M_PI : angle_ssa - M_PI; +} + +Eigen::Matrix3d get_skew_symmetric_matrix(const Eigen::Vector3d& vector) { + Eigen::Matrix3d skew_symmetric_matrix; + skew_symmetric_matrix << 0, -vector.z(), vector.y(), vector.z(), 0, + -vector.x(), -vector.y(), vector.x(), 0; + return skew_symmetric_matrix; +} + +Eigen::Matrix3d get_rotation_matrix(const double roll, + const double pitch, + const double yaw) { + Eigen::Matrix3d rotation_matrix = + (Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())) + .toRotationMatrix(); + return rotation_matrix; +} + +Eigen::Matrix3d get_transformation_matrix_attitude(const double roll, + const double pitch) { + double sin_r = sin(roll); + double cos_r = cos(roll); + double cos_p = cos(pitch); + if (cos_p == 0) { + throw std::runtime_error("Singular pitch"); + } + double tan_p = tan(pitch); + + Eigen::Matrix3d transformation_matrix; + transformation_matrix(0, 0) = 1; + transformation_matrix(0, 1) = sin_r * tan_p; + transformation_matrix(0, 2) = cos_r * tan_p; + transformation_matrix(1, 0) = 0; + transformation_matrix(1, 1) = cos_r; + transformation_matrix(1, 2) = -sin_r; + transformation_matrix(2, 0) = 0; + transformation_matrix(2, 1) = sin_r / cos_p; + transformation_matrix(2, 2) = cos_r / cos_p; + return transformation_matrix; +} + +Eigen::Vector3d quat_to_euler(const Eigen::Quaterniond& q) { + Eigen::Matrix3d rotation_matrix = q.toRotationMatrix(); + if (std::fabs(rotation_matrix(2, 0)) > 1.0) { + throw std::runtime_error("Singular value in pitch"); + } + double roll{atan2(rotation_matrix(2, 1), rotation_matrix(2, 2))}; + double pitch{-asin(rotation_matrix(2, 0))}; + double yaw{atan2(rotation_matrix(1, 0), rotation_matrix(0, 0))}; + return {roll, pitch, yaw}; +} + +Eigen::Quaterniond euler_to_quat(const double roll, + const double pitch, + const double yaw) { + const Eigen::AngleAxisd r_z(yaw, Eigen::Vector3d::UnitZ()); + const Eigen::AngleAxisd r_y(pitch, Eigen::Vector3d::UnitY()); + const Eigen::AngleAxisd r_x(roll, Eigen::Vector3d::UnitX()); + Eigen::Quaterniond q = r_z * r_y * r_x; + return q.normalized(); +} + +} // namespace vortex::utils::math diff --git a/test/test_cpp_utils.cpp b/test/test_cpp_utils.cpp deleted file mode 100644 index 9eb0f85..0000000 --- a/test/test_cpp_utils.cpp +++ /dev/null @@ -1,90 +0,0 @@ -#include -#include - -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(); -} diff --git a/vortex_utils/qos_profiles.py b/vortex_utils/qos_profiles.py new file mode 100644 index 0000000..f3c4f8c --- /dev/null +++ b/vortex_utils/qos_profiles.py @@ -0,0 +1,17 @@ +from rclpy import qos + + +def sensor_data_profile(depth: int = 5) -> qos.QoSProfile: + return qos.QoSProfile( + history=qos.HistoryPolicy.KEEP_LAST, + depth=depth, + reliability=qos.ReliabilityPolicy.BEST_EFFORT, + ) + + +def reliable_profile(depth: int = 10) -> qos.QoSProfile: + return qos.QoSProfile( + history=qos.HistoryPolicy.KEEP_LAST, + depth=depth, + reliability=qos.ReliabilityPolicy.RELIABLE, + )