From 639da9458864f9fff2757f4e2fbe490e03fd12c8 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Tue, 16 Apr 2019 09:04:28 +0200
Subject: [PATCH] Only write position commands if a position controller is
 active.

Fixes #19
---
 .../ur_rtde_driver/ros/hardware_interface.h    |  2 ++
 src/ros/hardware_interface.cpp                 | 18 ++++++++++++++++--
 2 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/include/ur_rtde_driver/ros/hardware_interface.h b/include/ur_rtde_driver/ros/hardware_interface.h
index d0d9678..874cf34 100644
--- a/include/ur_rtde_driver/ros/hardware_interface.h
+++ b/include/ur_rtde_driver/ros/hardware_interface.h
@@ -65,6 +65,8 @@ protected:
   vector6d_t joint_velocities_;
   vector6d_t joint_efforts_;
   std::vector<std::string> joint_names_;
+
+  bool position_controller_running_;
 };
 
 }  // namespace ur_driver
diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp
index be0d0b6..fad3d34 100644
--- a/src/ros/hardware_interface.cpp
+++ b/src/ros/hardware_interface.cpp
@@ -35,6 +35,7 @@ HardwareInterface::HardwareInterface()
   , joint_velocities_{ 0, 0, 0, 0, 0, 0 }
   , joint_efforts_{ 0, 0, 0, 0, 0, 0 }
   , joint_names_(6)
+  , position_controller_running_(false)
 
 {
 }
@@ -101,7 +102,10 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
 
 void HardwareInterface ::write(const ros::Time& time, const ros::Duration& period)
 {
-  ur_driver_->writeJointCommand(joint_position_command_);
+  if (position_controller_running_)
+  {
+    ur_driver_->writeJointCommand(joint_position_command_);
+  }
 }
 
 bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
@@ -114,7 +118,17 @@ bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::Contr
 void HardwareInterface ::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                                   const std::list<hardware_interface::ControllerInfo>& stop_list)
 {
-  // TODO: Implement
+  position_controller_running_ = false;
+  for (auto& controller_it : start_list)
+  {
+    for (auto& resource_it : controller_it.claimed_resources)
+    {
+      if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
+      {
+        position_controller_running_ = true;
+      }
+    }
+  }
 }
 
 const uint32_t HardwareInterface ::getControlFrequency() const