Try fixing trajectory length unit test

moveit · Mar 10, 2023 · 0793ed2 · 0793ed2
1 parent b6c9a6d
commit 0793ed2
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp
@@ -526,8 +526,15 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryIterator)
 
 TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength)
 {
+  const double epsilon = 1e-4;
+
+  // Create test trajectory and check that its length is zero.
   robot_trajectory::RobotTrajectoryPtr trajectory;
   initTestTrajectory(trajectory);
+  EXPECT_NEAR(robot_trajectory::path_length(*trajectory), 0.0, epsilon);
+
+  // Now modify the trajectory and check that the path length is greater than zero.
+  modifyFirstWaypointPtrAndCheckTrajectory(trajectory);
   EXPECT_GT(robot_trajectory::path_length(*trajectory), 0.0);
 }