From 014848ca27350081f821cf272e4412431c7564c7 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 18 Jun 2024 09:22:28 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=9A=80=20Add=20PID=20controller=20?= =?UTF-8?q?=F0=9F=8E=89=20(backport=20#434,=20#975,=20#899,=20#1084,=20#95?= =?UTF-8?q?1)=20(#1163)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ackermann_steering_controller/package.xml | 1 + bicycle_steering_controller/package.xml | 1 + doc/controllers_index.rst | 1 + pid_controller/CMakeLists.txt | 108 ++++ pid_controller/README.md | 7 + pid_controller/doc/userdoc.rst | 103 ++++ .../include/pid_controller/pid_controller.hpp | 140 +++++ .../pid_controller/visibility_control.h | 49 ++ pid_controller/package.xml | 37 ++ pid_controller/pid_controller.xml | 8 + pid_controller/src/pid_controller.cpp | 523 +++++++++++++++++ pid_controller/src/pid_controller.yaml | 87 +++ .../test/pid_controller_params.yaml | 11 + .../test/pid_controller_preceding_params.yaml | 11 + .../test/test_load_pid_controller.cpp | 43 ++ pid_controller/test/test_pid_controller.cpp | 532 ++++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 287 ++++++++++ .../test/test_pid_controller_preceding.cpp | 104 ++++ ros2_controllers/package.xml | 1 + tricycle_steering_controller/package.xml | 1 + 20 files changed, 2055 insertions(+) create mode 100644 pid_controller/CMakeLists.txt create mode 100644 pid_controller/README.md create mode 100644 pid_controller/doc/userdoc.rst create mode 100644 pid_controller/include/pid_controller/pid_controller.hpp create mode 100644 pid_controller/include/pid_controller/visibility_control.h create mode 100644 pid_controller/package.xml create mode 100644 pid_controller/pid_controller.xml create mode 100644 pid_controller/src/pid_controller.cpp create mode 100644 pid_controller/src/pid_controller.yaml create mode 100644 pid_controller/test/pid_controller_params.yaml create mode 100644 pid_controller/test/pid_controller_preceding_params.yaml create mode 100644 pid_controller/test/test_load_pid_controller.cpp create mode 100644 pid_controller/test/test_pid_controller.cpp create mode 100644 pid_controller/test/test_pid_controller.hpp create mode 100644 pid_controller/test/test_pid_controller_preceding.cpp diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 2f9cc2d22e..d074c6ff30 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -16,6 +16,7 @@ generate_parameter_library + backward_ros control_msgs controller_interface hardware_interface diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 1f1ec12562..25daab72d4 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -16,6 +16,7 @@ generate_parameter_library + backward_ros control_msgs controller_interface hardware_interface diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 8d6480f290..2cb55150ce 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -52,6 +52,7 @@ The controllers are using `common hardware interface definitions`_, and may use Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Gripper Controller <../gripper_controllers/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> + PID Controller <../pid_controller/doc/userdoc.rst> Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt new file mode 100644 index 0000000000..81cbe6f006 --- /dev/null +++ b/pid_controller/CMakeLists.txt @@ -0,0 +1,108 @@ +cmake_minimum_required(VERSION 3.16) +project(pid_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + angles + control_msgs + control_toolbox + controller_interface + generate_parameter_library + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(pid_controller_parameters + src/pid_controller.yaml +) + +add_library(pid_controller SHARED + src/pid_controller.cpp +) +target_compile_features(pid_controller PUBLIC cxx_std_17) +target_include_directories(pid_controller PUBLIC + $ + $ +) +target_link_libraries(pid_controller PUBLIC + pid_controller_parameters +) +ament_target_dependencies(pid_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface pid_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_rostest_with_parameters_gmock( + test_pid_controller + test/test_pid_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_params.yaml + ) + target_include_directories(test_pid_controller PRIVATE include) + target_link_libraries(test_pid_controller pid_controller) + ament_target_dependencies( + test_pid_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock( + test_pid_controller_preceding + test/test_pid_controller_preceding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceding_params.yaml + ) + target_include_directories(test_pid_controller_preceding PRIVATE include) + target_link_libraries(test_pid_controller_preceding pid_controller) + ament_target_dependencies( + test_pid_controller_preceding + controller_interface + hardware_interface + ) + + ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp) + target_include_directories(test_load_pid_controller PRIVATE include) + ament_target_dependencies( + test_load_pid_controller + controller_manager + ros2_control_test_assets + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/pid_controller +) + +install(TARGETS + pid_controller + pid_controller_parameters + EXPORT export_pid_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_pid_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pid_controller/README.md b/pid_controller/README.md new file mode 100644 index 0000000000..01e8fddac8 --- /dev/null +++ b/pid_controller/README.md @@ -0,0 +1,7 @@ +pid_controller +========================================== + +Controller based on PID implementation from control_toolbox package. + +Pluginlib-Library: pid_controller +Plugin: pid_controller/PidController (controller_interface::ControllerInterface) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst new file mode 100644 index 0000000000..fc94357ab3 --- /dev/null +++ b/pid_controller/doc/userdoc.rst @@ -0,0 +1,103 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pid_controller/doc/userdoc.rst + +.. _pid_controller_userdoc: + +PID Controller +-------------------------------- + +PID Controller implementation that uses PidROS implementation from `control_toolbox `_ package. +The controller can be used directly by sending references through a topic or in a chain having preceding or following controllers. +It also enables to use the first derivative of the reference and its feedback to have second-order PID control. + +Depending on the reference/state and command interface of the hardware a different parameter setup of PidROS should be used as for example: + +- reference/state POSITION; command VELOCITY --> PI CONTROLLER +- reference/state VELOCITY; command ACCELERATION --> PI CONTROLLER + +- reference/state VELOCITY; command POSITION --> PD CONTROLLER +- reference/state ACCELERATION; command VELOCITY --> PD CONTROLLER + +- reference/state POSITION; command POSITION --> PID CONTROLLER +- reference/state VELOCITY; command VELOCITY --> PID CONTROLLER +- reference/state ACCELERATION; command ACCELERATION --> PID CONTROLLER +- reference/state EFFORT; command EFFORT --> PID CONTROLLER + +.. note:: + + Theoretically one can misuse :ref:`Joint Trajectory Controller (JTC)` for the same purpose by sending only one reference point into it. + Nevertheless, this is not recommended. JTC should be used if you need to interpolate between trajectory points using linear, cubic or quintic interpolation. PID Controller doesn't do that. + PID term of JTC has different purpose - it enables commanding only ``velocity`` or ``effort`` interfaces to hardware. + +Execution logic of the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The controller can be also used in "feed-forward" mode where feed-forward gain is used to increase controllers dynamics. +If one type of the reference and state interfaces is used, only immediate error is used. If there are two, then the second interface type is considered to be the first derivative of the first type. +For example a valid combination would be ``position`` and ``velocity`` interface types. + +Using the controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Pluginlib-Library: pid_controller +Plugin name: pid_controller/PidController + +Description of controller's interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``. + +Commands +,,,,,,,,, +- / [double] + +States +,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``. + + +Subscribers +,,,,,,,,,,,, +If controller is not in chained mode (``in_chained_mode == false``): + +- /reference [control_msgs/msg/MultiDOFCommand] + +If controller parameter ``use_external_measured_states`` is true: + +- /measured_state [control_msgs/msg/MultiDOFCommand] + +Services +,,,,,,,,,,, + +- /set_feedforward_control [std_srvs/srv/SetBool] + +Publishers +,,,,,,,,,,, +- /controller_state [control_msgs/msg/MultiDOFStateStamped] + +Parameters +,,,,,,,,,,, + +The PID controller uses the `generate_parameter_library `_ to handle its parameters. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/pid_controller.yaml + + +An example parameter file +========================= + + +An example parameter file for this controller can be found in `the test folder (standalone) `_: + +.. literalinclude:: ../test/pid_controller_params.yaml + :language: yaml + +or as `preceding controller `_: + +.. literalinclude:: ../test/pid_controller_preceding_params.yaml + :language: yaml diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp new file mode 100644 index 0000000000..f7b8cc4491 --- /dev/null +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -0,0 +1,140 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschrä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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_ +#define PID_CONTROLLER__PID_CONTROLLER_HPP_ + +#include +#include +#include + +#include "control_msgs/msg/multi_dof_command.hpp" +#include "control_msgs/msg/multi_dof_state_stamped.hpp" +#include "control_toolbox/pid_ros.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "pid_controller/visibility_control.h" +#include "pid_controller_parameters.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +#include "control_msgs/msg/joint_controller_state.hpp" + +#include "control_msgs/msg/pid_state.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +namespace pid_controller +{ + +enum class feedforward_mode_type : std::uint8_t +{ + OFF = 0, + ON = 1, +}; + +class PidController : public controller_interface::ChainableControllerInterface +{ +public: + PID_CONTROLLER__VISIBILITY_PUBLIC + PidController(); + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + PID_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + using ControllerReferenceMsg = control_msgs::msg::MultiDOFCommand; + using ControllerMeasuredStateMsg = control_msgs::msg::MultiDOFCommand; + using ControllerModeSrvType = std_srvs::srv::SetBool; + using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped; + +protected: + std::shared_ptr param_listener_; + pid_controller::Params params_; + + std::vector reference_and_state_dof_names_; + size_t dof_; + std::vector measured_state_values_; + + using PidPtr = std::shared_ptr; + std::vector pids_; + // Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector feedforward_gain_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + + rclcpp::Subscription::SharedPtr measured_state_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> measured_state_; + + rclcpp::Service::SharedPtr set_feedforward_control_service_; + realtime_tools::RealtimeBuffer control_mode_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // override methods from ChainableControllerInterface + std::vector on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; + + // internal methods + void update_parameters(); + controller_interface::CallbackReturn configure_parameters(); + +private: + // callback for topic interface + PID_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); +}; + +} // namespace pid_controller + +#endif // PID_CONTROLLER__PID_CONTROLLER_HPP_ diff --git a/pid_controller/include/pid_controller/visibility_control.h b/pid_controller/include/pid_controller/visibility_control.h new file mode 100644 index 0000000000..1509364b5a --- /dev/null +++ b/pid_controller/include/pid_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschrä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 PID_CONTROLLER__VISIBILITY_CONTROL_H_ +#define PID_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define PID_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define PID_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define PID_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef PID_CONTROLLER__VISIBILITY_BUILDING_DLL +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_EXPORT +#else +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_IMPORT +#endif +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL +#else +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define PID_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL +#endif +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // PID_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/pid_controller/package.xml b/pid_controller/package.xml new file mode 100644 index 0000000000..eb2815225f --- /dev/null +++ b/pid_controller/package.xml @@ -0,0 +1,37 @@ + + + + pid_controller + 0.0.0 + Controller based on PID implememenation from control_toolbox package. + Bence Magyar + Denis Štogl + Denis Štogl + Apache-2.0 + + ament_cmake + + generate_parameter_library + + angles + backward_ros + control_msgs + control_toolbox + controller_interface + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/pid_controller/pid_controller.xml b/pid_controller/pid_controller.xml new file mode 100644 index 0000000000..5b90741ae6 --- /dev/null +++ b/pid_controller/pid_controller.xml @@ -0,0 +1,8 @@ + + + + PidController ros2_control controller. + + + diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp new file mode 100644 index 0000000000..c4b447b514 --- /dev/null +++ b/pid_controller/src/pid_controller.cpp @@ -0,0 +1,523 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschrä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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "pid_controller/pid_controller.hpp" + +#include +#include +#include +#include + +#include "angles/angles.h" +#include "control_msgs/msg/single_dof_state.hpp" +#include "controller_interface/helpers.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace +{ // utility + +// Changed services history QoS to keep all so we don't lose any client service calls +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); + +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + const std::shared_ptr & msg, const std::vector & dof_names) +{ + msg->dof_names = dof_names; + msg->values.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); + msg->values_dot.resize(dof_names.size(), std::numeric_limits::quiet_NaN()); +} + +void reset_controller_measured_state_msg( + const std::shared_ptr & msg, const std::vector & dof_names) +{ + reset_controller_reference_msg(msg, dof_names); +} + +} // namespace + +namespace pid_controller +{ +PidController::PidController() : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn PidController::on_init() +{ + control_mode_.initRT(feedforward_mode_type::OFF); + + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void PidController::update_parameters() +{ + if (!param_listener_->is_old(params_)) + { + return; + } + params_ = param_listener_->get_params(); +} + +controller_interface::CallbackReturn PidController::configure_parameters() +{ + update_parameters(); + + if (!params_.reference_and_state_dof_names.empty()) + { + reference_and_state_dof_names_ = params_.reference_and_state_dof_names; + } + else + { + reference_and_state_dof_names_ = params_.dof_names; + } + + if (params_.dof_names.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'dof_names' (%zu) and 'reference_and_state_dof_names' (%zu) parameters has to be " + "the same!", + params_.dof_names.size(), reference_and_state_dof_names_.size()); + return CallbackReturn::FAILURE; + } + + dof_ = params_.dof_names.size(); + + // TODO(destogl): is this even possible? Test it... + if (params_.gains.dof_names_map.size() != dof_) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'gains' (%zu) map and number or 'dof_names' (%zu) have to be the same!", + params_.gains.dof_names_map.size(), dof_); + return CallbackReturn::FAILURE; + } + + pids_.resize(dof_); + + for (size_t i = 0; i < dof_; ++i) + { + // prefix should be interpreted as parameters prefix + pids_[i] = + std::make_shared(get_node(), "gains." + params_.dof_names[i], true); + if (!pids_[i]->initPid()) + { + return CallbackReturn::FAILURE; + } + } + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reference_and_state_dof_names_.clear(); + pids_.clear(); + + return CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto ret = configure_parameters(); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&PidController::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, reference_and_state_dof_names_); + input_ref_.writeFromNonRT(msg); + + // input state Subscriber and callback + if (params_.use_external_measured_states) + { + auto measured_state_callback = + [&](const std::shared_ptr msg) -> void + { + // TODO(destogl): Sort the input values based on joint and interface names + measured_state_.writeFromNonRT(msg); + }; + measured_state_subscriber_ = get_node()->create_subscription( + "~/measured_state", subscribers_qos, measured_state_callback); + } + std::shared_ptr measured_state_msg = + std::make_shared(); + reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); + measured_state_.writeFromNonRT(measured_state_msg); + + measured_state_values_.resize( + dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + auto set_feedforward_control_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + if (request->data) + { + control_mode_.writeFromNonRT(feedforward_mode_type::ON); + } + else + { + control_mode_.writeFromNonRT(feedforward_mode_type::OFF); + } + response->success = true; + }; + + set_feedforward_control_service_ = get_node()->create_service( + "~/set_feedforward_control", set_feedforward_control_callback, qos_services); + + try + { + // State publisher + s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Reserve memory in state publisher + state_publisher_->lock(); + state_publisher_->msg_.dof_states.resize(reference_and_state_dof_names_.size()); + for (size_t i = 0; i < reference_and_state_dof_names_.size(); ++i) + { + state_publisher_->msg_.dof_states[i].name = reference_and_state_dof_names_[i]; + } + state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void PidController::reference_callback(const std::shared_ptr msg) +{ + if (msg->dof_names.empty() && msg->values.size() == reference_and_state_dof_names_.size()) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Reference massage does not have DoF names defined. " + "Assuming that value have order as defined state DoFs"); + auto ref_msg = msg; + ref_msg->dof_names = reference_and_state_dof_names_; + input_ref_.writeFromNonRT(ref_msg); + } + else if ( + msg->dof_names.size() == reference_and_state_dof_names_.size() && + msg->values.size() == reference_and_state_dof_names_.size()) + { + auto ref_msg = msg; // simple initialization + + // sort values in the ref_msg + reset_controller_reference_msg(msg, reference_and_state_dof_names_); + + bool all_found = true; + for (size_t i = 0; i < msg->dof_names.size(); ++i) + { + auto found_it = + std::find(ref_msg->dof_names.begin(), ref_msg->dof_names.end(), msg->dof_names[i]); + if (found_it == msg->dof_names.end()) + { + all_found = false; + RCLCPP_WARN( + get_node()->get_logger(), "DoF name '%s' not found in the defined list of state DoFs.", + msg->dof_names[i].c_str()); + break; + } + + auto position = std::distance(ref_msg->dof_names.begin(), found_it); + ref_msg->values[position] = msg->values[i]; + ref_msg->values_dot[position] = msg->values_dot[i]; + } + + if (all_found) + { + input_ref_.writeFromNonRT(ref_msg); + } + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data names (%zu) and/or values (%zu) is not matching the expected size (%zu).", + msg->dof_names.size(), msg->values.size(), reference_and_state_dof_names_.size()); + } +} + +controller_interface::InterfaceConfiguration PidController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(params_.dof_names.size()); + for (const auto & dof_name : params_.dof_names) + { + command_interfaces_config.names.push_back(dof_name + "/" + params_.command_interface); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PidController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + + if (params_.use_external_measured_states) + { + state_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + } + else + { + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(dof_ * params_.reference_and_state_interfaces.size()); + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + state_interfaces_config.names.push_back(dof_name + "/" + interface); + } + } + } + + return state_interfaces_config; +} + +std::vector PidController::on_export_reference_interfaces() +{ + reference_interfaces_.resize( + dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + size_t index = 0; + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), dof_name + "/" + interface, &reference_interfaces_[index])); + ++index; + } + } + + return reference_interfaces; +} + +bool PidController::on_set_chained_mode(bool chained_mode) +{ + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn PidController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Set default value in command (the same number as state interfaces) + reset_controller_reference_msg(*(input_ref_.readFromRT()), reference_and_state_dof_names_); + reset_controller_measured_state_msg( + *(measured_state_.readFromRT()), reference_and_state_dof_names_); + + reference_interfaces_.assign( + reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + measured_state_values_.assign( + measured_state_values_.size(), std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PidController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto current_ref = input_ref_.readFromRT(); + + for (size_t i = 0; i < dof_; ++i) + { + if (!std::isnan((*current_ref)->values[i])) + { + reference_interfaces_[i] = (*current_ref)->values[i]; + if (reference_interfaces_.size() == 2 * dof_ && !std::isnan((*current_ref)->values_dot[i])) + { + reference_interfaces_[dof_ + i] = (*current_ref)->values_dot[i]; + } + + (*current_ref)->values[i] = std::numeric_limits::quiet_NaN(); + } + } + return controller_interface::return_type::OK; +} + +controller_interface::return_type PidController::update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + // check for any parameter updates + update_parameters(); + + if (params_.use_external_measured_states) + { + const auto measured_state = *(measured_state_.readFromRT()); + for (size_t i = 0; i < dof_; ++i) + { + measured_state_values_[i] = measured_state->values[i]; + if (measured_state_values_.size() == 2 * dof_) + { + measured_state_values_[dof_ + i] = measured_state->values_dot[i]; + } + } + } + else + { + for (size_t i = 0; i < measured_state_values_.size(); ++i) + { + measured_state_values_[i] = state_interfaces_[i].get_value(); + } + } + + for (size_t i = 0; i < dof_; ++i) + { + double tmp_command = std::numeric_limits::quiet_NaN(); + + // Using feedforward + if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i])) + { + // calculate feed-forward + if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) + { + tmp_command = reference_interfaces_[dof_ + i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } + else + { + tmp_command = 0.0; + } + + double error = reference_interfaces_[i] - measured_state_values_[i]; + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) + { + // for continuous angles the error is normalized between -picomputeCommand( + error, reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); + } + else + { + // Fallback to calculation with 'error' only + tmp_command += pids_[i]->computeCommand(error, period); + } + } + else + { + // use calculation with 'error' only + tmp_command += pids_[i]->computeCommand(error, period); + } + + // write calculated values + command_interfaces_[i].set_value(tmp_command); + } + } + + if (state_publisher_ && state_publisher_->trylock()) + { + state_publisher_->msg_.header.stamp = time; + for (size_t i = 0; i < dof_; ++i) + { + state_publisher_->msg_.dof_states[i].reference = reference_interfaces_[i]; + state_publisher_->msg_.dof_states[i].feedback = measured_state_values_[i]; + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i]; + } + state_publisher_->msg_.dof_states[i].error = + reference_interfaces_[i] - measured_state_values_[i]; + if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) + { + // for continuous angles the error is normalized between -pimsg_.dof_states[i].error = + angles::shortest_angular_distance(measured_state_values_[i], reference_interfaces_[i]); + } + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].error_dot = + reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i]; + } + state_publisher_->msg_.dof_states[i].time_step = period.seconds(); + // Command can store the old calculated values. This should be obvious because at least one + // another value is NaN. + state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); + } + state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace pid_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + pid_controller::PidController, controller_interface::ChainableControllerInterface) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml new file mode 100644 index 0000000000..f645738862 --- /dev/null +++ b/pid_controller/src/pid_controller.yaml @@ -0,0 +1,87 @@ +pid_controller: + dof_names: { + type: string_array, + default_value: [], + description: "Specifies dof_names or axes used by the controller. If 'reference_and_state_dof_names' parameter is defined, then only command dof names are defined with this parameter.", + read_only: true, + validation: { + unique<>: null, + not_empty<>: null, + } + } + reference_and_state_dof_names: { + type: string_array, + default_value: [], + description: "(optional) Specifies dof_names or axes for getting reference and reading states. This parameter is only relevant when state dof names are different then command dof names, i.e., when a following controller is used.", + read_only: true, + validation: { + unique<>: null, + } + } + command_interface: { + type: string, + default_value: "", + description: "Name of the interface used by the controller for writing commands to the hardware.", + read_only: true, + validation: { + not_empty<>: null, + } + } + reference_and_state_interfaces: { + type: string_array, + default_value: [], + description: "Name of the interfaces used by the controller getting hardware states and reference commands. The second interface should be the derivative of the first.", + read_only: true, + validation: { + not_empty<>: null, + size_lt<>: 3, + } + } + use_external_measured_states: { + type: bool, + default_value: false, + description: "Use external states from a topic instead from state interfaces." + } + gains: + __map_dof_names: + p: { + type: double, + default_value: 0.0, + description: "Proportional gain for PID" + } + i: { + type: double, + default_value: 0.0, + description: "Integral gain for PID" + } + d: { + type: double, + default_value: 0.0, + description: "Derivative gain for PID" + } + antiwindup: { + type: bool, + default_value: false, + description: "Antiwindup functionality." + } + i_clamp_max: { + type: double, + default_value: 0.0, + description: "Upper integral clamp. Only used if antiwindup is activated." + } + i_clamp_min: { + type: double, + default_value: 0.0, + description: "Lower integral clamp. Only used if antiwindup is activated." + } + feedforward_gain: { + type: double, + default_value: 0.0, + description: "Gain for the feed-forward part." + } + angle_wraparound: { + type: bool, + default_value: false, + description: "For joints that wrap around (i.e., are continuous). + Normalizes position-error to -pi to pi." + } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml new file mode 100644 index 0000000000..7555cfc156 --- /dev/null +++ b/pid_controller/test/pid_controller_params.yaml @@ -0,0 +1,11 @@ +test_pid_controller: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + gains: + joint1: {p: 1.0, i: 2.0, d: 10.0, i_clamp_max: 5.0, i_clamp_min: -5.0} diff --git a/pid_controller/test/pid_controller_preceding_params.yaml b/pid_controller/test/pid_controller_preceding_params.yaml new file mode 100644 index 0000000000..673abfe08e --- /dev/null +++ b/pid_controller/test/pid_controller_preceding_params.yaml @@ -0,0 +1,11 @@ +test_pid_controller: + ros__parameters: + dof_names: + - joint1 + + command_interface: position + + reference_and_state_interfaces: ["position"] + + reference_and_state_dof_names: + - joint1state diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp new file mode 100644 index 0000000000..3a75f6e170 --- /dev/null +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschrä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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#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(TestLoadPidController, 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_pid_controller", "pid_controller/PidController")); + + rclcpp::shutdown(); +} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp new file mode 100644 index 0000000000..a44347f5f1 --- /dev/null +++ b/pid_controller/test/test_pid_controller.cpp @@ -0,0 +1,532 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschrä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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_THAT(controller_->reference_and_state_dof_names_, testing::ElementsAreArray(dof_names_)); + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 10.0); + ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].antiwindup); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_max, 5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_min, -5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); +} + +TEST_F(PidControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) + { + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); + } + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +TEST_F(PidControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_EQ((*msg)->values.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values) + { + EXPECT_TRUE(std::isnan(cmd)); + } + EXPECT_EQ((*msg)->values_dot.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values_dot) + { + EXPECT_TRUE(std::isnan(cmd)); + } + + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(PidControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(PidControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, test_feedforward_mode_service) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + // initially set to OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // should stay false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // set to true + ASSERT_NO_THROW(call_service(true, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // set back to false + ASSERT_NO_THROW(call_service(false, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); +} + +TEST_F(PidControllerTest, test_update_logic_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } +} + +TEST_F(PidControllerTest, test_update_logic_feedforward_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + } +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // write reference interface so that the values are would be wrapped + + // run update + + // check the result of the commands - the values are not wrapped +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // write reference interface so that the values are would be wrapped + + // run update + + // check the result of the commands - the values are wrapped +} + +TEST_F(PidControllerTest, subscribe_and_get_messages_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + ASSERT_EQ(controller_->reference_interfaces_[i], 0.45); + } + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + ASSERT_EQ(msg.dof_states[i].reference, 0.45); + ASSERT_NE(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp new file mode 100644 index 0000000000..24d04c2bc7 --- /dev/null +++ b/pid_controller/test/test_pid_controller.hpp @@ -0,0 +1,287 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschrä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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#ifndef TEST_PID_CONTROLLER_HPP_ +#define TEST_PID_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "pid_controller/pid_controller.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = pid_controller::PidController::ControllerStateMsg; +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; +using ControllerModeSrvType = pid_controller::PidController::ControllerModeSrvType; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestablePidController : public pid_controller::PidController +{ + FRIEND_TEST(PidControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(PidControllerTest, activate_success); + FRIEND_TEST(PidControllerTest, reactivate_success); + FRIEND_TEST(PidControllerTest, test_feedforward_mode_service); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_off); + FRIEND_TEST(PidControllerTest, test_update_logic_feedforward_on); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_off); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_feedforward_on); + FRIEND_TEST(PidControllerTest, subscribe_and_get_messages_success); + FRIEND_TEST(PidControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = pid_controller::PidController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return pid_controller::PidController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class PidControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + dof_names_ = {"joint1"}; + command_interface_ = "position"; + state_interfaces_ = {"position"}; + dof_state_values_ = {1.1}; + dof_command_values_ = {101.101}; + reference_and_state_dof_names_ = {"joint1state"}; + + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + feedforward_service_client_ = service_caller_node_->create_client( + "/test_pid_controller/set_feedforward_control"); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_pid_controller") + { + ASSERT_EQ( + controller_->init(controller_name, "", rclcpp::NodeOptions()), + controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(dof_names_.size()); + command_ifs.reserve(dof_names_.size()); + + for (size_t i = 0; i < dof_names_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + dof_names_[i], command_interface_, &dof_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + std::vector state_ifs; + state_ifs.reserve(dof_names_.size() * state_interfaces_.size()); + state_itfs_.reserve(dof_names_.size() * state_interfaces_.size()); + size_t index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface(dof_name, interface, &dof_state_values_[index])); + state_ifs.emplace_back(state_itfs_.back()); + ++index; + } + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_pid_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands( + const std::vector & values = {0.45}, const std::vector & values_dot = {0.0}) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerCommandMsg msg; + msg.dof_names = dof_names_; + msg.values = values; + msg.values_dot = values_dot; + + command_publisher_->publish(msg); + } + + std::shared_ptr call_service( + const bool feedforward, rclcpp::Executor & executor) + { + auto request = std::make_shared(); + request->data = feedforward; + + bool wait_for_service_ret = + feedforward_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Service is not available!"); + } + auto result = feedforward_service_client_->async_send_request(request); + EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + + return result.get(); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + std::vector dof_names_; + std::string command_interface_; + std::vector state_interfaces_; + std::vector dof_state_values_; + std::vector dof_command_values_; + std::vector reference_and_state_dof_names_; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr service_caller_node_; + rclcpp::Client::SharedPtr feedforward_service_client_; +}; + +#endif // TEST_PID_CONTROLLER_HPP_ diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp new file mode 100644 index 0000000000..3e17e69286 --- /dev/null +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschrä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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_THAT( + controller_->params_.reference_and_state_dof_names, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_THAT( + controller_->reference_and_state_dof_names_, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_EQ(controller_->params_.command_interface, command_interface_); +} + +TEST_F(PidControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) + { + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); + } + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 8b9ee5108e..571191c96f 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -22,6 +22,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index a5683f94ea..73c6313ea0 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -18,6 +18,7 @@ generate_parameter_library + backward_ros control_msgs controller_interface hardware_interface