Shutdown dashboard client when connected to PolyScopeX robot

Before trying to establish a connection to the dashboard server, check
the version of the connected robot. If it is a PolyScopeX robot,
shutdown the dashboard client.

Author: urfeex

ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp

@@ -51,7 +51,7 @@
 // UR client library
 #include "ur_client_library/ur/dashboard_client.h"
 #include "ur_client_library/exceptions.h"
-#include "ur_dashboard_msgs/msg/program_state.hpp"
+#include "ur_client_library/primary/primary_client.h"
 #include "ur_dashboard_msgs/srv/add_to_log.hpp"
 #include "ur_dashboard_msgs/srv/get_loaded_program.hpp"
 #include "ur_dashboard_msgs/srv/get_program_state.hpp"
@@ -120,6 +120,9 @@ class DashboardClientROS
   std::shared_ptr<rclcpp::Node> node_;
   urcl::DashboardClient client_;
 
+  urcl::comm::INotifier notifier_;
+  urcl::primary_interface::PrimaryClient primary_client_;
+
   // Commanding services
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr brake_release_service_;
   rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr clear_operational_mode_service_;

ur_robot_driver/src/dashboard_client_node.cpp

@@ -53,7 +53,14 @@ int main(int argc, char** argv)
 
   ur_robot_driver::registerUrclLogHandler("");  // Set empty tf_prefix at the moment
 
-  ur_robot_driver::DashboardClientROS client(node, robot_ip);
+  try {
+    ur_robot_driver::DashboardClientROS client(node, robot_ip);
+  } catch (const urcl::UrException& e) {
+    RCLCPP_WARN(rclcpp::get_logger("Dashboard_Client"),
+                "%s This warning is expected on a PolyScopeX robot. If you don't want to see this warning, "
+                "please don't start the dashboard client. Exiting dashboard client now.",
+                e.what());
+  }
 
   rclcpp::spin(node);
 

ur_robot_driver/src/dashboard_client_ros.cpp

@@ -37,16 +37,27 @@
  */
 //----------------------------------------------------------------------
 
-#include <ur_robot_driver/dashboard_client_ros.hpp>
+#include <ur_client_library/exceptions.h>
+#include <ur_client_library/primary/primary_client.h>
 
 #include <string>
 
+#include <ur_robot_driver/dashboard_client_ros.hpp>
+
 namespace ur_robot_driver
 {
 DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, const std::string& robot_ip)
-  : node_(node), client_(robot_ip)
+  : node_(node), client_(robot_ip), primary_client_(robot_ip, notifier_)
 {
   node_->declare_parameter<double>("receive_timeout", 1);
+
+  primary_client_.start(10, std::chrono::seconds(2));
+  auto robot_version = primary_client_.getRobotVersion();
+
+  if (robot_version->major > 5) {
+    throw(urcl::UrException("The dashboard server is only available for CB3 and e-Series robots."));
+  }
+
   connect();
 
   // Service to release the brakes. If the robot is currently powered off, it will get powered on on the fly.