diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index eabdb149cb..6b8113c402 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -453,6 +453,133 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
   executor.cancel();
 }
 
+// Floating-point value comparison threshold
+const double EPS = 1e-6;
+/**
+ * @brief check if position error of revolute joints are angle_wraparound if not configured so
+ */
+TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound)
+{
+  rclcpp::executors::MultiThreadedExecutor executor;
+  constexpr double k_p = 10.0;
+  std::vector<rclcpp::Parameter> params = {
+    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
+  bool angle_wraparound = false;
+  SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound);
+  subscribeToState();
+
+  size_t n_joints = joint_names_.size();
+
+  // send msg
+  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
+  builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
+  // *INDENT-OFF*
+  std::vector<std::vector<double>> points{
+    {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
+  std::vector<std::vector<double>> points_velocities{
+    {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
+  // *INDENT-ON*
+  publish(time_from_start, points, rclcpp::Time(), {}, {});
+  traj_controller_->wait_for_trajectory(executor);
+
+  // first update
+  updateController(rclcpp::Duration(FIRST_POINT_TIME));
+
+  // Spin to receive latest state
+  executor.spin_some();
+  auto state_msg = getState();
+  ASSERT_TRUE(state_msg);
+
+  const auto allowed_delta = 0.1;
+
+  // no update of state_interface
+  EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS);
+
+  // has the msg the correct vector sizes?
+  EXPECT_EQ(n_joints, state_msg->reference.positions.size());
+  EXPECT_EQ(n_joints, state_msg->feedback.positions.size());
+  EXPECT_EQ(n_joints, state_msg->error.positions.size());
+
+  // are the correct reference positions used?
+  EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta);
+  EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
+  EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);
+
+  // no normalization of position error
+  EXPECT_NEAR(
+    state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
+  EXPECT_NEAR(
+    state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
+  EXPECT_NEAR(
+    state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
+
+  if (traj_controller_->has_position_command_interface())
+  {
+    // check command interface
+    EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
+    EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
+    EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
+    // double check state msg
+    EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
+    EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
+    EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta);
+  }
+
+  if (traj_controller_->has_velocity_command_interface())
+  {
+    // use_closed_loop_pid_adapter_
+    if (traj_controller_->use_closed_loop_pid_adapter())
+    {
+      // we expect u = k_p * (s_d-s)
+      EXPECT_NEAR(
+        k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
+        k_p * allowed_delta);
+      EXPECT_NEAR(
+        k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
+        k_p * allowed_delta);
+      // no normalization of position error
+      EXPECT_NEAR(
+        k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
+        k_p * allowed_delta);
+    }
+    else
+    {
+      // check command interface
+      EXPECT_LT(0.0, joint_vel_[0]);
+      EXPECT_LT(0.0, joint_vel_[1]);
+      EXPECT_LT(0.0, joint_vel_[2]);
+      // double check state msg
+      EXPECT_LT(0.0, state_msg->output.velocities[0]);
+      EXPECT_LT(0.0, state_msg->output.velocities[1]);
+      EXPECT_LT(0.0, state_msg->output.velocities[2]);
+    }
+  }
+
+  if (traj_controller_->has_effort_command_interface())
+  {
+    // with effort command interface, use_closed_loop_pid_adapter is always true
+
+    // check command interface
+    // we expect u = k_p * (s_d-s) for joint0, joint1, joint2
+    EXPECT_NEAR(
+      k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
+      k_p * allowed_delta);
+    EXPECT_NEAR(
+      k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
+      k_p * allowed_delta);
+    EXPECT_NEAR(
+      k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2],
+      k_p * allowed_delta);
+
+    // double check state msg
+    EXPECT_LT(0.0, state_msg->output.effort[0]);
+    EXPECT_LT(0.0, state_msg->output.effort[1]);
+    EXPECT_LT(0.0, state_msg->output.effort[2]);
+  }
+
+  executor.cancel();
+}
+
 /**
  * @brief cmd_timeout must be greater than constraints.goal_time
  */
@@ -594,133 +721,6 @@ TEST_P(TrajectoryControllerTestParameterized, timeout)
   executor.cancel();
 }
 
-// Floating-point value comparison threshold
-const double EPS = 1e-6;
-/**
- * @brief check if position error of revolute joints are angle_wraparound if not configured so
- */
-TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparound)
-{
-  rclcpp::executors::MultiThreadedExecutor executor;
-  constexpr double k_p = 10.0;
-  std::vector<rclcpp::Parameter> params = {
-    rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
-  bool angle_wraparound = false;
-  SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, angle_wraparound);
-  subscribeToState();
-
-  size_t n_joints = joint_names_.size();
-
-  // send msg
-  constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
-  builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)};
-  // *INDENT-OFF*
-  std::vector<std::vector<double>> points{
-    {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
-  std::vector<std::vector<double>> points_velocities{
-    {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
-  // *INDENT-ON*
-  publish(time_from_start, points, rclcpp::Time(), {}, {});
-  traj_controller_->wait_for_trajectory(executor);
-
-  // first update
-  updateController(rclcpp::Duration(FIRST_POINT_TIME));
-
-  // Spin to receive latest state
-  executor.spin_some();
-  auto state_msg = getState();
-  ASSERT_TRUE(state_msg);
-
-  const auto allowed_delta = 0.1;
-
-  // no update of state_interface
-  EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS);
-
-  // has the msg the correct vector sizes?
-  EXPECT_EQ(n_joints, state_msg->reference.positions.size());
-  EXPECT_EQ(n_joints, state_msg->feedback.positions.size());
-  EXPECT_EQ(n_joints, state_msg->error.positions.size());
-
-  // are the correct reference positions used?
-  EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta);
-  EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta);
-  EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta);
-
-  // no normalization of position error
-  EXPECT_NEAR(
-    state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS);
-  EXPECT_NEAR(
-    state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
-  EXPECT_NEAR(
-    state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
-
-  if (traj_controller_->has_position_command_interface())
-  {
-    // check command interface
-    EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta);
-    EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta);
-    EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta);
-    // double check state msg
-    EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta);
-    EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta);
-    EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta);
-  }
-
-  if (traj_controller_->has_velocity_command_interface())
-  {
-    // use_closed_loop_pid_adapter_
-    if (traj_controller_->use_closed_loop_pid_adapter())
-    {
-      // we expect u = k_p * (s_d-s)
-      EXPECT_NEAR(
-        k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0],
-        k_p * allowed_delta);
-      EXPECT_NEAR(
-        k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1],
-        k_p * allowed_delta);
-      // no normalization of position error
-      EXPECT_NEAR(
-        k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2],
-        k_p * allowed_delta);
-    }
-    else
-    {
-      // check command interface
-      EXPECT_LT(0.0, joint_vel_[0]);
-      EXPECT_LT(0.0, joint_vel_[1]);
-      EXPECT_LT(0.0, joint_vel_[2]);
-      // double check state msg
-      EXPECT_LT(0.0, state_msg->output.velocities[0]);
-      EXPECT_LT(0.0, state_msg->output.velocities[1]);
-      EXPECT_LT(0.0, state_msg->output.velocities[2]);
-    }
-  }
-
-  if (traj_controller_->has_effort_command_interface())
-  {
-    // with effort command interface, use_closed_loop_pid_adapter is always true
-
-    // check command interface
-    // we expect u = k_p * (s_d-s) for joint0, joint1, joint2
-    EXPECT_NEAR(
-      k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0],
-      k_p * allowed_delta);
-    EXPECT_NEAR(
-      k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1],
-      k_p * allowed_delta);
-    EXPECT_NEAR(
-      k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2],
-      k_p * allowed_delta);
-
-    // double check state msg
-    EXPECT_LT(0.0, state_msg->output.effort[0]);
-    EXPECT_LT(0.0, state_msg->output.effort[1]);
-    EXPECT_LT(0.0, state_msg->output.effort[2]);
-  }
-
-  executor.cancel();
-}
-
 /**
  * @brief check if position error of revolute joints are normalized if configured so
  */