feat: made some tests

Author: Andeshog

CMakeLists.txt

@@ -34,4 +34,18 @@ 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()
+endif()
+
 ament_package()

tests/test_utils.py

@@ -0,0 +1,21 @@
+import pytest
+from vortex_utils.python_utils import *
+
+def test_ssa():
+    assert ssa(0) == 0
+    assert ssa(2 * np.pi) == 0
+    assert ssa(3.5) == pytest.approx(-2.78, rel=0.01)
+    assert ssa(-3.5) == pytest.approx(2.78, rel=0.01)
+
+def test_euler_to_quat():
+    quat = euler_to_quat(0, 0, 0)
+    assert quat == pytest.approx([0, 0, 0, 1], rel=0.01)
+    quat = euler_to_quat(1, 0, 0)
+    assert quat == pytest.approx([0.479, 0, 0, 0.877], rel=0.01)
+    quat = euler_to_quat(0, 1, 0)
+    assert quat == pytest.approx([0, 0.479, 0, 0.877], rel=0.01)
+    quat = euler_to_quat(0, 0, 1)
+    assert quat == pytest.approx([0, 0, 0.479, 0.877], rel=0.01)
+    quat = euler_to_quat(1, 1, 1)
+    assert quat == pytest.approx([0.5709, 0.167, 0.5709, 0.565], rel=0.01)
+