|
| 1 | +// -- BEGIN LICENSE BLOCK ---------------------------------------------- |
| 2 | +// Copyright 2026 Universal Robots A/S |
| 3 | +// |
| 4 | +// Redistribution and use in source and binary forms, with or without |
| 5 | +// modification, are permitted provided that the following conditions are met: |
| 6 | +// |
| 7 | +// * Redistributions of source code must retain the above copyright |
| 8 | +// notice, this list of conditions and the following disclaimer. |
| 9 | +// |
| 10 | +// * Redistributions in binary form must reproduce the above copyright |
| 11 | +// notice, this list of conditions and the following disclaimer in the |
| 12 | +// documentation and/or other materials provided with the distribution. |
| 13 | +// |
| 14 | +// * Neither the name of the {copyright_holder} nor the names of its |
| 15 | +// contributors may be used to endorse or promote products derived from |
| 16 | +// this software without specific prior written permission. |
| 17 | +// |
| 18 | +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| 19 | +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| 20 | +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| 21 | +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE |
| 22 | +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| 23 | +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| 24 | +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| 25 | +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| 26 | +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 27 | +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 28 | +// POSSIBILITY OF SUCH DAMAGE. |
| 29 | +// -- END LICENSE BLOCK ------------------------------------------------ |
| 30 | + |
| 31 | +#include <chrono> |
| 32 | +#include <condition_variable> |
| 33 | +#include <memory> |
| 34 | +#include <mutex> |
| 35 | +#include <string> |
| 36 | +#include <thread> |
| 37 | + |
| 38 | +#include <gtest/gtest.h> |
| 39 | + |
| 40 | +#include "ur_client_library/control/trajectory_point_interface.h" |
| 41 | +#include "ur_client_library/example_robot_wrapper.h" |
| 42 | +#include "ur_client_library/log.h" |
| 43 | +#include "ur_client_library/types.h" |
| 44 | + |
| 45 | +using namespace urcl; |
| 46 | + |
| 47 | +const std::string SCRIPT_FILE = "../resources/external_control.urscript"; |
| 48 | +const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe.txt"; |
| 49 | +const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt"; |
| 50 | +std::string g_ROBOT_IP = "192.168.56.101"; |
| 51 | +bool g_HEADLESS = true; |
| 52 | + |
| 53 | +std::unique_ptr<ExampleRobotWrapper> g_my_robot; |
| 54 | + |
| 55 | +std::condition_variable g_trajectory_result_cv; |
| 56 | +std::mutex g_trajectory_result_mutex; |
| 57 | +control::TrajectoryResult g_trajectory_result = control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN; |
| 58 | +bool g_trajectory_result_received = false; |
| 59 | + |
| 60 | +void handleTrajectoryState(control::TrajectoryResult state) |
| 61 | +{ |
| 62 | + std::lock_guard<std::mutex> lk(g_trajectory_result_mutex); |
| 63 | + g_trajectory_result = state; |
| 64 | + g_trajectory_result_received = true; |
| 65 | + g_trajectory_result_cv.notify_one(); |
| 66 | + URCL_LOG_INFO("Received trajectory result %s", control::trajectoryResultToString(state).c_str()); |
| 67 | +} |
| 68 | + |
| 69 | +class TrajectoryStreamingTest : public ::testing::Test |
| 70 | +{ |
| 71 | +protected: |
| 72 | + static void SetUpTestSuite() |
| 73 | + { |
| 74 | + // Streaming tests are headless only. The URCap path doesn't exercise |
| 75 | + // anything different at the URScript level for streaming, so there is |
| 76 | + // no value in maintaining a separate _urcap variant. |
| 77 | + ASSERT_TRUE(g_HEADLESS) << "trajectory streaming tests require headless mode"; |
| 78 | + |
| 79 | + g_my_robot = std::make_unique<ExampleRobotWrapper>(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, |
| 80 | + "external_control.urp", SCRIPT_FILE); |
| 81 | + ASSERT_TRUE(g_my_robot->isHealthy()); |
| 82 | + g_my_robot->getUrDriver()->registerTrajectoryDoneCallback(&handleTrajectoryState); |
| 83 | + } |
| 84 | + |
| 85 | + void SetUp() override |
| 86 | + { |
| 87 | + if (!g_my_robot->isHealthy()) |
| 88 | + { |
| 89 | + ASSERT_TRUE(g_my_robot->resendRobotProgram()); |
| 90 | + ASSERT_TRUE(g_my_robot->waitForProgramRunning(500)); |
| 91 | + } |
| 92 | + resetTrajectoryResultState(); |
| 93 | + } |
| 94 | + |
| 95 | + static void resetTrajectoryResultState() |
| 96 | + { |
| 97 | + std::lock_guard<std::mutex> lk(g_trajectory_result_mutex); |
| 98 | + g_trajectory_result = control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN; |
| 99 | + g_trajectory_result_received = false; |
| 100 | + } |
| 101 | + |
| 102 | + // Pump TRAJECTORY_NOOP on the reverse socket so the urscript's main |
| 103 | + // dispatcher read does not time out while we wait for the trajectory |
| 104 | + // result callback. Returns the captured result, or |
| 105 | + // TRAJECTORY_RESULT_UNKNOWN if the deadline elapsed first. |
| 106 | + control::TrajectoryResult waitForTrajectoryResultPumpingNoops(std::chrono::milliseconds total_timeout) |
| 107 | + { |
| 108 | + const auto deadline = std::chrono::steady_clock::now() + total_timeout; |
| 109 | + while (std::chrono::steady_clock::now() < deadline) |
| 110 | + { |
| 111 | + std::unique_lock<std::mutex> lk(g_trajectory_result_mutex); |
| 112 | + if (g_trajectory_result_cv.wait_for(lk, std::chrono::milliseconds(100), |
| 113 | + [] { return g_trajectory_result_received; })) |
| 114 | + { |
| 115 | + return g_trajectory_result; |
| 116 | + } |
| 117 | + lk.unlock(); |
| 118 | + g_my_robot->getUrDriver()->writeTrajectoryControlMessage( |
| 119 | + control::TrajectoryControlMessage::TRAJECTORY_NOOP); |
| 120 | + } |
| 121 | + return control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN; |
| 122 | + } |
| 123 | +}; |
| 124 | + |
| 125 | +// Clean end-to-end stream: STREAM_START, N spline points at a held pose, |
| 126 | +// STREAM_END(N). Expect TRAJECTORY_RESULT_SUCCESS via the end callback. |
| 127 | +// All points sit at the same pose so the test exercises the streaming |
| 128 | +// protocol without exercising motion dynamics. |
| 129 | +TEST_F(TrajectoryStreamingTest, stream_end_yields_success) |
| 130 | +{ |
| 131 | + const vector6d_t held_pose = { 0.0, -1.57, 0.0, -1.57, 0.0, 0.0 }; |
| 132 | + const vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; |
| 133 | + |
| 134 | + // Position the robot at held_pose with a one-shot finite trajectory. |
| 135 | + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( |
| 136 | + control::TrajectoryControlMessage::TRAJECTORY_START, 1)); |
| 137 | + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(held_pose, zero, zero, 2.0f)); |
| 138 | + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, |
| 139 | + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); |
| 140 | + resetTrajectoryResultState(); |
| 141 | + |
| 142 | + // Stream: STREAM_START, N points, STREAM_END(N). |
| 143 | + const int k_num_points = 50; |
| 144 | + const float k_step_time = 0.01f; |
| 145 | + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( |
| 146 | + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); |
| 147 | + for (int i = 0; i < k_num_points; ++i) |
| 148 | + { |
| 149 | + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(held_pose, zero, zero, k_step_time)); |
| 150 | + } |
| 151 | + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( |
| 152 | + control::TrajectoryControlMessage::TRAJECTORY_STREAM_END, k_num_points)); |
| 153 | + |
| 154 | + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, |
| 155 | + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); |
| 156 | +} |
| 157 | + |
| 158 | +// STREAM_START with no spline points and no STREAM_END. The trajectoryThread's |
| 159 | +// first socket_read on the trajectory socket times out (0.5s before any motion), |
| 160 | +// trips the underrun else-branch with both was_streaming_at_start and |
| 161 | +// trajectory_streaming still True, and emits TRAJECTORY_RESULT_FAILURE. |
| 162 | +TEST_F(TrajectoryStreamingTest, stream_underrun_yields_failure) |
| 163 | +{ |
| 164 | + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( |
| 165 | + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); |
| 166 | + |
| 167 | + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE, |
| 168 | + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(3))); |
| 169 | +} |
| 170 | + |
| 171 | +// STREAM_START followed by points, then TRAJECTORY_CANCEL before STREAM_END. |
| 172 | +// The existing cancel pathway tears down the trajectory thread, drains |
| 173 | +// remaining points via clearTrajectoryPointsThread (whose loop terminates |
| 174 | +// when the trajectory socket goes silent on first timeout), and emits |
| 175 | +// TRAJECTORY_RESULT_CANCELED. |
| 176 | +TEST_F(TrajectoryStreamingTest, stream_cancel_yields_canceled) |
| 177 | +{ |
| 178 | + const vector6d_t held_pose = { 0.0, -1.57, 0.0, -1.57, 0.0, 0.0 }; |
| 179 | + const vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; |
| 180 | + |
| 181 | + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( |
| 182 | + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); |
| 183 | + // Pile a batch of points into the OS socket buffer so the cleanup path |
| 184 | + // has something to drain. |
| 185 | + for (int i = 0; i < 100; ++i) |
| 186 | + { |
| 187 | + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(held_pose, zero, zero, 0.01f)); |
| 188 | + } |
| 189 | + // Give the trajectoryThread a moment to consume a handful of points so |
| 190 | + // the cancel hits mid-stream rather than racing the spawn. |
| 191 | + std::this_thread::sleep_for(std::chrono::milliseconds(50)); |
| 192 | + |
| 193 | + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( |
| 194 | + control::TrajectoryControlMessage::TRAJECTORY_CANCEL)); |
| 195 | + |
| 196 | + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, |
| 197 | + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); |
| 198 | +} |
| 199 | + |
| 200 | +int main(int argc, char* argv[]) |
| 201 | +{ |
| 202 | + ::testing::InitGoogleTest(&argc, argv); |
| 203 | + |
| 204 | + for (int i = 0; i < argc; i++) |
| 205 | + { |
| 206 | + if (std::string(argv[i]) == "--robot_ip" && i + 1 < argc) |
| 207 | + { |
| 208 | + g_ROBOT_IP = argv[i + 1]; |
| 209 | + ++i; |
| 210 | + } |
| 211 | + if (std::string(argv[i]) == "--headless" && i + 1 < argc) |
| 212 | + { |
| 213 | + std::string headless = argv[i + 1]; |
| 214 | + g_HEADLESS = headless == "true" || headless == "1" || headless == "True" || headless == "TRUE"; |
| 215 | + ++i; |
| 216 | + } |
| 217 | + } |
| 218 | + |
| 219 | + return RUN_ALL_TESTS(); |
| 220 | +} |
0 commit comments