diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 1117966ecf3..d345f3942b0 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 dacd6797d9b..fcb6f86b597 100644 --- 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); + 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);