Forward State Controller

forward_state_controller/CMakeLists.txt

@@ -0,0 +1,86 @@
+cmake_minimum_required(VERSION 3.16)
+project(forward_state_controller)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  generate_parameter_library
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(
+  forward_state_controller_parameters
+  src/forward_state_controller_parameters.yaml
+)
+
+add_library(forward_state_controller SHARED
+  src/forward_state_controller.cpp
+)
+target_compile_features(forward_state_controller PUBLIC cxx_std_17)
+target_include_directories(forward_state_controller PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/forward_state_controller>
+)
+target_link_libraries(forward_state_controller PUBLIC
+                      forward_state_controller_parameters
+                      controller_interface::controller_interface
+                      hardware_interface::hardware_interface
+                      pluginlib::pluginlib
+                      rclcpp::rclcpp
+                      rclcpp_lifecycle::rclcpp_lifecycle)
+pluginlib_export_plugin_description_file(controller_interface forward_state_plugin.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+
+  ament_add_gmock(test_load_forward_state_controller
+    test/test_load_forward_state_controller.cpp
+  )
+  target_link_libraries(test_load_forward_state_controller
+    forward_state_controller
+    controller_manager::controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+  ament_add_gmock(test_forward_state_controller
+    test/test_forward_state_controller.cpp
+  )
+  target_link_libraries(test_forward_state_controller
+    forward_state_controller
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/forward_state_controller
+)
+install(
+  TARGETS
+    forward_state_controller
+    forward_state_controller_parameters
+  EXPORT export_forward_state_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  INCLUDES DESTINATION include
+)
+
+ament_export_targets(export_forward_state_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

forward_state_controller/forward_state_plugin.xml

@@ -0,0 +1,7 @@
+<library path="forward_state_controller">
+  <class name="forward_state_controller/ForwardStateController" type="forward_state_controller::ForwardStateController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    Forward state controller that forwards state interface values to command interfaces.
+  </description>
+  </class>
+</library>

forward_state_controller/include/forward_state_controller/forward_state_controller.hpp

@@ -0,0 +1,86 @@
+// Copyright 2024 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef FORWARD_STATE_CONTROLLER__FORWARD_STATE_CONTROLLER_HPP_
+#define FORWARD_STATE_CONTROLLER__FORWARD_STATE_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "forward_state_controller/forward_state_controller_parameters.hpp"
+
+namespace forward_state_controller
+{
+/**
+ * \brief Forward state controller that forwards state interfaces to command interfaces.
+ *
+ *
+ * \param state_interfaces List of state interface names to read from.
+ * \param forward_state Map from each state interface name to the list of command interfaces
+ *   to forward its value to.
+ *
+ * Configuration example:
+ * \code{.yaml}
+ *   state_interfaces:
+ *     - joint1/position
+ *     - joint1/velocity
+ *   forward_state:
+ *     joint1/position:
+ *       to_command: ["joint2/position", "joint3/position"]
+ *     joint1/velocity:
+ *       to_command: ["joint2/velocity"]
+ * \endcode
+ */
+class ForwardStateController : public controller_interface::ControllerInterface
+{
+public:
+  ForwardStateController();
+
+  ~ForwardStateController() = 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:
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  std::vector<std::string> state_interface_names_;
+  std::vector<std::string> command_interface_names_;
+
+  std::unordered_map<std::size_t, std::vector<std::size_t>> state_to_command_map_;
+};
+
+}  // namespace forward_state_controller
+
+#endif  // FORWARD_STATE_CONTROLLER__FORWARD_STATE_CONTROLLER_HPP_

forward_state_controller/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>forward_state_controller</name>
+  <version>0.1.0</version>
+  <description>Controller that forwards state interface values to command interfaces.</description>
+  <maintainer email="nibanovic@gmail.com">Nikola Banovic</maintainer>
+  <license>Apache License 2.0</license>
+  <url type="website">https://control.ros.org</url>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ros2_control_cmake</build_depend>
+
+  <depend>backward_ros</depend>
+  <depend>controller_interface</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface_testing</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

forward_state_controller/src/forward_state_controller.cpp

@@ -0,0 +1,174 @@
+// Copyright 2024 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "forward_state_controller/forward_state_controller.hpp"
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "controller_interface/helpers.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
+#include "rclcpp/logging.hpp"
+
+namespace forward_state_controller
+{
+ForwardStateController::ForwardStateController()
+: controller_interface::ControllerInterface()
+{
+}
+
+controller_interface::CallbackReturn ForwardStateController::on_init()
+{
+  try
+  {
+    param_listener_ = std::make_shared<ParamListener>(get_node());
+  }
+  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 ForwardStateController::on_configure(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  params_ = param_listener_->get_params();
+
+  if (params_.state_interfaces.empty())
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "'state_interfaces' parameter is empty");
+    return controller_interface::CallbackReturn::ERROR;
+  }
+
+  state_interface_names_.clear();
+  command_interface_names_.clear();
+  state_to_command_map_.clear();
+
+  for (const auto & state_iface_name : params_.state_interfaces)
+  {
+    auto it = params_.forward_state.state_interfaces_map.find(state_iface_name);
+    if (it == params_.forward_state.state_interfaces_map.end())
+    {
+      RCLCPP_ERROR(
+        get_node()->get_logger(),
+        "No 'forward_state.%s' mapping found in parameters", state_iface_name.c_str());
+      return controller_interface::CallbackReturn::ERROR;
+    }
+
+    const auto & to_command = it->second.to_command;
+    if (to_command.empty())
+    {
+      RCLCPP_ERROR(
+        get_node()->get_logger(),
+        "'forward_state.%s.to_command' is empty", state_iface_name.c_str());
+      return controller_interface::CallbackReturn::ERROR;
+    }
+
+    const std::size_t state_idx = state_interface_names_.size();
+    state_interface_names_.push_back(state_iface_name);
+
+    std::vector<std::size_t> cmd_indices;
+    for (const auto & cmd_iface_name : to_command)
+    {
+      // deduplicate command_interface_names_
+      auto cmd_it = std::find(
+        command_interface_names_.begin(), command_interface_names_.end(), cmd_iface_name);
+      std::size_t cmd_idx;
+      if (cmd_it == command_interface_names_.end())
+      {
+        cmd_idx = command_interface_names_.size();
+        command_interface_names_.push_back(cmd_iface_name);
+      }
+      else
+      {
+        cmd_idx = static_cast<std::size_t>(std::distance(command_interface_names_.begin(), cmd_it));
+      }
+      cmd_indices.push_back(cmd_idx);
+    }
+    state_to_command_map_[state_idx] = cmd_indices;
+  }
+
+  RCLCPP_INFO(get_node()->get_logger(), "configure successful");
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration
+ForwardStateController::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_names_;
+
+  return command_interfaces_config;
+}
+
+controller_interface::InterfaceConfiguration
+ForwardStateController::state_interface_configuration() const
+{
+  controller_interface::InterfaceConfiguration state_interfaces_config;
+  state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  state_interfaces_config.names = state_interface_names_;
+
+  return state_interfaces_config;
+}
+
+controller_interface::CallbackReturn ForwardStateController::on_activate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn ForwardStateController::on_deactivate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type ForwardStateController::update(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+  for (const auto & [state_idx, cmd_indices] : state_to_command_map_)
+  {
+    const auto state_value_opt = state_interfaces_[state_idx].get_optional<double>();
+    if (!state_value_opt.has_value())
+    {
+      RCLCPP_WARN_THROTTLE(
+        get_node()->get_logger(), *(get_node()->get_clock()), 1000,
+        "Unable to get value from state interface '%s'",
+        state_interfaces_[state_idx].get_name().c_str());
+      continue;
+    }
+
+    const double value = state_value_opt.value();
+    for (const auto & cmd_idx : cmd_indices)
+    {
+      command_interfaces_[cmd_idx].set_value(value)
+    }
+  }
+
+  return controller_interface::return_type::OK;
+}
+
+}  // namespace forward_state_controller
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  forward_state_controller::ForwardStateController, controller_interface::ControllerInterface)