From a6a06526ac36225ddaa6e5b05385f7079693243b Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Thu, 27 Jun 2019 13:25:07 +0200
Subject: [PATCH] disable starting controllers when robot control is inactive

Since we use the external controller stopper to take down controllers when
robot control is inactive, we also do not want that users can start controllers
while robot control is deactivated. The only exception is at startup time.
---
 .../ur_rtde_driver/ros/hardware_interface.h   |  1 +
 ur_rtde_driver/src/ros/hardware_interface.cpp | 20 +++++++++++++++++--
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
index a4f0605..a8f8d58 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
@@ -133,6 +133,7 @@ protected:
   ros::Publisher program_state_pub_;
 
   bool controller_reset_necessary_;
+  bool controllers_initialized_;
 
   std::string robot_ip_;
 };
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index 2ea6bfc..e65edf6 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -47,6 +47,7 @@ HardwareInterface::HardwareInterface()
   , position_controller_running_(false)
   , pausing_state_(PausingState::RUNNING)
   , pausing_ramp_up_increment_(0.01)
+  , controllers_initialized_(false)
 {
 }
 
@@ -301,8 +302,23 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio
 bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                                        const std::list<hardware_interface::ControllerInfo>& stop_list)
 {
-  // TODO: Implement
-  return true;
+  bool ret_val = true;
+  if (controllers_initialized_ && !isRobotProgramRunning() && !start_list.empty())
+  {
+    for (auto& controller : start_list)
+    {
+      if (!controller.claimed_resources.empty())
+      {
+        ROS_ERROR_STREAM("Robot control is currently inactive. Starting controllers that claim resources is currently "
+                         "not possible. Not starting controller '"
+                         << controller.name << "'");
+        ret_val = false;
+      }
+    }
+  }
+
+  controllers_initialized_ = true;
+  return ret_val;
 }
 
 void HardwareInterface ::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,