Fix instruction executor idle (UniversalRobots#424)

When using the InstructionExecutor, sending the first trajectory puts
the external_control program in `MODE_FORWARD`. We then send keepalive
signals every 100ms with a timeout of 200 ms.

However, when the trajectory has finished, nothing is sent anymore,
hence reading the keepalive signal times out and program execution is
stopped. This is most likely the cause of UniversalRobots#423.

This PR explicitly sets the external_control program into `MODE_IDLE`
once a trajectory has finished. That allows waiting for a new package on
the reverse interface indefinitely. Thus, users can spend as much time
between trajectory calls as they like without having to manually send
keepalive signals of some sort.

Author: urfeex

src/ur/instruction_executor.cpp

@@ -91,6 +91,7 @@ bool urcl::InstructionExecutor::executeMotion(
   if (!cancel_requested_)
   {
     std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
+    driver_->writeJointCommand({ 0, 0, 0, 0, 0, 0 }, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::millisec(0));
     URCL_LOG_INFO("Trajectory done with result %s", control::trajectoryResultToString(trajectory_result_).c_str());
     return trajectory_result_ == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS;
   }

tests/test_instruction_executor.cpp

@@ -30,6 +30,7 @@
 
 #include <gtest/gtest.h>
 #include <algorithm>
+#include <future>
 #include <iostream>
 #include <thread>
 #include "ur_client_library/ur/instruction_executor.h"
@@ -455,6 +456,27 @@ TEST_F(InstructionExecutorTest, optimovel_with_illegal_parameters_fails)
   ASSERT_FALSE(executor_->optimoveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 0.4, 0.7, -0.1));
 }
 
+TEST_F(InstructionExecutorTest, no_new_trajectory_doesnt_stop_execution)
+{
+  // move to a feasible starting pose
+  ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }));
+
+  // The internal keepalive of the instruction executor is 200 ms
+  // The program should keep running.
+  ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(500));
+
+  auto op = [this]() mutable { return executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, -2.2 }); };
+  auto result = std::async(std::launch::async, op);
+
+  // Give motion some time to start, then cancel
+  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  executor_->cancelMotion();
+
+  result.wait();
+  ASSERT_FALSE(result.get());
+  ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(500));
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);
@@ -475,4 +497,4 @@ int main(int argc, char* argv[])
   }
 
   return RUN_ALL_TESTS();
-}
+}