From 9ad28ef6dfdc6121a26630c8a06d0cb9eb372179 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 16 Dec 2022 14:19:06 -0600 Subject: [PATCH] Don't add to the timestep, rather overwrite it --- moveit_core/robot_trajectory/src/robot_trajectory.cpp | 2 +- .../robot_trajectory/test/test_robot_trajectory.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) 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..000534d74f9 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); + 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);