changes based on cursor bot review

Author: acmorrow

resources/external_control.urscript

@@ -769,10 +769,16 @@ thread trajectoryThread():
         spline_qd = [0, 0, 0, 0, 0, 0]
       end
     else:
-      if was_streaming_at_start and not trajectory_streaming:
-        # Producer signalled STREAM_END between the predicate check and
-        # the socket read. Drop any pending unread points and exit cleanly
-        # rather than spin one timeout per missing point.
+      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!")
@@ -1157,6 +1163,9 @@ 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:
@@ -1167,21 +1176,32 @@ while control_mode > MODE_STOPPED:
         trajectory_points_left = STREAMING_SENTINEL
         thread_trajectory = run trajectoryThread()
       elif params_mult[2] == TRAJECTORY_MODE_STREAM_END:
-        # 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
+        # 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
         request_trajectory_cleanup()
         stopj(STOPJ_ACCELERATION)
         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
         socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
       end
     elif control_mode == MODE_SPEEDL:

tests/test_trajectory_streaming.cpp

@@ -30,6 +30,7 @@
 
 #include <chrono>
 #include <condition_variable>
+#include <functional>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -120,6 +121,41 @@ class TrajectoryStreamingTest : public ::testing::Test
     }
     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<void()> 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<std::mutex> 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,
@@ -197,6 +233,158 @@ TEST_F(TrajectoryStreamingTest, stream_cancel_yields_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);