Use ExampleRobotWrapper in test_ur_driver

Author: urfeex

include/ur_client_library/example_robot_wrapper.h

@@ -23,6 +23,7 @@ class ExampleRobotWrapper
   void startRTDECommununication(const bool consume_data = false);
   void startConsumingRTDEData();
   void stopConsumingRTDEData();
+  bool readDataPackage(std::unique_ptr<rtde_interface::DataPackage>& data_pkg);
 
   bool waitForProgramRunning(int milliseconds = 100);
   bool waitForProgramNotRunning(int milliseconds = 100);

src/example_robot_wrapper.cpp

@@ -101,6 +101,23 @@ void ExampleRobotWrapper::stopConsumingRTDEData()
   rtde_consumer_thread_.join();
 }
 
+bool ExampleRobotWrapper::readDataPackage(std::unique_ptr<rtde_interface::DataPackage>& data_pkg)
+{
+  if (consume_rtde_packages_ == true)
+  {
+    URCL_LOG_ERROR("Unable to read packages while consuming, this should not happen!");
+    return false;
+  }
+  std::lock_guard<std::mutex> lk(read_package_mutex_);
+  data_pkg = ur_driver_->getDataPackage();
+  if (data_pkg == nullptr)
+  {
+    URCL_LOG_ERROR("Timed out waiting for a new package from the robot");
+    return false;
+  }
+  return true;
+}
+
 bool ExampleRobotWrapper::waitForProgramRunning(int milliseconds)
 {
   std::unique_lock<std::mutex> lk(program_running_mutex_);