Totally non functional controller but it builds

spot_controllers/CMakeLists.txt

@@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   pluginlib
   rclcpp
   rclcpp_lifecycle
+  spot_msgs
 )
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
@@ -42,6 +43,7 @@ add_library(
   spot_controllers
   SHARED
   src/forward_state_controller.cpp
+  src/spot_forward_controller.cpp
 )
 target_compile_features(spot_controllers PUBLIC cxx_std_20)
 target_include_directories(spot_controllers PUBLIC
@@ -50,7 +52,8 @@ target_include_directories(spot_controllers PUBLIC
 )
 target_link_libraries(
   spot_controllers PUBLIC 
-  forward_state_controller_parameters forward_command_controller::forward_command_controller
+  forward_state_controller_parameters
+  forward_command_controller::forward_command_controller
 )
 ament_target_dependencies(
   spot_controllers PUBLIC

spot_controllers/include/spot_controllers/spot_forward_controller.hpp

@@ -1,8 +1,4 @@
-// File modified. Modifications Copyright (c) 2024 Boston Dynamics AI Institute LLC.
-// All rights reserved.
-
-// --------------------------------------------------------------
-// Copyright 2020 PAL Robotics S.L.
+// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt)
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -22,36 +18,53 @@
 #include <string>
 #include <vector>
 
-#include "forward_command_controller/forward_command_controller/forward_controllers_base.hpp"
-#include "forward_state_controller_parameters.hpp"  // NOLINT(build/include_subdir)
+#include "controller_interface/controller_interface.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_buffer.hpp"
 #include "spot_controllers/visibility_control.h"
+#include "spot_msgs/msg/joint_command.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
 
 namespace spot_controllers {
+using CmdType = spot_msgs::msg::JointCommand;
+
 /**
- * \brief Forward command controller for a set of interfaces.
- *
- * This class forwards the command signal for a set of interfaces over a set of joints.
+ * \brief Forward command controller for a set of joints and interfaces.
  *
- * \param joints Names of the joint to control.
- * \param interface_names Names of the interfaces to command.
+ * This class forwards the command signal down to a set of joints or interfaces.
  *
  * Subscribes to:
- * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply.
+ * - \b commands (spot_msgs::msg::JointCommand) : The commands to apply.
  */
-class SpotPassthroughController : public forward_command_controller::ForwardControllersBase {
+class SpotForwardController : public controller_interface::ControllerInterface {
  public:
-  SPOT_CONTROLLERS_PUBLIC
-  SpotPassthroughController();
+  SpotForwardController();
+
+  ~SpotForwardController() = default;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() 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;
+
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
  protected:
-  void declare_parameters() override;
-  controller_interface::CallbackReturn read_parameters() override;
+  std::vector<std::string> joint_names_;
+  std::string interface_name_;
 
-  using Params = forward_state_controller::Params;
-  using ParamListener = forward_state_controller::ParamListener;
+  std::vector<std::string> command_interface_types_;
 
-  std::shared_ptr<ParamListener> param_listener_;
-  Params params_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>> rt_command_ptr_;
+  rclcpp::Subscription<CmdType>::SharedPtr joints_command_subscriber_;
 };
 
 }  // namespace spot_controllers

spot_controllers/spot_controllers.xml

@@ -4,4 +4,9 @@
     General passthrough controller that can forward commands for a set of joints over a set of interfaces.
   </description>
   </class>
+  <class name="spot_controllers/SpotForwardController" type="spot_controllers::SpotForwardController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    Forwarding controller useful for forwarding sets of joint commands (including gains) to Spot's hardware interface.
+  </description>
+  </class>
 </library>

spot_controllers/src/spot_forward_controller.cpp

@@ -1,8 +1,8 @@
-// File modified. Modifications Copyright (c) 2024 Boston Dynamics AI Institute LLC.
+// File modified. Modifications Copyright (c) 2025 Boston Dynamics AI Institute LLC.
 // All rights reserved.
 
 // --------------------------------------------------------------
-// Copyright 2020 PAL Robotics S.L.
+// Copyright 2021 Stogl Robotics Consulting UG (haftungsbescrhänkt)
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -16,52 +16,119 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "spot_controllers/spot_passthrough_controller.hpp"
+#include "spot_controllers/spot_forward_controller.hpp"
 
-#include <algorithm>
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
 
+#include "controller_interface/helpers.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/qos.hpp"
 
 namespace spot_controllers {
-SpotForwardController::SpotForwardController() : forward_command_controller::ForwardControllersBase() {}
+SpotForwardController::SpotForwardController()
+    : controller_interface::ControllerInterface(), rt_command_ptr_(nullptr), joints_command_subscriber_(nullptr) {}
 
-void SpotForwardController::declare_parameters() {
-  param_listener_ = std::make_shared<ParamListener>(get_node());
+controller_interface::CallbackReturn SpotForwardController::on_init() {
+  // try
+  // {
+  //   declare_parameters();
+  // }
+  // catch (const std::exception & e)
+  // {
+  //   fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+  //   return controller_interface::CallbackReturn::ERROR;
+  // }
+
+  return controller_interface::CallbackReturn::SUCCESS;
 }
 
-controller_interface::CallbackReturn SpotForwardController::read_parameters() {
-  if (!param_listener_) {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
-    return controller_interface::CallbackReturn::ERROR;
-  }
-  params_ = param_listener_->get_params();
+controller_interface::CallbackReturn SpotForwardController::on_configure(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  // auto ret = this->read_parameters();
+  // if (ret != controller_interface::CallbackReturn::SUCCESS)
+  // {
+  //   return ret;
+  // }
 
-  if (params_.joints.empty()) {
-    RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty");
-    return controller_interface::CallbackReturn::ERROR;
-  }
+  joints_command_subscriber_ = get_node()->create_subscription<CmdType>("~/commands", rclcpp::SystemDefaultsQoS(),
+                                                                        [this](const CmdType::SharedPtr msg) {
+                                                                          rt_command_ptr_.writeFromNonRT(msg);
+                                                                        });
 
-  if (params_.interface_names.empty()) {
-    RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty");
+  RCLCPP_INFO(get_node()->get_logger(), "configure successful");
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration SpotForwardController::command_interface_configuration() const {
+  controller_interface::InterfaceConfiguration command_interfaces_config;
+  command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  command_interfaces_config.names = command_interface_types_;
+
+  return command_interfaces_config;
+}
+
+controller_interface::InterfaceConfiguration SpotForwardController::state_interface_configuration() const {
+  return controller_interface::InterfaceConfiguration{controller_interface::interface_configuration_type::NONE};
+}
+
+controller_interface::CallbackReturn SpotForwardController::on_activate(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  //  check if we have all resources defined in the "points" parameter
+  //  also verify that we *only* have the resources defined in the "points" parameter
+  // ATTENTION(destogl): Shouldn't we use ordered interface all the time?
+  std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> ordered_interfaces;
+  if (!controller_interface::get_ordered_interfaces(command_interfaces_, command_interface_types_, std::string(""),
+                                                    ordered_interfaces) ||
+      command_interface_types_.size() != ordered_interfaces.size()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu command interfaces, got %zu", command_interface_types_.size(),
+                 ordered_interfaces.size());
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  // Example: if you input joints [1,2,3] and interfaces [A,B,C] as parameters, the order of the command will be
-  // [1/A, 1/B, 1/C, 2/A, 2/B, 2/C, 3/A, 3/B, 3/C]
-  for (const auto& interface_name : params_.interface_names) {
-    for (const auto& joint : params_.joints) {
-      command_interface_types_.push_back(joint + "/" + interface_name);
-    }
-  }
+  // reset command buffer if a command came through callback when controller was inactive
+  rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>(nullptr);
+
+  RCLCPP_INFO(get_node()->get_logger(), "activate successful");
+  return controller_interface::CallbackReturn::SUCCESS;
+}
 
+controller_interface::CallbackReturn SpotForwardController::on_deactivate(
+    const rclcpp_lifecycle::State& /*previous_state*/) {
+  // reset command buffer
+  rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>(nullptr);
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
+controller_interface::return_type SpotForwardController::update(const rclcpp::Time& /*time*/,
+                                                                const rclcpp::Duration& /*period*/) {
+  auto joint_commands = rt_command_ptr_.readFromRT();
+
+  // no command received yet
+  if (!joint_commands || !(*joint_commands)) {
+    return controller_interface::return_type::OK;
+  }
+
+  // fix this for new command type
+  // if ((*joint_commands)->data.size() != command_interfaces_.size())
+  // {
+  //   RCLCPP_ERROR_THROTTLE(
+  //     get_node()->get_logger(), *(get_node()->get_clock()), 1000,
+  //     "command size (%zu) does not match number of interfaces (%zu)",
+  //     (*joint_commands)->data.size(), command_interfaces_.size());
+  //   return controller_interface::return_type::ERROR;
+  // }
+
+  // for (auto index = 0ul; index < command_interfaces_.size(); ++index)
+  // {
+  //   command_interfaces_[index].set_value((*joint_commands)->data[index]);
+  // }
+
+  return controller_interface::return_type::OK;
+}
+
 }  // namespace spot_controllers
 
 #include "pluginlib/class_list_macros.hpp"