Reduce usage of dashboard client in tests and examples (#296)

As we now have more capabilities in the primary interface, we don't need
to rely on the dashboard client to initialize the robot in the example
robot wrapper.

Author: urfeex

include/ur_client_library/example_robot_wrapper.h

@@ -85,6 +85,8 @@ class ExampleRobotWrapper
    */
   bool initializeRobotWithDashboard();
 
+  bool initializeRobotWithPrimaryClient();
+
   /**
    * @brief Starts RTDE communication with the robot.
    *
@@ -167,12 +169,20 @@ class ExampleRobotWrapper
 
   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 */
+  //! Dashboard client to interact with the robot
+  std::shared_ptr<urcl::DashboardClient> dashboard_client_;
+
+  //! primary client to interact with the robot
+  std::shared_ptr<urcl::primary_interface::PrimaryClient> primary_client_;
+
+  //! UR driver to interact with the robot
+  std::shared_ptr<urcl::UrDriver> ur_driver_;
 
 private:
   void handleRobotProgramState(bool program_running);
 
+  comm::INotifier notifier_;
+
   std::atomic<bool> rtde_communication_started_ = false;
   std::atomic<bool> consume_rtde_packages_ = false;
   std::mutex read_package_mutex_;

include/ur_client_library/primary/primary_client.h

@@ -171,6 +171,23 @@ class PrimaryClient
     return static_cast<RobotMode>(consumer_->getRobotModeData()->robot_mode_);
   }
 
+  /*!
+   * \brief Get the robot's software version as Major.Minor.Bugfix
+   *
+   * This function by default blocks until a VersionMessage has been received and returns that
+   * version information. If there is an older version message that has been received, this is
+   * returned directly.
+   *
+   * \param blocking If true, the function will block until there is a valid version information
+   * received by the client or the timeout passed by.
+   * \param timeout The maximum time to wait for a valid version message.
+   *
+   * \throws urcl::TimeoutException if no message was received until the given timeout passed by.
+   *
+   */
+  std::shared_ptr<VersionInformation>
+  getRobotVersion(bool wait_for_message = true, const std::chrono::milliseconds timeout = std::chrono::seconds(2));
+
   /*!
    * \brief Get the latest robot mode data.
    *

include/ur_client_library/primary/primary_consumer.h

@@ -31,6 +31,7 @@
 #include "ur_client_library/primary/abstract_primary_consumer.h"
 #include "ur_client_library/primary/robot_state/robot_mode_data.h"
 #include "ur_client_library/ur/datatypes.h"
+#include "ur_client_library/ur/version_information.h"
 
 #include <functional>
 #include <mutex>
@@ -87,6 +88,12 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
    */
   virtual bool consume(VersionMessage& pkg) override
   {
+    std::scoped_lock lock(version_information_mutex_);
+    version_information_ = std::make_shared<VersionInformation>();
+    version_information_->major = pkg.major_version_;
+    version_information_->minor = pkg.minor_version_;
+    version_information_->bugfix = pkg.svn_version_;
+    version_information_->build = pkg.build_number_;
     return true;
   }
 
@@ -196,11 +203,26 @@ class PrimaryConsumer : public AbstractPrimaryConsumer
     return robot_mode_;
   }
 
+  /*!
+   * \brief Get the latest version information.
+   *
+   * The version information will be updated in the background. This will always show the latest
+   * received version information independent of the time that has passed since receiving it. If no
+   * version information has been received yet, this will return a nullptr.
+   */
+  std::shared_ptr<VersionInformation> getVersionInformation()
+  {
+    std::scoped_lock lock(version_information_mutex_);
+    return version_information_;
+  }
+
 private:
   std::function<void(ErrorCode&)> error_code_message_callback_;
   std::shared_ptr<KinematicsInfo> kinematics_info_;
   std::mutex robot_mode_mutex_;
   std::shared_ptr<RobotModeData> robot_mode_;
+  std::mutex version_information_mutex_;
+  std::shared_ptr<VersionInformation> version_information_;
 };
 
 }  // namespace primary_interface

