Add a passthrough_controller demo

passthrough_controller/CMakeLists.txt

@@ -0,0 +1,87 @@
+cmake_minimum_required(VERSION 3.16)
+project(passthrough_controller LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_msgs
+  generate_parameter_library
+  controller_interface
+  hardware_interface
+  parameter_traits
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  std_msgs
+)
+
+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(passthrough_controller_parameters
+  src/passthrough_controller_parameters.yaml
+)
+
+add_library(passthrough_controller SHARED
+  src/passthrough_controller.cpp
+)
+target_compile_features(passthrough_controller PUBLIC cxx_std_17)
+target_include_directories(passthrough_controller PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/passthrough_controller>
+)
+ target_link_libraries(passthrough_controller PUBLIC
+   passthrough_controller_parameters
+ )
+ament_target_dependencies(passthrough_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(passthrough_controller PRIVATE "passthrough_controller_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(controller_interface passthrough_controller.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(test_load_controller test/test_load_controller.cpp)
+  target_include_directories(test_load_controller PRIVATE include)
+  ament_target_dependencies(
+    test_load_controller
+    controller_manager
+    ros2_control_test_assets
+  )
+
+  ament_add_gmock(test_passthrough_controller
+    test/test_passthrough_controller.cpp
+  )
+  target_link_libraries(test_passthrough_controller
+    passthrough_controller
+  )
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include/passthrough_controller
+)
+
+install(TARGETS
+    passthrough_controller
+    passthrough_controller_parameters
+  EXPORT export_passthrough_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+ament_export_targets(export_passthrough_controller HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

passthrough_controller/include/passthrough_controller/passthrough_controller.hpp

@@ -0,0 +1,222 @@
+// Copyright (c) 2023, PAL Robotics
+//
+// 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 PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_
+#define PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_
+
+#include <controller_interface/chainable_controller_interface.hpp>
+#include "controller_interface/helpers.hpp"
+#include "realtime_tools/realtime_buffer.h"
+#include "std_msgs/msg/float64_multi_array.hpp"
+// auto-generated by generate_parameter_library
+#include "passthrough_controller_parameters.hpp"
+
+#include "passthrough_controller/visibility_control.h"
+
+namespace passthrough_controller
+{
+using DataType = std_msgs::msg::Float64MultiArray;
+class PassthroughController : public controller_interface::ChainableControllerInterface
+{
+public:
+  PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+  PassthroughController() {}
+
+  PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_init() override
+  {
+    try
+    {
+      param_listener_ = std::make_shared<ParamListener>(get_node());
+      params_ = param_listener_->get_params();
+    }
+    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;
+  }
+
+  PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override
+  {
+    params_ = param_listener_->get_params();
+    command_interface_names_ = params_.interfaces;
+
+    if (command_interface_names_.empty())
+    {
+      RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter list was empty");
+      return controller_interface::CallbackReturn::ERROR;
+    }
+
+    joints_cmd_sub_ = this->get_node()->create_subscription<DataType>(
+      "~/commands", rclcpp::SystemDefaultsQoS(),
+      [this](const DataType::SharedPtr msg)
+      {
+        // check if message is correct size, if not ignore
+        if (msg->data.size() == command_interface_names_.size())
+        {
+          rt_buffer_ptr_.writeFromNonRT(msg);
+        }
+        else
+        {
+          RCLCPP_ERROR(
+            this->get_node()->get_logger(),
+            "Invalid command received of %zu size, expected %zu size", msg->data.size(),
+            command_interface_names_.size());
+        }
+      });
+
+    // pre-reserve command interfaces
+    command_interfaces_.reserve(command_interface_names_.size());
+
+    RCLCPP_INFO(this->get_node()->get_logger(), "configure successful");
+
+    // The names should be in the same order as for command interfaces for easier matching
+    reference_interface_names_ = command_interface_names_;
+    // for any case make reference interfaces size of command interfaces
+    reference_interfaces_.resize(
+      reference_interface_names_.size(), std::numeric_limits<double>::quiet_NaN());
+
+    return controller_interface::CallbackReturn::SUCCESS;
+  }
+
+  PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override
+  {
+    //  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_names_, std::string(""), ordered_interfaces) ||
+      command_interface_names_.size() != ordered_interfaces.size())
+    {
+      RCLCPP_ERROR(
+        this->get_node()->get_logger(), "Expected %zu command interfaces, got %zu",
+        command_interface_names_.size(), ordered_interfaces.size());
+      return controller_interface::CallbackReturn::ERROR;
+    }
+
+    // reset command buffer if a command came through callback when controller was inactive
+    rt_buffer_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<DataType>>(nullptr);
+
+    RCLCPP_INFO(this->get_node()->get_logger(), "activate successful");
+
+    std::fill(
+      reference_interfaces_.begin(), reference_interfaces_.end(),
+      std::numeric_limits<double>::quiet_NaN());
+
+    return controller_interface::CallbackReturn::SUCCESS;
+  }
+
+  PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override
+  {
+    // reset command buffer
+    rt_buffer_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<DataType>>(nullptr);
+    return controller_interface::CallbackReturn::SUCCESS;
+  }
+
+  PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override
+  {
+    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;
+  }
+
+  PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override
+  {
+    return controller_interface::InterfaceConfiguration{
+      controller_interface::interface_configuration_type::NONE};
+  }
+
+  PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override
+  {
+    auto joint_commands = rt_buffer_ptr_.readFromRT();
+    // message is valid
+    if (!(!joint_commands || !(*joint_commands)))
+    {
+      if (reference_interfaces_.size() != (*joint_commands)->data.size())
+      {
+        RCLCPP_ERROR_THROTTLE(
+          get_node()->get_logger(), *(get_node()->get_clock()), 1000,
+          "command size (%zu) does not match number of reference interfaces (%zu)",
+          (*joint_commands)->data.size(), reference_interfaces_.size());
+        return controller_interface::return_type::ERROR;
+      }
+      reference_interfaces_ = (*joint_commands)->data;
+    }
+
+    return controller_interface::return_type::OK;
+  }
+
+protected:
+  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override
+  {
+    std::vector<hardware_interface::CommandInterface> reference_interfaces;
+
+    for (size_t i = 0; i < reference_interface_names_.size(); ++i)
+    {
+      reference_interfaces.push_back(hardware_interface::CommandInterface(
+        get_node()->get_name(), reference_interface_names_[i], &reference_interfaces_[i]));
+    }
+
+    return reference_interfaces;
+  }
+
+  PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+  bool on_set_chained_mode(bool /* chained_mode */) override { return true; }
+
+  PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override
+  {
+    for (size_t i = 0; i < command_interfaces_.size(); ++i)
+    {
+      if (!std::isnan(reference_interfaces_[i]))
+      {
+        command_interfaces_[i].set_value(reference_interfaces_[i]);
+      }
+    }
+
+    return controller_interface::return_type::OK;
+  }
+
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  realtime_tools::RealtimeBuffer<std::shared_ptr<DataType>> rt_buffer_ptr_;
+  rclcpp::Subscription<DataType>::SharedPtr joints_cmd_sub_;
+
+  std::vector<std::string> reference_interface_names_;
+
+  std::vector<std::string> command_interface_names_;
+};
+
+}  // namespace passthrough_controller
+
+#endif  // PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_

passthrough_controller/include/passthrough_controller/visibility_control.h

@@ -0,0 +1,49 @@
+// Copyright (c) 2023, PAL Robotics
+//
+// 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 PASSTHROUGH_CONTROLLER__VISIBILITY_CONTROL_H_
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport))
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport))
+#else
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport)
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport)
+#endif
+#ifdef PASSTHROUGH_CONTROLLER__VISIBILITY_BUILDING_DLL
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT
+#else
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT
+#endif
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC_TYPE PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_LOCAL
+#else
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default")))
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT
+#if __GNUC__ >= 4
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default")))
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden")))
+#else
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_LOCAL
+#endif
+#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC_TYPE
+#endif
+
+#endif  // PASSTHROUGH_CONTROLLER__VISIBILITY_CONTROL_H_

passthrough_controller/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>passthrough_controller</name>
+  <version>3.3.0</version>
+  <description>A simple demo of chainable controllers. It passes commands through without change.</description>
+  <maintainer email="[email protected]">Sai Kishor Kothakota</maintainer>
+  <maintainer email="[email protected]">Andy Zelenak</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>control_msgs</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>parameter_traits</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>