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/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_;
 };

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
+  bool ret_val = true;
-  return 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,