Fixes controller switches to only act if necessary

all control communication was set to false when a switch was called. This
is not correct, as we might e.g. only start a reading controller such as
the FTS measurements.

Second, controllers were never checked for matching joints in this HW interface
which is problematic in combined-hw cases.

Author: Felix Exner

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -196,6 +196,14 @@ class HardwareInterface : public hardware_interface::RobotHW
   std::unique_ptr<UrDriver> ur_driver_;
   std::unique_ptr<DashboardClientROS> dashboard_client_;
 
+  /*!
+   * \brief Checks whether a resource list contains joints from this hardware interface
+   *
+   * True is returned as soon as one joint name from claimed_resources matches a joint from this
+   * hardware interface.
+   */
+  bool checkControllerClaims(const std::set<std::string>& claimed_resources);
+
   ros::ServiceServer deactivate_srv_;
   ros::ServiceServer tare_sensor_srv_;
 

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -526,27 +526,53 @@ bool HardwareInterface::prepareSwitch(const std::list<hardware_interface::Contro
 void HardwareInterface::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                                  const std::list<hardware_interface::ControllerInfo>& stop_list)
 {
-  position_controller_running_ = false;
-  velocity_controller_running_ = false;
-  for (auto& controller_it : start_list)
+  for (auto& controller_it : stop_list)
   {
     for (auto& resource_it : controller_it.claimed_resources)
     {
-      if (resource_it.hardware_interface == "ur_controllers::ScaledPositionJointInterface")
-      {
-        position_controller_running_ = true;
-      }
-      if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
-      {
-        position_controller_running_ = true;
-      }
-      if (resource_it.hardware_interface == "ur_controllers::ScaledVelocityJointInterface")
+      if (checkControllerClaims(resource_it.resources))
       {
-        velocity_controller_running_ = true;
+        if (resource_it.hardware_interface == "ur_controllers::ScaledPositionJointInterface")
+        {
+          position_controller_running_ = false;
+        }
+        if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
+        {
+          position_controller_running_ = false;
+        }
+        if (resource_it.hardware_interface == "ur_controllers::ScaledVelocityJointInterface")
+        {
+          velocity_controller_running_ = false;
+        }
+        if (resource_it.hardware_interface == "hardware_interface::VelocityJointInterface")
+        {
+          velocity_controller_running_ = false;
+        }
       }
-      if (resource_it.hardware_interface == "hardware_interface::VelocityJointInterface")
+    }
+  }
+  for (auto& controller_it : start_list)
+  {
+    for (auto& resource_it : controller_it.claimed_resources)
+    {
+      if (checkControllerClaims(resource_it.resources))
       {
-        velocity_controller_running_ = true;
+        if (resource_it.hardware_interface == "ur_controllers::ScaledPositionJointInterface")
+        {
+          position_controller_running_ = true;
+        }
+        if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
+        {
+          position_controller_running_ = true;
+        }
+        if (resource_it.hardware_interface == "ur_controllers::ScaledVelocityJointInterface")
+        {
+          velocity_controller_running_ = true;
+        }
+        if (resource_it.hardware_interface == "hardware_interface::VelocityJointInterface")
+        {
+          velocity_controller_running_ = true;
+        }
       }
     }
   }
@@ -830,6 +856,21 @@ void HardwareInterface::publishRobotAndSafetyMode()
     }
   }
 }
+
+bool HardwareInterface::checkControllerClaims(const std::set<std::string>& claimed_resources)
+{
+  for (const std::string& it : joint_names_)
+  {
+    for (const std::string& jt : claimed_resources)
+    {
+      if (it == jt)
+      {
+        return true;
+      }
+    }
+  }
+  return false;
+}
 }  // namespace ur_driver
 
 PLUGINLIB_EXPORT_CLASS(ur_driver::HardwareInterface, hardware_interface::RobotHW)