diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 512cbd0ac3..b404e41444 100644 --- 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 index b80ff79929..5eefa8e898 100644 --- 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;