Make sure to wait long enough before releasing the PSTOP

Author: urfeex

tests/test_instruction_executor.cpp

@@ -97,6 +97,7 @@ class InstructionExecutorTest : public ::testing::Test
 {
 protected:
   std::unique_ptr<InstructionExecutor> executor_;
+  bool is_protective_stopped_ = false;
 
   static void SetUpTestSuite()
   {
@@ -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 (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 }));
+  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));
+  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));
+  is_protective_stopped_ = true;
 }
 
 TEST_F(InstructionExecutorTest, unfeasible_times_succeeds)