feat: add cpp utils and ssa function

Author: Andeshog

CMakeLists.txt

@@ -0,0 +1,34 @@
+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()
+
+find_package(ament_cmake REQUIRED)
+
+include_directories(include)
+
+add_library(${PROJECT_NAME} src/cpp_utils.cpp)
+
+ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+ament_export_dependencies()
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_package()

include/vortex_utils/cpp_utils.hpp

@@ -0,0 +1,12 @@
+#ifndef VORTEX_UTILS_HPP
+#define VORTEX_UTILS_HPP
+
+#include <cmath>
+
+namespace vortex_utils
+{
+    // @brief Function to calculate the smallest signed angle between two angles
+    double ssa(const double angle);
+} // namespace vortex_utils
+
+#endif // VORTEX_UTILS_HPP

package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>vortex_utils</name>
+  <version>0.0.0</version>
+  <description>Package containing utility functions and data structures.</description>
+  <maintainer email="andeshog@stud.ntnu.no">andeshog</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/cpp_utils.cpp

@@ -0,0 +1,7 @@
+#include "vortex_utils/cpp_utils.hpp"
+
+double vortex_utils::ssa(const double angle)
+{
+    double angle_ssa = fmod(angle + M_PI, 2 * M_PI) - M_PI;
+    return angle_ssa;
+}