From 43d03c1b34ae8cd373cfacaaacf7a2f675be7f63 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Wed, 15 Nov 2023 17:25:33 +0000
Subject: [PATCH] Add test for wraparound_joint method

---
 .../test/test_trajectory.cpp                  | 51 +++++++++++++++++++
 1 file changed, 51 insertions(+)

diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp
index b52aa67a04..4ff4ac01bc 100644
--- a/joint_trajectory_controller/test/test_trajectory.cpp
+++ b/joint_trajectory_controller/test/test_trajectory.cpp
@@ -821,3 +821,54 @@ TEST(TestTrajectory, skip_interpolation)
     }
   }
 }
+
+// test wraparound_joint method
+TEST(TestTrajectory, test_wraparound_joint)
+{
+  const std::vector<double> initial_position(3, 0.);
+  std::vector<double> next_position(3, M_PI * 3. / 2.);
+
+  // no wraparound
+  {
+    std::vector<double> current_position(initial_position);
+    std::vector<bool> joints_angle_wraparound(3, false);
+    joint_trajectory_controller::wraparound_joint(
+      current_position, next_position, joints_angle_wraparound);
+    EXPECT_EQ(current_position[0], initial_position[0]);
+    EXPECT_EQ(current_position[1], initial_position[1]);
+    EXPECT_EQ(current_position[2], initial_position[2]);
+  }
+
+  // wraparound of a single joint
+  {
+    std::vector<double> current_position(initial_position);
+    std::vector<bool> joints_angle_wraparound{true, false, false};
+    joint_trajectory_controller::wraparound_joint(
+      current_position, next_position, joints_angle_wraparound);
+    EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI);
+    EXPECT_EQ(current_position[1], initial_position[1]);
+    EXPECT_EQ(current_position[2], initial_position[2]);
+  }
+
+  // wraparound of all joints
+  {
+    std::vector<double> current_position(initial_position);
+    std::vector<bool> joints_angle_wraparound(3, true);
+    joint_trajectory_controller::wraparound_joint(
+      current_position, next_position, joints_angle_wraparound);
+    EXPECT_EQ(current_position[0], initial_position[0] + 2 * M_PI);
+    EXPECT_EQ(current_position[1], initial_position[1] + 2 * M_PI);
+    EXPECT_EQ(current_position[2], initial_position[2] + 2 * M_PI);
+  }
+
+  // wraparound of all joints, but no offset
+  {
+    std::vector<double> current_position(next_position);
+    std::vector<bool> joints_angle_wraparound(3, true);
+    joint_trajectory_controller::wraparound_joint(
+      current_position, next_position, joints_angle_wraparound);
+    EXPECT_EQ(current_position[0], next_position[0]);
+    EXPECT_EQ(current_position[1], next_position[1]);
+    EXPECT_EQ(current_position[2], next_position[2]);
+  }
+}