From 72f5b76f7b650b0735048bc76773bbdf67aa9973 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 21 Mar 2022 16:24:41 +0100 Subject: [PATCH] Make forward controller chainable. Enabling chanable mode for forward command controllers. Fix all bugs in chained-controllers mode. Remove debug output from forwarding controller. Updated example ffwd chainable controller for new structure. --- forward_command_controller/CMakeLists.txt | 27 ++++ .../forward_command_plugin.xml | 54 ++++++- .../chainable_forward_controller.hpp | 85 +++++++++++ .../forward_command_controller.hpp | 70 ++++++++- .../forward_controller.hpp | 67 +++++++++ .../forward_controllers_base.hpp | 37 ++--- ...i_interface_forward_command_controller.hpp | 72 ++++++++- .../src/chainable_forward_controller.cpp | 139 ++++++++++++++++++ .../src/forward_command_controller.cpp | 55 +------ .../src/forward_controller.cpp | 80 ++++++++++ .../src/forward_controllers_base.cpp | 84 +++++------ ...i_interface_forward_command_controller.cpp | 47 +----- .../test/test_forward_command_controller.cpp | 1 + .../test/test_forward_command_controller.hpp | 1 + ...oad_chained_forward_command_controller.cpp | 42 ++++++ ...i_interface_forward_command_controller.cpp | 41 ++++++ .../test_load_forward_command_controller.cpp | 3 + ...i_interface_forward_command_controller.cpp | 1 + ...i_interface_forward_command_controller.hpp | 1 + .../joint_trajectory_controller.hpp | 1 + .../src/joint_trajectory_controller.cpp | 30 +++- ros2_controllers_test_nodes/setup.cfg | 4 +- 22 files changed, 763 insertions(+), 179 deletions(-) create mode 100644 forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp create mode 100644 forward_command_controller/include/forward_command_controller/forward_controller.hpp create mode 100644 forward_command_controller/src/chainable_forward_controller.cpp create mode 100644 forward_command_controller/src/forward_controller.cpp create mode 100644 forward_command_controller/test/test_load_chained_forward_command_controller.cpp create mode 100644 forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 2490b5c856..30fe885b5d 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(std_msgs REQUIRED) add_library(forward_command_controller SHARED + src/chainable_forward_controller.cpp + src/forward_controller.cpp src/forward_controllers_base.cpp src/forward_command_controller.cpp src/multi_interface_forward_command_controller.cpp @@ -101,6 +103,31 @@ if(BUILD_TESTING) target_link_libraries(test_multi_interface_forward_command_controller forward_command_controller ) + + ament_add_gmock( + test_load_chained_forward_command_controller + test/test_load_chained_forward_command_controller.cpp + ) + target_include_directories(test_load_chained_forward_command_controller PRIVATE include) + ament_target_dependencies( + test_load_chained_forward_command_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock( + test_load_chained_multi_interface_forward_command_controller + test/test_load_chained_multi_interface_forward_command_controller.cpp + ) + target_include_directories(test_load_chained_multi_interface_forward_command_controller PRIVATE include) + ament_target_dependencies( + test_load_chained_multi_interface_forward_command_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + endif() ament_export_dependencies( diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml index 48fa29936d..47d661d603 100644 --- a/forward_command_controller/forward_command_plugin.xml +++ b/forward_command_controller/forward_command_plugin.xml @@ -1,11 +1,61 @@ + + - + The forward command controller commands a group of joints in a given interface + type="forward_command_controller::MultiInterfaceForwardCommandController" + base_class_type="controller_interface::ControllerInterface"> + + MultiInterfaceForwardController ros2_control controller. + + + + + + The forward command controller commands a group of joints in a given interface + + + MultiInterfaceForwardController ros2_control controller. diff --git a/forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp b/forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp new file mode 100644 index 0000000000..e62b39237b --- /dev/null +++ b/forward_command_controller/include/forward_command_controller/chainable_forward_controller.hpp @@ -0,0 +1,85 @@ +// 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. +// 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_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_ +#define FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "forward_command_controller/forward_controllers_base.hpp" +#include "forward_command_controller/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +namespace forward_command_controller +{ +using CmdType = std_msgs::msg::Float64MultiArray; + +/** + * \brief Forward command controller for a set of joints and interfaces. + * + * 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. + */ +class ChainableForwardController : public ForwardControllersBase, + public controller_interface::ChainableControllerInterface +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ChainableForwardController(); + + FORWARD_COMMAND_CONTROLLER_PUBLIC + ~ChainableForwardController() = default; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + std::vector 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; + + std::vector reference_interface_names_; +}; + +} // namespace forward_command_controller + +#endif // FORWARD_COMMAND_CONTROLLER__CHAINABLE_FORWARD_CONTROLLER_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp index f50af328d8..d799be9245 100644 --- a/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_command_controller.hpp @@ -18,8 +18,11 @@ #include #include +#include "forward_command_controller/chainable_forward_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" +#include "rclcpp_lifecycle/lifecycle_node.hpp" namespace forward_command_controller { @@ -34,20 +37,79 @@ namespace forward_command_controller * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ -class ForwardCommandController : public ForwardControllersBase +template < + typename T, + typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr, + typename std::enable_if< + std::is_convertible::value, T>::type * = + nullptr> +class BaseForwardCommandController : public T { public: FORWARD_COMMAND_CONTROLLER_PUBLIC - ForwardCommandController(); + BaseForwardCommandController() : T() {} protected: - void declare_parameters() override; - controller_interface::CallbackReturn read_parameters() override; + void declare_parameters() override + { + controller_interface::ControllerInterfaceBase::auto_declare>( + "joints", std::vector()); + controller_interface::ControllerInterfaceBase::auto_declare("interface_name", ""); + }; + + controller_interface::CallbackReturn read_parameters() override + { + joint_names_ = T::get_node()->get_parameter("joints").as_string_array(); + + if (joint_names_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'joints' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + // Specialized, child controllers set interfaces before calling configure function. + if (interface_name_.empty()) + { + interface_name_ = T::get_node()->get_parameter("interface_name").as_string(); + } + + if (interface_name_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'interface_name' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & joint : joint_names_) + { + T::command_interface_names_.push_back(joint + "/" + interface_name_); + } + + return controller_interface::CallbackReturn::SUCCESS; + }; std::vector joint_names_; std::string interface_name_; }; +class ForwardCommandController : public BaseForwardCommandController +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ForwardCommandController() : BaseForwardCommandController() {} +}; + +class ChainableForwardCommandController +: public BaseForwardCommandController +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ChainableForwardCommandController() : BaseForwardCommandController() + { + } +}; + } // namespace forward_command_controller #endif // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/forward_controller.hpp b/forward_command_controller/include/forward_command_controller/forward_controller.hpp new file mode 100644 index 0000000000..5bf2d1f831 --- /dev/null +++ b/forward_command_controller/include/forward_command_controller/forward_controller.hpp @@ -0,0 +1,67 @@ +// 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. +// 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_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_ +#define FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_ + +#include "controller_interface/controller_interface.hpp" +#include "forward_command_controller/forward_controllers_base.hpp" +#include "forward_command_controller/visibility_control.h" + +namespace forward_command_controller +{ +using CmdType = std_msgs::msg::Float64MultiArray; + +/** + * \brief Forward command controller for a set of joints and interfaces. + */ +class ForwardController : public ForwardControllersBase, + public controller_interface::ControllerInterface +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ForwardController(); + + FORWARD_COMMAND_CONTROLLER_PUBLIC + ~ForwardController() = default; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + FORWARD_COMMAND_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; +}; + +} // namespace forward_command_controller + +#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLER_HPP_ diff --git a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp index 3e153d7e2e..c04ed7b985 100644 --- a/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp +++ b/forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp @@ -32,14 +32,14 @@ namespace forward_command_controller using CmdType = std_msgs::msg::Float64MultiArray; /** - * \brief Forward command controller for a set of joints and interfaces. + * \brief Forward command controller base class for shared implementation. * * 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. */ -class ForwardControllersBase : public controller_interface::ControllerInterface +class ForwardControllersBase { public: FORWARD_COMMAND_CONTROLLER_PUBLIC @@ -49,29 +49,27 @@ class ForwardControllersBase : public controller_interface::ControllerInterface ~ForwardControllersBase() = default; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration get_command_interface_configuration() const; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; + controller_interface::InterfaceConfiguration get_state_interface_configuration() const; FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_init() override; + controller_interface::CallbackReturn execute_init( + const std::shared_ptr & node); FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn execute_configure( + const rclcpp_lifecycle::State & previous_state, + std::vector & command_interfaces); FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn execute_activate( + const rclcpp_lifecycle::State & previous_state, + std::vector & command_interfaces); - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - FORWARD_COMMAND_CONTROLLER_PUBLIC - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + controller_interface::CallbackReturn execute_deactivate( + const rclcpp_lifecycle::State & previous_state); protected: /** @@ -81,7 +79,7 @@ class ForwardControllersBase : public controller_interface::ControllerInterface virtual void declare_parameters() = 0; /** - * Derived controllers have to read parameters in this method and set `command_interface_types_` + * Derived controllers have to read parameters in this method and set `command_interface_names_` * variable. The variable is then used to propagate the command interface configuration to * controller manager. The method is called from `on_configure`-method of this class. * @@ -95,10 +93,13 @@ class ForwardControllersBase : public controller_interface::ControllerInterface std::vector joint_names_; std::string interface_name_; - std::vector command_interface_types_; + std::vector command_interface_names_; realtime_tools::RealtimeBuffer> rt_command_ptr_; rclcpp::Subscription::SharedPtr joints_command_subscriber_; + +private: + std::shared_ptr node_; }; } // namespace forward_command_controller diff --git a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp index 8acec36c5c..273832e727 100644 --- a/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/include/forward_command_controller/multi_interface_forward_command_controller.hpp @@ -18,8 +18,11 @@ #include #include +#include "forward_command_controller/chainable_forward_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/forward_controllers_base.hpp" #include "forward_command_controller/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace forward_command_controller { @@ -34,21 +37,80 @@ namespace forward_command_controller * Subscribes to: * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. */ -class MultiInterfaceForwardCommandController -: public forward_command_controller::ForwardControllersBase +template < + typename T, + typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr, + typename std::enable_if< + std::is_convertible::value, T>::type * = + nullptr> +class BaseMultiInterfaceForwardCommandController : public T { public: FORWARD_COMMAND_CONTROLLER_PUBLIC - MultiInterfaceForwardCommandController(); + BaseMultiInterfaceForwardCommandController() : T() {} protected: - void declare_parameters() override; - controller_interface::CallbackReturn read_parameters() override; + void declare_parameters() override + { + controller_interface::ControllerInterfaceBase::auto_declare("joint", joint_name_); + controller_interface::ControllerInterfaceBase::auto_declare>( + "interface_names", interface_names_); + }; + + controller_interface::CallbackReturn read_parameters() override + { + joint_name_ = T::get_node()->get_parameter("joint").as_string(); + interface_names_ = T::get_node()->get_parameter("interface_names").as_string_array(); + + if (joint_name_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'joint' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + if (interface_names_.empty()) + { + RCLCPP_ERROR(T::get_node()->get_logger(), "'interfaces' parameter is empty"); + return controller_interface::CallbackReturn::ERROR; + } + + for (const auto & interface : interface_names_) + + { + T::command_interface_names_.push_back(joint_name_ + "/" + interface); + } + + return controller_interface::CallbackReturn::SUCCESS; + }; std::string joint_name_; std::vector interface_names_; }; +class MultiInterfaceForwardCommandController +: public BaseMultiInterfaceForwardCommandController +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + MultiInterfaceForwardCommandController() + : BaseMultiInterfaceForwardCommandController() + { + } +}; + +class ChainableMultiInterfaceForwardCommandController +: public BaseMultiInterfaceForwardCommandController +{ +public: + FORWARD_COMMAND_CONTROLLER_PUBLIC + ChainableMultiInterfaceForwardCommandController() + : BaseMultiInterfaceForwardCommandController() + { + } +}; + } // namespace forward_command_controller #endif // FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_ diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp new file mode 100644 index 0000000000..25a247dd2e --- /dev/null +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -0,0 +1,139 @@ +// 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. +// 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_command_controller/chainable_forward_controller.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +namespace forward_command_controller +{ +ChainableForwardController::ChainableForwardController() +: ForwardControllersBase(), controller_interface::ChainableControllerInterface() +{ +} + +controller_interface::CallbackReturn ChainableForwardController::on_init() +{ + return execute_init(get_node()); +} + +controller_interface::CallbackReturn ChainableForwardController::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = execute_configure(previous_state, command_interfaces_); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + // 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::quiet_NaN()); + + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +ChainableForwardController::command_interface_configuration() const +{ + return get_command_interface_configuration(); +} + +controller_interface::InterfaceConfiguration +ChainableForwardController::state_interface_configuration() const +{ + return get_state_interface_configuration(); +} + +std::vector +ChainableForwardController::on_export_reference_interfaces() +{ + std::vector 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; +} + +bool ChainableForwardController::on_set_chained_mode(bool chained_mode) +{ + // we can set chained mode in any situation + (void)chained_mode; + return true; +} + +controller_interface::CallbackReturn ChainableForwardController::on_activate( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = execute_activate(previous_state, command_interfaces_); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn ChainableForwardController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + return execute_deactivate(previous_state); +} + +controller_interface::return_type ChainableForwardController::update_reference_from_subscribers() +{ + auto joint_commands = rt_command_ptr_.readFromRT(); + // message is valid + if (!(!joint_commands || !(*joint_commands))) + { + reference_interfaces_ = (*joint_commands)->data; + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type ChainableForwardController::update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + 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; +} + +} // namespace forward_command_controller diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 46e90cd1c4..55be87dfae 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -14,58 +14,13 @@ #include "forward_command_controller/forward_command_controller.hpp" -#include -#include -#include -#include -#include - -#include "rclcpp/logging.hpp" -#include "rclcpp/qos.hpp" - -namespace forward_command_controller -{ -ForwardCommandController::ForwardCommandController() : ForwardControllersBase() {} - -void ForwardCommandController::declare_parameters() -{ - get_node()->declare_parameter>("joints", std::vector()); - get_node()->declare_parameter("interface_name", ""); -} - -controller_interface::CallbackReturn ForwardCommandController::read_parameters() -{ - joint_names_ = get_node()->get_parameter("joints").as_string_array(); - - if (joint_names_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } - - // Specialized, child controllers set interfaces before calling configure function. - if (interface_name_.empty()) - { - interface_name_ = get_node()->get_parameter("interface_name").as_string(); - } - - if (interface_name_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); - return controller_interface::CallbackReturn::ERROR; - } - - for (const auto & joint : joint_names_) - { - command_interface_types_.push_back(joint + "/" + interface_name_); - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -} // namespace forward_command_controller +#include "forward_command_controller/chainable_forward_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + forward_command_controller::ChainableForwardCommandController, + controller_interface::ChainableControllerInterface) diff --git a/forward_command_controller/src/forward_controller.cpp b/forward_command_controller/src/forward_controller.cpp new file mode 100644 index 0000000000..790738a9be --- /dev/null +++ b/forward_command_controller/src/forward_controller.cpp @@ -0,0 +1,80 @@ +// 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. +// 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_command_controller/forward_controller.hpp" + +#include "forward_command_controller/visibility_control.h" + +namespace forward_command_controller +{ +ForwardController::ForwardController() +: ForwardControllersBase(), controller_interface::ControllerInterface() +{ +} + +controller_interface::InterfaceConfiguration ForwardController::command_interface_configuration() + const +{ + return get_command_interface_configuration(); +} + +controller_interface::InterfaceConfiguration ForwardController::state_interface_configuration() + const +{ + return get_state_interface_configuration(); +} + +controller_interface::CallbackReturn ForwardController::on_init() +{ + return execute_init(get_node()); +} + +controller_interface::CallbackReturn ForwardController::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + return execute_configure(previous_state, command_interfaces_); +} + +controller_interface::CallbackReturn ForwardController::on_activate( + const rclcpp_lifecycle::State & previous_state) +{ + return execute_activate(previous_state, command_interfaces_); +} + +controller_interface::CallbackReturn ForwardController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + return execute_deactivate(previous_state); +} + +controller_interface::return_type ForwardController::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; + } + + 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 forward_command_controller diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index e4ea46fcc5..3aa76d4423 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -28,14 +28,15 @@ namespace forward_command_controller { ForwardControllersBase::ForwardControllersBase() -: controller_interface::ControllerInterface(), - rt_command_ptr_(nullptr), - joints_command_subscriber_(nullptr) +: rt_command_ptr_(nullptr), joints_command_subscriber_(nullptr) { } -controller_interface::CallbackReturn ForwardControllersBase::on_init() +controller_interface::CallbackReturn ForwardControllersBase::execute_init( + const std::shared_ptr & node) { + node_ = node; + try { declare_parameters(); @@ -49,42 +50,53 @@ controller_interface::CallbackReturn ForwardControllersBase::on_init() return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ForwardControllersBase::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn ForwardControllersBase::execute_configure( + const rclcpp_lifecycle::State & /*previous_state*/, + std::vector & command_interfaces) { auto ret = this->read_parameters(); if (ret != controller_interface::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(node_->get_logger(), "Error when reading parameters."); return ret; } - joints_command_subscriber_ = get_node()->create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); + joints_command_subscriber_ = node_->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { + // check if message is correct size, if not ignore + if (msg->data.size() == command_interface_names_.size()) + { + rt_command_ptr_.writeFromNonRT(msg); + } + }); + + // pre-reserve command interfaces + command_interfaces.reserve(command_interface_names_.size()); - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + RCLCPP_INFO(node_->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::InterfaceConfiguration -ForwardControllersBase::command_interface_configuration() const +ForwardControllersBase::get_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_; + command_interfaces_config.names = command_interface_names_; return command_interfaces_config; } -controller_interface::InterfaceConfiguration ForwardControllersBase::state_interface_configuration() - const +controller_interface::InterfaceConfiguration +ForwardControllersBase::get_state_interface_configuration() const { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } -controller_interface::CallbackReturn ForwardControllersBase::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn ForwardControllersBase::execute_activate( + const rclcpp_lifecycle::State & /*previous_state*/, + std::vector & command_interfaces) { // check if we have all resources defined in the "points" parameter // also verify that we *only* have the resources defined in the "points" parameter @@ -93,23 +105,23 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( ordered_interfaces; if ( !controller_interface::get_ordered_interfaces( - command_interfaces_, command_interface_types_, std::string(""), ordered_interfaces) || - command_interface_types_.size() != ordered_interfaces.size()) + command_interfaces, command_interface_names_, std::string(""), ordered_interfaces) || + command_interface_names_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu command interfaces, got %zu", - command_interface_types_.size(), ordered_interfaces.size()); + 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_command_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); - RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + RCLCPP_INFO(node_->get_logger(), "activate successful"); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( +controller_interface::CallbackReturn ForwardControllersBase::execute_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // reset command buffer @@ -117,32 +129,4 @@ controller_interface::CallbackReturn ForwardControllersBase::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type ForwardControllersBase::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; - } - - 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 forward_command_controller diff --git a/forward_command_controller/src/multi_interface_forward_command_controller.cpp b/forward_command_controller/src/multi_interface_forward_command_controller.cpp index 6cc22cf549..e6749bda9e 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -14,51 +14,14 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" -#include -#include - -namespace forward_command_controller -{ -MultiInterfaceForwardCommandController::MultiInterfaceForwardCommandController() -: ForwardControllersBase() -{ -} - -void MultiInterfaceForwardCommandController::declare_parameters() -{ - get_node()->declare_parameter("joint", joint_name_); - get_node()->declare_parameter>("interface_names", interface_names_); -} - -controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters() -{ - joint_name_ = get_node()->get_parameter("joint").as_string(); - interface_names_ = get_node()->get_parameter("interface_names").as_string_array(); - - if (joint_name_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'joint' parameter is empty"); - return controller_interface::CallbackReturn::ERROR; - } - - if (interface_names_.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter is empty"); - return controller_interface::CallbackReturn::ERROR; - } - - for (const auto & interface : interface_names_) - { - command_interface_types_.push_back(joint_name_ + "/" + interface); - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -} // namespace forward_command_controller +#include "forward_command_controller/chainable_forward_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( forward_command_controller::MultiInterfaceForwardCommandController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + forward_command_controller::ChainableMultiInterfaceForwardCommandController, + controller_interface::ChainableControllerInterface) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 697e42d671..0240dbb4e0 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -23,6 +23,7 @@ #include "test_forward_command_controller.hpp" #include "forward_command_controller/forward_command_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "lifecycle_msgs/msg/state.hpp" diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 9c6bd2a352..d9acbdf6c4 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -22,6 +22,7 @@ #include "gmock/gmock.h" #include "forward_command_controller/forward_command_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/forward_command_controller/test/test_load_chained_forward_command_controller.cpp b/forward_command_controller/test/test_load_chained_forward_command_controller.cpp new file mode 100644 index 0000000000..9c93fc8d7e --- /dev/null +++ b/forward_command_controller/test/test_load_chained_forward_command_controller.cpp @@ -0,0 +1,42 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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 +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadForwardCommandController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_forward_command_controller", + "forward_command_controller/ChainedForwardCommandController")); + + rclcpp::shutdown(); +} diff --git a/forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp new file mode 100644 index 0000000000..f73ba042de --- /dev/null +++ b/forward_command_controller/test/test_load_chained_multi_interface_forward_command_controller.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2021, PickNik, Inc. +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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 +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadMultiInterfaceForwardController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_forward_command_controller", + "forward_command_controller/ChainedMultiInterfaceForwardCommandController")); +} diff --git a/forward_command_controller/test/test_load_forward_command_controller.cpp b/forward_command_controller/test/test_load_forward_command_controller.cpp index 464b57b69d..03d735fb1c 100644 --- a/forward_command_controller/test/test_load_forward_command_controller.cpp +++ b/forward_command_controller/test/test_load_forward_command_controller.cpp @@ -34,6 +34,9 @@ TEST(TestLoadForwardCommandController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + cm.load_controller( + "test_forward_command_controller", "forward_command_controller/ForwardCommandController"); + ASSERT_NO_THROW(cm.load_controller( "test_forward_command_controller", "forward_command_controller/ForwardCommandController")); diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 0cada04859..bb4ded549e 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -24,6 +24,7 @@ #include "test_multi_interface_forward_command_controller.hpp" +#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 62a4d4e981..8846b2bcdc 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -23,6 +23,7 @@ #include "gmock/gmock.h" +#include "forward_command_controller/forward_controller.hpp" #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 29789b30d3..a1bfdf4469 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa protected: std::vector joint_names_; + std::vector command_joint_names_; std::vector command_interface_types_; std::vector state_interface_types_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index eecbecbb04..f171b018a8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -56,6 +56,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() { // with the lifecycle node being initialized, we can declare parameters auto_declare>("joints", joint_names_); + auto_declare>("command_joints", command_joint_names_); auto_declare>("command_interfaces", command_interface_types_); auto_declare>("state_interfaces", state_interface_types_); auto_declare("state_publish_rate", 50.0); @@ -81,8 +82,8 @@ JointTrajectoryController::command_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(joint_names_.size() * command_interface_types_.size()); - for (const auto & joint_name : joint_names_) + conf.names.reserve(command_joint_names_.size() * command_interface_types_.size()); + for (const auto & joint_name : command_joint_names_) { for (const auto & interface_type : command_interface_types_) { @@ -458,6 +459,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // update parameters joint_names_ = get_node()->get_parameter("joints").as_string_array(); + // TODO(destogl): why is this here? Add comment or move if (!reset()) { return CallbackReturn::FAILURE; @@ -465,9 +467,25 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (joint_names_.empty()) { + // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? RCLCPP_WARN(logger, "'joints' parameter is empty."); } + command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array(); + + if (command_joint_names_.empty()) + { + command_joint_names_ = joint_names_; + RCLCPP_INFO( + logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); + } + else if (command_joint_names_.size() != joint_names_.size()) + { + RCLCPP_ERROR( + logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); + return CallbackReturn::FAILURE; + } + // Specialized, child controllers set interfaces before calling configure function. if (command_interface_types_.empty()) { @@ -775,12 +793,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); auto index = std::distance(allowed_interface_types_.begin(), it); if (!controller_interface::get_ordered_interfaces( - command_interfaces_, joint_names_, interface, joint_command_interface_[index])) + command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", - joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); - return CallbackReturn::ERROR; + command_joint_names_.size(), interface.c_str(), joint_command_interface_[index].size()); + return controller_interface::CallbackReturn::ERROR; } } for (const auto & interface : state_interface_types_) @@ -794,7 +812,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( RCLCPP_ERROR( get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", joint_names_.size(), interface.c_str(), joint_state_interface_[index].size()); - return CallbackReturn::ERROR; + return controller_interface::CallbackReturn::ERROR; } } diff --git a/ros2_controllers_test_nodes/setup.cfg b/ros2_controllers_test_nodes/setup.cfg index c00c975bcf..ec192fb831 100644 --- a/ros2_controllers_test_nodes/setup.cfg +++ b/ros2_controllers_test_nodes/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/ros2_controllers_test_nodes +script_dir=$base/lib/ros2_controllers_test_nodes [install] -install-scripts=$base/lib/ros2_controllers_test_nodes +install_scripts=$base/lib/ros2_controllers_test_nodes