Support PolyScopeX robots (#1318)

* 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.

* Make robot_state_helper use primary_client whenever possible

(cherry picked from commit 5162bf4)

# Conflicts:
#	ur_robot_driver/src/robot_state_helper.cpp

Author: urfeex

ur_robot_driver/CMakeLists.txt

@@ -113,6 +113,7 @@ ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TAR
 add_executable(robot_state_helper
   src/robot_state_helper.cpp
   src/robot_state_helper_node.cpp
+  src/urcl_log_handler.cpp
 )
 target_link_libraries(robot_state_helper ur_client_library::urcl)
 ament_target_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})

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/include/ur_robot_driver/robot_state_helper.hpp

@@ -29,7 +29,7 @@
 #ifndef UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
 #define UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
 
-#include <chrono>
+#include <string>
 #include <memory>
 
 #include "rclcpp/rclcpp.hpp"
@@ -41,6 +41,7 @@
 #include "ur_dashboard_msgs/msg/safety_mode.hpp"
 #include "ur_dashboard_msgs/msg/robot_mode.hpp"
 #include "ur_client_library/ur/datatypes.h"
+#include "ur_client_library/primary/primary_client.h"
 
 namespace ur_robot_driver
 {
@@ -90,6 +91,10 @@ class RobotStateHelper
   std::atomic<bool> program_running_;
   std::mutex goal_mutex_;
 
+  std::string robot_ip_;
+  urcl::comm::INotifier notifier_;
+  std::shared_ptr<urcl::primary_interface::PrimaryClient> primary_client_;
+
   rclcpp_action::Server<ur_dashboard_msgs::action::SetMode>::SharedPtr set_mode_as_;
 
   rclcpp::CallbackGroup::SharedPtr robot_mode_sub_cb_;
@@ -100,12 +105,7 @@ class RobotStateHelper
 
   rclcpp::CallbackGroup::SharedPtr service_cb_grp_;
 
-  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr unlock_protective_stop_srv_;
   rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr restart_safety_srv_;
-  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_on_srv_;
-  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_off_srv_;
-  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr brake_release_srv_;
-  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr stop_program_srv_;
   rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr play_program_srv_;
   rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr resend_robot_program_srv_;
 };

ur_robot_driver/launch/ur_control.launch.py

@@ -262,8 +262,10 @@ def launch_setup(context, *args, **kwargs):
         executable="robot_state_helper",
         name="ur_robot_state_helper",
         output="screen",
+        condition=UnlessCondition(use_mock_hardware),
         parameters=[
             {"headless_mode": headless_mode},
+            {"robot_ip": robot_ip},
         ],
     )
 

ur_robot_driver/src/dashboard_client_node.cpp

@@ -37,9 +37,10 @@
 
 #include "ur_robot_driver/dashboard_client_ros.hpp"
 
+#include <memory>
 #include <string>
 
-#include "rclcpp/rclcpp.hpp"
+#include <rclcpp/logging.hpp>
 #include "ur_robot_driver/urcl_log_handler.hpp"
 
 int main(int argc, char** argv)
@@ -53,7 +54,16 @@ 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);
+  std::shared_ptr<ur_robot_driver::DashboardClientROS> client;
+  try {
+    client = std::make_shared<ur_robot_driver::DashboardClientROS>(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());
+    return 0;
+  }
 
   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(10));
+  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.

ur_robot_driver/src/robot_state_helper.cpp

@@ -27,6 +27,8 @@
 // POSSIBILITY OF SUCH DAMAGE.
 
 #include <mutex>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/logging.hpp>
 #include <ur_robot_driver/robot_state_helper.hpp>
 
 #include "rclcpp/rclcpp.hpp"
@@ -66,6 +68,7 @@ RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node)
   node->declare_parameter("headless_mode", false);
   headless_mode_ = node->get_parameter("headless_mode").as_bool();
 
+<<<<<<< HEAD
   // Service to unlock protective stop
   unlock_protective_stop_srv_ = node_->create_client<std_srvs::srv::Trigger>(
       "dashboard_client/unlock_protective_stop", rmw_qos_profile_services_default, service_cb_grp_);
@@ -88,6 +91,29 @@ RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node)
   play_program_srv_ = node_->create_client<std_srvs::srv::Trigger>("dashboard_client/play",
                                                                    rmw_qos_profile_services_default, service_cb_grp_);
   play_program_srv_->wait_for_service();
