instruction_executor: Allow canceling an instruction (#281)

This allows cancelling a running motion with the InstructionExecutor. As
executions are blocking, they have to be run in another thread to be
able to be canceled.

Author: urfeex

include/ur_client_library/control/reverse_interface.h

@@ -146,6 +146,16 @@ class ReverseInterface
     disconnection_callback_ = disconnection_fun;
   }
 
+  /*!
+   * \brief Checks if the reverse interface is connected to the robot.
+   *
+   * \returns True, if the interface is connected, false otherwise.
+   */
+  bool isConnected() const
+  {
+    return client_fd_ != INVALID_SOCKET;
+  }
+
 protected:
   virtual void connectionCallback(const socket_t filedescriptor);
 

include/ur_client_library/ur/instruction_executor.h

@@ -48,6 +48,12 @@ class InstructionExecutor
         std::bind(&InstructionExecutor::trajDisconnectCallback, this, std::placeholders::_1));
   }
 
+  ~InstructionExecutor()
+  {
+    driver_->registerTrajectoryDoneCallback(nullptr);
+    driver_->registerTrajectoryInterfaceDisconnectedCallback(nullptr);
+  }
+
   /**
    * \brief Execute a sequence of motion primitives.
    *
@@ -92,14 +98,35 @@ class InstructionExecutor
   bool moveL(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04,
              const double time = 0, const double blend_radius = 0);
 
+  /**
+   * \brief Cancel the current motion.
+   *
+   * If no motion is running, false will be returned.
+   * If a motion is running, it will be canceled and it will wait for a TRAJECTORY_CANCELED result
+   * with a timeout of one second.
+   *
+   * If another result or no result is received, false will be returned.
+   */
+  bool cancelMotion();
+
+  /**
+   * \brief Check if a trajectory is currently running.
+   */
+  bool isTrajectoryRunning() const
+  {
+    return trajectory_running_;
+  }
+
 private:
   void trajDoneCallback(const urcl::control::TrajectoryResult& result);
   void trajDisconnectCallback(const int filedescriptor);
   std::shared_ptr<urcl::UrDriver> driver_;
   std::atomic<bool> trajectory_running_ = false;
+  std::atomic<bool> cancel_requested_ = false;
   std::mutex trajectory_result_mutex_;
+  std::condition_variable trajectory_done_cv_;
   urcl::control::TrajectoryResult trajectory_result_;
 };
 }  // namespace urcl
 
-#endif  // UR_CLIENT_LIBRARY_INSTRUCTION_EXECUTOR_H_INCLUDED
+#endif  // UR_CLIENT_LIBRARY_INSTRUCTION_EXECUTOR_H_INCLUDED

include/ur_client_library/ur/ur_driver.h

@@ -880,6 +880,16 @@ class UrDriver
     secondary_stream_->close();
   }
 
+  bool isReverseInterfaceConnected() const
+  {
+    return reverse_interface_->isConnected();
+  }
+
+  bool isTrajectoryInterfaceConnected() const
+  {
+    return trajectory_interface_->isConnected();
+  }
+
 private:
   void init(const UrDriverConfiguration& config);
 

src/control/trajectory_point_interface.cpp

@@ -30,6 +30,7 @@
 #include <ur_client_library/exceptions.h>
 #include <math.h>
 #include <stdexcept>
+#include "ur_client_library/comm/socket_t.h"
 
 namespace urcl
 {
@@ -217,11 +218,11 @@ void TrajectoryPointInterface::connectionCallback(const socket_t filedescriptor)
 void TrajectoryPointInterface::disconnectionCallback(const socket_t filedescriptor)
 {
   URCL_LOG_DEBUG("Connection to trajectory interface dropped.");
-  client_fd_ = -1;
   if (disconnection_callback_ != nullptr)
   {
     disconnection_callback_(filedescriptor);
   }
+  client_fd_ = INVALID_SOCKET;
 }
 
 void TrajectoryPointInterface::messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv)

src/ur/instruction_executor.cpp

@@ -29,17 +29,23 @@
 // -- END LICENSE BLOCK ------------------------------------------------
 
 #include "ur_client_library/ur/instruction_executor.h"
+#include <mutex>
 #include "ur_client_library/control/trajectory_point_interface.h"
