Don't add to the timestep, rather overwrite it

moveit · Dec 16, 2022 · 9ad28ef · 9ad28ef
1 parent 9cf5148
commit 9ad28ef
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 4 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,17 +204,18 @@ 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);
+  initial_trajectory->append(*traj2, 0.1, 0, 5);
+  EXPECT_EQ(initial_trajectory->getWayPointCount(), size_t(10));
 
-  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);