diff --git a/ur_dashboard_msgs/CMakeLists.txt b/ur_dashboard_msgs/CMakeLists.txt index b04e37fd4..db943ea71 100644 --- a/ur_dashboard_msgs/CMakeLists.txt +++ b/ur_dashboard_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ set(srv_files srv/Load.srv srv/Popup.srv srv/RawRequest.srv + srv/IsInRemoteControl.srv ) set(action_files diff --git a/ur_dashboard_msgs/srv/IsInRemoteControl.srv b/ur_dashboard_msgs/srv/IsInRemoteControl.srv new file mode 100644 index 000000000..622bbd4af --- /dev/null +++ b/ur_dashboard_msgs/srv/IsInRemoteControl.srv @@ -0,0 +1,4 @@ +--- +string answer +bool remote_control # is the robot in remote control? +bool success # Did the dashboard server call succeed? diff --git a/ur_robot_driver/doc/ROS_INTERFACE.md b/ur_robot_driver/doc/ROS_INTERFACE.md index 21bb3d2bc..00c7082a4 100644 --- a/ur_robot_driver/doc/ROS_INTERFACE.md +++ b/ur_robot_driver/doc/ROS_INTERFACE.md @@ -202,6 +202,10 @@ Stop program execution on the robot Dismiss a protective stop to continue robot movements. NOTE: It is the responsibility of the user to ensure the cause of the protective stop is resolved before calling this service. +##### is_in_remote_control [ur_dashboard_msgs/IsInRemoteControl](http://docs.ros.org/en/rolling/p/ur_dashboard_msgs/srv/IsInRemoteControl.html) + +Service to query whether the robot is in remote control + #### Parameters ##### receive_timeout (Required) diff --git a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp index 6c493246f..7fb4c913f 100644 --- a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp +++ b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.hpp @@ -62,6 +62,7 @@ #include "ur_dashboard_msgs/srv/load.hpp" #include "ur_dashboard_msgs/srv/popup.hpp" #include "ur_dashboard_msgs/srv/raw_request.hpp" +#include "ur_dashboard_msgs/srv/is_in_remote_control.hpp" namespace ur_robot_driver { @@ -115,6 +116,9 @@ class DashboardClientROS bool handleRobotModeQuery(ur_dashboard_msgs::srv::GetRobotMode::Request::SharedPtr req, ur_dashboard_msgs::srv::GetRobotMode::Response::SharedPtr resp); + bool handleRemoteControlQuery(ur_dashboard_msgs::srv::IsInRemoteControl::Request::SharedPtr req, + ur_dashboard_msgs::srv::IsInRemoteControl::Response::SharedPtr resp); + bool connect(); std::shared_ptr node_; @@ -151,6 +155,7 @@ class DashboardClientROS rclcpp::Service::SharedPtr program_state_service_; rclcpp::Service::SharedPtr safety_mode_service_; rclcpp::Service::SharedPtr robot_mode_service_; + rclcpp::Service::SharedPtr is_in_remote_control_service_; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/src/dashboard_client_ros.cpp b/ur_robot_driver/src/dashboard_client_ros.cpp index 77068fdc1..3a729c8c0 100644 --- a/ur_robot_driver/src/dashboard_client_ros.cpp +++ b/ur_robot_driver/src/dashboard_client_ros.cpp @@ -269,6 +269,11 @@ DashboardClientROS::DashboardClientROS(const rclcpp::Node::SharedPtr& node, cons } return true; }); + + // Service to query whether the robot is in remote control. + is_in_remote_control_service_ = node_->create_service( + "~/is_in_remote_control", + std::bind(&DashboardClientROS::handleRemoteControlQuery, this, std::placeholders::_1, std::placeholders::_2)); } bool DashboardClientROS::connect() @@ -410,4 +415,20 @@ bool DashboardClientROS::handleRobotModeQuery(const ur_dashboard_msgs::srv::GetR } return true; } + +bool DashboardClientROS::handleRemoteControlQuery( + const ur_dashboard_msgs::srv::IsInRemoteControl::Request::SharedPtr req, + ur_dashboard_msgs::srv::IsInRemoteControl::Response::SharedPtr resp) +{ + try { + resp->remote_control = this->client_.commandIsInRemoteControl(); + resp->success = true; + } catch (const urcl::UrException& e) { + RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what()); + resp->answer = e.what(); + resp->success = false; + } + return true; +} + } // namespace ur_robot_driver