Make sure to wait long enough before releasing the PSTOP

Author: urfeex

tests/test_instruction_executor.cpp

@@ -44,6 +44,7 @@ const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
 std::string ROBOT_IP = "192.168.56.101";
 
 bool g_program_running;
+bool g_is_protective_stopped = false;
 std::condition_variable g_program_not_running_cv;
 std::mutex g_program_not_running_mutex;
 std::condition_variable g_program_running_cv;
@@ -135,13 +136,17 @@ class InstructionExecutorTest : public ::testing::Test
   void SetUp() override
   {
     executor_ = std::make_unique<InstructionExecutor>(g_ur_driver);
-    // Make sure script is running on the robot
-    if (g_program_running == false)
+    if (g_is_protective_stopped)
     {
+      // We forced a protective stop above. Some versions require waiting 5 seconds before releasing
+      // the protective stop.
+      std::this_thread::sleep_for(std::chrono::seconds(5));
       g_dashboard_client->commandCloseSafetyPopup();
-      std::this_thread::sleep_for(std::chrono::milliseconds(500));
       ASSERT_TRUE(g_dashboard_client->commandUnlockProtectiveStop());
-
+    }
+    // Make sure script is running on the robot
+    if (g_program_running == false)
+    {
       g_ur_driver->sendRobotProgram();
       ASSERT_TRUE(waitForProgramRunning(1000));
     }
@@ -293,6 +298,7 @@ TEST_F(InstructionExecutorTest, unfeasible_movej_target_results_in_failure)
 
   // move to an unfeasible pose
   ASSERT_FALSE(executor_->moveJ({ -123, 0, 0, 0, 0, 0 }));
+  g_is_protective_stopped = true;
 }
 
 TEST_F(InstructionExecutorTest, unfeasible_movel_target_results_in_failure)
@@ -302,6 +308,7 @@ TEST_F(InstructionExecutorTest, unfeasible_movel_target_results_in_failure)
 
   // move to an unfeasible pose
   ASSERT_FALSE(executor_->moveL({ -10.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 1.4, 1.04, 0.1));
+  g_is_protective_stopped = true;
 }
 
 TEST_F(InstructionExecutorTest, unfeasible_sequence_targets_results_in_failure)
@@ -312,6 +319,7 @@ TEST_F(InstructionExecutorTest, unfeasible_sequence_targets_results_in_failure)
 
   };
   ASSERT_FALSE(executor_->executeMotion(motion_sequence));
+  g_is_protective_stopped = true;
 }
 
 TEST_F(InstructionExecutorTest, unfeasible_times_succeeds)