From fe6d3c7300278341a6093740a73c4779410a43f7 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 20 Dec 2022 09:03:30 -0600 Subject: [PATCH] Bug fix: RobotTrajectory append() (#1813) * Add a test for append() * Don't add to the timestep, rather overwrite it (cherry picked from commit 35bb662a6656f08101f0c1bd6aff5496d2172bf9) --- .../robot_trajectory/src/robot_trajectory.cpp | 2 +- .../test/test_robot_trajectory.cpp | 21 +++++++++++++++++++ 2 files changed, 22 insertions(+), 1 deletion(-) 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;