diff --git a/joint_trajectory_controller/test/test_trajectory.cpp b/joint_trajectory_controller/test/test_trajectory.cpp index 4ff4ac01bc..6e0c53ac77 100644 --- a/joint_trajectory_controller/test/test_trajectory.cpp +++ b/joint_trajectory_controller/test/test_trajectory.cpp @@ -822,53 +822,58 @@ TEST(TestTrajectory, skip_interpolation) } } -// test wraparound_joint method -TEST(TestTrajectory, test_wraparound_joint) +TEST(TestWrapAroundJoint, no_wraparound) { const std::vector initial_position(3, 0.); std::vector next_position(3, M_PI * 3. / 2.); - // no wraparound - { - std::vector current_position(initial_position); - std::vector 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]); - } + std::vector current_position(initial_position); + std::vector 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 current_position(initial_position); - std::vector 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]); - } +TEST(TestWrapAroundJoint, wraparound_single_joint) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); - // wraparound of all joints - { - std::vector current_position(initial_position); - std::vector 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); - } + std::vector current_position(initial_position); + std::vector 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, but no offset - { - std::vector current_position(next_position); - std::vector 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]); - } +TEST(TestWrapAroundJoint, wraparound_all_joints) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(initial_position); + std::vector 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); +} + +TEST(TestWrapAroundJoint, wraparound_all_joints_no_offset) +{ + const std::vector initial_position(3, 0.); + std::vector next_position(3, M_PI * 3. / 2.); + + std::vector current_position(next_position); + std::vector 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]); }