From 045e57e8727299a631ec6109e99a3d120e73d964 Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Wed, 19 Aug 2026 13:49:10 +0000 Subject: [PATCH 1/7] Fix flaky spline interpolation test Made new method: sendTrajectoryAndConfirmStart which launches an async thread to confirm that a new trajectory resets the spline_travel_time and then starts executing. This is more robust than the previous implementation. I have replaced all the calls to sendTrajectory where they were immediately followed by waitForTrajectoryStarted, with one call to sendTrajectoryAndConfirmStart. waitForTrajectoryStarted is no longer used, but I havent removed it yet. --- tests/test_spline_interpolation.cpp | 178 ++++++++++++++++++++++------ 1 file changed, 139 insertions(+), 39 deletions(-) diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 68c1ef18f..c39a5de73 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include "test_utils.h" using namespace urcl; @@ -171,7 +172,6 @@ class SplineInterpolationTest : public ::testing::Test // Send trajectory to robot for execution ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_START, s_pos.size())); - for (size_t i = 0; i < s_pos.size(); i++) { // QUINTIC @@ -187,6 +187,39 @@ class SplineInterpolationTest : public ::testing::Test } } + bool sendTrajectoryAndConfirmStart(const std::vector& s_pos, + const std::vector& s_vel, + const std::vector& s_acc, const std::vector& s_time) + { + async_ready = false; + async_stop = false; + // Launch async thread to check that trajectory resets spline_travel_time + auto confirm_future = std::async(std::launch::async, &SplineInterpolationTest::confirmTrajectoryStarted, this); + + // Wait for the thread to be ready + std::unique_lock lk(async_mutex); + if (!async_cv.wait_for(lk, std::chrono::milliseconds(1000), + [this] { return async_ready; })) // Setting up the thread might take some time + { + async_stop = true; + confirm_future.wait(); + return false; + } + + // Send trajectory + sendTrajectory(s_pos, s_vel, s_acc, s_time); + + // Wait for trajectory to start (should be quick) + if (confirm_future.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) + { + std::cout << "Trajectory could not be confirmed to have started." << std::endl; + async_stop = true; + confirm_future.wait(); + return false; + } + return confirm_future.get(); + } + void interpolate(const double& time, urcl::vector6d_t& positions, const std::vector& coefficients) { for (unsigned int i = 0; i < 6; ++i) @@ -254,26 +287,105 @@ class SplineInterpolationTest : public ::testing::Test return coefficients; } + bool confirmTrajectoryStarted() + { + // Timers + double trajectory_start_timeout = 1; + double trajectory_start_time = 0.0; + double travel_time_reset_timeout = 1; + double travel_time_reset_time = 0.0; + + // Data package pre-allocation + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + + // Spline travel times + double spline_travel_time = 0.0; + double start_spline_travel_time = 0.0; + + // Check what the current spline_travel_time is + if (g_my_robot->getUrDriver()->getDataPackage(data_pkg)) + { + data_pkg.getData("output_double_register_1", start_spline_travel_time); + } + else + { + std::cout << "Could not get previous spline travel time" << std::endl; + return false; + } + + // Allow new trajectory to be sent + { + std::lock_guard lk(async_mutex); + async_ready = true; + async_cv.notify_one(); + } + + bool spline_travel_time_reset = false; + while (g_my_robot->getUrDriver()->getDataPackage(data_pkg)) + { + // Caller wants the thread to stop for whatever reason + if (async_stop) + { + return false; + } + + // Continously check spline travel time + data_pkg.getData("output_double_register_1", spline_travel_time); + // Confirm that a new trajectory was transferred (Travel time = 0.0) + if (spline_travel_time < start_spline_travel_time || spline_travel_time == 0.0) + { + spline_travel_time_reset = true; + } + if (spline_travel_time_reset) + { + // Confirm that the new trajectory is running + if (std::abs(spline_travel_time) > 0.002) + { + return true; + } + if (trajectory_start_time > trajectory_start_timeout) + { + std::cout << "Trajectory didn't start within timeout, is the spline travel time written to output float " + "register 1?" + << std::endl; + return false; + } + trajectory_start_time += step_time_; + } + else + { + if (travel_time_reset_time > travel_time_reset_timeout) + { + std::cout << "Spline travel time was not reset, was trajectory transferred to robot, and the spline travel " + "time written to output float register 1?" + << std::endl; + return false; + } + travel_time_reset_time += step_time_; + } + } + return false; + } + void waitForTrajectoryStarted() { - bool trajectory_started = false; double timeout = 1; double cur_time = 0.0; rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); - while (trajectory_started == false && g_my_robot->getUrDriver()->getDataPackage(data_pkg)) + while (g_my_robot->getUrDriver()->getDataPackage(data_pkg)) { double spline_travel_time = 0.0; data_pkg.getData("output_double_register_1", spline_travel_time); - // Keep connection alive ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); - if (std::abs(spline_travel_time - 0.0) < 0.01) + if (std::abs(spline_travel_time) < 0.01) { return; } if (cur_time > timeout) { + std::cout << std::endl; std::cout << "Trajectory didn't start within timeout, is the spline travel time written to output float " "register 1?" << std::endl; @@ -348,6 +460,12 @@ class SplineInterpolationTest : public ::testing::Test // Deceleration variables if changed in the script, they should be changed here as well double deceleration_time_ = 0.4189; double max_deceleration_ = 15; + + // Async variables + bool async_ready = false; + std::mutex async_mutex; + std::condition_variable async_cv; + std::atomic async_stop = false; }; TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) @@ -381,12 +499,10 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) std::vector time_vec, spline_time; // Send the trajectory to the robot - sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_trajectory_running = true; - // Make sure that the trajectory has started before we start testing for trajectory execution - waitForTrajectoryStarted(); + ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, std::vector(), s_time)); + g_trajectory_running = true; double old_spline_travel_time = 0.0; double plot_time = 0.0; while (g_trajectory_running) @@ -499,12 +615,9 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee std::vector time_vec, spline_time; // Send trajectory to the robot - sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_trajectory_running = true; - // Make sure that trajectory has started before we start testing for trajectory execution - waitForTrajectoryStarted(); - + ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time)); + g_trajectory_running = true; double old_spline_travel_time = 0.0; double plot_time = 0.0; unsigned int loop_count = 0; @@ -663,12 +776,9 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) std::vector time_vec, spline_time; // Send the trajectory to the robot - sendTrajectory(s_pos, s_vel, std::vector(), s_time); - g_trajectory_running = true; - // Make sure that trajectory has started before we start testing for trajectory execution - waitForTrajectoryStarted(); - + ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, std::vector(), s_time)); + g_trajectory_running = true; int segment_idx = 0; double old_spline_travel_time = 0.0; double plot_time = 0.0; @@ -774,12 +884,8 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) std::vector time_vec, spline_time; // Send the trajectory to the robot - sendTrajectory(s_pos, s_vel, s_acc, s_time); - g_trajectory_running = true; - // Make sure that trajectory has started before we start testing for trajectory execution - waitForTrajectoryStarted(); - + ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time)); int segment_idx = 0; double old_spline_travel_time = 0.0; double plot_time = 0.0; @@ -894,9 +1000,9 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) s_pos = { first_point, second_point, third_point }; s_vel = { zeros, zeros, zeros }; s_time = { 0.0, 1.0, 2.0 }; - sendTrajectory(s_pos, s_vel, std::vector(), s_time); + + ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, std::vector(), s_time)); g_trajectory_running = true; - waitForTrajectoryStarted(); urcl::vector6d_t joint_positions; while (g_trajectory_running) { @@ -968,9 +1074,9 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) s_vel = { zeros, zeros, zeros }; s_acc = { zeros, zeros, zeros }; s_time = { 0.0, 1.0, 2.0 }; - sendTrajectory(s_pos, s_vel, s_acc, s_time); + + ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time)); g_trajectory_running = true; - waitForTrajectoryStarted(); urcl::vector6d_t joint_positions; while (g_trajectory_running) { @@ -1145,14 +1251,11 @@ TEST_F(SplineInterpolationTest, new_trajectory_received_without_cancelling_the_o } // Send the trajectory - sendTrajectory(s_pos, s_vel, s_acc, s_time); - waitForTrajectoryStarted(); + ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time)); // Send the trajectory again without canceling the before one - sendTrajectory(s_pos, s_vel, s_acc, s_time); + ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time)); g_trajectory_running = true; - waitForTrajectoryStarted(); - // Ensure that everything goes as expected and the robot reaches the target position urcl::vector6d_t joint_positions; while (g_trajectory_running) @@ -1203,9 +1306,7 @@ TEST_F(SplineInterpolationTest, cancel_trajectory_while_being_executed_and_sendi } // Send the trajectory and cancel it right away - sendTrajectory(s_pos, s_vel, s_acc, s_time); - waitForTrajectoryStarted(); - + ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time)); ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL)); EXPECT_TRUE(waitForTrajectoryResult(std::chrono::milliseconds(500))); @@ -1214,13 +1315,12 @@ TEST_F(SplineInterpolationTest, cancel_trajectory_while_being_executed_and_sendi // Do the same procesure again to ensure that a new trajectory can be send after canceling the before // one in the middle of its execution - sendTrajectory(s_pos, s_vel, s_acc, s_time); - waitForTrajectoryStarted(); - + ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time)); ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL)); EXPECT_TRUE(waitForTrajectoryResult(std::chrono::milliseconds(500))); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); + std::cout << "Canceled second trajectory" << std::endl; } int main(int argc, char* argv[]) From bebf33e672a1cc29c001f24deca7a501bbd40909 Mon Sep 17 00:00:00 2001 From: URJala <159417921+URJala@users.noreply.github.com> Date: Wed, 19 Aug 2026 16:04:41 +0200 Subject: [PATCH 2/7] Release mutex in failure path Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- tests/test_spline_interpolation.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index c39a5de73..0094bbf89 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -202,6 +202,7 @@ class SplineInterpolationTest : public ::testing::Test [this] { return async_ready; })) // Setting up the thread might take some time { async_stop = true; + lk.unlock(); confirm_future.wait(); return false; } From 98a3c16489d58e7665127266f3db2580ee5b3905 Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Thu, 20 Aug 2026 07:10:47 +0000 Subject: [PATCH 3/7] Add keep alive writes --- tests/test_spline_interpolation.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 0094bbf89..ad174da99 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -193,6 +193,7 @@ class SplineInterpolationTest : public ::testing::Test { async_ready = false; async_stop = false; + trajectory_sent = false; // Launch async thread to check that trajectory resets spline_travel_time auto confirm_future = std::async(std::launch::async, &SplineInterpolationTest::confirmTrajectoryStarted, this); @@ -210,6 +211,8 @@ class SplineInterpolationTest : public ::testing::Test // Send trajectory sendTrajectory(s_pos, s_vel, s_acc, s_time); + // Inform thread that trajectory is sent + trajectory_sent = true; // Wait for trajectory to start (should be quick) if (confirm_future.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) { @@ -339,6 +342,15 @@ class SplineInterpolationTest : public ::testing::Test } if (spline_travel_time_reset) { + // Keep script alive when the trajectory has been sent + if (trajectory_sent) + { + if (!g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)) + { + return false; + } + } // Confirm that the new trajectory is running if (std::abs(spline_travel_time) > 0.002) { @@ -467,6 +479,7 @@ class SplineInterpolationTest : public ::testing::Test std::mutex async_mutex; std::condition_variable async_cv; std::atomic async_stop = false; + std::atomic trajectory_sent = false; }; TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) From d4cd1819f50dd43341d178c053663b2ff293f4f4 Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Thu, 20 Aug 2026 07:22:00 +0000 Subject: [PATCH 4/7] Increase future timeout to match timer in thread --- tests/test_spline_interpolation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index ad174da99..f3433500e 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -214,7 +214,7 @@ class SplineInterpolationTest : public ::testing::Test // Inform thread that trajectory is sent trajectory_sent = true; // Wait for trajectory to start (should be quick) - if (confirm_future.wait_for(std::chrono::milliseconds(500)) != std::future_status::ready) + if (confirm_future.wait_for(std::chrono::milliseconds(1000)) != std::future_status::ready) { std::cout << "Trajectory could not be confirmed to have started." << std::endl; async_stop = true; From 5c36b82a78c8534c482009dcd3d51e1acaf89b79 Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Thu, 20 Aug 2026 08:24:19 +0000 Subject: [PATCH 5/7] Remove stray debugging print --- tests/test_spline_interpolation.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index f3433500e..2d8fa9e04 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -1334,7 +1334,6 @@ TEST_F(SplineInterpolationTest, cancel_trajectory_while_being_executed_and_sendi urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL)); EXPECT_TRUE(waitForTrajectoryResult(std::chrono::milliseconds(500))); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); - std::cout << "Canceled second trajectory" << std::endl; } int main(int argc, char* argv[]) From e1aa95b8263f32db685e7db509e6dc8277950bce Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Thu, 20 Aug 2026 09:02:01 +0000 Subject: [PATCH 6/7] Ensure that a trajectory always runs for at least one cycle Also add debug messages when failing Also make reverse socket read blocking --- tests/test_spline_interpolation.cpp | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 2d8fa9e04..6d893bde1 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -171,7 +171,7 @@ class SplineInterpolationTest : public ::testing::Test // Send trajectory to robot for execution ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( - urcl::control::TrajectoryControlMessage::TRAJECTORY_START, s_pos.size())); + urcl::control::TrajectoryControlMessage::TRAJECTORY_START, s_pos.size(), RobotReceiveTimeout::off())); for (size_t i = 0; i < s_pos.size(); i++) { // QUINTIC @@ -330,29 +330,31 @@ class SplineInterpolationTest : public ::testing::Test // Caller wants the thread to stop for whatever reason if (async_stop) { + std::cout << "Async stop triggered, exiting confirm thread" << std::endl; return false; } - // Continously check spline travel time + // Continuously check spline travel time data_pkg.getData("output_double_register_1", spline_travel_time); - // Confirm that a new trajectory was transferred (Travel time = 0.0) + + // Confirm that a new trajectory was transferred (Travel time = 0.0 or lower than it was before) if (spline_travel_time < start_spline_travel_time || spline_travel_time == 0.0) { spline_travel_time_reset = true; + start_spline_travel_time = spline_travel_time; } if (spline_travel_time_reset) { // Keep script alive when the trajectory has been sent - if (trajectory_sent) + if (!g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)) { - if (!g_my_robot->getUrDriver()->writeTrajectoryControlMessage( - urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)) - { - return false; - } + std::cout << "Failed to write trajectory NOOP control message, exiting confirm thread" << std::endl; + return false; } + // Confirm that the new trajectory is running - if (std::abs(spline_travel_time) > 0.002) + if (std::abs(spline_travel_time) > start_spline_travel_time) { return true; } @@ -377,6 +379,7 @@ class SplineInterpolationTest : public ::testing::Test travel_time_reset_time += step_time_; } } + std::cout << "Failed to get data package from RTDE, exiting confirm thread" << std::endl; return false; } @@ -398,7 +401,6 @@ class SplineInterpolationTest : public ::testing::Test } if (cur_time > timeout) { - std::cout << std::endl; std::cout << "Trajectory didn't start within timeout, is the spline travel time written to output float " "register 1?" << std::endl; From 4e94127b46f04e1c8373cc777dd18580917f148f Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Thu, 20 Aug 2026 09:10:51 +0000 Subject: [PATCH 7/7] Only write trajectory noop after trajectory has been sent --- tests/test_spline_interpolation.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 6d893bde1..ca2e42fd2 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -346,11 +346,14 @@ class SplineInterpolationTest : public ::testing::Test if (spline_travel_time_reset) { // Keep script alive when the trajectory has been sent - if (!g_my_robot->getUrDriver()->writeTrajectoryControlMessage( - urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)) + if (trajectory_sent) { - std::cout << "Failed to write trajectory NOOP control message, exiting confirm thread" << std::endl; - return false; + if (!g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)) + { + std::cout << "Failed to write trajectory NOOP control message, exiting confirm thread" << std::endl; + return false; + } } // Confirm that the new trajectory is running