From 0206ee68b6f5e46bbc9f7a31f776dd8e0e483322 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 01/12] 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 | 36 ++--- ...i_interface_forward_command_controller.hpp | 72 ++++++++- .../src/chainable_forward_controller.cpp | 140 ++++++++++++++++++ .../src/forward_command_controller.cpp | 64 ++------ .../src/forward_controller.cpp | 74 +++++++++ .../src/forward_controllers_base.cpp | 78 ++++------ ...i_interface_forward_command_controller.cpp | 56 ++----- .../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.hpp | 1 + .../joint_trajectory_controller.hpp | 1 + .../src/joint_trajectory_controller.cpp | 30 +++- 20 files changed, 769 insertions(+), 174 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..58026d62b5 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..c8f8dcca23 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::ControllerInterface::auto_declare>( + "joints", std::vector()); + controller_interface::ControllerInterface::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..ad6e1a5d23 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,28 @@ 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 +80,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 +94,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..30cabfa6d6 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::ControllerInterface::auto_declare("joint", joint_name_); + controller_interface::ControllerInterface::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..796040516c --- /dev/null +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -0,0 +1,140 @@ +// 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() +{ +} + +CallbackReturn ChainableForwardController::on_init() { return execute_init(get_node()); } + +CallbackReturn ChainableForwardController::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + auto ret = execute_configure(previous_state, command_interfaces_); + RCLCPP_ERROR(get_node()->get_logger(), "Configure is successful, now tryting configure ref_itfs"); + 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()); + + for (const auto & value : reference_interfaces_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Reference interface value is %f", value); + } + + 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; +} + +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 CallbackReturn::SUCCESS; +} + +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; + } +} + +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..9bd27e0215 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -14,58 +14,20 @@ #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< +// forward_command_controller::ForwardController>, +// controller_interface::ControllerInterface) +// PLUGINLIB_EXPORT_CLASS( +// forward_command_controller::ForwardCommandController< +// forward_command_controller::ChainableForwardController>, +// controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + forward_command_controller::ChainableForwardCommandController, + controller_interface::ControllerInterface) diff --git a/forward_command_controller/src/forward_controller.cpp b/forward_command_controller/src/forward_controller.cpp new file mode 100644 index 0000000000..0574ce7b5c --- /dev/null +++ b/forward_command_controller/src/forward_controller.cpp @@ -0,0 +1,74 @@ +// 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(); +} + +CallbackReturn ForwardController::on_init() { return execute_init(get_node()); } + +CallbackReturn ForwardController::on_configure(const rclcpp_lifecycle::State & previous_state) +{ + return execute_configure(previous_state, command_interfaces_); +} + +CallbackReturn ForwardController::on_activate(const rclcpp_lifecycle::State & previous_state) +{ + return execute_activate(previous_state, command_interfaces_); +} + +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..bea74aeb6f 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(get_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"); 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,12 +105,12 @@ 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()); + command_interface_names_.size(), ordered_interfaces.size()); return controller_interface::CallbackReturn::ERROR; } @@ -109,7 +121,7 @@ controller_interface::CallbackReturn ForwardControllersBase::on_activate( 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..4828049857 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,23 @@ #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< +// forward_command_controller::ForwardController>, +// controller_interface::ControllerInterface) +// PLUGINLIB_EXPORT_CLASS( +// forward_command_controller::MultiInterfaceForwardCommandController< +// forward_command_controller::ChainableForwardController>, +// controller_interface::ControllerInterface) + PLUGINLIB_EXPORT_CLASS( forward_command_controller::MultiInterfaceForwardCommandController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + forward_command_controller::ChainableMultiInterfaceForwardCommandController, + controller_interface::ControllerInterface) 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.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; } } From 43f39a090f284c0295ae20453fdc2b9211f5f599 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 23 Mar 2022 18:23:11 +0100 Subject: [PATCH 02/12] Enabling chanable mode for forward command controllers. --- .../forward_controllers_base.hpp | 1 - .../src/chainable_forward_controller.cpp | 15 +++++++++------ 2 files changed, 9 insertions(+), 7 deletions(-) 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 ad6e1a5d23..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 @@ -68,7 +68,6 @@ class ForwardControllersBase const rclcpp_lifecycle::State & previous_state, std::vector & command_interfaces); - FORWARD_COMMAND_CONTROLLER_PUBLIC controller_interface::CallbackReturn execute_deactivate( const rclcpp_lifecycle::State & previous_state); diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index 796040516c..3f126244d9 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -32,9 +32,12 @@ ChainableForwardController::ChainableForwardController() { } -CallbackReturn ChainableForwardController::on_init() { return execute_init(get_node()); } +controller_interface::CallbackReturn ChainableForwardController::on_init() +{ + return execute_init(get_node()); +} -CallbackReturn ChainableForwardController::on_configure( +controller_interface::CallbackReturn ChainableForwardController::on_configure( const rclcpp_lifecycle::State & previous_state) { auto ret = execute_configure(previous_state, command_interfaces_); @@ -48,7 +51,7 @@ CallbackReturn ChainableForwardController::on_configure( 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()); + command_interfaces_.size(), std::numeric_limits::quiet_NaN()); for (const auto & value : reference_interfaces_) { @@ -91,7 +94,7 @@ bool ChainableForwardController::on_set_chained_mode(bool chained_mode) return true; } -CallbackReturn ChainableForwardController::on_activate( +controller_interface::CallbackReturn ChainableForwardController::on_activate( const rclcpp_lifecycle::State & previous_state) { auto ret = execute_activate(previous_state, command_interfaces_); @@ -104,10 +107,10 @@ CallbackReturn ChainableForwardController::on_activate( reference_interfaces_.begin(), reference_interfaces_.end(), std::numeric_limits::quiet_NaN()); - return CallbackReturn::SUCCESS; + return controller_interface::CallbackReturn::SUCCESS; } -CallbackReturn ChainableForwardController::on_deactivate( +controller_interface::CallbackReturn ChainableForwardController::on_deactivate( const rclcpp_lifecycle::State & previous_state) { return execute_deactivate(previous_state); From 29c4393a2b5c6d2d7c32be228ba1611afdf42ab7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Mar 2022 17:16:28 +0100 Subject: [PATCH 03/12] Fix all bugs in chained-controllers mode. --- .../src/chainable_forward_controller.cpp | 11 +++++------ .../src/forward_controllers_base.cpp | 2 +- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index 3f126244d9..7f59cd50c3 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -51,12 +51,7 @@ controller_interface::CallbackReturn ChainableForwardController::on_configure( reference_interface_names_ = command_interface_names_; // for any case make reference interfaces size of command interfaces reference_interfaces_.resize( - command_interfaces_.size(), std::numeric_limits::quiet_NaN()); - - for (const auto & value : reference_interfaces_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Reference interface value is %f", value); - } + reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); return CallbackReturn::SUCCESS; } @@ -133,6 +128,10 @@ controller_interface::return_type ChainableForwardController::update_and_write_c { if (!std::isnan(reference_interfaces_[i])) { + RCLCPP_ERROR( + get_node()->get_logger(), "Reference interface value is %f; Command interface name is '%s'", + reference_interfaces_[i], command_interfaces_[i].get_full_name().c_str()); + command_interfaces_[i].set_value(reference_interfaces_[i]); } } diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index bea74aeb6f..fcb7bb2ab8 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -57,7 +57,7 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_configure( auto ret = this->read_parameters(); if (ret != controller_interface::CallbackReturn::SUCCESS) { - RCLCPP_ERROR(get_node()->get_logger(), "Error when reading parameters."); + RCLCPP_ERROR(node_->get_logger(), "Error when reading parameters."); return ret; } From 6f2f956818866a77e8413a8b8b47f7bdfea1acf6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 29 Mar 2022 19:24:27 +0200 Subject: [PATCH 04/12] Remove debug output from forwarding controller. --- .../src/chainable_forward_controller.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index 7f59cd50c3..83f7ceacae 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -128,10 +128,6 @@ controller_interface::return_type ChainableForwardController::update_and_write_c { if (!std::isnan(reference_interfaces_[i])) { - RCLCPP_ERROR( - get_node()->get_logger(), "Reference interface value is %f; Command interface name is '%s'", - reference_interfaces_[i], command_interfaces_[i].get_full_name().c_str()); - command_interfaces_[i].set_value(reference_interfaces_[i]); } } From 704ba5dea205de5a4214fece0bc0073441dd6bcd 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 05/12] Make forward controller chainable. --- .../forward_command_plugin.xml | 32 ------------------- .../chainable_forward_controller.hpp | 4 +++ ...i_interface_forward_command_controller.hpp | 1 - .../src/chainable_forward_controller.cpp | 1 - .../src/forward_command_controller.cpp | 9 +----- ...i_interface_forward_command_controller.cpp | 9 ------ .../test/test_forward_command_controller.hpp | 3 +- ...i_interface_forward_command_controller.cpp | 1 + ...i_interface_forward_command_controller.hpp | 3 +- 9 files changed, 10 insertions(+), 53 deletions(-) diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml index 58026d62b5..37ddedc035 100644 --- a/forward_command_controller/forward_command_plugin.xml +++ b/forward_command_controller/forward_command_plugin.xml @@ -1,35 +1,3 @@ - - ::quiet_NaN()); diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 9bd27e0215..82319bbc65 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -16,16 +16,9 @@ #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< -// forward_command_controller::ForwardController>, -// controller_interface::ControllerInterface) -// PLUGINLIB_EXPORT_CLASS( -// forward_command_controller::ForwardCommandController< -// forward_command_controller::ChainableForwardController>, -// controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( 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 4828049857..a1020fcb17 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -19,15 +19,6 @@ #include "pluginlib/class_list_macros.hpp" -// PLUGINLIB_EXPORT_CLASS( -// forward_command_controller::MultiInterfaceForwardCommandController< -// forward_command_controller::ForwardController>, -// controller_interface::ControllerInterface) -// PLUGINLIB_EXPORT_CLASS( -// forward_command_controller::MultiInterfaceForwardCommandController< -// forward_command_controller::ChainableForwardController>, -// controller_interface::ControllerInterface) - PLUGINLIB_EXPORT_CLASS( forward_command_controller::MultiInterfaceForwardCommandController, controller_interface::ControllerInterface) diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index d9acbdf6c4..66a5df629b 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -30,7 +30,8 @@ using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; // subclassing and friending so we can access member variables -class FriendForwardCommandController : public forward_command_controller::ForwardCommandController +class FriendForwardCommandController : public forward_command_controller::ForwardCommandController< + forward_command_controller::ForwardController> { FRIEND_TEST(ForwardCommandControllerTest, JointsParameterNotSet); FRIEND_TEST(ForwardCommandControllerTest, InterfaceParameterNotSet); 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 8846b2bcdc..74cf5a7550 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 @@ -35,7 +35,8 @@ using hardware_interface::HW_IF_VELOCITY; // subclassing and friending so we can access member variables class FriendMultiInterfaceForwardCommandController -: public forward_command_controller::MultiInterfaceForwardCommandController +: public forward_command_controller::MultiInterfaceForwardCommandController< + forward_command_controller::ForwardController> { FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, JointsParameterNotSet); FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, InterfaceParameterNotSet); From 6759bb583973a394b3b30c1dda1e1bc40e726548 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 23 Mar 2022 18:23:11 +0100 Subject: [PATCH 06/12] Enabling chanable mode for forward command controllers. --- .../forward_command_plugin.xml | 32 +++++++++++++++++++ .../chainable_forward_controller.hpp | 4 --- ...i_interface_forward_command_controller.hpp | 1 + .../src/chainable_forward_controller.cpp | 3 +- .../test/test_forward_command_controller.hpp | 3 +- ...i_interface_forward_command_controller.hpp | 3 +- 6 files changed, 37 insertions(+), 9 deletions(-) diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml index 37ddedc035..3697404a34 100644 --- a/forward_command_controller/forward_command_plugin.xml +++ b/forward_command_controller/forward_command_plugin.xml @@ -1,3 +1,35 @@ + + ::quiet_NaN()); + command_interfaces_.size(), std::numeric_limits::quiet_NaN()); return CallbackReturn::SUCCESS; } diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 66a5df629b..d9acbdf6c4 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -30,8 +30,7 @@ using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; // subclassing and friending so we can access member variables -class FriendForwardCommandController : public forward_command_controller::ForwardCommandController< - forward_command_controller::ForwardController> +class FriendForwardCommandController : public forward_command_controller::ForwardCommandController { FRIEND_TEST(ForwardCommandControllerTest, JointsParameterNotSet); FRIEND_TEST(ForwardCommandControllerTest, InterfaceParameterNotSet); 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 74cf5a7550..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 @@ -35,8 +35,7 @@ using hardware_interface::HW_IF_VELOCITY; // subclassing and friending so we can access member variables class FriendMultiInterfaceForwardCommandController -: public forward_command_controller::MultiInterfaceForwardCommandController< - forward_command_controller::ForwardController> +: public forward_command_controller::MultiInterfaceForwardCommandController { FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, JointsParameterNotSet); FRIEND_TEST(MultiInterfaceForwardCommandControllerTest, InterfaceParameterNotSet); From 7afdefc95a4ae1c1f44d91cb78b593bd183928b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 24 Mar 2022 17:16:28 +0100 Subject: [PATCH 07/12] Fix all bugs in chained-controllers mode. --- .../src/chainable_forward_controller.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index bef1ceffda..09c62ad5b3 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -51,7 +51,12 @@ controller_interface::CallbackReturn ChainableForwardController::on_configure( reference_interface_names_ = command_interface_names_; // for any case make reference interfaces size of command interfaces reference_interfaces_.resize( - command_interfaces_.size(), std::numeric_limits::quiet_NaN()); + reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); + + for (const auto & value : reference_interfaces_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Reference interface value is %f", value); + } return CallbackReturn::SUCCESS; } @@ -128,6 +133,10 @@ controller_interface::return_type ChainableForwardController::update_and_write_c { if (!std::isnan(reference_interfaces_[i])) { + RCLCPP_ERROR( + get_node()->get_logger(), "Reference interface value is %f; Command interface name is '%s'", + reference_interfaces_[i], command_interfaces_[i].get_full_name().c_str()); + command_interfaces_[i].set_value(reference_interfaces_[i]); } } From 7c03f79e76dfa1a8e2cda945e03e64ea16792c07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 29 Mar 2022 19:24:27 +0200 Subject: [PATCH 08/12] Remove debug output from forwarding controller. --- .../src/chainable_forward_controller.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index 09c62ad5b3..e1ffa0553b 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -133,10 +133,6 @@ controller_interface::return_type ChainableForwardController::update_and_write_c { if (!std::isnan(reference_interfaces_[i])) { - RCLCPP_ERROR( - get_node()->get_logger(), "Reference interface value is %f; Command interface name is '%s'", - reference_interfaces_[i], command_interfaces_[i].get_full_name().c_str()); - command_interfaces_[i].set_value(reference_interfaces_[i]); } } From f8556eabd73a91e823442e71f23cd764b15b40ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 26 Apr 2022 21:53:24 +0200 Subject: [PATCH 09/12] Some compile fixes --- .../src/forward_controller.cpp | 14 ++++++++++---- .../src/forward_controllers_base.cpp | 6 +++--- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/forward_command_controller/src/forward_controller.cpp b/forward_command_controller/src/forward_controller.cpp index 0574ce7b5c..790738a9be 100644 --- a/forward_command_controller/src/forward_controller.cpp +++ b/forward_command_controller/src/forward_controller.cpp @@ -35,19 +35,25 @@ controller_interface::InterfaceConfiguration ForwardController::state_interface_ return get_state_interface_configuration(); } -CallbackReturn ForwardController::on_init() { return execute_init(get_node()); } +controller_interface::CallbackReturn ForwardController::on_init() +{ + return execute_init(get_node()); +} -CallbackReturn ForwardController::on_configure(const rclcpp_lifecycle::State & previous_state) +controller_interface::CallbackReturn ForwardController::on_configure( + const rclcpp_lifecycle::State & previous_state) { return execute_configure(previous_state, command_interfaces_); } -CallbackReturn ForwardController::on_activate(const rclcpp_lifecycle::State & previous_state) +controller_interface::CallbackReturn ForwardController::on_activate( + const rclcpp_lifecycle::State & previous_state) { return execute_activate(previous_state, command_interfaces_); } -CallbackReturn ForwardController::on_deactivate(const rclcpp_lifecycle::State & previous_state) +controller_interface::CallbackReturn ForwardController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) { return execute_deactivate(previous_state); } diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp index fcb7bb2ab8..3aa76d4423 100644 --- a/forward_command_controller/src/forward_controllers_base.cpp +++ b/forward_command_controller/src/forward_controllers_base.cpp @@ -73,7 +73,7 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_configure( // 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; } @@ -109,7 +109,7 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_activate( command_interface_names_.size() != ordered_interfaces.size()) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu command interfaces, got %zu", + node_->get_logger(), "Expected %zu command interfaces, got %zu", command_interface_names_.size(), ordered_interfaces.size()); return controller_interface::CallbackReturn::ERROR; } @@ -117,7 +117,7 @@ controller_interface::CallbackReturn ForwardControllersBase::execute_activate( // 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; } From d77b4b6b1c9723c8d955855e547fefa95d2fbcde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 26 Apr 2022 22:20:24 +0200 Subject: [PATCH 10/12] Fix some small issues. --- .../src/chainable_forward_controller.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/forward_command_controller/src/chainable_forward_controller.cpp b/forward_command_controller/src/chainable_forward_controller.cpp index e1ffa0553b..25a247dd2e 100644 --- a/forward_command_controller/src/chainable_forward_controller.cpp +++ b/forward_command_controller/src/chainable_forward_controller.cpp @@ -41,7 +41,6 @@ controller_interface::CallbackReturn ChainableForwardController::on_configure( const rclcpp_lifecycle::State & previous_state) { auto ret = execute_configure(previous_state, command_interfaces_); - RCLCPP_ERROR(get_node()->get_logger(), "Configure is successful, now tryting configure ref_itfs"); if (ret != CallbackReturn::SUCCESS) { return ret; @@ -53,11 +52,6 @@ controller_interface::CallbackReturn ChainableForwardController::on_configure( reference_interfaces_.resize( reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); - for (const auto & value : reference_interfaces_) - { - RCLCPP_ERROR(get_node()->get_logger(), "Reference interface value is %f", value); - } - return CallbackReturn::SUCCESS; } @@ -124,6 +118,8 @@ controller_interface::return_type ChainableForwardController::update_reference_f { reference_interfaces_ = (*joint_commands)->data; } + + return controller_interface::return_type::OK; } controller_interface::return_type ChainableForwardController::update_and_write_commands( From 778ed130ee8ab951509c18cce57ae1e41bdbb45d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 16 May 2022 20:56:42 +0200 Subject: [PATCH 11/12] Update to use class. --- forward_command_controller/forward_command_plugin.xml | 4 ++-- .../forward_command_controller.hpp | 6 +++--- .../multi_interface_forward_command_controller.hpp | 6 +++--- .../src/forward_command_controller.cpp | 2 +- .../src/multi_interface_forward_command_controller.cpp | 2 +- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/forward_command_controller/forward_command_plugin.xml b/forward_command_controller/forward_command_plugin.xml index 3697404a34..47d661d603 100644 --- a/forward_command_controller/forward_command_plugin.xml +++ b/forward_command_controller/forward_command_plugin.xml @@ -48,14 +48,14 @@ + base_class_type="controller_interface::ChainableControllerInterface"> The forward command controller commands a group of joints in a given interface + base_class_type="controller_interface::ChainableControllerInterface"> MultiInterfaceForwardController ros2_control controller. 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 c8f8dcca23..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 @@ -43,7 +43,7 @@ template < std::is_convertible::value, T>::type * = nullptr, typename std::enable_if< - std::is_convertible::value, T>::type * = + std::is_convertible::value, T>::type * = nullptr> class BaseForwardCommandController : public T { @@ -54,9 +54,9 @@ class BaseForwardCommandController : public T protected: void declare_parameters() override { - controller_interface::ControllerInterface::auto_declare>( + controller_interface::ControllerInterfaceBase::auto_declare>( "joints", std::vector()); - controller_interface::ControllerInterface::auto_declare("interface_name", ""); + controller_interface::ControllerInterfaceBase::auto_declare("interface_name", ""); }; controller_interface::CallbackReturn read_parameters() override 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 30cabfa6d6..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 @@ -43,7 +43,7 @@ template < std::is_convertible::value, T>::type * = nullptr, typename std::enable_if< - std::is_convertible::value, T>::type * = + std::is_convertible::value, T>::type * = nullptr> class BaseMultiInterfaceForwardCommandController : public T { @@ -54,8 +54,8 @@ class BaseMultiInterfaceForwardCommandController : public T protected: void declare_parameters() override { - controller_interface::ControllerInterface::auto_declare("joint", joint_name_); - controller_interface::ControllerInterface::auto_declare>( + controller_interface::ControllerInterfaceBase::auto_declare("joint", joint_name_); + controller_interface::ControllerInterfaceBase::auto_declare>( "interface_names", interface_names_); }; diff --git a/forward_command_controller/src/forward_command_controller.cpp b/forward_command_controller/src/forward_command_controller.cpp index 82319bbc65..55be87dfae 100644 --- a/forward_command_controller/src/forward_command_controller.cpp +++ b/forward_command_controller/src/forward_command_controller.cpp @@ -23,4 +23,4 @@ PLUGINLIB_EXPORT_CLASS( forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( forward_command_controller::ChainableForwardCommandController, - controller_interface::ControllerInterface) + controller_interface::ChainableControllerInterface) 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 a1020fcb17..e6749bda9e 100644 --- a/forward_command_controller/src/multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/src/multi_interface_forward_command_controller.cpp @@ -24,4 +24,4 @@ PLUGINLIB_EXPORT_CLASS( controller_interface::ControllerInterface) PLUGINLIB_EXPORT_CLASS( forward_command_controller::ChainableMultiInterfaceForwardCommandController, - controller_interface::ControllerInterface) + controller_interface::ChainableControllerInterface) From 6d960fede7ad1b489cf8f90b7c306bddc9cf5bd2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 2 Jul 2022 14:52:20 +0200 Subject: [PATCH 12/12] Fix deprecation of setup.cfg --- ros2_controllers_test_nodes/setup.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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