Skip to content

Commit 979d564

Browse files
AndeshogAndeshog
authored
refactor: restructure and more C++ utilities (#10)
* refactor: separate utilities into more specific files * feat: add qos profile implementation * refactor(tests): better organize python and cpp tests * refactor: remove commented out code in cmakelist * add header files to library * add rclcpp depend to package.xml * add back eigen? * feat: add python qos profiles * refactor: quat and euler functions and tests * test(cpp_math): add more euler to quat tests * test(cpp_math): write brief on every test * feat: add transformation matrix from Fossen * update documentation * formatting * remove unused half sentence at the end * doc: add python qos to readme * refactor: rename include path to vortex/utils/ * test: add tests for types and remove unnecessary typedefs * [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci * fix typo in readme * explicitly set reliability in reliable profile * refactor: simplify to_vector methods of Eta and Nu structs * update max index in quat test loops
1 parent f109e3b commit 979d564

File tree

17 files changed

+521
-192
lines changed

17 files changed

+521
-192
lines changed

‎CMakeLists.txt‎

Lines changed: 13 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,24 @@
11
cmake_minimum_required(VERSION 3.8)
22
project(vortex_utils)
33

4-
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5-
add_compile_options(-Wall -Wextra -Wpedantic)
6-
endif()
4+
set(CMAKE_CXX_STANDARD 17)
5+
add_compile_options(-Wall -Wextra -Wpedantic)
76

87
find_package(ament_cmake REQUIRED)
8+
find_package(rclcpp)
99
find_package(ament_cmake_python REQUIRED)
1010
find_package(Eigen3 REQUIRED)
1111

1212
include_directories(include)
1313

14-
add_library(${PROJECT_NAME} src/cpp_utils.cpp)
14+
add_library(${PROJECT_NAME} SHARED
15+
src/math.cpp)
16+
17+
target_include_directories(${PROJECT_NAME} PUBLIC
18+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
19+
$<INSTALL_INTERFACE:include>)
20+
21+
ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp Eigen3)
1522

1623
ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
1724

@@ -36,28 +43,8 @@ ament_export_libraries(${PROJECT_NAME})
3643
ament_python_install_package(${PROJECT_NAME})
3744

3845
if(BUILD_TESTING)
39-
find_package(ament_cmake_pytest REQUIRED)
40-
set(_pytest_tests
41-
tests/test_utils.py
42-
)
43-
foreach(_test_path ${_pytest_tests})
44-
get_filename_component(_test_name ${_test_path} NAME_WE)
45-
ament_add_pytest_test(${_test_name} ${_test_path}
46-
APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
47-
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
48-
)
49-
endforeach()
50-
51-
find_package(ament_cmake_gtest REQUIRED)
52-
ament_add_gtest(${PROJECT_NAME}_test
53-
test/test_cpp_utils.cpp
54-
)
55-
target_include_directories(${PROJECT_NAME}_test PUBLIC
56-
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
57-
$<INSTALL_INTERFACE:include>
58-
)
59-
target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME})
60-
46+
add_subdirectory(py_test)
47+
add_subdirectory(cpp_test)
6148
endif()
6249

6350
ament_package()

‎README.md‎

