diff --git a/doc/architecture/reverse_interface.rst b/doc/architecture/reverse_interface.rst index 8f048d376..bc28faf11 100644 --- a/doc/architecture/reverse_interface.rst +++ b/doc/architecture/reverse_interface.rst @@ -51,6 +51,7 @@ meaning: - 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. + - field 3: The identifier of the move that is being started, for the two control modes which start one. The robot adopts this identifier and then executes only those trajectory points which carry it, and that is what prevents points left over from a move that has already ended from running as part of the next one. The remaining control modes do not start a move, and the robot ignores the field for them. See :ref:`trajectory_point_interface` for the point record which carries the identifier back. - Cartesian velocities (SPEEDL) - Cartesian pose (POSE) diff --git a/doc/architecture/trajectory_point_interface.rst b/doc/architecture/trajectory_point_interface.rst index f16900d84..12c93094d 100644 --- a/doc/architecture/trajectory_point_interface.rst +++ b/doc/architecture/trajectory_point_interface.rst @@ -33,7 +33,7 @@ Communication protocol ---------------------- The ``TrajectoryPointInterface``'s "trajectory_socket" on the robot is expecting 32 bit integer -representations in 21 datafields. The data fields have the following meaning: +representations in 22 datafields. The data fields have the following meaning: .. table:: trajectory_socket message format :widths: auto @@ -106,6 +106,15 @@ representations in 21 datafields. The data fields have the following meaning: - 12: OPTIMOVEJ_POSE – ``optimovej`` to a pose target - 13: OPTIMOVEL_JOINT – ``optimovel`` to the pose implied by a joint target - 51: SPLINE - Cubic or quintic spline. + 21 the identifier of the move this point belongs to, as a raw integer. The robot executes + only those points whose identifier matches the move it is currently running, and + discards the rest. This is what keeps points that a producer wrote for a move which has + already failed or been cancelled from being executed by whichever move runs next, since + the producer cannot learn of the end of a move instantly and may keep writing for a + short while afterwards. The identifier is announced to the robot in the control message + that begins the move, and is assigned by ``UrDriver``, which increments it once per + move for the life of a connection. A value of 0 belongs to no move and is what the + robot holds before it has been told about any. ===== ===== where diff --git a/doc/migration_notes.rst b/doc/migration_notes.rst index bb271b5f0..ef49cc5b7 100644 --- a/doc/migration_notes.rst +++ b/doc/migration_notes.rst @@ -5,6 +5,23 @@ This document contains notes on the migration of the ur_client_library between m It contains only breaking changes. +Migrating from 2.x.x to 3.x.x +----------------------------- + +- Trajectory point records sent on the "trajectory_socket" have gained a trailing field, and the + trajectory control message sent on the "reverse_socket" now carries a move identifier in a slot + that was previously zero padding. See :ref:`trajectory_point_interface` and + :ref:`reverse_interface` for the two layouts. The library and the ``external_control.urscript`` + that it sends to the robot must be upgraded together, because neither one can read the other's + record layout. This only affects you if you supply your own copy of the script rather than + using the one shipped alongside the library. + +- ``urcl::control::ReverseInterface::writeTrajectoryControlMessage()`` takes a new ``move_id`` + argument, which sits before the existing ``robot_receive_timeout`` argument. Callers that + passed a receive timeout positionally will no longer compile until they are updated. Callers + that go through ``urcl::UrDriver`` need no change, since it assigns move identifiers on their + behalf. + Migrating from 1.x.x to 2.x.x ----------------------------- diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h index c31c877ac..84cc6568e 100644 --- a/include/ur_client_library/control/reverse_interface.h +++ b/include/ur_client_library/control/reverse_interface.h @@ -147,6 +147,13 @@ class ReverseInterface * 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. * + * \param move_id The identifier of the move which TRAJECTORY_START and TRAJECTORY_STREAM_START + * begin. The robot executes only those trajectory points which carry the identifier of the move + * it is currently running, and discards any that were left behind by an earlier one, so this + * must be the same identifier that is given to TrajectoryPointInterface::setMoveId() before that + * move's points are written. The remaining actions do not begin a move, and the robot ignores + * the field for them. A value of 0 belongs to no move. + * * \param point_number Mode-dependent point-count argument. See the description of \p * trajectory_action for the per-mode semantics. * @@ -159,9 +166,9 @@ class ReverseInterface * \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, - const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200)); + bool writeTrajectoryControlMessage( + const TrajectoryControlMessage trajectory_action, const int move_id, const int point_number, + const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200)); /*! * \brief Writes needed information to the robot to be read by the URScript program. diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h index 26de4d89f..26e27b86a 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -29,6 +29,7 @@ #ifndef UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED #define UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED +#include #include #include @@ -63,7 +64,7 @@ std::string trajectoryResultToString(const TrajectoryResult result); class TrajectoryPointInterface : public ReverseInterface { public: - static const int MESSAGE_LENGTH = 21; + static const int MESSAGE_LENGTH = 22; TrajectoryPointInterface() = delete; /*! @@ -139,6 +140,21 @@ class TrajectoryPointInterface : public ReverseInterface void removeTrajectoryEndCallback(const uint32_t callback_id); + /*! + * \brief Sets the identifier that will be written into every subsequent trajectory point. + * + * The robot discards any point whose identifier is not that of the move it is currently + * executing. That is what prevents the points of a move which has failed or been cancelled from + * being executed by the move that follows it. A caller must set this to the identifier that was + * announced in the control message which began the move, and must do so before writing any of + * that move's points. Callers working through UrDriver have no need to call this, because it + * assigns and applies the identifiers itself. + * + * \param move_id The identifier of the move that the following points belong to. A value of 0 + * belongs to no move. + */ + void setMoveId(int move_id); + protected: virtual void connectionCallback(const socket_t filedescriptor) override; @@ -151,6 +167,10 @@ class TrajectoryPointInterface : public ReverseInterface std::list> trajectory_end_callbacks_; uint32_t next_done_callback_id_ = 0; + + // Written into each outgoing point. It stays zero until some move announces an identifier, which + // is also the value the script starts out with, so the two of them agree that no move is running. + std::atomic move_id_ = { 0 }; }; } // namespace control diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 2c5da0655..49b5b18b6 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -28,6 +28,7 @@ #ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED #define UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED +#include #include #include #include @@ -1128,6 +1129,13 @@ class UrDriver std::unique_ptr script_sender_; std::unique_ptr script_reader_; + // Identifies the move that is currently being sent to the robot. The driver is what assigns these + // identifiers, because it is the only object which observes both the boundaries between moves and + // every point written within them. It issues one identifier for each command that begins a move, + // and never reuses one for the life of a connection. A value of 0 belongs to no move, and is what + // the robot holds before it has been told about any. + std::atomic trajectory_move_id_ = { 0 }; + size_t socket_connection_attempts_ = 0; std::chrono::milliseconds socket_reconnection_timeout_ = std::chrono::milliseconds(10000); diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 3423bbe52..59c75c07b 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -61,6 +61,11 @@ MOTION_TYPE_OPTIMOVEL_JOINT = 13 MOTION_TYPE_SPLINE = 51 TRAJECTORY_DATA_DIMENSION = 3 * 6 + 1 +# How many points may be discarded in a row before the discarding thread yields. +# A long run of points left over from an earlier move would otherwise starve the +# other threads, which is why clearTrajectoryPointsThread yields as it drains. +TRAJECTORY_DISCARD_SYNC_LIMIT = 10 + TRAJECTORY_RESULT_SUCCESS = 0 TRAJECTORY_RESULT_CANCELED = 1 TRAJECTORY_RESULT_FAILURE = 2 @@ -113,6 +118,11 @@ global extrapolate_max_count = 0 global control_mode = MODE_UNINITIALIZED global trajectory_points_left = 0 global trajectory_streaming = False +# The identifier of the move being executed, announced by the control message +# that begins it. Every trajectory point carries the identifier of the move it +# was written for, so we can recognize points belonging to a move that has +# already ended and discard them. A value of 0 belongs to no move. +global trajectory_move_id = 0 global spline_qdd = [0, 0, 0, 0, 0, 0] global spline_qd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False @@ -553,11 +563,14 @@ thread trajectoryThread(): # 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 my_move_id = trajectory_move_id + local discarded_points = 0 local INDEX_TIME = TRAJECTORY_DATA_DIMENSION local INDEX_BLEND = INDEX_TIME + 1 # same index as blend parameter, depending on point type local INDEX_SPLINE_TYPE = INDEX_BLEND local INDEX_POINT_TYPE = INDEX_BLEND + 1 + local INDEX_MOVE_ID = INDEX_POINT_TYPE + 1 spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] enter_critical @@ -568,11 +581,28 @@ thread trajectoryThread(): if is_robot_moving: timeout = get_steptime() end - #reading trajectory point + blend radius + type of point (cartesian/joint based) - local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket", timeout) + #reading trajectory point + blend radius + type of point (cartesian/joint based) + move id + local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1+1, "trajectory_socket", timeout) + # Note that we count the point down before knowing whether one arrived, so a + # read which timed out still spends one. Moving this into the success branch + # below would mean revisiting the underrun predicate that was written against + # its placement here, and nothing depends on the count being exact any longer + # now that points are recognized by the move they name. trajectory_points_left = trajectory_points_left - 1 - if raw_point[0] > 0: + if raw_point[0] > 0 and raw_point[INDEX_MOVE_ID] != my_move_id: + # This point belongs to some other move, whose producer had not yet learned + # that it had ended. It is not ours to execute and was never part of our + # total, so we give back the count we spent reading it. An identifier ahead + # of our own would mean points arriving for a move we were never told + # about, which we can do no better with than discard as well. + trajectory_points_left = trajectory_points_left + 1 + discarded_points = discarded_points + 1 + if discarded_points >= TRAJECTORY_DISCARD_SYNC_LIMIT: + sync() + discarded_points = 0 + end + elif raw_point[0] > 0: local q = [raw_point[1] / MULT_jointstate, raw_point[2] / MULT_jointstate, raw_point[3] / MULT_jointstate, raw_point[4] / MULT_jointstate, raw_point[5] / MULT_jointstate, raw_point[6] / MULT_jointstate] local tmptime = raw_point[INDEX_TIME] / MULT_time local blend_radius = raw_point[INDEX_BLEND] / MULT_time @@ -768,6 +798,11 @@ thread trajectoryThread(): spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] end + # Only a point we actually executed can end the first point of the move. + # Sitting after the branches instead, as this once did, a discarded point + # would clear the flag and the first point we do execute would lose its + # first point handling. The failed read branches below all end the loop. + is_first_point = False else: if was_streaming_at_start and not trajectory_streaming and trajectory_points_left < 0: # Race: STREAM_END was processed after the consumer had already @@ -783,9 +818,19 @@ thread trajectoryThread(): else: textmsg("Receiving trajectory point failed!") trajectory_result = TRAJECTORY_RESULT_FAILURE + if trajectory_streaming: + # We underran mid-stream, with no STREAM_END, so trajectory_points_left + # still holds the open ended STREAMING_SENTINEL rather than a real + # count. Left that way, the cleanup before the next trajectory command + # bounds its drain by it, runs unbounded, and swallows that command's + # points. A finite trajectory, or a stream STREAM_END has closed, leaves + # a genuine count that the cleanup needs, so we reset only while a + # stream is still open. + trajectory_streaming = False + trajectory_points_left = 0 + end end end - is_first_point = False end exit_critical stopj(STOPJ_ACCELERATION) @@ -806,7 +851,10 @@ thread clearTrajectoryPointsThread(): {% endif %} while trajectory_points_left > 0: - raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION + 2, "trajectory_socket", timeout) + # We have to read a whole record here, move identifier included. One field + # short would leave the trailing integer of every drained record behind and + # put every later read on this socket out of step with the stream. + raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION + 3, "trajectory_socket", timeout) if raw_point[0] <= 0: trajectory_points_left = 0 textmsg("No more trajectory points in the buffer. Resetting trajectory_points_left to 0.") @@ -1166,6 +1214,9 @@ while control_mode > MODE_STOPPED: # This is a legacy finite trajectory; ensure trajectory_streaming # does not leak in from any prior streaming session. trajectory_streaming = False + # Adopt the identifier this move announced before spawning the thread, so + # that it knows which points are its own from its first read. + trajectory_move_id = params_mult[4] trajectory_points_left = params_mult[3] thread_trajectory = run trajectoryThread() elif params_mult[2] == TRAJECTORY_MODE_STREAM_START: @@ -1173,6 +1224,7 @@ while control_mode > MODE_STOPPED: request_trajectory_cleanup() wait_for_trajectory_cleanup() trajectory_streaming = True + trajectory_move_id = params_mult[4] trajectory_points_left = STREAMING_SENTINEL thread_trajectory = run trajectoryThread() elif params_mult[2] == TRAJECTORY_MODE_STREAM_END: diff --git a/src/control/reverse_interface.cpp b/src/control/reverse_interface.cpp index 0cd8ad928..e527d6a84 100644 --- a/src/control/reverse_interface.cpp +++ b/src/control/reverse_interface.cpp @@ -121,10 +121,13 @@ bool ReverseInterface::write(const vector6d_t* positions, const comm::ControlMod } bool ReverseInterface::writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, - const int point_number, + const int move_id, const int point_number, const RobotReceiveTimeout& robot_receive_timeout) { - const int message_length = 3; + // The move identifier occupies the slot which used to be the first of the padding, so the zero + // fill below has to begin one slot further along than it did. Otherwise it would write over the + // identifier we just placed there. + const int message_length = 4; if (client_fd_ == INVALID_SOCKET) { return false; @@ -153,6 +156,9 @@ bool ReverseInterface::writeTrajectoryControlMessage(const TrajectoryControlMess val = htobe32(point_number); b_pos += append(b_pos, val); + val = htobe32(move_id); + b_pos += append(b_pos, val); + // writing zeros to allow usage with other script commands for (size_t i = message_length; i < MAX_MESSAGE_LENGTH - 1; i++) { diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 83ca2c8cb..3e54a8210 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -218,6 +218,12 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr& h) { return h.id == handler_id; }); } +void TrajectoryPointInterface::setMoveId(const int move_id) +{ + move_id_.store(move_id); +} + } // namespace control } // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 69352ac50..7ada46c77 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -252,7 +252,18 @@ bool UrDriver::writeTrajectorySplinePoint(const vector6d_t& positions, const flo bool UrDriver::writeTrajectoryControlMessage(const control::TrajectoryControlMessage trajectory_action, const int point_number, const RobotReceiveTimeout& robot_receive_timeout) { - return reverse_interface_->writeTrajectoryControlMessage(trajectory_action, point_number, robot_receive_timeout); + // The identifier a point carries is what distinguishes it from the points left behind by an + // earlier move, so a command which begins a move takes the next identifier and gives it to both + // of the parties that need it. The robot learns of it from the control message itself, and the + // trajectory interface needs it in order to write it into the points which follow. The commands + // that do not begin a move carry the current identifier along with them, and the robot ignores it. + if (trajectory_action == control::TrajectoryControlMessage::TRAJECTORY_START || + trajectory_action == control::TrajectoryControlMessage::TRAJECTORY_STREAM_START) + { + trajectory_interface_->setMoveId(++trajectory_move_id_); + } + return reverse_interface_->writeTrajectoryControlMessage(trajectory_action, trajectory_move_id_, point_number, + robot_receive_timeout); } bool UrDriver::writeMotionPrimitive(const std::shared_ptr motion_instruction) diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index 7fa0a6c44..7123a276e 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -278,19 +278,19 @@ TEST_F(ReverseInterfaceTest, write_trajectory_control_message) EXPECT_TRUE(waitForProgramState(1000, true)); control::TrajectoryControlMessage written_control_message = control::TrajectoryControlMessage::TRAJECTORY_CANCEL; - reverse_interface_->writeTrajectoryControlMessage(written_control_message, 1); + reverse_interface_->writeTrajectoryControlMessage(written_control_message, 0, 1); int32_t received_control_message = client_->getTrajectoryControlMode(); EXPECT_EQ(toUnderlying(written_control_message), received_control_message); written_control_message = control::TrajectoryControlMessage::TRAJECTORY_NOOP; - reverse_interface_->writeTrajectoryControlMessage(written_control_message, 1); + reverse_interface_->writeTrajectoryControlMessage(written_control_message, 0, 1); received_control_message = client_->getTrajectoryControlMode(); EXPECT_EQ(toUnderlying(written_control_message), received_control_message); written_control_message = control::TrajectoryControlMessage::TRAJECTORY_START; - reverse_interface_->writeTrajectoryControlMessage(written_control_message, 1); + reverse_interface_->writeTrajectoryControlMessage(written_control_message, 0, 1); received_control_message = client_->getTrajectoryControlMode(); EXPECT_EQ(toUnderlying(written_control_message), received_control_message); @@ -302,7 +302,7 @@ TEST_F(ReverseInterfaceTest, write_trajectory_point_number) EXPECT_TRUE(waitForProgramState(1000, true)); int32_t written_point_number = 2; - reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, + reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 0, written_point_number); int32_t received_point_number = client_->getTrajectoryPointNumber(); @@ -316,7 +316,7 @@ TEST_F(ReverseInterfaceTest, control_mode_is_forward) // When writing trajectory control message, the control mode should always be mode forward comm::ControlMode expected_control_mode = comm::ControlMode::MODE_FORWARD; - reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1); + reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 0, 1); int32_t received_control_mode = client_->getControlMode(); EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); @@ -328,17 +328,32 @@ TEST_F(ReverseInterfaceTest, remaining_message_points_are_zeros) EXPECT_TRUE(waitForProgramState(1000, true)); // When using trajectory control message, the received message is keep_alive_signal=keep_alive_signal, - // received_pos[0]=control::TrajectoryControlMessage, received_pos[1]=point_number and received_pos[2]-received_pos[5] - // should be zeros. - reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1); + // received_pos[0]=control::TrajectoryControlMessage, received_pos[1]=point_number, + // received_pos[2]=move_id and received_pos[3]-received_pos[5] should be zeros. + reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 0, 1); vector6int32_t received_pos = client_->getPositions(); - EXPECT_EQ(0, received_pos[2]); EXPECT_EQ(0, received_pos[3]); EXPECT_EQ(0, received_pos[4]); EXPECT_EQ(0, received_pos[5]); } +TEST_F(ReverseInterfaceTest, write_trajectory_move_id) +{ + // Wait for the client to connect to the server + EXPECT_TRUE(waitForProgramState(1000, true)); + + // The move identifier occupies the slot immediately after the point number, which used to be + // part of the padding. A move announces its identifier here, and the robot then runs only those + // trajectory points which carry it. + const int32_t written_move_id = 7; + reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, + written_move_id, 1); + vector6int32_t received_pos = client_->getPositions(); + + EXPECT_EQ(written_move_id, received_pos[2]); +} + TEST_F(ReverseInterfaceTest, read_timeout) { // Wait for the client to connect to the server @@ -354,7 +369,7 @@ TEST_F(ReverseInterfaceTest, read_timeout) EXPECT_EQ(expected_read_timeout, received_read_timeout); // Test that read timeout works with trajectory control message as well - reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1, + reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 0, 1, RobotReceiveTimeout::millisec(expected_read_timeout)); received_read_timeout = client_->getReadTimeout(); @@ -383,7 +398,7 @@ TEST_F(ReverseInterfaceTest, default_read_timeout) // Test that read timeout works with trajectory control message as well expected_read_timeout = 200; - reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1); + reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 0, 1); received_read_timeout = client_->getReadTimeout(); EXPECT_EQ(expected_read_timeout, received_read_timeout); @@ -495,7 +510,7 @@ TEST_F(ReverseInterfaceTest, deprecated_set_keep_alive_count) int32_t received_read_timeout = client_->getReadTimeout(); EXPECT_EQ(expected_read_timeout, received_read_timeout); - reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1); + reverse_interface_->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 0, 1); received_read_timeout = client_->getReadTimeout(); EXPECT_EQ(expected_read_timeout, received_read_timeout); diff --git a/tests/test_trajectory_point_interface.cpp b/tests/test_trajectory_point_interface.cpp index f648eac78..297503ab3 100644 --- a/tests/test_trajectory_point_interface.cpp +++ b/tests/test_trajectory_point_interface.cpp @@ -96,7 +96,7 @@ class TrajectoryPointInterfaceTest : public ::testing::Test } void readMessage(vector6int32_t& pos, vector6int32_t& vel, vector6int32_t& acc, int32_t& goal_time, - int32_t& blend_radius_or_spline_type, int32_t& motion_type) + int32_t& blend_radius_or_spline_type, int32_t& motion_type, int32_t& move_id) { // Read message uint8_t buf[sizeof(int32_t) * urcl::control::TrajectoryPointInterface::MESSAGE_LENGTH]; @@ -152,66 +152,80 @@ class TrajectoryPointInterfaceTest : public ::testing::Test // Decode motion type std::memcpy(&val, b_pos, sizeof(int32_t)); motion_type = be32toh(val); + b_pos += sizeof(int32_t); + + // Decode the id of the move this point belongs to + std::memcpy(&val, b_pos, sizeof(int32_t)); + move_id = be32toh(val); } vector6int32_t getPosition() { - int32_t goal_time, blend_radius_or_spline_type, motion_type; + int32_t goal_time, blend_radius_or_spline_type, motion_type, move_id; vector6int32_t pos, vel, acc; - readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type); + readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type, move_id); return pos; } vector6int32_t getVelocity() { - int32_t goal_time, blend_radius_or_spline_type, motion_type; + int32_t goal_time, blend_radius_or_spline_type, motion_type, move_id; vector6int32_t pos, vel, acc; - readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type); + readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type, move_id); return vel; } vector6int32_t getAcceleration() { - int32_t goal_time, blend_radius_or_spline_type, motion_type; + int32_t goal_time, blend_radius_or_spline_type, motion_type, move_id; vector6int32_t pos, vel, acc; - readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type); + readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type, move_id); return acc; } int32_t getGoalTime() { - int32_t goal_time, blend_radius_or_spline_type, motion_type; + int32_t goal_time, blend_radius_or_spline_type, motion_type, move_id; vector6int32_t pos, vel, acc; - readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type); + readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type, move_id); return goal_time; } int32_t getBlendRadius() { - int32_t goal_time, blend_radius_or_spline_type, motion_type; + int32_t goal_time, blend_radius_or_spline_type, motion_type, move_id; vector6int32_t pos, vel, acc; - readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type); + readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type, move_id); return blend_radius_or_spline_type; } int32_t getMotionType() { - int32_t goal_time, blend_radius_or_spline_type, motion_type; + int32_t goal_time, blend_radius_or_spline_type, motion_type, move_id; vector6int32_t pos, vel, acc; - readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type); + readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type, move_id); return motion_type; } + int32_t getMoveId() + { + int32_t goal_time, blend_radius_or_spline_type, motion_type, move_id; + vector6int32_t pos, vel, acc; + readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type, move_id); + return move_id; + } + struct TrajData { vector6int32_t pos, vel, acc; - int32_t goal_time, blend_radius_or_spline_type, motion_type; + int32_t goal_time, blend_radius_or_spline_type, motion_type, move_id; }; TrajData getData() { TrajData spl; - readMessage(spl.pos, spl.vel, spl.acc, spl.goal_time, spl.blend_radius_or_spline_type, spl.motion_type); + readMessage(spl.pos, spl.vel, spl.acc, spl.goal_time, spl.blend_radius_or_spline_type, spl.motion_type, + spl.move_id); return spl; } @@ -623,6 +637,33 @@ TEST_F(TrajectoryPointInterfaceTest, write_blend_radius) EXPECT_EQ(send_blend_radius, ((float)received_blend_radius) / traj_point_interface_->MULT_TIME); } +TEST_F(TrajectoryPointInterfaceTest, write_move_id) +{ + urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 }; + const int32_t send_move_id = 3; + traj_point_interface_->setMoveId(send_move_id); + traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, false); + + EXPECT_EQ(send_move_id, client_->getMoveId()); +} + +// A point must carry the identifier of the move that is being written at the time, and not one +// which was captured when the interface was created. If the interface captured it only once, then +// every point would appear to belong to the first move, and the robot would discard the points of +// every move after that one. +TEST_F(TrajectoryPointInterfaceTest, successive_moves_carry_different_move_ids) +{ + urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 }; + + traj_point_interface_->setMoveId(1); + traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, false); + EXPECT_EQ(1, client_->getMoveId()); + + traj_point_interface_->setMoveId(2); + traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, false); + EXPECT_EQ(2, client_->getMoveId()); +} + TEST_F(TrajectoryPointInterfaceTest, write_cartesian) { // Write cartesian point diff --git a/tests/test_trajectory_streaming.cpp b/tests/test_trajectory_streaming.cpp index 35dc136db..60be14994 100644 --- a/tests/test_trajectory_streaming.cpp +++ b/tests/test_trajectory_streaming.cpp @@ -29,6 +29,7 @@ // -- END LICENSE BLOCK ------------------------------------------------ #include +#include #include #include #include @@ -41,6 +42,7 @@ #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/rtde/data_package.h" #include "ur_client_library/types.h" using namespace urcl; @@ -80,6 +82,10 @@ class TrajectoryStreamingTest : public ::testing::Test 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()); + // The tests which assert on motion read the joint positions from RTDE, and + // getDataPackage() fails immediately unless the background reader has been + // started. + g_my_robot->startRTDECommununication(true); g_my_robot->getUrDriver()->registerTrajectoryDoneCallback(&handleTrajectoryState); } @@ -100,6 +106,67 @@ class TrajectoryStreamingTest : public ::testing::Test g_trajectory_result_received = false; } + // Read the current joint positions of the arm. The output recipe this fixture + // uses already carries actual_q, so observing motion requires no additional + // configuration. We report a read which produced nothing rather than asserting + // on it, so that a caller polling for a position can treat it as a reason to + // try again instead of as a failure. + static bool readJointPositions(vector6d_t& joint_positions) + { + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg)) + { + return false; + } + return data_pkg.getData("actual_q", joint_positions); + } + + static bool jointsNear(const vector6d_t& lhs, const vector6d_t& rhs, double tolerance) + { + for (size_t i = 0; i < lhs.size(); ++i) + { + if (std::fabs(lhs[i] - rhs[i]) > tolerance) + { + return false; + } + } + return true; + } + + // Poll until every joint lies within `tolerance` of `target`, or until the + // deadline passes, and return the last sample we read in either case. The + // background reader returns the newest package it has seen without blocking, + // so this loop has to pace itself. We return the sample rather than a bool + // because callers follow this with EXPECT_NEAR, which then reports the + // position the arm actually reached when it fails. + static vector6d_t waitForJointsNear(const vector6d_t& target, double tolerance, std::chrono::milliseconds timeout) + { + const auto deadline = std::chrono::steady_clock::now() + timeout; + vector6d_t joint_positions = { 0, 0, 0, 0, 0, 0 }; + while (std::chrono::steady_clock::now() < deadline) + { + if (readJointPositions(joint_positions) && jointsNear(joint_positions, target, tolerance)) + { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + return joint_positions; + } + + // Assert that the arm came to rest at `target`. We have to assert on motion + // because the trajectory result on its own cannot distinguish a move which ran + // from a move which reported success without ever moving the arm. + static void expectArmReached(const vector6d_t& target) + { + const double k_position_tolerance = 0.01; + const vector6d_t reached = waitForJointsNear(target, k_position_tolerance, std::chrono::seconds(2)); + for (size_t i = 0; i < target.size(); ++i) + { + EXPECT_NEAR(target[i], reached[i], k_position_tolerance) << "joint " << i << " did not reach its target"; + } + } + // 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 @@ -383,6 +450,361 @@ TEST_F(TrajectoryStreamingTest, stream_end_with_overcount_yields_failure) waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); } +// A streaming producer is obliged to keep the trajectory socket fed, but that +// obligation is about trajectory time rather than about how often it writes. +// Here the producer writes twenty points of 0.1s each and then stops writing for +// 400ms, while continuing to answer the reverse interface with NOOPs. The gap is +// much longer than the per-point socket read timeout, which is `get_steptime` +// (2-8ms) once the robot is moving, but much shorter than the two seconds of +// trajectory time already in the socket. The `trajectoryThread` should read +// straight across the gap and the stream should end in SUCCESS. A FAILURE would +// mean the read timeout starves the thread while unread points remain. +TEST_F(TrajectoryStreamingTest, stream_survives_send_gap_while_unread_points_remain) +{ + // A small wrist-3 twist the arm can actually execute, with zero velocity and + // acceleration at each endpoint. Because the arm moves, `is_robot_moving` + // stays true and the per-point read uses the `get_steptime` timeout rather + // than the 0.5s timeout that applies before motion starts. + 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.57, 0.0, -1.57, 0.0, 0.15 }; + const vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + // Position the robot at `pose_a` 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(pose_a, zero, zero, 2.0f)); + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + // Write two seconds of trajectory time to the socket up front, alternating + // between `pose_b` and `pose_a`. + const int k_num_points = 20; + const float k_step_time = 0.1f; + 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(i % 2 == 0 ? pose_b : pose_a, zero, zero, k_step_time)); + } + + // Stop writing to the trajectory socket for 400ms while the thread consumes + // what we already sent, still answering the reverse interface with NOOPs so + // that only the trajectory socket goes quiet. The gap stays under the script's + // 0.5s constant, which sleep_for could otherwise overshoot. + for (int i = 0; i < 8; ++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_num_points)); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); +} + +// The companion to `stream_survives_send_gap_while_unread_points_remain`. Rather +// than writing the whole stream up front, the producer writes one point per +// segment duration, so nothing is ever in the socket ahead of what the robot is +// executing. The producer keeps pace on average, one 0.1s point every 100ms, so +// trajectory time is never actually missing. The question is whether the +// per-point read timeout, `get_steptime` (2-8ms) once the robot is moving, is +// wide enough to absorb the jitter between our write and the robot's read. +TEST_F(TrajectoryStreamingTest, stream_survives_producer_with_no_lookahead) +{ + // The same poses as `stream_survives_send_gap_while_unread_points_remain`. + 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.57, 0.0, -1.57, 0.0, 0.15 }; + const vector6d_t zero = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + // Position the robot at pose_a 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(pose_a, zero, zero, 2.0f)); + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + const int k_num_points = 20; + const float k_step_time = 0.1f; + const auto k_send_period = std::chrono::milliseconds(100); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); + // Write one point, then sleep a full segment before writing the next, so no + // point arrives before the robot needs it. + for (int i = 0; i < k_num_points; ++i) + { + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectorySplinePoint(i % 2 == 0 ? pose_b : pose_a, zero, zero, k_step_time)); + // Answer the reverse interface without writing anything further to the + // trajectory socket. + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + std::this_thread::sleep_for(k_send_period); + } + 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))); +} + +// A control test, and the one case in this group that passes. When a finite +// trajectory underruns, `trajectory_points_left` is left at the declared count +// minus the points actually read and `trajectory_streaming` is false, so the +// cleanup that runs before the next trajectory has a real bound and stops once +// it has drained that many points. The next trajectory then runs normally. The +// streaming cases below differ only in that no count is ever declared, so we pin +// the finite behavior here to keep it from regressing. +TEST_F(TrajectoryStreamingTest, finite_underrun_then_finite_move_recovers) +{ + 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.57, 0.0, -1.57, 0.0, 0.15 }; + 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(); + + // Starve a finite trajectory: declare two points, send only one. The thread + // consumes the first, starts moving, then underruns on the missing second. + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 2)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(pose_b, zero, zero, 0.1f)); + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + // Recovery: a plain finite move must succeed. + 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)); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); +} + +// Recovery from a streaming underrun by way of a finite move, taking the +// `TRAJECTORY_MODE_RECEIVE` branch of the dispatcher. +// +// A stream which aborted used to leave `trajectory_points_left` holding +// `STREAMING_SENTINEL` and `trajectory_streaming` still true, which gave +// `clearTrajectoryPointsThread` no real bound to drain against, so it went on +// reading until the socket fell quiet. Those reads consumed the point belonging +// to the recovery move on the next trajectory command, and so the recovery move +// starved in its turn. The underrun now resets both of those globals before it +// reports failure, which is what this test holds in place. +// +// See https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/550. +TEST_F(TrajectoryStreamingTest, stream_underrun_then_finite_move_recovers) +{ + 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.57, 0.0, -1.57, 0.0, 0.15 }; + 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(); + + // Starve the stream while the arm is moving: open it, write enough points to + // get the arm going, then stop writing. The thread consumes what we sent and + // then underruns. We never send a STREAM_END, so `trajectory_points_left` is + // still `STREAMING_SENTINEL` when the thread reports FAILURE. + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); + for (int i = 0; i < 3; ++i) + { + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(i % 2 == 0 ? pose_b : pose_a, zero, zero, 0.1f)); + } + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + // Recovery: a plain finite move must succeed. + 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)); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); +} + +// The same underrun as `stream_underrun_then_finite_move_recovers`, except that +// the recovery is itself a stream, so the dispatcher takes the +// `TRAJECTORY_MODE_STREAM_START` branch rather than `TRAJECTORY_MODE_RECEIVE`. +// The two branches used to fail in the same way, and a change which repaired +// only one of them would still be caught here. +// +// See https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/550. +TEST_F(TrajectoryStreamingTest, stream_underrun_then_stream_recovers) +{ + 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.57, 0.0, -1.57, 0.0, 0.15 }; + 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(); + + // Starve the stream while the arm is moving, as in + // `stream_underrun_then_finite_move_recovers`. + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); + for (int i = 0; i < 3; ++i) + { + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(i % 2 == 0 ? pose_b : pose_a, zero, zero, 0.1f)); + } + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + // Recovery: a new stream that we keep fed must complete with SUCCESS. + const int k_num_points = 20; + const float k_step_time = 0.1f; + 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(i % 2 == 0 ? pose_b : pose_a, 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))); +} + +// After a stream underruns, the producer does not learn of the failure +// immediately and carries on writing. Those leftover points stay in the +// trajectory socket, and the next trajectory must not execute them. This is the +// complaint that the issue below was filed about. +// +// We can observe it without reading any positions, by making the leftover point +// one that the robot will refuse. A wrist move of about one radian demanded in +// 1ms is roughly 1000 rad/s, far above `JOINT_IGNORE_SPEED`, so +// `targetWithinLimits` rejects it and whichever trajectory reads it ends in +// CANCELED. A result of SUCCESS therefore tells us the recovery ran its own +// point, and CANCELED tells us it ran the leftover one. +// +// That distinction is not sufficient on its own, because it cannot tell either +// of those outcomes apart from a recovery which ran nothing at all. The loop in +// the trajectory thread runs for as long as the result is SUCCESS and points +// remain, and the result begins as SUCCESS, so a recovery which consumed the +// leftover record, reached zero and exited without moving would report SUCCESS +// as well. We therefore have the recovery target a pose which neither the stream +// nor the leftover point ever asked for, and assert that the arm arrived at it. +// +// See https://github.com/UniversalRobots/Universal_Robots_Client_Library/issues/550. +TEST_F(TrajectoryStreamingTest, stream_underrun_stale_points_not_executed_by_recovery) +{ + 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.57, 0.0, -1.57, 0.0, 0.15 }; + const vector6d_t recovery_pose = { 0.0, -1.57, 0.0, -1.57, 0.0, -0.30 }; + 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(); + + // Starve the stream while the arm is moving. The thread consumes what we sent, + // underruns, and exits, leaving the trajectory socket empty and unattended. + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_STREAM_START)); + for (int i = 0; i < 3; ++i) + { + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(i % 2 == 0 ? pose_b : pose_a, zero, zero, 0.1f)); + } + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + // Stand in for a producer that has not yet learned of the failure by writing + // one more point, the invalid one described above. + const vector6d_t stale_fast_pose = { 0.0, -1.57, 0.0, -1.57, 0.0, 1.0 }; + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(stale_fast_pose, zero, zero, 0.001f)); + + // Recovery: a finite move with one valid point. It must run its own point, end + // in SUCCESS, and actually move the arm to the pose that point asked for. + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(recovery_pose, zero, zero, 2.0f)); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + expectArmReached(recovery_pose); +} + +// A client which cancels a stream with a count smaller than the number of points +// it actually wrote leaves those records queued on the trajectory socket. The +// dispatcher works the bound for the cleanup out as the count it was given less +// the number already consumed, so a count of zero produces a negative bound, and +// `clearTrajectoryPointsThread`, whose loop runs only while +// `trajectory_points_left` is above zero, drains nothing at all. This is the bug +// that the Viam module shipped, which sent a count of zero. It degrades rather +// than breaks, because the eager drain misses those records and the move which +// follows disposes of them instead, by recognizing that they name an earlier +// move. +// +// Were the identifiers not there, the recovery would read the first of the +// leftover records as though it were its own point, execute it, and report +// SUCCESS from the wrong pose. Only the assertion on motion catches that, which +// is why the recovery targets a pose the cancelled stream never asked for. +// +// The cancel leaves a run of records queued rather than only one, so a skip +// which worked for the first record alone would not be enough to pass here. +TEST_F(TrajectoryStreamingTest, cancel_with_short_count_then_finite_move_recovers) +{ + const vector6d_t pose_a = { 0.0, -1.57, 0.0, -1.57, 0.0, 0.0 }; + const vector6d_t stale_pose = { 0.0, -1.57, 0.0, -1.57, 0.0, 0.30 }; + const vector6d_t recovery_pose = { 0.0, -1.57, 0.0, -1.57, 0.0, -0.30 }; + 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(); + + // Pile points into the socket faster than the arm can run them, so that a + // cancel leaves most of them unread. + const int k_num_points = 20; + 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(stale_pose, zero, zero, 0.1f)); + } + + // Let the thread consume a point or two so that the cancel arrives in the + // middle of the stream rather than racing the spawn of the thread, and then + // cancel with a count which understates what we wrote. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( + control::TrajectoryControlMessage::TRAJECTORY_CANCEL, 0)); + ASSERT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + resetTrajectoryResultState(); + + // Recovery: one valid point, to a pose the cancelled stream never asked for. + ASSERT_TRUE( + g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_START, 1)); + ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectorySplinePoint(recovery_pose, zero, zero, 2.0f)); + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, + waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5))); + expectArmReached(recovery_pose); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv);