Added Chained controller #1

chaining_controller/include/chaining_controller/chained_controller.hpp

@@ -0,0 +1,56 @@
+#ifndef CHAINED_CONTROLLER_HPP
+#define CHAINED_CONTROLLER_HPP
+
+#include <controller_interface/chainable_controller_interface.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+#include "chaining_controller/visibility_control.h"
+#include "pluginlib/class_list_macros.hpp"
+
+namespace chaining_controller
+{
+
+class ChainedController : public controller_interface::ChainableControllerInterface
+{
+public:
+  CHAINING_CONTROLLER_PUBLIC
+  controller_interface::CallbackReturn on_init() override;
+
+  CHAINING_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  CHAINING_CONTROLLER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+protected:
+  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+  bool on_set_chained_mode(bool chained_mode) override;
+
+  controller_interface::return_type update_reference_from_subscribers() override;
+
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  bool update();
+
+private:
+  double EFFORT_MAX = 15;
+  double EFFORT_MIN = 5;
+
+  void registerJointCommand(const std::vector<double> & effort);
+
+  std::vector<double> joint_effort_command_;
+
+  int n_joints_;
+  std::vector<std::string> joint_names_;
+};
+
+template <typename T>
+T clip(const T & n, const T & lower, const T & upper)
+{
+  return std::max(lower, std::min(n, upper));
+}
+
+}  // namespace chaining_controller
+
+#endif

chaining_controller/src/chained_controller.cpp

@@ -0,0 +1,123 @@
+#include "chaining_controller/chained_controller.hpp"
+#include "chained_controller.hpp"
+
+controller_interface::CallbackReturn chaining_controller::ChainedController::on_init()
+{
+  std::string param_name = "joints";
+
+  if (!get_node()->get_parameter(param_name, joint_names_))
+  {
+    RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Failed to get parameter: " << param_name);
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  n_joints_ = joint_names_.size();
+
+  if (n_joints_ == 0)
+  {
+    RCLCPP_ERROR_STREAM(get_node()->get_logger(), "List of joint names is empty.");
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  reference_interfaces_.resize(n_joints_, std::numeric_limits<double>::quiet_NaN());
+  joint_effort_command_.resize(n_joints_, std::numeric_limits<double>::quiet_NaN());
+
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration
+chaining_controller::ChainedController::command_interface_configuration() const
+{
+  RCLCPP_INFO(
+    get_node()->get_logger(),
+    "Command Interface ChainedController. Command Interface Required: Effort");
+
+  controller_interface::InterfaceConfiguration command_interfaces_config;
+  command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  std::for_each(
+    joint_names_.begin(), joint_names_.end(),
+    [&command_interfaces_config](auto & joint_name)
+    {
+      command_interfaces_config.names.push_back(
+        joint_name + "/" + hardware_interface::HW_IF_EFFORT);
+    });
+
+  return command_interfaces_config;
+}
+
+controller_interface::InterfaceConfiguration
+chaining_controller::ChainedController::state_interface_configuration() const
+{
+  RCLCPP_INFO(get_node()->get_logger(), "State Interface ChainedController.");
+
+  controller_interface::InterfaceConfiguration state_interfaces_config;
+  state_interfaces_config.type = controller_interface::interface_configuration_type::NONE;
+
+  return state_interfaces_config;
+}
+
+std::vector<hardware_interface::CommandInterface>
+chaining_controller::ChainedController::on_export_reference_interfaces()
+{
+  RCLCPP_INFO(get_node()->get_logger(), "export_reference_interfaces");
+
+  std::vector<hardware_interface::CommandInterface> reference_interfaces;
+
+  std::string reference_interface_name = "effort";
+
+  for (size_t i = 0; i < n_joints_; i++)
+  {
+    reference_interfaces.push_back(hardware_interface::CommandInterface(
+      std::string(get_node()->get_name()), reference_interface_name, &reference_interfaces_[i]));
+  }
+
+  return reference_interfaces;
+}
+
+bool chaining_controller::ChainedController::on_set_chained_mode(bool chained_mode) { return true; }
+
+controller_interface::return_type
+chaining_controller::ChainedController::update_reference_from_subscribers()
+{
+  return update() ? controller_interface::return_type::OK
+                  : controller_interface::return_type::ERROR;
+}
+
+controller_interface::return_type chaining_controller::ChainedController::update_and_write_commands(
+  const rclcpp::Time & time, const rclcpp::Duration & period)
+{
+  return update() ? controller_interface::return_type::OK
+                  : controller_interface::return_type::ERROR;
+}
+
+bool chaining_controller::ChainedController::update()
+{
+  for (size_t i = 0; i < n_joints_; i++)
+  {
+    reference_interfaces_[i] = clip(reference_interfaces_[i], EFFORT_MIN, EFFORT_MAX);
+  }
+  registerJointCommand(reference_interfaces_);
+  return true;
+}
+void chaining_controller::ChainedController::registerJointCommand(
+  const std::vector<double> & effort)
+{
+  for (int i = 0; i < n_joints_; i++)
+  {
+    std::string joint_name(joint_names_[i]);
+
+    auto effort_command = std::find_if(
+      command_interfaces_.begin(), command_interfaces_.end(),
+      [&joint_name](const hardware_interface::LoanedCommandInterface & interface)
+      {
+        return interface.get_prefix_name() == joint_name &&
+               interface.get_interface_name() == hardware_interface::HW_IF_EFFORT;
+      });
+
+    effort_command->set_value(effort[i]);
+  }
+}
+
+PLUGINLIB_EXPORT_CLASS(
+  chaining_controller::ChainedController, controller_interface::ChainableControllerInterface)