Add waitForProgramNotRunning to wrapper

Author: urfeex

include/ur_client_library/example_robot_wrapper.h

@@ -25,6 +25,11 @@ class ExampleRobotWrapper
   void stopConsumingRTDEData();
 
   bool waitForProgramRunning(int milliseconds = 100);
+  bool waitForProgramNotRunning(int milliseconds = 100);
+
+  // Depending on whether it is headless or not start autostart_program or call driver's
+  // resendRobotProgram function.
+  bool resendRobotProgram();
 
   bool startRobotProgram(const std::string& program_file_name);
 
@@ -41,9 +46,14 @@ class ExampleRobotWrapper
 
   bool program_running_;
   std::condition_variable program_running_cv_;
+  std::condition_variable program_not_running_cv_;
   std::mutex program_running_mutex_;
+  std::mutex program_not_running_mutex_;
 
   std::thread rtde_consumer_thread_;
+
+  bool headless_mode_;
+  std::string autostart_program_;
 };
 }  // namespace urcl
 

src/example_robot_wrapper.cpp

@@ -6,6 +6,7 @@ namespace urcl
 ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std::string& output_recipe_file,
                                          const std::string& input_recipe_file, const bool headless_mode,
                                          const std::string& autostart_program, const std::string& script_file)
+  : headless_mode_(headless_mode), autostart_program_(autostart_program)
 {
   dashboard_client_ = std::make_shared<DashboardClient>(robot_ip);
 
@@ -111,6 +112,17 @@ bool ExampleRobotWrapper::waitForProgramRunning(int milliseconds)
   return false;
 }
 
+bool ExampleRobotWrapper::waitForProgramNotRunning(int milliseconds)
+{
+  std::unique_lock<std::mutex> lk(program_not_running_mutex_);
+  if (program_not_running_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout ||
+      program_running_ == false)
+  {
+    return true;
+  }
+  return false;
+}
+
 bool ExampleRobotWrapper::startRobotProgram(const std::string& program_file_name)
 {
   if (!dashboard_client_->commandLoadProgram(program_file_name))
@@ -121,5 +133,13 @@ bool ExampleRobotWrapper::startRobotProgram(const std::string& program_file_name
 
   return dashboard_client_->commandPlay();
 }
+bool ExampleRobotWrapper::resendRobotProgram()
+{
+  if (headless_mode_)
+  {
+    return ur_driver_->sendRobotProgram();
+  }
+  return startRobotProgram(autostart_program_);
+}
 
 }  // namespace urcl