Add test for cancel timeout

Author: urfeex

src/ur/instruction_executor.cpp

@@ -31,6 +31,8 @@
 #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());
@@ -124,12 +126,19 @@ bool urcl::InstructionExecutor::cancelMotion()
   if (trajectory_running_)
   {
     URCL_LOG_INFO("Cancel motion");
-    driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL);
+    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(1000)) == std::cv_status::no_timeout)
+    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/test_instruction_executor.cpp

@@ -29,7 +29,9 @@
 // -- END LICENSE BLOCK ------------------------------------------------
 
 #include <gtest/gtest.h>
+#include <algorithm>
 #include <iostream>
+#include <regex>
 #include <thread>
 #include "ur_client_library/ur/instruction_executor.h"
 #include "ur_client_library/control/motion_primitives.h"
@@ -70,6 +72,16 @@ class InstructionExecutorTest : public ::testing::Test
       ASSERT_TRUE(g_my_robot->waitForProgramRunning());
     }
   }
+  void TearDown()
+  {
+    g_my_robot->ur_driver_->stopControl();
+    g_my_robot->waitForProgramNotRunning(1000);
+  }
+
+  static void TearDownTestSuite()
+  {
+    g_my_robot.reset();
+  }
 };
 
 TEST_F(InstructionExecutorTest, execute_motion_sequence_success)
@@ -225,6 +237,49 @@ TEST_F(InstructionExecutorTest, canceling_without_running_trajectory_returns_fal
   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::regex delete_me("socket_send_int\\(TRAJECTORY_RESULT_CANCELED,\\s*\"trajectory_socket\"\\)");
+  while (std::getline(in_file, line))
+  {
+    std::smatch delete_match;
+    if (!std::regex_search(line, delete_match, delete_me))
+    {
+      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));