+#include "ur_client_library/log.h"
+#include "ur_client_library/ur/robot_receive_timeout.h"
 void urcl::InstructionExecutor::trajDoneCallback(const urcl::control::TrajectoryResult& result)
 {
+  URCL_LOG_DEBUG("Trajectory result received: %s", control::trajectoryResultToString(result).c_str());
   std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
+  trajectory_done_cv_.notify_all();
   trajectory_result_ = result;
   trajectory_running_ = false;
 }
 void urcl::InstructionExecutor::trajDisconnectCallback(const int filedescriptor)
 {
   URCL_LOG_INFO("Trajectory disconnect");
   std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
+  trajectory_done_cv_.notify_all();
   if (trajectory_running_)
   {
     trajectory_result_ = urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE;
@@ -86,17 +92,20 @@ bool urcl::InstructionExecutor::executeMotion(
     }
   }
   trajectory_running_ = true;
+  cancel_requested_ = false;
 
-  while (trajectory_running_)
+  while (trajectory_running_ && !cancel_requested_)
   {
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
     driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
+  if (!cancel_requested_)
   {
     std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
     URCL_LOG_INFO("Trajectory done with result %s", control::trajectoryResultToString(trajectory_result_).c_str());
     return trajectory_result_ == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS;
   }
+  return false;
 }
 bool urcl::InstructionExecutor::moveJ(const urcl::vector6d_t& target, const double acceleration, const double velocity,
                                       const double time, const double blend_radius)
@@ -110,3 +119,26 @@ bool urcl::InstructionExecutor::moveL(const urcl::Pose& target, const double acc
   return executeMotion({ std::make_shared<control::MoveLPrimitive>(
       target, blend_radius, std::chrono::milliseconds(static_cast<int>(time * 1000)), acceleration, velocity) });
 }
+
+bool urcl::InstructionExecutor::cancelMotion()
+{
+  cancel_requested_ = true;
+  if (trajectory_running_)
+  {
+    URCL_LOG_INFO("Cancel motion");
+    driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL, -1,
+                                           RobotReceiveTimeout::millisec(2000));
+    std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
+    if (trajectory_done_cv_.wait_for(lock, std::chrono::milliseconds(200)) == std::cv_status::no_timeout)
+    {
+      return trajectory_result_ == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED;
+    }
+    else
+    {
+      URCL_LOG_ERROR("Sent a canceling request to the robot but waiting for the answer timed out.");
+      return false;
+    }
+  }
+  URCL_LOG_WARN("Canceling motion requested without a motion running.");
+  return false;
+}

tests/resources/dockerursim/programs/cb3/external_control.urp

@@ -29,6 +29,7 @@
 // -- END LICENSE BLOCK ------------------------------------------------
 
 #include <gtest/gtest.h>
+#include <algorithm>
 #include <iostream>
 #include <thread>
 #include "ur_client_library/ur/instruction_executor.h"
@@ -70,6 +71,21 @@ class InstructionExecutorTest : public ::testing::Test
       ASSERT_TRUE(g_my_robot->waitForProgramRunning());
     }
   }
+  void TearDown() override
+  {
+    g_my_robot->ur_driver_->stopControl();
+    g_my_robot->waitForProgramNotRunning(1000);
+    while (g_my_robot->ur_driver_->isTrajectoryInterfaceConnected())
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    URCL_LOG_INFO("Stopped robot control.");
+  }
+
+  static void TearDownTestSuite()
+  {
+    g_my_robot.reset();
+  }
 };
 
 TEST_F(InstructionExecutorTest, execute_motion_sequence_success)
@@ -219,6 +235,69 @@ TEST_F(InstructionExecutorTest, unfeasible_movej_target_results_in_failure)
   ASSERT_FALSE(executor_->moveJ({ -123, 0, 0, 0, 0, 0 }));
 }
 
+TEST_F(InstructionExecutorTest, canceling_without_running_trajectory_returns_false)
+{
+  ASSERT_FALSE(executor_->isTrajectoryRunning());
+  ASSERT_FALSE(executor_->cancelMotion());
+}
+
+TEST(InstructionExecutorTestStandalone, canceling_without_receiving_answer_returns_false)
+{
+  std::ifstream in_file(SCRIPT_FILE);
+  std::ofstream out_file;
+  const std::string test_script_file = "test_script.urscript";
+  out_file.open(test_script_file);
+
+  std::string line;
+  std::string pattern = "socket_send_int(TRAJECTORY_RESULT_CANCELED, \"trajectory_socket\")";
+  while (std::getline(in_file, line))
+  {
+    if (line.find(pattern) == std::string::npos)
+    {
+      out_file << line << std::endl;
+    }
+  }
+  out_file.close();
+  auto my_robot = std::make_unique<ExampleRobotWrapper>(ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS,
+                                                        "external_control.urp", test_script_file);
+  auto executor = std::make_unique<InstructionExecutor>(my_robot->ur_driver_);
+  my_robot->clearProtectiveStop();
+  // Make sure script is running on the robot
+  if (!my_robot->waitForProgramRunning())
+  {
+    my_robot->resendRobotProgram();
+    ASSERT_TRUE(my_robot->waitForProgramRunning());
+  }
+  ASSERT_TRUE(executor->moveJ({ -1.59, -1.72, -2.2, -0.8, 1.6, 0.2 }, 2.0, 2.0));
+  std::thread move_thread([&executor]() { executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 2.7 }, 0.1, 0.1); });
+  bool is_trajectory_running = false;
+  std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
+  while (!is_trajectory_running || std::chrono::steady_clock::now() > start + std::chrono::seconds(5))
+  {
+    is_trajectory_running = executor->isTrajectoryRunning();
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  ASSERT_TRUE(executor->isTrajectoryRunning());
+  ASSERT_FALSE(executor->cancelMotion());
+  move_thread.join();
+  std::remove(test_script_file.c_str());
+}
+
+TEST_F(InstructionExecutorTest, canceling_with_running_trajectory_succeeds)
+{
+  ASSERT_TRUE(executor_->moveJ({ -1.59, -1.72, -2.2, -0.8, 1.6, 0.2 }, 2.0, 2.0));
+  std::thread move_thread([this]() { executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 2.7 }); });
+  bool is_trajectory_running = false;
+  std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
+  while (!is_trajectory_running || std::chrono::steady_clock::now() > start + std::chrono::seconds(5))
+  {
+    is_trajectory_running = executor_->isTrajectoryRunning();
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+  ASSERT_TRUE(executor_->cancelMotion());
+  move_thread.join();
+}
+
 TEST_F(InstructionExecutorTest, unfeasible_movel_target_results_in_failure)
 {
   // move to a feasible starting pose