Fix trajectory result in trajectory forward mode when no trajectory is running (#276)

Currently, when switching the control mode away from FORWARD, there will
always a trajectory result CANCELED sent to the trajectory socket. This
commit modifies the script code such that this will only be done when there
actually is a trajectory running.

It also adds a test for that.

Author: urfeex

resources/external_control.urscript

@@ -698,9 +698,11 @@ while control_mode > MODE_STOPPED:
       # Clear remaining trajectory points
       if control_mode == MODE_FORWARD:
         kill thread_trajectory
-        clear_remaining_trajectory_points()
-        stopj(STOPJ_ACCELERATION)
-        socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
+        if trajectory_points_left > 0:
+          clear_remaining_trajectory_points()
+          stopj(STOPJ_ACCELERATION)
+          socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
+        end
         # Stop freedrive
       elif control_mode == MODE_FREEDRIVE:
         textmsg("Leaving freedrive mode")

tests/test_spline_interpolation.cpp

@@ -36,6 +36,7 @@
 #include <ur_client_library/ur/ur_driver.h>
 #include <ur_client_library/types.h>
 
+#include <chrono>
 #include <iostream>
 #include <memory>
 #include <math.h>
@@ -56,10 +57,15 @@ bool g_HEADLESS = true;
 std::unique_ptr<ExampleRobotWrapper> g_my_robot;
 
 bool g_trajectory_running;
+std::condition_variable g_trajectory_result_cv;
+std::mutex g_trajectory_result_mutex;
 control::TrajectoryResult g_trajectory_result;
 void handleTrajectoryState(control::TrajectoryResult state)
 {
+  std::lock_guard<std::mutex> lk(g_trajectory_result_mutex);
+  g_trajectory_result_cv.notify_one();
   g_trajectory_result = state;
+  URCL_LOG_INFO("Received trajectory result %s", control::trajectoryResultToString(state).c_str());
   g_trajectory_running = false;
 }
 
@@ -74,6 +80,12 @@ bool nearlyEqual(double a, double b, double eps = 1e-15)
   return c < eps || -c < eps;
 }
 
+bool waitForTrajectoryResult(std::chrono::milliseconds timeout)
+{
+  std::unique_lock<std::mutex> lk(g_trajectory_result_mutex);
+  return g_trajectory_result_cv.wait_for(lk, timeout) == std::cv_status::no_timeout;
+}
+
 class SplineInterpolationTest : public ::testing::Test
 {
 protected:
@@ -1085,6 +1097,52 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline)
   }
 }
 
+TEST_F(SplineInterpolationTest, switching_control_mode_without_trajectory_produces_no_result)
+{
+  // We start by putting the robot into trajectory control mode
+  ASSERT_TRUE(
+      g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP));
+
+  // Then we switch to idle
+  ASSERT_TRUE(g_my_robot->ur_driver_->writeKeepalive(RobotReceiveTimeout::sec(2)));
+
+  EXPECT_FALSE(waitForTrajectoryResult(std::chrono::milliseconds(500)));
+}
+
+TEST_F(SplineInterpolationTest, switching_control_mode_with_trajectory_produces_result)
+{
+  g_my_robot->stopConsumingRTDEData();
+  std::unique_ptr<rtde_interface::DataPackage> data_pkg;
+  g_my_robot->readDataPackage(data_pkg);
+
+  urcl::vector6d_t joint_positions_before;
+  ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before));
+
+  // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly
+  g_my_robot->startConsumingRTDEData();
+  // Create a trajectory that cannot be executed within the robots limits
+  std::vector<urcl::vector6d_t> s_pos, s_vel, s_acc;
+  urcl::vector6d_t first_point = {
+    joint_positions_before[0], joint_positions_before[1], joint_positions_before[2],
+    joint_positions_before[3], joint_positions_before[4], joint_positions_before[5] + 0.5
+  };
+  s_pos.push_back(first_point);
+
+  urcl::vector6d_t zeros = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+  s_vel.push_back(zeros);
+  s_acc.push_back(zeros);
+
+  std::vector<double> s_time = { 0.02 };
+  sendTrajectory(s_pos, s_vel, s_acc, s_time);
+  g_my_robot->ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1,
+                                                        RobotReceiveTimeout::off());
+
+  // Then we switch to idle
+  ASSERT_TRUE(g_my_robot->ur_driver_->writeKeepalive(RobotReceiveTimeout::sec(2)));
+
+  EXPECT_TRUE(waitForTrajectoryResult(std::chrono::milliseconds(500)));
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);