Implemented getDataPackage function

include/ur_rtde_driver/ur/ur_driver.h

@@ -25,18 +25,40 @@
  */
 //----------------------------------------------------------------------
 
-
 #include "ur_rtde_driver/rtde/rtde_client.h"
 
-namespace ur_driver{
-  class UrDriver
-  {
-  public:
-    UrDriver (const std::string& ROBOT_IP);
-    virtual ~UrDriver () = default;
+namespace ur_driver
+{
+/*!
+ * \brief This is the main class for interfacing the driver.
+ *
+ * It sets up all the necessary socket connections and handles the data exchange with the robot.
+ * Use this classes methods to access and write data.
+ */
+class UrDriver
+{
+public:
+  /*!
+   * \brief Constructs a new UrDriver object.
+   *
+   * \param ROBOT_IP IP-address under which the robot is reachable.
+   */
+  UrDriver(const std::string& ROBOT_IP);
+  virtual ~UrDriver() = default;
 
-  private:
-    comm::INotifier notifier_;
-    std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
-  };
-}
+  /*!
+   * \brief Access function to receive the latest data package sent from the robot through RTDE
+   * interface.
+   *
+   * \returns The latest data package on success, a nullptr if no package can be found inside the
+   * interface's cycle time. See the private parameter #rtde_frequency_
+   */
+  std::unique_ptr<rtde_interface::DataPackage> getDataPackage();
+
+private:
+  //! This frequency is used for the rtde interface. By default, it will be 125Hz on CB3 and 500Hz on the e-series.
+  uint32_t rtde_frequency_;
+  comm::INotifier notifier_;
+  std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
+};
+}  // namespace ur_driver

src/main_plain_driver.cpp

@@ -16,7 +16,7 @@
 
 using namespace ur_driver;
 
-int main(int argc, char *argv[])
+int main(int argc, char* argv[])
 {
   std::string ROBOT_IP = "192.168.56.101";
 
@@ -30,7 +30,11 @@ int main(int argc, char *argv[])
   while (true)
   {
     sleep(1);
-    // LOG_INFO("Still running");
+    std::unique_ptr<rtde_interface::DataPackage> data_pkg = driver.getDataPackage();
+    if (data_pkg)
+    {
+      data_pkg->toString();
+    }
   }
   return 0;
 }

src/ur/ur_driver.cpp

@@ -28,11 +28,28 @@
 #include "ur_rtde_driver/ur/ur_driver.h"
 #include <memory>
 
-namespace ur_driver{
-
-  ur_driver::UrDriver::UrDriver (const std::string& ROBOT_IP)
-  {
-    rtde_client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_));
-  }
-
+namespace ur_driver
+{
+ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP)
+{
+  rtde_client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_));
 }
+
+std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
+{
+  // TODO: This goes into the rtde_client
+  std::unique_ptr<comm::URPackage<rtde_interface::PackageHeader>> urpackage;
+  uint32_t period_ms = (1.0 / rtde_frequency_) / 1000;
+  std::chrono::milliseconds timeout(period_ms);
+  if (rtde_client_->getDataPackage(urpackage, timeout))
+  {
+    rtde_interface::DataPackage* tmp = dynamic_cast<rtde_interface::DataPackage*>(urpackage.get());
+    if (tmp != nullptr)
+    {
+      urpackage.release();
+      return std::move(std::unique_ptr<rtde_interface::DataPackage>(tmp));
+    }
+  }
+  return nullptr;
+}
+}  // namespace ur_driver