Bug fix: RobotTrajectory append() (moveit#1813)

* Add a test for append() * Don't add to the timestep, rather overwrite it
KanRobotics · Dec 20, 2022 · 35bb662 · 35bb662
1 parent 1f32ab0
commit 35bb662
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 1 deletion.
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
@@ -200,6 +200,27 @@ TEST_F(RobotTrajectoryTestFixture, ChainEdits)
   EXPECT_EQ(trajectory.getWayPointCount(), initial_trajectory->getWayPointCount() * 2 + 3);
 }
 
+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, 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)
 {
   bool deepcopy = false;