Add tests demonstrating failure to recover from stream starvation

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)

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

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
 -----------------------------
 

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.

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 <atomic>
 #include <list>
 #include <optional>
 
@@ -63,9 +64,10 @@ std::string trajectoryResultToString(const TrajectoryResult result);
 class TrajectoryPointInterface : public ReverseInterface
 {
 public:
+  static const int MESSAGE_LENGTH = 22;
   // Spline vel/acc need finer resolution: quantising near-zero values at 1e-6 causes controller faults.
   static constexpr int32_t MULT_VEL_ACC = 100000000;
-  static const int MESSAGE_LENGTH = 21;
+
 
   TrajectoryPointInterface() = delete;
   /*!
@@ -163,6 +165,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;
 
@@ -177,6 +194,10 @@ class TrajectoryPointInterface : public ReverseInterface
 
   std::list<HandlerFunction<void(TrajectoryResult)>> 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<int32_t> move_id_ = { 0 };
 };
 
 }  // namespace control

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 <atomic>
 #include <chrono>
 #include <functional>
 #include <memory>
@@ -1136,6 +1137,13 @@ class UrDriver
   std::unique_ptr<control::ScriptSender> script_sender_;
   std::unique_ptr<control::ScriptReader> 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<int32_t> trajectory_move_id_ = { 0 };
+
   size_t socket_connection_attempts_ = 0;
   std::chrono::milliseconds socket_reconnection_timeout_ = std::chrono::milliseconds(10000);
 

resources/external_control.urscript

@@ -62,6 +62,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
@@ -114,6 +119,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
@@ -554,11 +564,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
@@ -569,11 +582,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
@@ -769,6 +799,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
@@ -784,9 +819,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)
@@ -807,7 +852,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.")
@@ -1167,13 +1215,17 @@ 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:
         kill thread_trajectory
         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:

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++)
   {

src/control/trajectory_point_interface.cpp

@@ -237,6 +237,12 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
   buffer[index] = htobe32(val);
   index++;
 
+  // Record the move that this point belongs to. The robot uses it to recognize the points which
+  // were left over from a move that has already ended, and discards them rather than executing them
+  // as though they were part of the move it is running now.
+  buffer[index] = htobe32(move_id_.load());
+  index++;
+
   size_t written;
 
   // We stored the data in a int32_t vector, but write needs a uint8_t buffer
@@ -373,5 +379,10 @@ void TrajectoryPointInterface::removeTrajectoryEndCallback(const uint32_t handle
       [handler_id](const HandlerFunction<void(TrajectoryResult)>& h) { return h.id == handler_id; });
 }
 
+void TrajectoryPointInterface::setMoveId(const int move_id)
+{
+  move_id_.store(move_id);
+}
+
 }  // namespace control
 }  // namespace urcl

src/ur/ur_driver.cpp

@@ -268,7 +268,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<control::MotionPrimitive> motion_instruction)