feat: add python utility module

Author: Andeshog

CMakeLists.txt

@@ -6,6 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 
 include_directories(include)
 
@@ -31,4 +32,6 @@ ament_export_dependencies()
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 
+ament_python_install_package(${PROJECT_NAME})
+
 ament_package()

package.xml

@@ -8,6 +8,7 @@
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

vortex_utils/__init__.py

@@ -0,0 +1,4 @@
+import numpy as np
+
+def ssa(angle: float) -> float:
+        return (angle + np.pi) % (2 * np.pi) - np.pi