diff --git a/CMakeLists.txt b/CMakeLists.txt
index 72db238..a6d161c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -47,6 +47,17 @@ if(BUILD_TESTING)
       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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME})
+
 endif()
 
 ament_package()
diff --git a/README.md b/README.md
index 249528d..a588a77 100644
--- a/README.md
+++ b/README.md
@@ -3,12 +3,23 @@
 [![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)
 
-```
-TODO: Write a simple description / introduction to the repository
-```
+This repository contains general, often-used utility functions and structs for both C++ and Python.
+
 
-# Setup
+# Usage
+
+
+In Python, import your desired function/dataclass like for example:
+```python
+from vortex_utils.python_utils import ssa
+```
 
+In C++, include vortex_utils like
+```C++
+#include <vortex_utils/cpp_utils.hpp>
 ```
-TODO: Write a setup guide
+and in the code
+```C++
+double some_angle = 3.14;
+double angle_ssa = vortex_utils::ssa(some_angle);
 ```
diff --git a/include/vortex_utils/cpp_utils.hpp b/include/vortex_utils/cpp_utils.hpp
index 9389589..85fb65d 100644
--- a/include/vortex_utils/cpp_utils.hpp
+++ b/include/vortex_utils/cpp_utils.hpp
@@ -9,13 +9,18 @@ typedef Eigen::Matrix<double, 3, 3> Matrix3d;
 typedef Eigen::Matrix<double, 6, 1> Vector6d;
 
 namespace vortex_utils {
-// @brief Function to calculate the smallest signed angle between two angles
+// @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);
 
+// @brief Struct to represent the state vector eta,
+// containing the position and orientation.
 struct Eta {
     double x = 0.0;
     double y = 0.0;
diff --git a/package.xml b/package.xml
index 11a3e18..4697281 100644
--- a/package.xml
+++ b/package.xml
@@ -13,8 +13,10 @@
   <depend>eigen</depend>
   <depend>python3-numpy</depend>
   <depend>python3-scipy</depend>
+  <depend>geometry_msgs</depend>
 
   <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/src/cpp_utils.cpp b/src/cpp_utils.cpp
index 62708a8..576446f 100644
--- a/src/cpp_utils.cpp
+++ b/src/cpp_utils.cpp
@@ -2,7 +2,8 @@
 
 namespace vortex_utils {
 double ssa(const double angle) {
-    double angle_ssa = fmod(angle + M_PI, 2 * M_PI) - M_PI;
+    double result = fmod(angle + M_PI, 2 * M_PI);
+    double angle_ssa = result < 0 ? result + M_PI : result - M_PI;
     return angle_ssa;
 }
 
diff --git a/test/test_cpp_utils.cpp b/test/test_cpp_utils.cpp
new file mode 100644
index 0000000..9eb0f85
--- /dev/null
+++ b/test/test_cpp_utils.cpp
@@ -0,0 +1,90 @@
+#include <gtest/gtest.h>
+#include <vortex_utils/cpp_utils.hpp>
+
+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/tests/test_utils.py b/tests/test_utils.py
index a1fe409..38b3d1b 100644
--- a/tests/test_utils.py
+++ b/tests/test_utils.py
@@ -1,5 +1,6 @@
 import numpy as np
 import pytest
+from geometry_msgs.msg import Pose, Twist
 
 from vortex_utils.python_utils import (
     PoseData,
@@ -9,47 +10,95 @@
     quat_to_euler,
     ssa,
 )
+from vortex_utils.ros_converter import pose_from_ros, twist_from_ros
 
 
-def test_ssa():
+def test_ssa_zero():
     assert ssa(0) == 0
+
+
+def test_ssa_two_pi():
     assert ssa(2 * np.pi) == 0
+
+
+def test_ssa_positive():
     assert ssa(3.5) == pytest.approx(-2.78, abs=0.01)
+
+
+def test_ssa_negative():
     assert ssa(-3.5) == pytest.approx(2.78, abs=0.01)
 
 
-def test_euler_to_quat():
+def test_euler_to_quat_zero():
     quat = euler_to_quat(0, 0, 0)
     assert quat == pytest.approx([0, 0, 0, 1], abs=0.01)
+
+
+def test_euler_to_quat_x():
     quat = euler_to_quat(1, 0, 0)
     assert quat == pytest.approx([0.479, 0, 0, 0.877], abs=0.01)
+
+
+def test_euler_to_quat_y():
     quat = euler_to_quat(0, 1, 0)
     assert quat == pytest.approx([0, 0.479, 0, 0.877], abs=0.01)
+
+
+def test_euler_to_quat_z():
     quat = euler_to_quat(0, 0, 1)
     assert quat == pytest.approx([0, 0, 0.479, 0.877], abs=0.01)
+
+
+def test_euler_to_quat_xyz():
     quat = euler_to_quat(1, 1, 1)
     assert quat == pytest.approx([0.5709, 0.167, 0.5709, 0.565], abs=0.01)
 
 
-def test_quat_to_euler():
+def test_quat_to_euler_identity():
     euler = quat_to_euler(0, 0, 0, 1)
     assert euler == pytest.approx([0, 0, 0], abs=0.01)
+
+
+def test_quat_to_euler_x():
     euler = quat_to_euler(0.707, 0, 0, 0.707)
     assert euler == pytest.approx([1.57, 0, 0], abs=0.01)
+
+
+def test_quat_to_euler_z():
     euler = quat_to_euler(0, 0, 0.707, 0.707)
     assert euler == pytest.approx([0, 0, 1.57], abs=0.01)
+
+
+def test_quat_to_euler_xyz():
     euler = quat_to_euler(0.4207, 0.4207, 0.229, 0.770)
     assert euler == pytest.approx([1, 1, 0], abs=0.01)
+
+
+def test_quat_to_euler_xyz_negative():
     euler = quat_to_euler(0.4207, -0.4207, -0.229, 0.770)
     assert euler == pytest.approx([1, -1, 0], abs=0.01)
 
 
-def test_pose():
+def test_pose_addition():
     pose = PoseData(1, 2, 3, 0.1, 0.2, 0.3)
     pose2 = PoseData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
     assert (pose + pose2) == PoseData(1.1, 2.2, 3.3, 0.2, 0.4, 0.6)
+
+
+def test_pose_subtraction():
+    pose = PoseData(1, 2, 3, 0.1, 0.2, 0.3)
+    pose2 = PoseData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
     assert (pose - pose2) == PoseData(0.9, 1.8, 2.7, 0, 0, 0)
+
+
+def test_pose_multiplication_scalar():
+    pose = PoseData(1, 2, 3, 0.1, 0.2, 0.3)
     assert (pose * 2) == PoseData(2, 4, 6, 0.2, 0.4, 0.6)
+
+
+def test_pose_multiplication_pose():
+    pose = PoseData(1, 2, 3, 0.1, 0.2, 0.3)
+    pose2 = PoseData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
     pose3 = pose * pose2
     assert pose3.x == pytest.approx(0.1, abs=0.01)
     assert pose3.y == pytest.approx(0.4, abs=0.01)
@@ -58,8 +107,16 @@ def test_pose():
     assert pose3.pitch == pytest.approx(0.04, abs=0.001)
     assert pose3.yaw == pytest.approx(0.09, abs=0.001)
 
+
+def test_pose_rotation_matrix_shape():
+    pose = PoseData(1, 2, 3, 0.1, 0.2, 0.3)
     rotation_matrix = pose.as_rotation_matrix()
     assert rotation_matrix.shape == (3, 3)
+
+
+def test_pose_rotation_matrix_values():
+    pose = PoseData(1, 2, 3, 0.1, 0.2, 0.3)
+    rotation_matrix = pose.as_rotation_matrix()
     assert rotation_matrix[0, 0] == pytest.approx(0.935, abs=0.05)
     assert rotation_matrix[0, 1] == pytest.approx(-0.283, abs=0.05)
     assert rotation_matrix[0, 2] == pytest.approx(0.2101, abs=0.05)
@@ -70,6 +127,8 @@ def test_pose():
     assert rotation_matrix[2, 1] == pytest.approx(0.127, abs=0.05)
     assert rotation_matrix[2, 2] == pytest.approx(0.975, abs=0.05)
 
+
+def test_pose_rotation_matrix_values_different_pose():
     pose = PoseData(1, 2, 3, 0.5, -0.5, 0.9)
     rotation_matrix = pose.as_rotation_matrix()
     assert rotation_matrix[0] == pytest.approx(
@@ -83,15 +142,36 @@ def test_pose():
     )
 
 
-def test_twist():
+def test_twist_addition():
     twist1 = TwistData(1, 2, 3, 0.1, 0.2, 0.3)
     twist2 = TwistData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
     assert (twist1 + twist2) == TwistData(1.1, 2.2, 3.3, 0.2, 0.4, 0.6)
+
+
+def test_twist_subtraction():
+    twist1 = TwistData(1, 2, 3, 0.1, 0.2, 0.3)
+    twist2 = TwistData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
     assert (twist1 - twist2) == TwistData(0.9, 1.8, 2.7, 0, 0, 0)
+
+
+def test_twist_multiplication_scalar():
+    twist1 = TwistData(1, 2, 3, 0.1, 0.2, 0.3)
     assert (twist1 * 2) == TwistData(2, 4, 6, 0.2, 0.4, 0.6)
 
 
-def test_state():
+def test_twist_multiplication_twist():
+    twist1 = TwistData(1, 2, 3, 0.1, 0.2, 0.3)
+    twist2 = TwistData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
+    twist3 = twist1 * twist2
+    assert twist3.linear_x == pytest.approx(0.1, abs=0.01)
+    assert twist3.linear_y == pytest.approx(0.4, abs=0.01)
+    assert twist3.linear_z == pytest.approx(0.9, abs=0.01)
+    assert twist3.angular_x == pytest.approx(0.01, abs=0.001)
+    assert twist3.angular_y == pytest.approx(0.04, abs=0.001)
+    assert twist3.angular_z == pytest.approx(0.09, abs=0.001)
+
+
+def test_state_addition_pose():
     pose1 = PoseData(1, 2, 3, 0.1, 0.2, 0.3)
     twist1 = TwistData(1, 2, 3, 0.1, 0.2, 0.3)
     state1 = State(pose1, twist1)
@@ -99,6 +179,69 @@ def test_state():
     twist2 = TwistData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
     state2 = State(pose2, twist2)
     assert (state1 + state2).pose == PoseData(1.1, 2.2, 3.3, 0.2, 0.4, 0.6)
+
+
+def test_state_subtraction_pose():
+    pose1 = PoseData(1, 2, 3, 0.1, 0.2, 0.3)
+    twist1 = TwistData(1, 2, 3, 0.1, 0.2, 0.3)
+    state1 = State(pose1, twist1)
+    pose2 = PoseData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
+    twist2 = TwistData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
+    state2 = State(pose2, twist2)
     assert (state1 - state2).pose == PoseData(0.9, 1.8, 2.7, 0, 0, 0)
+
+
+def test_state_addition_twist():
+    pose1 = PoseData(1, 2, 3, 0.1, 0.2, 0.3)
+    twist1 = TwistData(1, 2, 3, 0.1, 0.2, 0.3)
+    state1 = State(pose1, twist1)
+    pose2 = PoseData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
+    twist2 = TwistData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
+    state2 = State(pose2, twist2)
     assert (state1 + state2).twist == TwistData(1.1, 2.2, 3.3, 0.2, 0.4, 0.6)
+
+
+def test_state_subtraction_twist():
+    pose1 = PoseData(1, 2, 3, 0.1, 0.2, 0.3)
+    twist1 = TwistData(1, 2, 3, 0.1, 0.2, 0.3)
+    state1 = State(pose1, twist1)
+    pose2 = PoseData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
+    twist2 = TwistData(0.1, 0.2, 0.3, 0.1, 0.2, 0.3)
+    state2 = State(pose2, twist2)
     assert (state1 - state2).twist == TwistData(0.9, 1.8, 2.7, 0, 0, 0)
+
+
+def test_pose_from_ros():
+    pose_msg = Pose()
+    pose_msg.position.x = 1.0
+    pose_msg.position.y = 2.0
+    pose_msg.position.z = 3.0
+    pose_msg.orientation.x = 0.1
+    pose_msg.orientation.y = 0.2
+    pose_msg.orientation.z = 0.3
+    pose_msg.orientation.w = 0.4
+    euler = quat_to_euler(0.1, 0.2, 0.3, 0.4)
+    pose = pose_from_ros(pose_msg)
+    assert pose.x == 1.0
+    assert pose.y == 2.0
+    assert pose.z == 3.0
+    assert pose.roll == pytest.approx(euler[0], abs=0.01)
+    assert pose.pitch == pytest.approx(euler[1], abs=0.01)
+    assert pose.yaw == pytest.approx(euler[2], abs=0.01)
+
+
+def test_twist_from_ros():
+    twist_msg = Twist()
+    twist_msg.linear.x = 1.0
+    twist_msg.linear.y = 2.0
+    twist_msg.linear.z = 3.0
+    twist_msg.angular.x = 0.1
+    twist_msg.angular.y = 0.2
+    twist_msg.angular.z = 0.3
+    twist = twist_from_ros(twist_msg)
+    assert twist.linear_x == 1.0
+    assert twist.linear_y == 2.0
+    assert twist.linear_z == 3.0
+    assert twist.angular_x == 0.1
+    assert twist.angular_y == 0.2
+    assert twist.angular_z == 0.3