added a forward torque command controller

lbr_ros2_control/CMakeLists.txt

@@ -37,6 +37,7 @@ add_library(
   SHARED
   src/lbr_estimated_ft_broadcaster.cpp
   src/lbr_forward_position_command_controller.cpp
+  src/lbr_forward_torque_command_controller.cpp
   src/lbr_state_broadcaster.cpp
   src/lbr_system_interface.cpp
 )

lbr_ros2_control/include/lbr_ros2_control/lbr_forward_torque_command_controller.hpp

@@ -0,0 +1,60 @@
+#ifndef LBR_ROS2_CONTROL__LBR_FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_
+#define LBR_ROS2_CONTROL__LBR_FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_
+
+#include <array>
+#include <functional>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/realtime_buffer.h"
+
+#include "friLBRState.h"
+
+#include "lbr_fri_msgs/msg/lbr_torque_command.hpp"
+
+namespace lbr_ros2_control {
+class LBRForwardTorqueCommandController : public controller_interface::ControllerInterface {
+public:
+  LBRForwardTorqueCommandController();
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::return_type update(const rclcpp::Time &time,
+                                           const rclcpp::Duration &period) override;
+
+  controller_interface::CallbackReturn
+  on_configure(const rclcpp_lifecycle::State &previous_state) override;
+
+  controller_interface::CallbackReturn
+  on_activate(const rclcpp_lifecycle::State &previous_state) override;
+
+  controller_interface::CallbackReturn
+  on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+
+protected:
+  bool reference_command_interfaces_();
+  void clear_command_interfaces_();
+
+  std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS> joint_names_ = {
+      "A1", "A2", "A3", "A4", "A5", "A6", "A7"};
+
+  std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+      joint_position_command_interfaces_, torque_command_interfaces_;
+
+  realtime_tools::RealtimeBuffer<lbr_fri_msgs::msg::LBRTorqueCommand::SharedPtr>
+      rt_lbr_torque_command_ptr_;
+  rclcpp::Subscription<lbr_fri_msgs::msg::LBRTorqueCommand>::SharedPtr
+      lbr_torque_command_subscription_ptr_;
+};
+} // end of namespace lbr_ros2_control
+#endif // LBR_ROS2_CONTROL__LBR_FORWARD_TORQUE_COMMAND_CONTROLLER_HPP_

lbr_ros2_control/src/lbr_forward_torque_command_controller.cpp

@@ -0,0 +1,111 @@
+#include "lbr_ros2_control/lbr_forward_torque_command_controller.hpp"
+
+namespace lbr_ros2_control {
+LBRForwardTorqueCommandController::LBRForwardTorqueCommandController()
+    : rt_lbr_torque_command_ptr_(nullptr), lbr_torque_command_subscription_ptr_(nullptr) {}
+
+controller_interface::InterfaceConfiguration
+LBRForwardTorqueCommandController::command_interface_configuration() const {
+  controller_interface::InterfaceConfiguration interface_configuration;
+  interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto &joint_name : joint_names_) {
+    interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION);
+    interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_EFFORT);
+  }
+  return interface_configuration;
+}
+
+controller_interface::InterfaceConfiguration
+LBRForwardTorqueCommandController::state_interface_configuration() const {
+  return controller_interface::InterfaceConfiguration{
+      controller_interface::interface_configuration_type::NONE};
+}
+
+controller_interface::CallbackReturn LBRForwardTorqueCommandController::on_init() {
+  try {
+    lbr_torque_command_subscription_ptr_ =
+        this->get_node()->create_subscription<lbr_fri_msgs::msg::LBRTorqueCommand>(
+            "command/torque", rclcpp::SystemDefaultsQoS(),
+            [this](const lbr_fri_msgs::msg::LBRTorqueCommand::SharedPtr msg) {
+              rt_lbr_torque_command_ptr_.writeFromNonRT(msg);
+            });
+  } catch (const std::exception &e) {
+    RCLCPP_ERROR(this->get_node()->get_logger(),
+                 "Failed to initialize LBR forward torque command controller with: %s.", e.what());
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type
+LBRForwardTorqueCommandController::update(const rclcpp::Time & /*time*/,
+                                          const rclcpp::Duration & /*period*/) {
+  auto lbr_torque_command = rt_lbr_torque_command_ptr_.readFromRT();
+  if (!lbr_torque_command || !(*lbr_torque_command)) {
+    return controller_interface::return_type::OK;
+  }
+  for (std::size_t idx = 0; idx < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++idx) {
+    joint_position_command_interfaces_[idx].get().set_value(
+        (*lbr_torque_command)->joint_position[idx]);
+    torque_command_interfaces_[idx].get().set_value((*lbr_torque_command)->torque[idx]);
+  }
+  return controller_interface::return_type::OK;
+}
+
+controller_interface::CallbackReturn LBRForwardTorqueCommandController::on_configure(
+    const rclcpp_lifecycle::State & /*previous_state*/) {
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+LBRForwardTorqueCommandController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
+  if (!reference_command_interfaces_()) {
+    return controller_interface::CallbackReturn::ERROR;
+  }
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn LBRForwardTorqueCommandController::on_deactivate(
+    const rclcpp_lifecycle::State & /*previous_state*/) {
+  clear_command_interfaces_();
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+bool LBRForwardTorqueCommandController::reference_command_interfaces_() {
+  for (auto &command_interface : command_interfaces_) {
+    if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) {
+      joint_position_command_interfaces_.emplace_back(std::ref(command_interface));
+    }
+    if (command_interface.get_interface_name() == hardware_interface::HW_IF_EFFORT) {
+      torque_command_interfaces_.emplace_back(std::ref(command_interface));
+    }
+  }
+  if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+    RCLCPP_ERROR(
+        this->get_node()->get_logger(),
+        "Number of joint position command interfaces (%ld) does not match the number of joints "
+        "in the robot (%d).",
+        joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+    return false;
+  }
+  if (torque_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) {
+    RCLCPP_ERROR(this->get_node()->get_logger(),
+                 "Number of torque command interfaces (%ld) does not match the number of joints "
+                 "in the robot (%d).",
+                 torque_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
+    return false;
+  }
+  return true;
+}
+
+void LBRForwardTorqueCommandController::clear_command_interfaces_() {
+  joint_position_command_interfaces_.clear();
+  torque_command_interfaces_.clear();
+}
+} // end of namespace lbr_ros2_control
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::LBRForwardTorqueCommandController,
+                       controller_interface::ControllerInterface)