diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 68c1ef18f..ca2e42fd2 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; @@ -170,8 +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 @@ -187,6 +187,43 @@ 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; + 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); + + // 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; + lk.unlock(); + confirm_future.wait(); + return false; + } + + // 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(1000)) != 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,21 +291,114 @@ 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) + { + std::cout << "Async stop triggered, exiting confirm thread" << std::endl; + return false; + } + + // 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 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)) + { + 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) > start_spline_travel_time) + { + 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_; + } + } + std::cout << "Failed to get data package from RTDE, exiting confirm thread" << std::endl; + 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; } @@ -348,6 +478,13 @@ 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; + std::atomic trajectory_sent = false; }; TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) @@ -381,12 +518,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 +634,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 +795,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 +903,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 +1019,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 +1093,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 +1270,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 +1325,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,9 +1334,7 @@ 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)));