cancels for streams should carry the final primitive count as well

Author: acmorrow

include/ur_client_library/control/reverse_interface.h

@@ -129,7 +129,10 @@ class ReverseInterface
    * \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. \p point_number is unused.
+   * - 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

resources/external_control.urscript

@@ -1201,7 +1201,11 @@ while control_mode > MODE_STOPPED:
         wait_for_trajectory_cleanup()
         # The streaming session, if any, has been forcibly ended; clear
         # the flag so a subsequent stray STREAM_END is a no-op.
-        trajectory_streaming = False
+	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
         socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
       end
     elif control_mode == MODE_SPEEDL:

tests/test_trajectory_streaming.cpp

@@ -227,7 +227,7 @@ TEST_F(TrajectoryStreamingTest, stream_cancel_yields_canceled)
   std::this_thread::sleep_for(std::chrono::milliseconds(50));
 
   ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
-      control::TrajectoryControlMessage::TRAJECTORY_CANCEL));
+      control::TrajectoryControlMessage::TRAJECTORY_CANCEL, 100));
 
   EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED,
             waitForTrajectoryResultPumpingNoops(std::chrono::seconds(5)));