Ensure backwards compatibility

This ensures backwards compatibility in multiple ways:

- When using this with a description not containing the effort
interface, it will simply not be exported. Therefore, when trying to
start an effort controller that will not work, since the interfaces
aren't available.
- When using this with a robot software version not supporting torque
control this will reject activating effort-controllers.

Author: urfeex

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -231,6 +231,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   bool initialized_;
   double system_interface_initialized_;
   std::atomic_bool async_thread_shutdown_;
+  urcl::VersionInformation version_info_;
   double get_robot_software_version_major_;
   double get_robot_software_version_minor_;
   double get_robot_software_version_bugfix_;

ur_robot_driver/src/hardware_interface.cpp

@@ -157,51 +157,55 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareComponent
   trajectory_joint_accelerations_.reserve(32768);
 
   for (const hardware_interface::ComponentInfo& joint : info_.joints) {
-    if (joint.command_interfaces.size() != 3) {
+    auto has_cmd_interface = [](const hardware_interface::ComponentInfo& joint, const std::string& interface_name) {
+      auto it = find_if(
+          joint.command_interfaces.begin(), joint.command_interfaces.end(),
+          [&interface_name](const hardware_interface::InterfaceInfo& obj) { return obj.name == interface_name; });
+      return it != joint.command_interfaces.end();
+    };
+
+    if (!has_cmd_interface(joint, hardware_interface::HW_IF_POSITION)) {
       RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"),
-                   "Joint '%s' has %zu command interfaces found. 3 expected.", joint.name.c_str(),
-                   joint.command_interfaces.size());
+                   "Joint '%s' does not contain a '%s' command interface.", joint.name.c_str(),
+                   hardware_interface::HW_IF_POSITION);
       return hardware_interface::CallbackReturn::ERROR;
     }
 
-    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
+    if (!has_cmd_interface(joint, hardware_interface::HW_IF_VELOCITY)) {
       RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"),
-                   "Joint '%s' have %s command interfaces found as first command interface. '%s' expected.",
-                   joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+                   "Joint '%s' does not contain a '%s' command interface.", joint.name.c_str(),
+                   hardware_interface::HW_IF_VELOCITY);
       return hardware_interface::CallbackReturn::ERROR;
     }
 
-    if (joint.command_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
-      RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"),
-                   "Joint '%s' have %s command interfaces found as second command interface. '%s' expected.",
-                   joint.name.c_str(), joint.command_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
-      return hardware_interface::CallbackReturn::ERROR;
-    }
+    // We treat the torque command interface as optional here for now. This should ensure backwards
+    // compatibility with older descriptions.
 
-    if (joint.state_interfaces.size() != 3) {
-      RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), "Joint '%s' has %zu state interface. 3 expected.",
-                   joint.name.c_str(), joint.state_interfaces.size());
-      return hardware_interface::CallbackReturn::ERROR;
-    }
+    auto has_state_interface = [](const hardware_interface::ComponentInfo& joint, const std::string& interface_name) {
+      auto it = find_if(
+          joint.state_interfaces.begin(), joint.state_interfaces.end(),
+          [&interface_name](const hardware_interface::InterfaceInfo& obj) { return obj.name == interface_name; });
+      return it != joint.state_interfaces.end();
+    };
 
-    if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
+    if (!has_state_interface(joint, hardware_interface::HW_IF_POSITION)) {
       RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"),
-                   "Joint '%s' have %s state interface as first state interface. '%s' expected.", joint.name.c_str(),
-                   joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+                   "Joint '%s' does not contain a '%s' state interface.", joint.name.c_str(),
+                   hardware_interface::HW_IF_POSITION);
       return hardware_interface::CallbackReturn::ERROR;
     }
 
-    if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
+    if (!has_state_interface(joint, hardware_interface::HW_IF_VELOCITY)) {
       RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"),
-                   "Joint '%s' have %s state interface as second state interface. '%s' expected.", joint.name.c_str(),
-                   joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
+                   "Joint '%s' does not contain a '%s' state interface.", joint.name.c_str(),
+                   hardware_interface::HW_IF_VELOCITY);
       return hardware_interface::CallbackReturn::ERROR;
     }
 
-    if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) {
+    if (!has_state_interface(joint, hardware_interface::HW_IF_EFFORT)) {
       RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"),
-                   "Joint '%s' have %s state interface as third state interface. '%s' expected.", joint.name.c_str(),
-                   joint.state_interfaces[2].name.c_str(), hardware_interface::HW_IF_EFFORT);
+                   "Joint '%s' does not contain a '%s' state interface.", joint.name.c_str(),
+                   hardware_interface::HW_IF_EFFORT);
       return hardware_interface::CallbackReturn::ERROR;
     }
   }
@@ -335,6 +339,12 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
 
 std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::export_command_interfaces()
 {
+  auto has_cmd_interface = [](const hardware_interface::ComponentInfo& joint, const std::string& interface_name) {
+    auto it =
+        find_if(joint.command_interfaces.begin(), joint.command_interfaces.end(),
+                [&interface_name](const hardware_interface::InterfaceInfo& obj) { return obj.name == interface_name; });
+    return it != joint.command_interfaces.end();
+  };
   std::vector<hardware_interface::CommandInterface> command_interfaces;
   for (size_t i = 0; i < info_.joints.size(); ++i) {
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
@@ -343,8 +353,10 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
         info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &urcl_velocity_commands_[i]));
 
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &urcl_torque_commands_[i]));
+    if (has_cmd_interface(info_.joints[i], hardware_interface::HW_IF_EFFORT)) {
+      command_interfaces.emplace_back(hardware_interface::CommandInterface(
+          info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &urcl_torque_commands_[i]));
+    }
   }
   // Obtain the tf_prefix from the urdf so that we can have the general interface multiple times
   // NOTE using the tf_prefix at this point is some kind of workaround. One should actually go through the list of gpio
@@ -646,11 +658,11 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou
   }
 
   // Export version information to state interfaces
-  urcl::VersionInformation version_info = ur_driver_->getVersion();
-  get_robot_software_version_major_ = version_info.major;
-  get_robot_software_version_minor_ = version_info.minor;
-  get_robot_software_version_build_ = version_info.build;
-  get_robot_software_version_bugfix_ = version_info.bugfix;
+  version_info_ = ur_driver_->getVersion();
+  get_robot_software_version_major_ = version_info_.major;
+  get_robot_software_version_minor_ = version_info_.minor;
+  get_robot_software_version_build_ = version_info_.build;
+  get_robot_software_version_bugfix_ = version_info_.bugfix;
 
   async_thread_ = std::make_shared<std::thread>(&URPositionHardwareInterface::asyncThread, this);
 
@@ -1200,6 +1212,19 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
     }
   }
 
+  // effort control is only available from 5.23.0 / 10.10.0
+  if (std::any_of(start_modes_.begin(), start_modes_.end(), [&](const std::vector<std::string>& modes) {
+        return std::any_of(modes.begin(), modes.end(),
+                           [&](const std::string& mode) { return mode == hardware_interface::HW_IF_EFFORT; });
+      })) {
+    if ((version_info_.major == 5 && version_info_.minor < 23) ||
+        (version_info_.major == 10 && version_info_.minor < 10) || version_info_.major < 5) {
+      RCLCPP_ERROR(get_logger(), "Requested to use effort interface on a robot version that doesn't support it. Torque "
+                                 "control is available from robot software 5.23.0 / 10.10.0 on.");
+      return hardware_interface::return_type::ERROR;
+    }
+  }
+
   if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(),
                    [&](const std::vector<std::string>& other) { return other == start_modes_[0]; })) {
     RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same.");