Move waitForProgramRunning to robot initialization

Author: urfeex

examples/force_mode_example.cpp

@@ -86,6 +86,11 @@ int main(int argc, char* argv[])
   g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                      "external_control.urp");
 
+  if (!g_my_robot->isHealthy())
+  {
+    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
+    return 1;
+  }
   if (!g_my_robot->ur_driver_->checkCalibration(CALIBRATION_CHECKSUM))
   {
     URCL_LOG_ERROR("Calibration checksum does not match actual robot.");
@@ -95,12 +100,6 @@ int main(int argc, char* argv[])
                    "for details.");
   }
 
-  // Make sure that external control script is running
-  if (!g_my_robot->waitForProgramRunning())
-  {
-    URCL_LOG_ERROR("External Control script not running.");
-    return 1;
-  }
   // End of initialization -- We've started the external control program, which means we have to
   // write keepalive signals from now on. Otherwise the connection will be dropped.
 

examples/freedrive_example.cpp

@@ -85,9 +85,9 @@ int main(int argc, char* argv[])
   g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                      "external_control.urp");
 
-  if (!g_my_robot->waitForProgramRunning())
+  if (!g_my_robot->isHealthy())
   {
-    URCL_LOG_ERROR("External Control script not running.");
+    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
     return 1;
   }
 

examples/full_driver.cpp

@@ -62,10 +62,9 @@ int main(int argc, char* argv[])
   bool headless_mode = true;
   g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                      "external_control.urp");
-  // Make sure that external control script is running
-  if (!g_my_robot->waitForProgramRunning())
+  if (!g_my_robot->isHealthy())
   {
-    URCL_LOG_ERROR("External Control script not running.");
+    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
     return 1;
   }
   // --------------- INITIALIZATION END -------------------

examples/instruction_executor.cpp

@@ -69,9 +69,9 @@ int main(int argc, char* argv[])
                    "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] "
                    "for details.");
   }
-  if (!g_my_robot->waitForProgramRunning(1000))
+  if (!g_my_robot->isHealthy())
   {
-    std::cout << "Program did not start running. Is the robot in remote control?" << std::endl;
+    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
     return 1;
   }
 

examples/spline_example.cpp

@@ -125,10 +125,9 @@ int main(int argc, char* argv[])
   bool headless_mode = true;
   g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                      "external_control.urp");
-  // Make sure that external control script is running
-  if (!g_my_robot->waitForProgramRunning())
+  if (!g_my_robot->isHealthy())
   {
-    URCL_LOG_ERROR("External Control script not running.");
+    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
     return 1;
   }
 

examples/tool_contact_example.cpp

@@ -80,9 +80,9 @@ int main(int argc, char* argv[])
   bool headless_mode = true;
   g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                      "external_control.urp");
-  if (!g_my_robot->waitForProgramRunning(1000))
+  if (!g_my_robot->isHealthy())
   {
-    std::cout << "Program did not start running. Is the robot in remote control?" << std::endl;
+    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
     return 1;
   }
   g_my_robot->ur_driver_->registerToolContactResultCallback(&handleToolContactResult);

examples/trajectory_point_interface.cpp

@@ -66,9 +66,9 @@ int main(int argc, char* argv[])
   bool headless_mode = true;
   g_my_robot = std::make_unique<urcl::ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode,
                                                            "external_control.urp");
-  if (!g_my_robot->waitForProgramRunning(1000))
+  if (!g_my_robot->isHealthy())
   {
-    URCL_LOG_ERROR("Program did not start running. Is the robot in remote control?");
+    URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above.");
     return 1;
   }
   // --------------- INITIALIZATION END -------------------

include/ur_client_library/example_robot_wrapper.h

@@ -83,7 +83,7 @@ class ExampleRobotWrapper
    *
    * The robot will be power-cycled once and end up switched on, breaks released.
    */
-  void initializeRobotWithDashboard();
+  bool initializeRobotWithDashboard();
 
   /**
    * @brief Starts RTDE communication with the robot.
@@ -157,6 +157,8 @@ class ExampleRobotWrapper
    */
   bool startRobotProgram(const std::string& program_file_name);
 
+  bool isHealthy() const;
+
   std::shared_ptr<urcl::DashboardClient> dashboard_client_; /*!< Dashboard client to interact with the robot */
   std::shared_ptr<urcl::UrDriver> ur_driver_;               /*!< UR driver to interact with the robot */
 
@@ -168,6 +170,8 @@ class ExampleRobotWrapper
   std::mutex read_package_mutex_;
   std::unique_ptr<rtde_interface::DataPackage> data_pkg_;
 
+  bool robot_initialized_ = false;
+
   bool program_running_;
   std::condition_variable program_running_cv_;
   std::condition_variable program_not_running_cv_;

src/example_robot_wrapper.cpp

@@ -57,6 +57,14 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std:
   {
     startRobotProgram(autostart_program);
   }
+
+  if (headless_mode | !std::empty(autostart_program))
+  {
+    if (!waitForProgramRunning(500))
+    {
+      URCL_LOG_ERROR("Program did not start running. Is the robot in remote control?");
+    }
+  }
 }
 
 ExampleRobotWrapper::~ExampleRobotWrapper()
@@ -66,34 +74,41 @@ ExampleRobotWrapper::~ExampleRobotWrapper()
     stopConsumingRTDEData();
   }
 }
-void ExampleRobotWrapper::initializeRobotWithDashboard()
+
+bool ExampleRobotWrapper::initializeRobotWithDashboard()
 {
   // // Stop program, if there is one running
   if (!dashboard_client_->commandStop())
   {
     URCL_LOG_ERROR("Could not send stop program command");
+    return false;
   }
 
   // Power it off
   if (!dashboard_client_->commandPowerOff())
   {
     URCL_LOG_ERROR("Could not send Power off command");
+    return false;
   }
 
   // Power it on
   if (!dashboard_client_->commandPowerOn())
   {
     URCL_LOG_ERROR("Could not send Power on command");
+    return false;
   }
 
   // Release the brakes
   if (!dashboard_client_->commandBrakeRelease())
   {
     URCL_LOG_ERROR("Could not send BrakeRelease command");
+    return false;
   }
 
   // Now the robot is ready to receive a program
   URCL_LOG_INFO("Robot ready to start a program");
+  robot_initialized_ = true;
+  return true;
 }
 
 void ExampleRobotWrapper::handleRobotProgramState(bool program_running)
@@ -210,4 +225,19 @@ bool ExampleRobotWrapper::resendRobotProgram()
   return startRobotProgram(autostart_program_);
 }
 
+bool ExampleRobotWrapper::isHealthy() const
+{
+  if (!robot_initialized_)
+  {
+    URCL_LOG_ERROR("Robot is not initialized");
+    return false;
+  }
+  if (!program_running_)
+  {
+    URCL_LOG_ERROR("Robot program is not running");
+    return false;
+  }
+  return true;
+}
+
 }  // namespace urcl

tests/test_spline_interpolation.cpp

@@ -133,12 +133,7 @@ class SplineInterpolationTest : public ::testing::Test
       g_my_robot->dashboard_client_->commandCloseSafetyPopup();
       ASSERT_TRUE(g_my_robot->dashboard_client_->commandUnlockProtectiveStop());
     }
-    // Make sure script is running on the robot
-    if (!g_my_robot->waitForProgramRunning())
-    {
-      g_my_robot->resendRobotProgram();
-      ASSERT_TRUE(g_my_robot->waitForProgramRunning());
-    }
+    ASSERT_TRUE(g_my_robot->isHealthy());
   }
 
   void sendIdle()