Check for protective stop in ExampleRobotWrapper initialization

Author: urfeex

include/ur_client_library/example_robot_wrapper.h

@@ -157,6 +157,14 @@ class ExampleRobotWrapper
    */
   bool startRobotProgram(const std::string& program_file_name);
 
+  /**
+   * @brief Clear protective stop on the robot.
+   *
+   * This will try to clear a protective stop on the robot. If the robot is not in protective stop
+   * this call will do nothing.
+   */
+  bool clearProtectiveStop();
+
   bool isHealthy() const;
 
   std::shared_ptr<urcl::DashboardClient> dashboard_client_; /*!< Dashboard client to interact with the robot */

src/example_robot_wrapper.cpp

@@ -31,6 +31,7 @@
 #include <ur_client_library/example_robot_wrapper.h>
 #include <iostream>
 #include "ur_client_library/exceptions.h"
+#include "ur_client_library/log.h"
 
 namespace urcl
 {
@@ -80,9 +81,38 @@ ExampleRobotWrapper::~ExampleRobotWrapper()
   }
 }
 
+bool ExampleRobotWrapper::clearProtectiveStop()
+{
+  std::string safety_status;
+  dashboard_client_->commandSafetyStatus(safety_status);
+  bool is_protective_stopped = safety_status.find("PROTECTIVE_STOP") != std::string::npos;
+  if (is_protective_stopped)
+  {
+    URCL_LOG_INFO("Robot is in protective stop, trying to release it");
+    dashboard_client_->commandClosePopup();
+    dashboard_client_->commandCloseSafetyPopup();
+    if (!dashboard_client_->commandUnlockProtectiveStop())
+    {
+      std::this_thread::sleep_for(std::chrono::seconds(5));
+      if (!dashboard_client_->commandUnlockProtectiveStop())
+      {
+        URCL_LOG_ERROR("Could not unlock protective stop");
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
 bool ExampleRobotWrapper::initializeRobotWithDashboard()
 {
-  // // Stop program, if there is one running
+  if (!clearProtectiveStop())
+  {
+    URCL_LOG_ERROR("Could not clear protective stop");
+    return false;
+  }
+
+  // Stop program, if there is one running
   if (!dashboard_client_->commandStop())
   {
     URCL_LOG_ERROR("Could not send stop program command");

tests/test_instruction_executor.cpp

@@ -62,17 +62,7 @@ class InstructionExecutorTest : public ::testing::Test
   void SetUp() override
   {
     executor_ = std::make_unique<InstructionExecutor>(g_my_robot->ur_driver_);
-    std::string safety_status;
-    g_my_robot->dashboard_client_->commandSafetyStatus(safety_status);
-    bool is_protective_stopped = safety_status.find("PROTECTIVE_STOP") != std::string::npos;
-    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_my_robot->dashboard_client_->commandCloseSafetyPopup();
-      ASSERT_TRUE(g_my_robot->dashboard_client_->commandUnlockProtectiveStop());
-    }
+    g_my_robot->clearProtectiveStop();
     // Make sure script is running on the robot
     if (!g_my_robot->waitForProgramRunning())
     {