Added 'is in remote control' call as a dashboard service (#1433)

(cherry picked from commit 868f240)

Author: urmahp

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

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?

ur_robot_driver/doc/dashboard_client.rst

@@ -133,6 +133,11 @@ unlock_protective_stop (`std_srvs/Trigger <http://docs.ros.org/en/rolling/p/std_
 
 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
 ----------
 

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<rclcpp::Node> node_;
@@ -151,6 +155,7 @@ class DashboardClientROS
   rclcpp::Service<ur_dashboard_msgs::srv::GetProgramState>::SharedPtr program_state_service_;
   rclcpp::Service<ur_dashboard_msgs::srv::GetSafetyMode>::SharedPtr safety_mode_service_;
   rclcpp::Service<ur_dashboard_msgs::srv::GetRobotMode>::SharedPtr robot_mode_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::IsInRemoteControl>::SharedPtr is_in_remote_control_service_;
 };
 }  // namespace ur_robot_driver
 

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<ur_dashboard_msgs::srv::IsInRemoteControl>(
+      "~/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