+=======
+  node->declare_parameter("robot_ip", "192.168.56.101");
+  robot_ip_ = node->get_parameter("robot_ip").as_string();
+
+  primary_client_ = std::make_shared<urcl::primary_interface::PrimaryClient>(robot_ip_, notifier_);
+
+  primary_client_->start(0, std::chrono::seconds(10));
+  auto robot_version = primary_client_->getRobotVersion();
+
+  if (robot_version->major > 5) {
+    RCLCPP_WARN(rclcpp::get_logger("robot_state_helper"), "Running on a PolyScopeX robot. The dashboard server is not "
+                                                          "available, therefore the robot_state_helper cannot start "
+                                                          "PolyScope programs and restart the safety.");
+  } else {
+    // Service to restart safety
+    restart_safety_srv_ = node_->create_client<std_srvs::srv::Trigger>(
+        "dashboard_client/restart_safety", rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
+    // Service to start UR program execution on the robot
+    play_program_srv_ = node_->create_client<std_srvs::srv::Trigger>(
+        "dashboard_client/play", rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_);
+    play_program_srv_->wait_for_service();
+  }
+>>>>>>> 5162bf4 (Support PolyScopeX robots (#1318))
 
   resend_robot_program_srv_ = node_->create_client<std_srvs::srv::Trigger>(
       "io_and_status_controller/resend_robot_program", rmw_qos_profile_services_default, service_cb_grp_);
@@ -136,7 +162,13 @@ bool RobotStateHelper::recoverFromSafety()
 {
   switch (safety_mode_) {
     case urcl::SafetyMode::PROTECTIVE_STOP:
-      return safeDashboardTrigger(this->unlock_protective_stop_srv_);
+      try {
+        primary_client_->commandUnlockProtectiveStop();
+      } catch (const urcl::UrException& e) {
+        RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), e.what());
+        return false;
+      }
+      return true;
     case urcl::SafetyMode::SYSTEM_EMERGENCY_STOP:;
     case urcl::SafetyMode::ROBOT_EMERGENCY_STOP:
       RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in safety mode."
@@ -145,7 +177,11 @@ bool RobotStateHelper::recoverFromSafety()
       return false;
     case urcl::SafetyMode::VIOLATION:;
     case urcl::SafetyMode::FAULT:
-      return safeDashboardTrigger(this->restart_safety_srv_);
+      if (restart_safety_srv_ != nullptr) {
+        return safeDashboardTrigger(this->restart_safety_srv_);
+      } else {
+        return false;
+      }
     default:
       // nothing to do
       RCLCPP_DEBUG_STREAM(rclcpp::get_logger("robot_state_helper"), "No safety recovery needed.");
@@ -155,15 +191,22 @@ bool RobotStateHelper::recoverFromSafety()
 
 bool RobotStateHelper::jumpToRobotMode(const urcl::RobotMode target_mode)
 {
-  switch (target_mode) {
-    case urcl::RobotMode::POWER_OFF:
-      return safeDashboardTrigger(this->power_off_srv_);
-    case urcl::RobotMode::IDLE:
-      return safeDashboardTrigger(this->power_on_srv_);
-    case urcl::RobotMode::RUNNING:
-      return safeDashboardTrigger(this->brake_release_srv_);
-    default:
-      RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Unreachable target robot mode.");
+  try {
+    switch (target_mode) {
+      case urcl::RobotMode::POWER_OFF:
+        primary_client_->commandPowerOff();
+        return true;
+      case urcl::RobotMode::IDLE:
+        primary_client_->commandPowerOn();
+        return true;
+      case urcl::RobotMode::RUNNING:
+        primary_client_->commandBrakeRelease();
+        return true;
+      default:
+        RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Unreachable target robot mode.");
+    }
+  } catch (const urcl::UrException& e) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), e.what());
   }
   return false;
 }
@@ -247,14 +290,13 @@ void RobotStateHelper::setModeAcceptCallback(const std::shared_ptr<RobotStateHel
 
 bool RobotStateHelper::stopProgram()
 {
-  if (safeDashboardTrigger(this->stop_program_srv_)) {
-    auto start = std::chrono::steady_clock::now();
-    while (program_running_ && std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) {
-      std::this_thread::sleep_for(std::chrono::milliseconds(2));
-      return true;
-    }
+  try {
+    primary_client_->commandStop();
+  } catch (const urcl::UrException& e) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), e.what());
+    return false;
   }
-  return false;
+  return true;
 }
 
 void RobotStateHelper::setModeExecute(const std::shared_ptr<RobotStateHelper::SetModeGoalHandle> goal_handle)
@@ -344,9 +386,14 @@ void RobotStateHelper::setModeExecute(const std::shared_ptr<RobotStateHelper::Se
       if (headless_mode_) {
         result_->success = safeDashboardTrigger(this->resend_robot_program_srv_);
       } else {
-        // The dashboard denies playing immediately after switching the mode to RUNNING
-        sleep(1);
-        result_->success = safeDashboardTrigger(this->play_program_srv_);
+        if (play_program_srv_ == nullptr) {
+          result_->success = false;
+          result_->message = "Play program service not available on this robot.";
+        } else {
+          // The dashboard denies playing immediately after switching the mode to RUNNING
+          sleep(1);
+          result_->success = safeDashboardTrigger(this->play_program_srv_);
+        }
       }
     }
     if (result_->success) {

ur_robot_driver/src/robot_state_helper_node.cpp

@@ -27,12 +27,19 @@
 // POSSIBILITY OF SUCH DAMAGE.
 
 #include "ur_robot_driver/robot_state_helper.hpp"
+#include "ur_robot_driver/urcl_log_handler.hpp"
 
 int main(int argc, char** argv)
 {
   rclcpp::init(argc, argv);
   rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("robot_state_helper");
-  ur_robot_driver::RobotStateHelper state_helper(node);
+  ur_robot_driver::registerUrclLogHandler("");  // Set empty tf_prefix at the moment
+  std::shared_ptr<ur_robot_driver::RobotStateHelper> robot_state_helper;
+  try {
+    robot_state_helper = std::make_shared<ur_robot_driver::RobotStateHelper>(node);
+  } catch (const urcl::UrException& e) {
+    RCLCPP_ERROR(rclcpp::get_logger("robot_state_helper"), "%s", e.what());
+  }
 
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(node);