include/ur_client_library/ur/ur_driver.h

@@ -890,6 +890,11 @@ class UrDriver
     return trajectory_interface_->isConnected();
   }
 
+  std::shared_ptr<urcl::primary_interface::PrimaryClient> getPrimaryClient()
+  {
+    return primary_client_;
+  }
+
 private:
   void init(const UrDriverConfiguration& config);
 
@@ -898,7 +903,7 @@ class UrDriver
 
   comm::INotifier notifier_;
   std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
-  std::unique_ptr<urcl::primary_interface::PrimaryClient> primary_client_;
+  std::shared_ptr<urcl::primary_interface::PrimaryClient> primary_client_;
   std::unique_ptr<control::ReverseInterface> reverse_interface_;
   std::unique_ptr<control::TrajectoryPointInterface> trajectory_interface_;
   std::unique_ptr<control::ScriptCommandInterface> script_command_interface_;

src/example_robot_wrapper.cpp

@@ -32,6 +32,7 @@
 #include <iostream>
 #include "ur_client_library/exceptions.h"
 #include "ur_client_library/log.h"
+#include "ur_client_library/ur/version_information.h"
 
 namespace urcl
 {
@@ -40,24 +41,31 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std:
                                          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);
+  primary_client_ = std::make_shared<urcl::primary_interface::PrimaryClient>(robot_ip, notifier_);
 
-  // Connect the robot Dashboard
-  if (!dashboard_client_->connect())
+  primary_client_->start();
+
+  auto robot_version = primary_client_->getRobotVersion();
+  if (*robot_version < VersionInformation::fromString("10.0.0"))
   {
-    URCL_LOG_ERROR("Could not connect to dashboard");
-  }
+    dashboard_client_ = std::make_shared<DashboardClient>(robot_ip);
+    // Connect the robot Dashboard
+    if (!dashboard_client_->connect())
+    {
+      URCL_LOG_ERROR("Could not connect to dashboard");
+    }
 
-  // In CI we the dashboard client times out for no obvious reason. Hence we increase the timeout
-  // here.
-  timeval tv;
-  tv.tv_sec = 10;
-  tv.tv_usec = 0;
-  dashboard_client_->setReceiveTimeout(tv);
+    // In CI we the dashboard client times out for no obvious reason. Hence we increase the timeout
+    // here.
+    timeval tv;
+    tv.tv_sec = 10;
+    tv.tv_usec = 0;
+    dashboard_client_->setReceiveTimeout(tv);
+  }
 
-  if (!initializeRobotWithDashboard())
+  if (!initializeRobotWithPrimaryClient())
   {
-    throw UrException("Could not initialize robot with dashboard");
+    throw UrException("Could not initialize robot with primary client");
   }
 
   UrDriverConfiguration driver_config;
@@ -75,7 +83,7 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std:
     startRobotProgram(autostart_program);
   }
 