Lines changed: 13 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -3,23 +3,28 @@
33
[![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)
44
[![codecov](https://codecov.io/github/vortexntnu/vortex-utils/graph/badge.svg?token=d6D7d5xNdf)](https://codecov.io/github/vortexntnu/vortex-utils)
55

6-
This repository contains general, often-used utility functions and structs for both C++ and Python.
7-
6+
This package contains common definitions and often-used utility functions in C++ and Python.
87

98
# Usage
109

11-
1210
In Python, import your desired function/dataclass like for example:
1311
```python
1412
from vortex_utils.python_utils import ssa
1513
```
14+
```python
15+
from vortex_utils.qos_profiles import sensor_data_profile, reliable_profile
16+
```
1617

17-
In C++, include vortex_utils like
18+
In C++, include
19+
```C++
20+
#include <vortex/utils/math.hpp>
21+
```
22+
for mathematical functions,
1823
```C++
19-
#include <vortex_utils/cpp_utils.hpp>
24+
#include <vortex/utils/qos_profiles.hpp>
2025
```
21-
and in the code
26+
for common QoS profile definitions, and
2227
```C++
23-
double some_angle = 3.14;
24-
double angle_ssa = vortex_utils::ssa(some_angle);
28+
#include <vortex/utils/types.hpp>
2529
```
30+
for common structs like 6DOF `Eta` and `Nu`.

‎cpp_test/CMakeLists.txt‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
3+
find_package(GTest REQUIRED)
4+
include(GoogleTest)
5+
6+
set(TEST_BINARY_NAME ${PROJECT_NAME}_test)
7+
add_executable(
8+
${TEST_BINARY_NAME}
9+
test_math.cpp
10+
test_types.cpp
11+
)
12+
13+
target_link_libraries(
14+
${TEST_BINARY_NAME}
15+
PRIVATE
16+
${PROJECT_NAME}
17+
GTest::GTest
18+
GTest::gtest_main
19+
)
20+
21+
ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3)
22+
23+
gtest_discover_tests(${TEST_BINARY_NAME})

‎cpp_test/test_main.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
#include <gtest/gtest.h>
2+
3+
int main(int argc, char** argv) {
4+
::testing::InitGoogleTest(&argc, argv);
5+
return RUN_ALL_TESTS();
6+
}

‎cpp_test/test_math.cpp‎

Lines changed: 171 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,171 @@
1+
#include <gtest/gtest.h>
2+
#include "vortex/utils/math.hpp"
3+
4+
namespace vortex::utils::math {
5+
6+
// Test that the value does not change when already in the interval [-pi, pi]
7+
TEST(ssa, test_ssa_0) {
8+
EXPECT_EQ(0, ssa(0));
9+
}
10+
11+
// Test that 2 pi correctly maps to 0
12+
TEST(ssa, test_ssa_2pi) {
13+
EXPECT_EQ(0, ssa(2 * M_PI));
14+
}
15+
16+
// Test that values over pi gets mapped to the negative interval
17+
TEST(ssa, test_ssa_3_5) {
18+
EXPECT_NEAR(-2.78, ssa(3.5), 0.01);
19+
}
20+
21+
// Test that values under -pi gets mapped to the positive interval
22+
TEST(ssa, test_ssa_minus_3_5) {
23+
EXPECT_NEAR(2.78, ssa(-3.5), 0.01);
24+
}
25+
26+
// Test that the skew-symmetric matrix is correctly calculated
27+
TEST(get_skew_symmetric_matrix, test_skew_symmetric) {
28+
Eigen::Vector3d vector(1, 2, 3);
29+
Eigen::Matrix3d expected;
30+
expected << 0, -3, 2, 3, 0, -1, -2, 1, 0;
31+
32+
Eigen::Matrix3d result = get_skew_symmetric_matrix(vector);
33+
EXPECT_EQ(expected, result);
34+
}
35+
36+
// Test that rotation matrix is correctly constructed
37+
TEST(get_rotation_matrix, test_rotation_matrix) {
38+
double roll{1.0};
39+
double pitch{2.0};
40+
double yaw{3.0};
41+
Eigen::Matrix3d expected;
42+
expected << 0.41198225, -0.83373765, -0.36763046, -0.05872664, -0.42691762,
43+
0.90238159, -0.90929743, -0.35017549, -0.2248451;
44+
Eigen::Matrix3d result = get_rotation_matrix(roll, pitch, yaw);
45+
EXPECT_NEAR(0.0, matrix_norm_diff(expected, result), 0.01);
46+
}
47+
48+
TEST(get_transformation_matrix_attitude, test_transformation_matrix_zeros) {
49+
Eigen::Matrix3d transformation_matrix{
50+
get_transformation_matrix_attitude(0.0, 0.0)};
51+
Eigen::Matrix3d expected{Eigen::Matrix3d::Identity()};
52+
EXPECT_NEAR(0.0, matrix_norm_diff(expected, transformation_matrix), 0.01);
53+
}
54+
55+
// Test that the identity quaternion correctly maps to 0, 0, 0
56+
TEST(quat_to_euler, test_quat_to_euler_1) {
57+
Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0);
58+
Eigen::Vector3d expected(0.0, 0.0, 0.0);
59+
Eigen::Vector3d result = quat_to_euler(q);
60+
for (int i = 0; i < 3; ++i) {
61+
EXPECT_NEAR(expected[i], result[i], 0.01);
62+
}
63+
}
64+
65+
// Test that only changing w and x in the quat only affects roll
66+
TEST(quat_to_euler, test_quat_to_euler_2) {
67+
Eigen::Quaterniond q2(0.707, 0.707, 0.0, 0.0);
68+
Eigen::Vector3d expected2(1.57, 0.0, 0.0);
69+
Eigen::Vector3d result2 = quat_to_euler(q2);
70+
for (int i = 0; i < 3; ++i) {
71+
EXPECT_NEAR(expected2[i], result2[i], 0.01);
72+
}
73+
}
74+
75+
// Test that only changing w and z in the quat only affects yaw
76+
TEST(quat_to_euler, test_quat_to_euler_3) {
77+
Eigen::Quaterniond q4(0.707, 0.0, 0.0, 0.707);
78+
Eigen::Vector3d expected4(0.0, 0.0, 1.57);
79+
Eigen::Vector3d result4 = quat_to_euler(q4);
80+
for (int i = 0; i < 3; ++i) {
81+
EXPECT_NEAR(expected4[i], result4[i], 0.01);
82+
}
83+
}
84+
85+
// Test that a quaternion is correctly converted to euler angles
86+
TEST(quat_to_euler, test_quat_to_euler_4) {
87+
Eigen::Quaterniond q5(0.770, 0.4207, -0.4207, -0.229);
88+
Eigen::Vector3d expected5(1.237, -0.4729, -0.9179);
89+
Eigen::Vector3d result5 = quat_to_euler(q5);
90+
for (int i = 0; i < 3; ++i) {
91+
EXPECT_NEAR(expected5[i], result5[i], 0.01);
92+
}
93+
}
94+
95+
// Test that a quaternion with flipped signs is correctly converted to euler
96+
// angles
97+
TEST(quat_to_euler, test_quat_to_euler_5) {
98+
Eigen::Quaterniond q5(0.770, 0.4207, 0.4207, 0.229);
99+
Eigen::Vector3d expected5(1.237, 0.4729, 0.9179);
100+
Eigen::Vector3d result5 = quat_to_euler(q5);
101+
for (int i = 0; i < 3; ++i) {
102+
EXPECT_NEAR(expected5[i], result5[i], 0.01);
103+
}
104+
}
105+
106+
// Test that zero euler angles construct the correct quaternion
107+
TEST(euler_to_quat, test_euler_to_quat_1) {
108+
double roll{};
109+
double pitch{};
110+
double yaw{};
111+
Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
112+
Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
113+
Eigen::Vector4d expected{0.0, 0.0, 0.0, 1.0};
114+
for (int i = 0; i < 4; ++i) {
115+
EXPECT_NEAR(expected[i], result[i], 0.01);
116+
}
117+
}
118+
119+
// Test that non-zero roll constructs the correct quaternion
120+
TEST(euler_to_quat, test_euler_to_quat_2) {
121+
double roll{1.0};
122+
double pitch{};
123+
double yaw{};
124+
Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
125+
Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
126+
Eigen::Vector4d expected{0.479, 0.0, 0.0, 0.877};
127+
for (int i = 0; i < 4; ++i) {
128+
EXPECT_NEAR(expected[i], result[i], 0.01);
129+
}
130+
}
131+
132+
// Test that non-zero pitch constructs the correct quaternion
133+
TEST(euler_to_quat, test_euler_to_quat_3) {
134+
double roll{};
135+
double pitch{1.0};
136+
double yaw{};
137+
Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
138+
Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
139+
Eigen::Vector4d expected{0.0, 0.479, 0.0, 0.877};
140+
for (int i = 0; i < 4; ++i) {
141+
EXPECT_NEAR(expected[i], result[i], 0.01);
142+
}
143+
}
144+
145+
// Test that non-zero yaw constructs the correct quaternion
146+
TEST(euler_to_quat, test_euler_to_quat_4) {
147+
double roll{};
148+
double pitch{};
149+
double yaw{1.0};
150+
Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
151+
Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
152+
Eigen::Vector4d expected{0.0, 0.0, 0.479, 0.877};
153+
for (int i = 0; i < 4; ++i) {
154+
EXPECT_NEAR(expected[i], result[i], 0.01);
155+
}
156+
}
157+
158+
// Test that non-zero euler angles constructs the correct quaternion
159+
TEST(euler_to_quat, test_euler_to_quat_5) {
160+
double roll{1.0};
161+
double pitch{1.0};
162+
double yaw{1.0};
163+
Eigen::Quaterniond q{euler_to_quat(roll, pitch, yaw)};
164+
Eigen::Vector4d result{q.x(), q.y(), q.z(), q.w()};
165+
Eigen::Vector4d expected{0.1675, 0.5709, 0.1675, 0.786};
166+
for (int i = 0; i < 4; ++i) {
167+
EXPECT_NEAR(expected[i], result[i], 0.01);
168+
}
169+
}
170+
171+
} // namespace vortex::utils::math

‎cpp_test/test_types.cpp‎

Lines changed: 93 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
#include <gtest/gtest.h>
2+
3+
#include "vortex/utils/math.hpp"
4+
#include "vortex/utils/types.hpp"
5+
6+
class TypesTests : public ::testing::Test {
7+
public:
8+
TypesTests() = default;
9+
void SetUp() override {}
10+
};
11+
12+
TEST_F(TypesTests, test_eta) {
13+
vortex::utils::types::Eta eta;
14+
// Test correct zero initialization
15+
EXPECT_EQ(eta.x, 0.0);
16+
EXPECT_EQ(eta.y, 0.0);
17+
EXPECT_EQ(eta.z, 0.0);
18+
EXPECT_EQ(eta.roll, 0.0);
19+
EXPECT_EQ(eta.pitch, 0.0);
20+
EXPECT_EQ(eta.yaw, 0.0);
21+
22+
// Test rotation and transformation matrix
23+
eta.roll = 1.0;
24+
eta.pitch = 0.5;
25+
eta.yaw = 1.7;
26+
Eigen::Matrix3d expected_rm{
27+
vortex::utils::math::get_rotation_matrix(1.0, 0.5, 1.7)};
28+
Eigen::Matrix3d result_rm{eta.as_rotation_matrix()};
29+
double diff_rm{
30+
vortex::utils::math::matrix_norm_diff(expected_rm, result_rm)};
31+
EXPECT_NEAR(diff_rm, 0.0, 1e-12);
32+
33+
Eigen::Matrix3d expected_tm{
34+
vortex::utils::math::get_transformation_matrix_attitude(1.0, 0.5)};
35+
Eigen::Matrix3d result_tm{eta.as_transformation_matrix()};
36+
double diff_tm{
37+
vortex::utils::math::matrix_norm_diff(expected_tm, result_tm)};
38+
EXPECT_NEAR(diff_tm, 0.0, 1e-12);
39+
40+
// Test to_vector
41+
eta.x = 5.0;
42+
eta.y = -4.0;
43+
eta.z = 2.1;
44+
Eigen::Vector6d result_v{eta.to_vector()};
45+
Eigen::Vector6d expected_v{5.0, -4.0, 2.1, 1.0, 0.5, 1.7};
46+
for (int i = 0; i < 6; ++i) {
47+
EXPECT_NEAR(expected_v[i], result_v[i], 1e-12);
48+
}
49+
50+
// Test operator-
51+
vortex::utils::types::Eta other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3};
52+
vortex::utils::types::Eta diff{eta - other};
53+
EXPECT_NEAR(diff.x, 4.0, 1e-12);
54+
EXPECT_NEAR(diff.y, -6.0, 1e-12);
55+
EXPECT_NEAR(diff.z, -0.9, 1e-12);
56+
EXPECT_NEAR(diff.roll, 0.9, 1e-12);
57+
EXPECT_NEAR(diff.pitch, 0.3, 1e-12);
58+
EXPECT_NEAR(diff.yaw, 1.4, 1e-12);
59+
}
60+
61+
TEST_F(TypesTests, test_nu) {
62+
vortex::utils::types::Nu nu;
63+
// Test correct zero initialization
64+
EXPECT_EQ(nu.u, 0.0);
65+
EXPECT_EQ(nu.v, 0.0);
66+
EXPECT_EQ(nu.w, 0.0);
67+
EXPECT_EQ(nu.p, 0.0);
68+
EXPECT_EQ(nu.q, 0.0);
69+
EXPECT_EQ(nu.r, 0.0);
70+
71+
// Test to_vector
72+
nu.u = 5.0;
73+
nu.v = -4.0;
74+
nu.w = 2.1;
75+
nu.p = 1.0;
76+
nu.q = 0.5;
77+
nu.r = 1.7;
78+
Eigen::Vector6d result_v{nu.to_vector()};
79+
Eigen::Vector6d expected_v{5.0, -4.0, 2.1, 1.0, 0.5, 1.7};
80+
for (int i = 0; i < 6; ++i) {
81+
EXPECT_NEAR(expected_v[i], result_v[i], 1e-12);
82+
}
83+
84+
// Test operator-
85+
vortex::utils::types::Nu other{1.0, 2.0, 3.0, 0.1, 0.2, 0.3};
86+
vortex::utils::types::Nu diff{nu - other};
87+
EXPECT_NEAR(diff.u, 4.0, 1e-12);
88+
EXPECT_NEAR(diff.v, -6.0, 1e-12);
89+
EXPECT_NEAR(diff.w, -0.9, 1e-12);
90+
EXPECT_NEAR(diff.p, 0.9, 1e-12);
91+
EXPECT_NEAR(diff.q, 0.3, 1e-12);
92+
EXPECT_NEAR(diff.r, 1.4, 1e-12);
93+
}

0 commit comments

Comments
 (0)