diff --git a/doc/architecture/reverse_interface.rst b/doc/architecture/reverse_interface.rst index 05514afe0..25f320e04 100644 --- a/doc/architecture/reverse_interface.rst +++ b/doc/architecture/reverse_interface.rst @@ -49,8 +49,8 @@ meaning: - joint velocities (SPEEDJ) - trajectory instructions (FORWARD) - - field 1: Trajectory control mode(1: TRAJECTORY_MODE_RECEIVE, -1: TRAJECTORY_MODE_CANCEL) - - field 2: Number of trajectory points left to transfer + - field 1: Trajectory control mode (1: TRAJECTORY_MODE_RECEIVE, 2: TRAJECTORY_MODE_STREAM_START, 3: TRAJECTORY_MODE_STREAM_END, -1: TRAJECTORY_MODE_CANCEL). See :ref:`streaming_trajectories` for the streaming modes. + - field 2: Trajectory point count. Its interpretation depends on the control mode in field 1. - Cartesian velocities (SPEEDL) - Cartesian pose (POSE) @@ -88,6 +88,63 @@ meaning: Depending on the control mode one can use the ``write()`` (SERVOJ, SPEEDJ, SPEEDL, POSE, TORQUE), ``writeTrajectoryControlMessage()`` (FORWARD) or ``writeFreedriveControlMessage()`` (FREEDRIVE) function to write a message to the "reverse_socket". +.. _streaming_trajectories: + +Streaming trajectories +~~~~~~~~~~~~~~~~~~~~~~~~ + +A ``FORWARD``-mode trajectory can be executed in one of two ways. + +A *finite* trajectory declares its total length up front: +``writeTrajectoryControlMessage(TRAJECTORY_START, n)`` promises exactly ``n`` points, and execution +completes once they have been consumed. This is the mode used by +:ref:`trajecotry_joint_interface_example`. + +A *streaming* trajectory is open-ended. The producer does not commit to a point count in advance; it +opens the stream, writes points for as long as it likes, and signals end-of-stream explicitly. This +suits points produced on the fly -- from a planner or a teleoperation source -- where the total length +is unknown when motion begins. The message sequence is:: + + writeTrajectoryControlMessage(TRAJECTORY_STREAM_START) // open the stream + // write a motion primitive, any number of times: + // writeTrajectoryPoint(...) / writeTrajectorySplinePoint(...) / writeMotionPrimitive(...) + writeTrajectoryControlMessage(TRAJECTORY_STREAM_END, n) // close the stream + +``TRAJECTORY_STREAM_START`` opens the trajectory; its point-count argument is unused. The producer then +writes motion primitives -- via ``writeTrajectoryPoint()``, ``writeTrajectorySplinePoint()`` or +``writeMotionPrimitive()`` -- for as long as it likes. ``TRAJECTORY_STREAM_END`` closes it. + +.. important:: + The ``n`` passed to ``TRAJECTORY_STREAM_END`` must equal the total number of motion primitives the + producer wrote since ``TRAJECTORY_STREAM_START``. The controller consumes primitives asynchronously + as it executes them and cannot otherwise tell how many are still outstanding; it uses ``n`` to + account for those still in flight before completing. Aborting a stream with ``TRAJECTORY_CANCEL`` + requires the same count, for the same reason. (For a finite ``TRAJECTORY_START`` trajectory the + cancel argument is unused.) + +Two obligations fall on the producer: + +- **Do not starve the controller.** The robot consumes the stream as it executes it, so the producer + must keep work available: the next primitive must arrive before the robot finishes the ones it + already holds. The budget for delivering it is simply the duration of the work in hand -- a primitive + that runs for five minutes gives five minutes to produce its successor, while a stream of + eight-millisecond primitives demands one every eight milliseconds. If the robot runs out of submitted + work before the next primitive arrives and before ``TRAJECTORY_STREAM_END`` is sent, the trajectory + ends with ``TRAJECTORY_RESULT_FAILURE``. Delivering primitives as they are produced, without holding + them back, is the natural way to satisfy this. + +- **End at rest.** Streaming adds no wind-down deceleration of its own. Make the final point a + controlled stop, with zero velocity and acceleration, so the spline interpolates smoothly to rest at + the goal. The trajectory thread issues ``stopj`` on exit regardless, so a non-zero terminal point + still stops the robot, but the transition will be less smooth. + +On completion the callback registered with ``setTrajectoryEndCallback()`` reports +``TRAJECTORY_RESULT_SUCCESS`` for a clean end (``TRAJECTORY_STREAM_END`` processed, all points +consumed), ``TRAJECTORY_RESULT_FAILURE`` for a producer underrun, or ``TRAJECTORY_RESULT_CANCELED`` if +the stream was aborted with ``TRAJECTORY_CANCEL``. + +See :ref:`trajectory_streaming_example` for a complete, working example. + .. _direct_torque_control_mode: Direct torque control mode diff --git a/doc/examples.rst b/doc/examples.rst index fcfa352a4..fae5a85e9 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -31,4 +31,5 @@ may be running forever until manually stopped. examples/tool_contact_example examples/direct_torque_control examples/trajectory_point_interface + examples/trajectory_streaming examples/ur_driver diff --git a/doc/examples/trajectory_streaming.rst b/doc/examples/trajectory_streaming.rst new file mode 100644 index 000000000..41a51a4dd --- /dev/null +++ b/doc/examples/trajectory_streaming.rst @@ -0,0 +1,121 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/trajectory_streaming.rst + +.. _trajectory_streaming_example: + +Trajectory streaming example +============================ + +This example demonstrates open-ended *trajectory streaming*: a producer sends points to the +controller without declaring a total point count up front, then signals end-of-stream when it is +finished. Contrast this with the :ref:`trajecotry_joint_interface_example`, which uses the finite +trajectory path where the point count is fixed in the initial ``TRAJECTORY_START`` message. + +The streaming communication contract -- the ``STREAM_START`` / ``STREAM_END`` handshake, the meaning +of the end-of-stream point count, the producer's obligation not to starve the controller, and the +trajectory result codes -- is described in :ref:`streaming_trajectories`. This page walks through the +example code and refers back to that section for the authoritative contract. + +Setup +----- + +As with the other examples, we create a ``UrDriver`` (here through ``ExampleRobotWrapper``) and +register a callback that fires when a trajectory finishes: + +.. literalinclude:: ../../examples/trajectory_streaming.cpp + :language: c++ + :caption: examples/trajectory_streaming.cpp + :linenos: + :lineno-match: + :start-at: const bool headless_mode = true; + :end-before: // --------------- PRE-POSITION VIA FINITE TRAJECTORY + +Trajectory execution is asynchronous, so completion is reported through a callback. The result +distinguishes a successful run from a cancellation or a failure: + +.. literalinclude:: ../../examples/trajectory_streaming.cpp + :language: c++ + :caption: examples/trajectory_streaming.cpp + :linenos: + :lineno-match: + :start-at: void trajDoneCallback + :end-at: } + +While a trajectory runs, the control PC must keep telling the robot program that the connection is +still alive. We do this by pumping ``TRAJECTORY_NOOP`` messages while we wait; without them the robot +program stops waiting for input and the ``external_control`` program exits: + +.. literalinclude:: ../../examples/trajectory_streaming.cpp + :language: c++ + :caption: examples/trajectory_streaming.cpp + :linenos: + :lineno-match: + :start-at: // Pump TRAJECTORY_NOOP + :end-before: // Sample a quintic-Hermite + +Generating a motion +------------------- + +To have something to stream, we sample a quintic-Hermite blend between two joint configurations. The +blend has zero velocity and acceleration at both endpoints, which -- as the streaming contract +recommends -- gives the final point the controlled-stop property needed to bring the robot cleanly to +rest: + +.. literalinclude:: ../../examples/trajectory_streaming.cpp + :language: c++ + :caption: examples/trajectory_streaming.cpp + :linenos: + :lineno-match: + :start-at: // Sample a quintic-Hermite + :end-before: int main( + +Pre-positioning +--------------- + +Before streaming, we park the robot at a known start pose using an ordinary finite trajectory. This +doubles as a demonstration of the ``TRAJECTORY_START`` path for comparison: + +.. literalinclude:: ../../examples/trajectory_streaming.cpp + :language: c++ + :caption: examples/trajectory_streaming.cpp + :linenos: + :lineno-match: + :start-after: // --------------- PRE-POSITION VIA FINITE TRAJECTORY + :end-before: // ----------------- STREAMING TRAJECTORY DEMO + +Streaming the trajectory +------------------------ + +We precompute all of the points up front. The final point inherits the ``qd = 0``, ``qdd = 0`` +boundary condition of the quintic blend, so it is a compliant controlled-stop terminal: + +.. literalinclude:: ../../examples/trajectory_streaming.cpp + :language: c++ + :caption: examples/trajectory_streaming.cpp + :linenos: + :lineno-match: + :start-after: // ----------------- STREAMING TRAJECTORY DEMO + :end-before: URCL_LOG_INFO("Streaming %d points + +The stream itself is the ``STREAM_START`` -> points -> ``STREAM_END`` handshake. We open the stream, +write every point back-to-back, then close it, passing the total number of points written so the +controller knows how many are still outstanding before it reports completion: + +.. literalinclude:: ../../examples/trajectory_streaming.cpp + :language: c++ + :caption: examples/trajectory_streaming.cpp + :linenos: + :lineno-match: + :start-at: URCL_LOG_INFO("Streaming %d points + :end-before: g_my_robot->getUrDriver()->stopControl(); + +Because this example delivers the whole motion up front, it can never starve the controller: all the +work is in hand before execution finishes, so the robot simply plays through it. The example also +measures the time from sending ``STREAM_END`` to the trajectory-done callback. That time is large +here, but not because ending a stream is costly: the producer sends every point and then +``STREAM_END`` within milliseconds, long before the robot has finished moving, so the callback cannot +fire until the robot has worked through the entire backlog. A producer that instead fed points in step +with the robot's motion would send ``STREAM_END`` just as the robot reached the final point, and the +callback would follow almost immediately. + +For the general case, where points are produced on the fly and pacing does matter, see the producer +obligations in :ref:`streaming_trajectories`. diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b273c2998..10007e75f 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -59,6 +59,10 @@ add_executable(trajectory_point_interface_example trajectory_point_interface.cpp) target_link_libraries(trajectory_point_interface_example ur_client_library::urcl) +add_executable(trajectory_streaming_example +trajectory_streaming.cpp) +target_link_libraries(trajectory_streaming_example ur_client_library::urcl) + add_executable(instruction_executor instruction_executor.cpp) target_link_libraries(instruction_executor ur_client_library::urcl) diff --git a/examples/trajectory_streaming.cpp b/examples/trajectory_streaming.cpp new file mode 100644 index 000000000..730cf38f2 --- /dev/null +++ b/examples/trajectory_streaming.cpp @@ -0,0 +1,231 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +// ---------------------------------------------------------------------------- +// Trajectory streaming example +// +// Demonstrates the open-ended (streaming) trajectory feature: a producer +// streams points to the controller without declaring a total point count up +// front, then signals end-of-stream when finished. Contrast with +// examples/trajectory_point_interface.cpp, which uses the finite-trajectory +// path where the total point count is fixed in the initial TRAJECTORY_START +// message. +// +// The streaming communication contract -- the STREAM_START / STREAM_END +// handshake, the end-of-stream point count, the producer's obligation not to +// starve the controller, the controlled-stop terminal point, and the +// trajectory result codes -- is documented in +// doc/architecture/reverse_interface.rst. A narrative walkthrough of this +// example lives in doc/examples/trajectory_streaming.rst. +// ---------------------------------------------------------------------------- + +#include +#include +#include +#include +#include +#include + +#include +#include "ur_client_library/control/trajectory_point_interface.h" +#include "ur_client_library/log.h" +#include "ur_client_library/types.h" + +const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; +const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; +const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; + +std::unique_ptr g_my_robot; +std::atomic g_trajectory_done{ false }; +std::atomic g_trajectory_result{ + urcl::control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN +}; + +void trajDoneCallback(const urcl::control::TrajectoryResult& result) +{ + g_trajectory_result = result; + g_trajectory_done = true; + URCL_LOG_INFO("Trajectory done with result %s", urcl::control::trajectoryResultToString(result).c_str()); +} + +// Pump TRAJECTORY_NOOP on the reverse socket while waiting for the +// trajectory-done callback. Without this the URScript dispatcher's +// reverse_socket read times out, which terminates the external_control +// program entirely. +void waitForTrajectoryDoneWithKeepalives() +{ + while (!g_trajectory_done) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + } +} + +// Sample a quintic-Hermite interpolation between q_start and q_end with zero +// boundary velocity and acceleration, evaluated at fractional time s in [0, 1] +// for a segment of total duration t_total: +// +// q(s) = q_start + (q_end - q_start) * (10s^3 - 15s^4 + 6s^5) +// qd(t) = (q_end - q_start) * (30s^2 - 60s^3 + 30s^4) / t_total +// qdd(t) = (q_end - q_start) * (60s - 180s^2 + 120s^3) / t_total^2 +struct SampledPoint +{ + urcl::vector6d_t q; + urcl::vector6d_t qd; + urcl::vector6d_t qdd; +}; + +SampledPoint sampleQuinticHermite(const urcl::vector6d_t& q_start, const urcl::vector6d_t& q_end, const double s, + const double t_total) +{ + const double s2 = s * s; + const double s3 = s2 * s; + const double s4 = s3 * s; + const double s5 = s4 * s; + const double pos_blend = 10.0 * s3 - 15.0 * s4 + 6.0 * s5; + const double vel_blend = (30.0 * s2 - 60.0 * s3 + 30.0 * s4) / t_total; + const double acc_blend = (60.0 * s - 180.0 * s2 + 120.0 * s3) / (t_total * t_total); + + SampledPoint out{}; + for (size_t i = 0; i < q_start.size(); ++i) + { + const double delta = q_end[i] - q_start[i]; + out.q[i] = q_start[i] + delta * pos_blend; + out.qd[i] = delta * vel_blend; + out.qdd[i] = delta * acc_blend; + } + return out; +} + +int main(int argc, char* argv[]) +{ + urcl::setLogLevel(urcl::LogLevel::INFO); + + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = argv[1]; + } + + const bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->isHealthy()) + { + URCL_LOG_ERROR("Robot initialization failed. See preceding output for details."); + return 1; + } + + g_my_robot->getUrDriver()->registerTrajectoryDoneCallback(&trajDoneCallback); + + // --------------- PRE-POSITION VIA FINITE TRAJECTORY ---------------- + // Park the robot at a known starting pose before the streaming demo. This + // also exercises the finite TRAJECTORY_START path for comparison. + + const urcl::vector6d_t pose_a = { -1.57, -1.57, 0.0, -1.57, 0.0, 0.0 }; + const urcl::vector6d_t pose_b = { -1.57, -1.20, 0.5, -1.57, 0.0, 0.0 }; + const urcl::vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + URCL_LOG_INFO("Pre-positioning to pose A via finite trajectory"); + g_trajectory_done = false; + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + 1); + g_my_robot->getUrDriver()->writeTrajectorySplinePoint(pose_a, zero, zero, 3.0f); + waitForTrajectoryDoneWithKeepalives(); + if (g_trajectory_result != urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) + { + URCL_LOG_ERROR("Pre-positioning failed"); + return 1; + } + + // ----------------- STREAMING TRAJECTORY DEMO ----------------------------- + // Stream a quintic-Hermite motion from pose A to pose B. All points are + // precomputed and written back-to-back; the robot works through them at its + // own pace. See doc/examples/trajectory_streaming.rst for details. + + const int k_num_points = 200; + const double k_motion_duration_s = 2.0; + const double dt = k_motion_duration_s / k_num_points; + + std::vector points; + points.reserve(k_num_points); + for (int i = 1; i <= k_num_points; ++i) + { + const double s = static_cast(i) / k_num_points; + points.push_back(sampleQuinticHermite(pose_a, pose_b, s, k_motion_duration_s)); + } + // The final quintic-Hermite point has qd=0, qdd=0: the controlled-stop + // terminal the streaming contract requires. + + URCL_LOG_INFO("Streaming %d points over %.2f s nominal motion duration", k_num_points, k_motion_duration_s); + g_trajectory_done = false; + g_trajectory_result = urcl::control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN; + + const auto stream_start_wall = std::chrono::steady_clock::now(); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_STREAM_START); + for (const auto& p : points) + { + if (!g_my_robot->getUrDriver()->writeTrajectorySplinePoint(p.q, p.qd, p.qdd, static_cast(dt))) + { + URCL_LOG_ERROR("writeTrajectorySplinePoint failed mid-stream"); + return 1; + } + } + const auto stream_end_send_wall = std::chrono::steady_clock::now(); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + urcl::control::TrajectoryControlMessage::TRAJECTORY_STREAM_END, k_num_points); + + waitForTrajectoryDoneWithKeepalives(); + const auto callback_wall = std::chrono::steady_clock::now(); + + const auto send_duration = + std::chrono::duration_cast(stream_end_send_wall - stream_start_wall); + const auto end_to_callback = + std::chrono::duration_cast(callback_wall - stream_end_send_wall); + URCL_LOG_INFO("All %d points written in %lld ms wall-clock", k_num_points, + static_cast(send_duration.count())); + // Time from STREAM_END to the done callback. Here it is dominated by the + // consumer draining the batched backlog, not by STREAM_END processing; a + // steady-state producer would see roughly one controller step plus the + // stopj at trajectory thread exit. See doc/examples/trajectory_streaming.rst. + URCL_LOG_INFO("Time from sending STREAM_END to trajectory-done callback: %lld ms", + static_cast(end_to_callback.count())); + URCL_LOG_INFO("Final result: %s", urcl::control::trajectoryResultToString(g_trajectory_result).c_str()); + + if (g_trajectory_result != urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) + { + URCL_LOG_ERROR("Streaming trajectory did not complete successfully"); + return 1; + } + + g_my_robot->getUrDriver()->stopControl(); + return 0; +} diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h index 18b7b4799..c31c877ac 100644 --- a/include/ur_client_library/control/reverse_interface.h +++ b/include/ur_client_library/control/reverse_interface.h @@ -48,9 +48,13 @@ namespace control */ enum class TrajectoryControlMessage : int32_t { - TRAJECTORY_CANCEL = -1, ///< Represents command to cancel currently active trajectory. - TRAJECTORY_NOOP = 0, ///< Represents no new control command. - TRAJECTORY_START = 1, ///< Represents command to start a new trajectory. + TRAJECTORY_CANCEL = -1, ///< Represents command to cancel currently active trajectory. + TRAJECTORY_NOOP = 0, ///< Represents no new control command. + TRAJECTORY_START = 1, ///< Represents command to start a new finite trajectory of declared length. + TRAJECTORY_STREAM_START = 2, ///< Represents command to start an open-ended streaming trajectory. + TRAJECTORY_STREAM_END = 3, ///< Represents command to terminate an open-ended streaming trajectory. + ///< The point_number argument must equal the total number of motion primitives + ///< the caller wrote on the trajectory socket since TRAJECTORY_STREAM_START. }; /*! @@ -120,15 +124,40 @@ class ReverseInterface const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(20)); /*! - * \brief Writes needed information to the robot to be read by the URScript program. + * \brief Sends a trajectory-control message to the URScript program over the reverse socket. * - * \param trajectory_action 1 if a trajectory is to be started, -1 if it should be stopped - * \param point_number The number of points of the trajectory to be executed - * \param robot_receive_timeout The read timeout configuration for the reverse socket running in the external - * control script on the robot. If you want to make the read function blocking then use RobotReceiveTimeout::off() - * function to create the RobotReceiveTimeout object + * \param trajectory_action One of the values of TrajectoryControlMessage. The value selects which + * trajectory-control action the URScript dispatcher takes, and dictates how \p point_number is + * interpreted: + * - TRAJECTORY_CANCEL (-1): Cancels the currently executing trajectory. If the trajectory was started + * with TRAJECTORY_START then \p point_number is unused. For streaming trajectories, \p point_number + * must equal the total number of motion primitives the producer wrote on the trajectory socket + * since TRAJECTORY_STREAM_START. + * - TRAJECTORY_NOOP (0): No-op; serves as a keepalive on the reverse socket while a trajectory + * executes. The producer must send these (or another trajectory-control message) periodically + * while waiting for trajectory completion so that the URScript dispatcher's read does not + * time out and exit the external_control program. \p point_number is unused. + * - TRAJECTORY_START (1): Begins a finite trajectory of declared length. \p point_number is the + * total number of subsequent writeTrajectoryPoint / writeTrajectorySplinePoint / writeMotionPrimitive + * calls the producer will issue on the trajectory socket. URScript reads exactly that many points + * and then completes. + * - TRAJECTORY_STREAM_START (2): Begins an open-ended (streaming) trajectory. The producer streams + * motion primitives and signals end-of-stream with TRAJECTORY_STREAM_END. \p point_number is unused. + * - TRAJECTORY_STREAM_END (3): Ends an open-ended streaming trajectory started by + * TRAJECTORY_STREAM_START. \p point_number must equal the total number of motion primitives the + * producer wrote on the trajectory socket since TRAJECTORY_STREAM_START. * - * \returns True, if the write was performed successfully, false otherwise. + * \param point_number Mode-dependent point-count argument. See the description of \p + * trajectory_action for the per-mode semantics. + * + * \param robot_receive_timeout The read timeout configuration for the reverse socket running in the + * external control script on the robot. If you want to make the read function blocking then use + * RobotReceiveTimeout::off() function to create the RobotReceiveTimeout object. + * + * \returns True if the write was performed successfully, false otherwise. + * + * \see examples/trajectory_point_interface.cpp for a finite-trajectory usage example, or + * examples/trajectory_streaming.cpp for an open-ended streaming trajectory example. */ bool writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, const int point_number = 0, diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 9ad65933c..2c5da0655 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -585,13 +585,23 @@ class UrDriver /*! * \brief Writes a control message in trajectory forward mode. * - * \param trajectory_action The action to be taken, such as starting a new trajectory - * \param point_number The number of points of a new trajectory to be sent - * \param robot_receive_timeout The read timeout configuration for the reverse socket running in the external - * control script on the robot. If you want to make the read function blocking then use RobotReceiveTimeout::off() - * function to create the RobotReceiveTimeout object + * Selects which trajectory-control action the URScript dispatcher takes (start a finite + * trajectory, begin or end an open-ended streaming trajectory, cancel, or no-op keepalive) + * and, where applicable, communicates a point count. See + * control::ReverseInterface::writeTrajectoryControlMessage for the per-mode semantics of + * \p trajectory_action and \p point_number. + * + * \param trajectory_action The trajectory-control action to take. + * \param point_number Mode-dependent point-count argument; see the underlying interface. + * \param robot_receive_timeout The read timeout configuration for the reverse socket running in + * the external control script on the robot. If you want to make the read function blocking + * then use RobotReceiveTimeout::off() function to create the RobotReceiveTimeout object. * * \returns True on successful write. + * + * \see control::ReverseInterface::writeTrajectoryControlMessage + * \see examples/trajectory_point_interface.cpp (finite trajectory) + * \see examples/trajectory_streaming.cpp (open-ended streaming trajectory) */ bool writeTrajectoryControlMessage(const control::TrajectoryControlMessage trajectory_action, const int point_number = 0, diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 3986f20f2..3423bbe52 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -33,6 +33,16 @@ REVERSE_INTERFACE_DATA_DIMENSION = 8 TRAJECTORY_MODE_RECEIVE = 1 TRAJECTORY_MODE_CANCEL = -1 +TRAJECTORY_MODE_STREAM_START = 2 +TRAJECTORY_MODE_STREAM_END = 3 + +# Sentinel value used for trajectory_points_left while an open-ended +# streaming trajectory is in progress. The trajectoryThread predicate +# "trajectory_points_left > 0" stays as written; this large starting +# value keeps the loop running until either STREAM_END converts the +# remaining unread points to a finite count, or the producer underruns. +# 2^31 - 1 is exactly representable in URScript (double precision). +STREAMING_SENTINEL = 2147483647 MOTION_TYPE_MOVEJ = 0 MOTION_TYPE_MOVEL = 1 @@ -102,6 +112,7 @@ global extrapolate_count = 0 global extrapolate_max_count = 0 global control_mode = MODE_UNINITIALIZED global trajectory_points_left = 0 +global trajectory_streaming = False global spline_qdd = [0, 0, 0, 0, 0, 0] global spline_qd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False @@ -538,6 +549,10 @@ thread trajectoryThread(): textmsg("Executing trajectory. Number of points: ", trajectory_points_left) local is_first_point = True local is_robot_moving = False + # Snapshot streaming-ness at thread spawn so the underrun else-branch + # can distinguish a clean post-STREAM_END drain (flag has since been + # cleared) from a mid-stream underrun (flag still set). + local was_streaming_at_start = trajectory_streaming local INDEX_TIME = TRAJECTORY_DATA_DIMENSION local INDEX_BLEND = INDEX_TIME + 1 # same index as blend parameter, depending on point type @@ -754,8 +769,21 @@ thread trajectoryThread(): spline_qd = [0, 0, 0, 0, 0, 0] end else: - textmsg("Receiving trajectory point failed!") - trajectory_result = TRAJECTORY_RESULT_FAILURE + if was_streaming_at_start and not trajectory_streaming and trajectory_points_left < 0: + # Race: STREAM_END was processed after the consumer had already + # drained every streamed point. The dispatcher's math zeroed + # trajectory_points_left, then this iteration's decrement pushed + # it below zero. No further points are expected; exit cleanly. + # Note: trajectory_points_left >= 0 here means the consumer still + # expected unread points - that is a genuine underrun (either + # producer/STREAM_END count mismatch or a delayed STREAM_END + # arrived during a real underrun) and must be reported as + # FAILURE rather than silently truncated. + trajectory_points_left = 0 + else: + textmsg("Receiving trajectory point failed!") + trajectory_result = TRAJECTORY_RESULT_FAILURE + end end is_first_point = False end @@ -939,11 +967,11 @@ thread script_commands(): friction_compensation_mode = FRICTION_COMP_MODE_FRICTION_SCALES viscous_scale = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] coulomb_scale = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate] - elif command == SET_TARGET_PAYLOAD: - mass = raw_command[2] / MULT_jointstate - cog = [raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate] + elif command == SET_TARGET_PAYLOAD: + mass = raw_command[2] / MULT_jointstate + cog = [raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate] inertia = [raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate, raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate] - transition_time = raw_command[12] / MULT_time + transition_time = raw_command[12] / MULT_time {% if ROBOT_SOFTWARE_VERSION >= v5.10.0 %} set_target_payload(mass, cog, inertia, transition_time) {% else %} @@ -1135,11 +1163,46 @@ while control_mode > MODE_STOPPED: kill thread_trajectory request_trajectory_cleanup() wait_for_trajectory_cleanup() + # This is a legacy finite trajectory; ensure trajectory_streaming + # does not leak in from any prior streaming session. + trajectory_streaming = False trajectory_points_left = params_mult[3] thread_trajectory = run trajectoryThread() + elif params_mult[2] == TRAJECTORY_MODE_STREAM_START: + kill thread_trajectory + request_trajectory_cleanup() + wait_for_trajectory_cleanup() + trajectory_streaming = True + trajectory_points_left = STREAMING_SENTINEL + thread_trajectory = run trajectoryThread() + elif params_mult[2] == TRAJECTORY_MODE_STREAM_END: + # Guard against a stray STREAM_END applied outside an active + # streaming session (during a finite trajectory, after a cancel, + # or as a second STREAM_END after the first). The sentinel-based + # math below is only meaningful while trajectory_points_left is + # known to be derived from STREAMING_SENTINEL; outside of that, + # applying it would corrupt trajectory_points_left. + if trajectory_streaming: + # params_mult[3] is the total count of spline points the producer + # wrote to the trajectory socket since STREAM_START. The + # trajectoryThread has decremented trajectory_points_left once per + # consumed point starting from STREAMING_SENTINEL, so the consumed + # count is (STREAMING_SENTINEL - trajectory_points_left). The + # unread remainder still in the OS socket buffer is therefore + # the producer's total minus the consumer's consumed count. + trajectory_points_left = params_mult[3] - (STREAMING_SENTINEL - trajectory_points_left) + trajectory_streaming = False + end elif params_mult[2] == TRAJECTORY_MODE_CANCEL: textmsg("cancel received") kill thread_trajectory + # The streaming session, if any, has been forcibly ended; clear + # the flag so a subsequent stray STREAM_END is a no-op. + if trajectory_streaming: + # In streaming mode, a cancel should come with the total count, just like a STREAM_END. + trajectory_points_left = params_mult[3] - (STREAMING_SENTINEL - trajectory_points_left) + trajectory_streaming = False + end request_trajectory_cleanup() stopj(STOPJ_ACCELERATION) wait_for_trajectory_cleanup() diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 5582edf8b..51f2502aa 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -114,6 +114,16 @@ if (INTEGRATION_TESTS) TEST_SUFFIX _headless ) + # Streaming trajectory tests. Headless-only; the URCap path does not + # exercise anything different at the URScript level for streaming. + add_executable(trajectory_streaming_tests_headless test_trajectory_streaming.cpp) + target_link_libraries(trajectory_streaming_tests_headless PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET trajectory_streaming_tests_headless + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless true ${INTEGRATION_TESTS_ROBOT_IP_ARG} + TEST_SUFFIX _headless + ) + # InstructionExecutor tests add_executable(instruction_executor_test_urcap test_instruction_executor.cpp) target_link_libraries(instruction_executor_test_urcap PRIVATE ur_client_library::urcl GTest::gtest_main) diff --git a/tests/test_trajectory_streaming.cpp b/tests/test_trajectory_streaming.cpp new file mode 100644 index 000000000..35dc136db --- /dev/null +++ b/tests/test_trajectory_streaming.cpp @@ -0,0 +1,406 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "ur_client_library/control/trajectory_point_interface.h" +#include "ur_client_library/example_robot_wrapper.h" +#include "ur_client_library/log.h" +#include "ur_client_library/types.h" + +using namespace urcl; + +const std::string SCRIPT_FILE = "../resources/external_control.urscript"; +const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe.txt"; +const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt"; +std::string g_ROBOT_IP = "192.168.56.101"; +bool g_HEADLESS = true; + +std::unique_ptr g_my_robot; + +std::condition_variable g_trajectory_result_cv; +std::mutex g_trajectory_result_mutex; +control::TrajectoryResult g_trajectory_result = control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN; +bool g_trajectory_result_received = false; + +void handleTrajectoryState(control::TrajectoryResult state) +{ + std::lock_guard lk(g_trajectory_result_mutex); + g_trajectory_result = state; + g_trajectory_result_received = true; + g_trajectory_result_cv.notify_one(); + URCL_LOG_INFO("Received trajectory result %s", control::trajectoryResultToString(state).c_str()); +} + +class TrajectoryStreamingTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + // Streaming tests are headless only. The URCap path doesn't exercise + // anything different at the URScript level for streaming, so there is + // no value in maintaining a separate _urcap variant. + ASSERT_TRUE(g_HEADLESS) << "trajectory streaming tests require headless mode"; + + g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, + "external_control.urp", SCRIPT_FILE); + ASSERT_TRUE(g_my_robot->isHealthy()); + g_my_robot->getUrDriver()->registerTrajectoryDoneCallback(&handleTrajectoryState); + } + + void SetUp() override + { + if (!g_my_robot->isHealthy()) + { + ASSERT_TRUE(g_my_robot->resendRobotProgram()); + ASSERT_TRUE(g_my_robot->waitForProgramRunning(500)); + } + resetTrajectoryResultState(); + } + + static void resetTrajectoryResultState() + { + std::lock_guard lk(g_trajectory_result_mutex); + g_trajectory_result = control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN; + g_trajectory_result_received = false; + } + + // Pump TRAJECTORY_NOOP on the reverse socket so the urscript's main + // dispatcher read does not time out while we wait for the trajectory + // result callback. Returns the captured result, or + // TRAJECTORY_RESULT_UNKNOWN if the deadline elapsed first. + control::TrajectoryResult waitForTrajectoryResultPumpingNoops(std::chrono::milliseconds total_timeout) + { + const auto deadline = std::chrono::steady_clock::now() + total_timeout; + while (std::chrono::steady_clock::now() < deadline) + { + std::unique_lock lk(g_trajectory_result_mutex); + if (g_trajectory_result_cv.wait_for(lk, std::chrono::milliseconds(100), + [] { return g_trajectory_result_received; })) + { + return g_trajectory_result; + } + lk.unlock(); + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + } + return control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN; + } + + // Variant that fires a single user-supplied action exactly once after + // `inject_at` has elapsed since the call started. The action takes the + // place of one NOOP-pump iteration. Used to inject a stray trajectory + // control message mid-flight without spinning up a second thread. + control::TrajectoryResult waitForTrajectoryResultPumpingNoopsWithInjection(std::chrono::milliseconds total_timeout, + std::chrono::milliseconds inject_at, + std::function inject_action) + { + const auto start = std::chrono::steady_clock::now(); + const auto deadline = start + total_timeout; + const auto inject_deadline = start + inject_at; + bool injected = false; + while (std::chrono::steady_clock::now() < deadline) + { + std::unique_lock lk(g_trajectory_result_mutex); + if (g_trajectory_result_cv.wait_for(lk, std::chrono::milliseconds(50), + [] { return g_trajectory_result_received; })) + { + return g_trajectory_result; + } + lk.unlock(); + if (!injected && std::chrono::steady_clock::now() >= inject_deadline) + { + inject_action(); + injected = true; + } + else + { + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP); + } + } + return control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN; + } +}; + +// Clean end-to-end stream: STREAM_START, N spline points at a held pose, +// STREAM_END(N). Expect TRAJECTORY_RESULT_SUCCESS via the end callback. +// All points sit at the same pose so the test exercises the streaming +// protocol without exercising motion dynamics. +TEST_F(TrajectoryStreamingTest, stream_end_yields_success) +{ + const vector6d_t held_pose = { 0.0, -1.57, 0.0, -1.57, 0.0, 0.0 }; + const vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // Position the robot at held_pose with a one-shot finite trajectory. + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(held_pose, zero, zero, 2.0f)); + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + // Stream: STREAM_START, N points, STREAM_END(N). + const int k_num_points = 50; + const float k_step_time = 0.01f; + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); + for (int i = 0; i < k_num_points; ++i) + { + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(held_pose, zero, zero, k_step_time)); + } + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_END, k_num_points)); + + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); +} + +// STREAM_START with no spline points and no STREAM_END. The trajectoryThread's +// first socket_read on the trajectory socket times out (0.5s before any motion), +// trips the underrun else-branch with both was_streaming_at_start and +// trajectory_streaming still True, and emits TRAJECTORY_RESULT_FAILURE. +TEST_F(TrajectoryStreamingTest, stream_underrun_yields_failure) +{ + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); + + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(3))); +} + +// STREAM_START followed by points, then TRAJECTORY_CANCEL before STREAM_END. +// The existing cancel pathway tears down the trajectory thread, drains +// remaining points via clearTrajectoryPointsThread (whose loop terminates +// when the trajectory socket goes silent on first timeout), and emits +// TRAJECTORY_RESULT_CANCELED. +TEST_F(TrajectoryStreamingTest, stream_cancel_yields_canceled) +{ + const vector6d_t held_pose = { 0.0, -1.57, 0.0, -1.57, 0.0, 0.0 }; + const vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); + // Pile a batch of points into the OS socket buffer so the cleanup path + // has something to drain. + for (int i = 0; i < 100; ++i) + { + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(held_pose, zero, zero, 0.01f)); + } + // Give the trajectoryThread a moment to consume a handful of points so + // the cancel hits mid-stream rather than racing the spawn. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_CANCEL, 100)); + + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); +} + +// Regression test for PR #528 cursor-bot review comment 1a. +// A stray TRAJECTORY_STREAM_END dispatched during a legacy finite +// TRAJECTORY_START trajectory must not corrupt trajectory_points_left. +// Without the streaming-guard fix, the dispatcher's STREAM_END math +// (which assumes trajectory_points_left is a sentinel-based credit +// budget) computes a huge negative value, the trajectoryThread's +// "trajectory_points_left > 0" predicate fails on the next iteration, +// the loop exits while trajectory_result is still SUCCESS, and the +// callback fires SUCCESS even though motion was truncated. +TEST_F(TrajectoryStreamingTest, stray_stream_end_during_finite_trajectory_does_not_truncate) +{ + const vector6d_t pose_a = { 0.0, -1.57, 0.0, -1.57, 0.0, 0.0 }; + const vector6d_t pose_b = { 0.0, -1.40, 0.0, -1.57, 0.0, 0.0 }; + const vector6d_t pose_c = { 0.0, -1.20, 0.0, -1.57, 0.0, 0.0 }; + const vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // Pre-position + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(pose_a, zero, zero, 2.0f)); + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + // Send a 3-point finite trajectory, each segment ~1 second. + const float k_segment_time = 1.0f; + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 3)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(pose_b, zero, zero, k_segment_time)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(pose_c, zero, zero, k_segment_time)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(pose_a, zero, zero, k_segment_time)); + + const auto motion_start = std::chrono::steady_clock::now(); + const auto result = + waitForTrajectoryResultPumpingNoopsWithInjection(std::chrono::seconds(6), std::chrono::milliseconds(500), [] { + g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_END, 0); + }); + const auto elapsed = std::chrono::steady_clock::now() - motion_start; + + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, result); + // Three 1-second segments expect ~3 s of motion. Without the fix, the + // stray STREAM_END truncates the loop after segment 1, callback fires + // in ~1 s. + EXPECT_GE(elapsed, std::chrono::milliseconds(2500)) << "Expected ~3 s of motion; stray STREAM_END appears to have " + "truncated the trajectory."; +} + +// Regression test for PR #528 cursor-bot review comment 1c. +// A second TRAJECTORY_STREAM_END dispatched during the drain phase +// re-applies the sentinel-based math to a non-sentinel +// trajectory_points_left value, producing a huge negative value and +// truncating the drain while reporting SUCCESS. Without the streaming- +// guard fix the second STREAM_END is destructive; with the fix it is +// a no-op because trajectory_streaming was cleared by the first. +TEST_F(TrajectoryStreamingTest, double_stream_end_does_not_truncate) +{ + const vector6d_t held_pose = { 0.0, -1.57, 0.0, -1.57, 0.0, 0.0 }; + const vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // Pre-position + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(held_pose, zero, zero, 2.0f)); + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + // Stream 20 points with tmptime=0.1 s each. Nominal consumer-side + // execution is ~2 s. + const int k_num_points = 20; + const float k_step_time = 0.1f; + + const auto motion_start = std::chrono::steady_clock::now(); + + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); + for (int i = 0; i < k_num_points; ++i) + { + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(held_pose, zero, zero, k_step_time)); + } + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_END, k_num_points)); + // Second STREAM_END immediately after the first. Without the guard fix + // this re-applies the sentinel-based math to the already-recomputed + // counter and truncates the drain. + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_END, k_num_points)); + + const auto result = waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5)); + const auto elapsed = std::chrono::steady_clock::now() - motion_start; + + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, result); + EXPECT_GE(elapsed, std::chrono::milliseconds(1500)) << "Expected ~2 s of consumer-side execution; double STREAM_END " + "appears to have truncated the drain."; +} + +// Regression test for PR #528 cursor-bot review comment 2. +// A post-STREAM_END timeout-on-read with trajectory_points_left still +// positive must be reported as TRAJECTORY_RESULT_FAILURE, not silently +// swallowed as SUCCESS. The current clean-end shortcut fires whenever +// trajectory_streaming was just cleared, regardless of whether the +// consumer still expects more points. The fix narrows the shortcut to +// the only legitimate case (count_after_decrement < 0), which can only +// arise via a race where STREAM_END landed during the read block after +// the consumer had already drained the buffer. +TEST_F(TrajectoryStreamingTest, stream_end_with_overcount_yields_failure) +{ + const vector6d_t held_pose = { 0.0, -1.57, 0.0, -1.57, 0.0, 0.0 }; + const vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // Pre-position + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(held_pose, zero, zero, 2.0f)); + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + // Send 50 real points but tell STREAM_END the producer wrote 100. + const int k_actually_sent = 50; + const int k_announced = 100; + const float k_step_time = 0.01f; + + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); + for (int i = 0; i < k_actually_sent; ++i) + { + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(held_pose, zero, zero, k_step_time)); + } + // The STREAM_END must arrive after the trajectoryThread has executed + // its `was_streaming_at_start = trajectory_streaming` assignment with + // the streaming flag still True. Otherwise a race - URScript + // scheduling the dispatcher's next iteration before the new thread's + // first statement runs - causes was_streaming_at_start to be captured + // as False, which sends any underrun straight down the legacy FAILURE + // arm regardless of the bug under test. Pump NOOPs for ~250 ms to + // give the thread plenty of time to start and consume some points + // before STREAM_END is dispatched. + for (int i = 0; i < 5; ++i) + { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + } + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_END, k_announced)); + + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + for (int i = 0; i < argc; i++) + { + if (std::string(argv[i]) == "--robot_ip" && i + 1 < argc) + { + g_ROBOT_IP = argv[i + 1]; + ++i; + } + if (std::string(argv[i]) == "--headless" && i + 1 < argc) + { + std::string headless = argv[i + 1]; + g_HEADLESS = headless == "true" || headless == "1" || headless == "True" || headless == "TRUE"; + ++i; + } + } + + return RUN_ALL_TESTS(); +}