-  if (headless_mode | !std::empty(autostart_program))
+  if (headless_mode || !std::empty(autostart_program))
   {
     if (!waitForProgramRunning(500))
     {
@@ -94,20 +102,28 @@ 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)
+  if (primary_client_->isRobotProtectiveStopped())
   {
     URCL_LOG_INFO("Robot is in protective stop, trying to release it");
-    dashboard_client_->commandClosePopup();
-    dashboard_client_->commandCloseSafetyPopup();
-    if (!dashboard_client_->commandUnlockProtectiveStop())
+    if (dashboard_client_ != nullptr)
+    {
+      dashboard_client_->commandClosePopup();
+      dashboard_client_->commandCloseSafetyPopup();
+    }
+    try
+    {
+      primary_client_->commandUnlockProtectiveStop();
+    }
+    catch (const TimeoutException&)
     {
       std::this_thread::sleep_for(std::chrono::seconds(5));
-      if (!dashboard_client_->commandUnlockProtectiveStop())
+      try
       {
-        URCL_LOG_ERROR("Could not unlock protective stop");
+        primary_client_->commandUnlockProtectiveStop();
+      }
+      catch (const TimeoutException&)
+      {
+        URCL_LOG_ERROR("Robot could not unlock the protective stop");
         return false;
       }
     }
@@ -157,6 +173,36 @@ bool ExampleRobotWrapper::initializeRobotWithDashboard()
   return true;
 }
 
+bool ExampleRobotWrapper::initializeRobotWithPrimaryClient()
+{
+  try
+  {
+    waitFor([&]() { return primary_client_->getRobotModeData() != nullptr; }, std::chrono::seconds(5));
+    clearProtectiveStop();
+  }
+  catch (const std::exception& exc)
+  {
+    URCL_LOG_ERROR("Could not clear protective stop (%s)", exc.what());
+    return false;
+  }
+
+  try
+  {
+    primary_client_->commandStop();
+    primary_client_->commandBrakeRelease();
+  }
+  catch (const TimeoutException& exc)
+  {
+    URCL_LOG_ERROR(exc.what());
+    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)
 {
   // Print the text in green so we see it better
@@ -254,13 +300,20 @@ bool ExampleRobotWrapper::waitForProgramNotRunning(int milliseconds)
 
 bool ExampleRobotWrapper::startRobotProgram(const std::string& program_file_name)
 {
-  if (!dashboard_client_->commandLoadProgram(program_file_name))
+  if (dashboard_client_ != nullptr)
   {
-    URCL_LOG_ERROR("Could not load program '%s'", program_file_name.c_str());
-    return false;
-  }
+    if (!dashboard_client_->commandLoadProgram(program_file_name))
+    {
+      URCL_LOG_ERROR("Could not load program '%s'", program_file_name.c_str());
+      return false;
+    }
 
-  return dashboard_client_->commandPlay();
+    return dashboard_client_->commandPlay();
+  }
+  URCL_LOG_ERROR("Dashboard client is not initialized. If you are running a PolyScope X robot, the dashboard server is "
+                 "not available. Loading and running polyscope programs isn't possible. Please use the headless mode "
+                 "or the teach pendant instead.");
+  return false;
 }
 bool ExampleRobotWrapper::resendRobotProgram()
 {

src/primary/primary_client.cpp

@@ -33,6 +33,7 @@
 #include <ur_client_library/primary/robot_state.h>
 #include "ur_client_library/exceptions.h"
 #include <ur_client_library/helpers.h>
+#include <chrono>
 namespace urcl
 {
 namespace primary_interface
@@ -267,6 +268,16 @@ void PrimaryClient::commandStop(const bool validate, const std::chrono::millisec
     }
   }
 }
+std::shared_ptr<VersionInformation> PrimaryClient::getRobotVersion(bool blocking,
+                                                                   const std::chrono::milliseconds timeout)
+{
+  if (blocking)
+  {
+    waitFor([this]() { return consumer_->getVersionInformation() != nullptr; }, timeout);
+  }
+
+  return consumer_->getVersionInformation();
+}
 
 }  // namespace primary_interface
 }  // namespace urcl

tests/test_dashboard_client.cpp

@@ -32,6 +32,8 @@
 #include <ur_client_library/exceptions.h>
 #include <chrono>
 #include <thread>
+#include "gtest/gtest.h"
+#include "test_utils.h"
 #include "ur_client_library/comm/tcp_socket.h"
 #include "ur_client_library/ur/version_information.h"
 #include <ur_client_library/ur/dashboard_client.h>
@@ -61,6 +63,12 @@ class DashboardClientTest : public ::testing::Test
 protected:
   void SetUp()
   {
+    if (!robotVersionLessThan(g_ROBOT_IP, "10.0.0"))
+    {
+      GTEST_SKIP_("Running DashboardClient tests for PolyScope X is not supported as it doesn't have a dashboard "
+                  "server.");
+    }
+
     dashboard_client_.reset(new TestableDashboardClient(g_ROBOT_IP));
     // In CI we the dashboard client times out for no obvious reason. Hence we increase the timeout
     // here.
@@ -287,4 +295,4 @@ int main(int argc, char* argv[])
   }
 
   return RUN_ALL_TESTS();
-}
+}