Don't add to the timestep, rather overwrite it

moveit · Dec 18, 2022 · 3385acc · 3385acc
1 parent 9cf5148
commit 3385acc
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 9 deletions.
diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp
@@ -132,7 +132,7 @@ RobotTrajectory& RobotTrajectory::append(const RobotTrajectory& source, double d
                                  std::next(source.duration_from_previous_.begin(), start_index),
                                  std::next(source.duration_from_previous_.begin(), end_index));
   if (duration_from_previous_.size() > index)
-    duration_from_previous_[index] += dt;
+    duration_from_previous_[index] = dt;
 
   return *this;
 }

diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp
@@ -204,20 +204,21 @@ TEST_F(RobotTrajectoryTestFixture, Append)
 {
   robot_trajectory::RobotTrajectoryPtr initial_trajectory;
   initTestTrajectory(initial_trajectory);
+  EXPECT_EQ(initial_trajectory->getWayPointCount(), size_t(5));
 
   // Append to the first
   robot_trajectory::RobotTrajectoryPtr traj2;
   initTestTrajectory(traj2);
+  EXPECT_EQ(traj2->getWayPointCount(), size_t(5));
 
   // After append() we should have 10 waypoints, all with 0.1s duration
-  const double EXPECTED_DURATION = 0.1;
-  initial_trajectory->append(*traj2, 0.1, 0, 4);
-
-  EXPECT_EQ(traj2->getWayPointCount(), 5);
-  EXPECT_EQ(initial_trajectory->getWayPointCount(), 10);
-  EXPECT_EQ(initial_trajectory->getWayPointDurationFromPrevious(4), EXPECTED_DURATION);
-  EXPECT_EQ(initial_trajectory->getWayPointDurationFromPrevious(5), EXPECTED_DURATION);
-  EXPECT_EQ(initial_trajectory->getWayPointDurationFromPrevious(6), EXPECTED_DURATION);
+  const double expected_duration = 0.1;
+  initial_trajectory->append(*traj2, expected_duration, 0, 5);
+  EXPECT_EQ(initial_trajectory->getWayPointCount(), size_t(10));
+
+  EXPECT_EQ(initial_trajectory->getWayPointDurationFromPrevious(4), expected_duration);
+  EXPECT_EQ(initial_trajectory->getWayPointDurationFromPrevious(5), expected_duration);
+  EXPECT_EQ(initial_trajectory->getWayPointDurationFromPrevious(6), expected_duration);
 }
 
 TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryShallowCopy)