Allow canceling an instruction

Author: urfeex

include/ur_client_library/ur/instruction_executor.h

@@ -92,14 +92,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

src/ur/instruction_executor.cpp

@@ -29,9 +29,11 @@
 // -- END LICENSE BLOCK ------------------------------------------------
 
 #include "ur_client_library/ur/instruction_executor.h"
+#include <mutex>
 #include "ur_client_library/control/trajectory_point_interface.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_result_ = result;
   trajectory_running_ = false;
@@ -40,6 +42,7 @@ 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 +89,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 +116,19 @@ 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);
+    std::unique_lock<std::mutex> lock(trajectory_result_mutex_);
+    if (trajectory_done_cv_.wait_for(lock, std::chrono::milliseconds(1000)) == std::cv_status::no_timeout)
+    {
+      return trajectory_result_ == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED;
+    }
+  }
+  return false;
+}

tests/test_instruction_executor.cpp

@@ -219,6 +219,27 @@ 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_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