Dashboard service to query whether the robot is in remote control (Un…

…iversalRobots#561) * new dashboard msg to check remote control
najmehellme0 · Sep 13, 2022 · 3853a30 · 3853a30
1 parent 1eb7bf9
commit 3853a30
Show file tree

Hide file tree

Showing 5 changed files with 33 additions and 0 deletions.
diff --git a/ur_dashboard_msgs/CMakeLists.txt b/ur_dashboard_msgs/CMakeLists.txt
@@ -62,6 +62,7 @@ add_service_files(
   GetProgramState.srv
   GetRobotMode.srv
   GetSafetyMode.srv
+  IsInRemoteControl.srv
   IsProgramRunning.srv
   IsProgramSaved.srv
   Load.srv

diff --git a/ur_dashboard_msgs/srv/IsInRemoteControl.srv b/ur_dashboard_msgs/srv/IsInRemoteControl.srv
@@ -0,0 +1,4 @@
+---
+string answer
+bool in_remote_control # is the robot currently in remote control mode?
+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
@@ -736,6 +736,10 @@ Query whether there is currently a program running
 
 Query whether the current program is saved
 
+##### dashboard/is_in_remote_control ([ur_dashboard_msgs/IsInRemoteControl](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/IsInRemoteControl.html))
+
+Query whether the robot is currently in remote control mode. This is only available on e-Series models.
+
 ##### dashboard/program_state ([ur_dashboard_msgs/GetProgramState](http://docs.ros.org/api/ur_dashboard_msgs/html/srv/GetProgramState.html))
 
 Service to query the current program state

diff --git a/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.h b/ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.h
@@ -47,6 +47,7 @@
 #include <ur_dashboard_msgs/ProgramState.h>
 #include <ur_dashboard_msgs/GetRobotMode.h>
 #include <ur_dashboard_msgs/GetSafetyMode.h>
+#include <ur_dashboard_msgs/IsInRemoteControl.h>
 #include <ur_dashboard_msgs/RawRequest.h>
 
 namespace ur_driver
@@ -89,6 +90,8 @@ class DashboardClientROS
                              ur_dashboard_msgs::GetSafetyMode::Response& resp);
   bool handleRobotModeQuery(ur_dashboard_msgs::GetRobotMode::Request& req,
                             ur_dashboard_msgs::GetRobotMode::Response& resp);
+  bool handleRemoteControlQuery(ur_dashboard_msgs::IsInRemoteControl::Request& req,
+                                ur_dashboard_msgs::IsInRemoteControl::Response& resp);
 
   bool connect();
 
@@ -118,6 +121,7 @@ class DashboardClientROS
   ros::ServiceServer running_service_;
   ros::ServiceServer get_loaded_program_service_;
   ros::ServiceServer is_program_saved_service_;
+  ros::ServiceServer is_in_remote_control_service_;
   ros::ServiceServer program_state_service_;
   ros::ServiceServer polyscope_version_service_;
   ros::ServiceServer safety_mode_service_;

diff --git a/ur_robot_driver/src/dashboard_client_ros.cpp b/ur_robot_driver/src/dashboard_client_ros.cpp
@@ -120,6 +120,10 @@ DashboardClientROS::DashboardClientROS(const ros::NodeHandle& nh, const std::str
   // Query whether the current program is saved
   is_program_saved_service_ = nh_.advertiseService("program_saved", &DashboardClientROS::handleSavedQuery, this);
 
+  // Query whether the robot is in remote control
+  is_in_remote_control_service_ =
+      nh_.advertiseService("is_in_remote_control", &DashboardClientROS::handleRemoteControlQuery, this);
+
   // Service to show a popup on the UR Teach pendant.
   popup_service_ = nh_.advertiseService<ur_dashboard_msgs::Popup::Request, ur_dashboard_msgs::Popup::Response>(
       "popup", [&](ur_dashboard_msgs::Popup::Request& req, ur_dashboard_msgs::Popup::Response& resp) {
@@ -221,6 +225,22 @@ bool DashboardClientROS::handleSavedQuery(ur_dashboard_msgs::IsProgramSaved::Req
   return true;
 }
 
+bool DashboardClientROS::handleRemoteControlQuery(ur_dashboard_msgs::IsInRemoteControl::Request& req,
+                                                  ur_dashboard_msgs::IsInRemoteControl::Response& resp)
+{
+  resp.answer = this->client_.sendAndReceive("is in remote control\n");
+  std::regex expected("(true|false)");
+  std::smatch match;
+  resp.success = std::regex_match(resp.answer, match, expected);
+
+  if (resp.success)
+  {
+    resp.in_remote_control = (match[1] == "true");
+  }
+
+  return true;
+}
+
 bool DashboardClientROS::handleSafetyModeQuery(ur_dashboard_msgs::GetSafetyMode::Request& req,
                                                ur_dashboard_msgs::GetSafetyMode::Response& resp)
 {