From f3d8b66a8b6356ffe542a5ace4bd0e39c3796808 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 10 Sep 2022 13:49:10 +0200 Subject: [PATCH 01/37] Create PID Controller package. --- pid_controller/CMakeLists.txt | 126 ++++++ pid_controller/LICENSE | 202 +++++++++ pid_controller/README.md | 7 + .../include/pid_controller/pid_controller.hpp | 116 ++++++ .../validate_pid_controller_parameters.hpp | 38 ++ .../pid_controller/visibility_control.h | 50 +++ pid_controller/package.xml | 39 ++ pid_controller/pid_controller.xml | 8 + pid_controller/src/pid_controller.cpp | 279 +++++++++++++ pid_controller/src/pid_controller.yaml | 31 ++ .../test/pid_controller_params.yaml | 7 + .../pid_controller_preceeding_params.yaml | 9 + .../test/test_load_pid_controller.cpp | 41 ++ pid_controller/test/test_pid_controller.cpp | 386 ++++++++++++++++++ pid_controller/test/test_pid_controller.hpp | 275 +++++++++++++ .../test/test_pid_controller_preceeding.cpp | 89 ++++ ros2_controllers/package.xml | 1 + 17 files changed, 1704 insertions(+) create mode 100644 pid_controller/CMakeLists.txt create mode 100644 pid_controller/LICENSE create mode 100644 pid_controller/README.md create mode 100644 pid_controller/include/pid_controller/pid_controller.hpp create mode 100644 pid_controller/include/pid_controller/validate_pid_controller_parameters.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_preceeding_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_preceeding.cpp diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt new file mode 100644 index 0000000000..31d20a0f66 --- /dev/null +++ b/pid_controller/CMakeLists.txt @@ -0,0 +1,126 @@ +cmake_minimum_required(VERSION 3.8) +project(pid_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs +) + +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# find dependencies +find_package(ament_cmake REQUIRED) + +# Add pid_controller library related compile commands +generate_parameter_library(pid_controller_parameters + src/pid_controller.yaml + include/pid_controller/validate_pid_controller_parameters.hpp +) +add_library( + pid_controller + SHARED + src/pid_controller.cpp +) +target_include_directories(pid_controller PRIVATE include) +target_link_libraries(pid_controller pid_controller_parameters) +ament_target_dependencies(pid_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface pid_controller.xml) + +install( + TARGETS + pid_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + 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 + hardware_interface + ros2_control_test_assets + ) + + 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_preceeding + test/test_pid_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceeding_params.yaml + ) + target_include_directories(test_pid_controller_preceeding PRIVATE include) + target_link_libraries(test_pid_controller_preceeding pid_controller) + ament_target_dependencies( + test_pid_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + +) +ament_export_libraries( + pid_controller +) + +ament_package() diff --git a/pid_controller/LICENSE b/pid_controller/LICENSE new file mode 100644 index 0000000000..d645695673 --- /dev/null +++ b/pid_controller/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/pid_controller/README.md b/pid_controller/README.md new file mode 100644 index 0000000000..2544278e5f --- /dev/null +++ b/pid_controller/README.md @@ -0,0 +1,7 @@ +pid_controller +========================================== + +Controller based on PID implememenation from control_toolbox package. + +Pluginlib-Library: pid_controller +Plugin: pid_controller/PidController (controller_interface::ControllerInterface) 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..f5151d1a0f --- /dev/null +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -0,0 +1,116 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2022, 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. + +#ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_ +#define PID_CONTROLLER__PID_CONTROLLER_HPP_ + +#include +#include +#include + +#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" + +// TODO(anyone): Replace with controller specific messages +#include "control_msgs/msg/joint_controller_state.hpp" +#include "control_msgs/msg/joint_jog.hpp" + +namespace pid_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_MY_ITFS = 0; + +// name constants for command interfaces +static constexpr size_t CMD_MY_ITFS = 0; + +// TODO(anyone: xample setup for control mode (usually you will use some enums defined in messages) +enum class control_mode_type : std::uint8_t +{ + FAST = 0, + SLOW = 1, +}; + +class PidController : public controller_interface::ChainableControllerInterface +{ +public: + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + PidController(); + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + // TODO(anyone): replace the state and command message types + using ControllerCommandMsg = control_msgs::msg::JointJog; + using ControllerModeSrvType = std_srvs::srv::SetBool; + using ControllerStateMsg = control_msgs::msg::JointControllerState; + +protected: + std::shared_ptr param_listener_; + pid_controller::Params params_; + + std::vector state_joints_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr cmd_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_cmd_; + + rclcpp::Service::SharedPtr set_slow_control_mode_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; +}; + +} // namespace pid_controller + +#endif // PID_CONTROLLER__PID_CONTROLLER_HPP_ diff --git a/pid_controller/include/pid_controller/validate_pid_controller_parameters.hpp b/pid_controller/include/pid_controller/validate_pid_controller_parameters.hpp new file mode 100644 index 0000000000..9f64c4e3b7 --- /dev/null +++ b/pid_controller/include/pid_controller/validate_pid_controller_parameters.hpp @@ -0,0 +1,38 @@ +// Copyright (c) 2022, 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__VALIDATE_PID_CONTROLLER_PARAMETERS_HPP_ +#define PID_CONTROLLER__VALIDATE_PID_CONTROLLER_PARAMETERS_HPP_ + +#include + +#include "parameter_traits/parameter_traits.hpp" + +namespace parameter_traits +{ +Result forbidden_interface_name_prefix(rclcpp::Parameter const & parameter) +{ + auto const & interface_name = parameter.as_string(); + + if (interface_name.rfind("blup_", 0) == 0) + { + return ERROR("'interface_name' parameter can not start with 'blup_'"); + } + + return OK; +} + +} // namespace parameter_traits + +#endif // PID_CONTROLLER__VALIDATE_PID_CONTROLLER_PARAMETERS_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..89fa6ad28c --- /dev/null +++ b/pid_controller/include/pid_controller/visibility_control.h @@ -0,0 +1,50 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2022, 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. + +#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 TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#endif +#define TEMPLATES__ROS2_CONTROL__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..176d2cf52d --- /dev/null +++ b/pid_controller/package.xml @@ -0,0 +1,39 @@ + + + + pid_controller + 0.0.0 + Controller based on PID implememenation from control_toolbox package. + Denis Štogl + Apache-2.0 + + ament_cmake + + generate_parameter_library + + control_msgs + + controller_interface + + hardware_interface + + pluginlib + + rclcpp + + rclcpp_lifecycle + + realtime_tools + + std_srvs + + ament_lint_auto + ament_cmake_gmock + controller_manager + hardware_interface + 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..ee29da64a9 --- /dev/null +++ b/pid_controller/src/pid_controller.cpp @@ -0,0 +1,279 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2022, 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 "pid_controller/pid_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" + +namespace +{ // utility + +// TODO(destogl): remove this when merged upstream +// Changed services history QoS to keep all so we don't lose any client service calls +static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +using ControllerCommandMsg = pid_controller::PidController::ControllerCommandMsg; + +// called from RT control loop +void reset_controller_command_msg( + const std::shared_ptr & msg, const std::vector & joint_names) +{ + msg->joint_names = joint_names; + msg->displacements.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); + msg->velocities.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace pid_controller +{ +PidController::PidController() : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn PidController::on_init() +{ + control_mode_.initRT(control_mode_type::FAST); + + 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; +} + +controller_interface::CallbackReturn PidController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + if (!params_.state_joints.empty()) + { + state_joints_ = params_.state_joints; + } + else + { + state_joints_ = params_.joints; + } + + // Command Subscriber and callbacks + auto callback_cmd = [&](const std::shared_ptr msg) -> void { + if (msg->joint_names.size() == params_.joints.size()) + { + input_cmd_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received %zu , but expected %zu joints in command. Ignoring message.", + msg->joint_names.size(), params_.joints.size()); + } + }; + cmd_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), callback_cmd); + + std::shared_ptr msg = std::make_shared(); + reset_controller_command_msg(msg, params_.joints); + input_cmd_.writeFromNonRT(msg); + + auto set_slow_mode_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) { + if (request->data) + { + control_mode_.writeFromNonRT(control_mode_type::SLOW); + } + else + { + control_mode_.writeFromNonRT(control_mode_type::FAST); + } + response->success = true; + }; + + set_slow_control_mode_service_ = get_node()->create_service( + "~/set_slow_control_mode", set_slow_mode_service_callback, + rmw_qos_profile_services_hist_keep_all); + + try + { + // State publisher + s_publisher_ = + get_node()->create_publisher("~/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; + } + + // TODO(anyone): Reserve memory in state publisher depending on the message type + state_publisher_->lock(); + state_publisher_->msg_.header.frame_id = params_.joints[0]; + state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +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_.joints.size()); + for (const auto & joint : params_.joints) + { + command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PidController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_joints_.size()); + for (const auto & joint : state_joints_) + { + state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + } + + return state_interfaces_config; +} + +std::vector PidController::on_export_reference_interfaces() +{ + reference_interfaces_.resize(state_joints_.size(), std::numeric_limits::quiet_NaN()); + + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), state_joints_[i] + "/" + params_.interface_name, + &reference_interfaces_[i])); + } + + 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 + reset_controller_command_msg(*(input_cmd_.readFromRT()), state_joints_); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PidController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PidController::update_reference_from_subscribers() +{ + auto current_cmd = input_cmd_.readFromRT(); + + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + if (!std::isnan((*current_cmd)->displacements[i])) + { + reference_interfaces_[i] = (*current_cmd)->displacements[i]; + + (*current_cmd)->displacements[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*/) +{ + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + if (!std::isnan(reference_interfaces_[i])) + { + if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) + { + reference_interfaces_[i] /= 2; + } + command_interfaces_[i].set_value(reference_interfaces_[i]); + + reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); + } + } + + if (state_publisher_ && state_publisher_->trylock()) + { + state_publisher_->msg_.header.stamp = time; + state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].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..03069bc73c --- /dev/null +++ b/pid_controller/src/pid_controller.yaml @@ -0,0 +1,31 @@ +pid_controller: + joints: { + type: string_array, + default_value: [], + description: "Specifies joints used by the controller. If state joints parameter is defined, then only command joints are defined with this parameter.", + read_only: true, + validation: { + unique<>: null, + not_empty<>: null, + } + } + state_joints: { + type: string_array, + default_value: [], + description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + validation: { + unique<>: null, + } + } + interface_name: { + type: string, + default_value: "", + description: "Name of the interface used by the controller on joints and command_joints.", + read_only: true, + validation: { + not_empty<>: null, + one_of<>: [["position", "velocity", "acceleration", "effort",]], + forbidden_interface_name_prefix: null + } + } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml new file mode 100644 index 0000000000..8a72de7e8e --- /dev/null +++ b/pid_controller/test/pid_controller_params.yaml @@ -0,0 +1,7 @@ +test_pid_controller: + ros__parameters: + + joints: + - joint1 + + interface_name: acceleration diff --git a/pid_controller/test/pid_controller_preceeding_params.yaml b/pid_controller/test/pid_controller_preceeding_params.yaml new file mode 100644 index 0000000000..b81fdf9a5d --- /dev/null +++ b/pid_controller/test/pid_controller_preceeding_params.yaml @@ -0,0 +1,9 @@ +test_pid_controller: + ros__parameters: + joints: + - joint1 + + interface_name: acceleration + + state_joints: + - 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..5ac338cd74 --- /dev/null +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2022, 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(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..c4448d6f31 --- /dev/null +++ b/pid_controller/test/test_pid_controller.cpp @@ -0,0 +1,386 @@ +// Copyright (c) 2022, 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. + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::CMD_MY_ITFS; +using pid_controller::control_mode_type; +using pid_controller::STATE_MY_ITFS; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.joints.empty()); + ASSERT_TRUE(controller_->params_.state_joints.empty()); + ASSERT_TRUE(controller_->params_.interface_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); + ASSERT_TRUE(controller_->params_.state_joints.empty()); + ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(joint_names_)); + ASSERT_EQ(controller_->params_.interface_name, interface_name_); +} + +TEST_F(PidControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + + joint_names_[i] + "/" + interface_name_; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[i].get_interface_name(), joint_names_[i] + "/" + interface_name_); + } +} + +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_cmd_.readFromNonRT(); + EXPECT_EQ((*msg)->displacements.size(), joint_names_.size()); + for (const auto & cmd : (*msg)->displacements) + { + EXPECT_TRUE(std::isnan(cmd)); + } + EXPECT_EQ((*msg)->velocities.size(), joint_names_.size()); + for (const auto & cmd : (*msg)->velocities) + { + EXPECT_TRUE(std::isnan(cmd)); + } + + ASSERT_TRUE(std::isnan((*msg)->duration)); + + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.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_EQ(controller_->command_interfaces_[CMD_MY_ITFS].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, test_setting_slow_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 false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + + 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()), control_mode_type::FAST); + + // set to true + ASSERT_NO_THROW(call_service(true, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + + // set back to false + ASSERT_NO_THROW(call_service(false, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); +} + +TEST_F(PidControllerTest, test_update_logic_fast) +{ + 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()); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_cmd_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); + EXPECT_TRUE(std::isnan((*(controller_->input_cmd_.readFromRT()))->displacements[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(PidControllerTest, test_update_logic_slow) +{ + 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()); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + // When slow mode is enabled command ends up being half of the value + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_cmd_.writeFromNonRT(msg); + controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + ASSERT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); + EXPECT_TRUE(std::isnan((*(controller_->input_cmd_.readFromRT()))->displacements[0])); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_fast) +{ + 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()); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_cmd_.writeFromNonRT(msg); + // this is input source in chained mode + controller_->reference_interfaces_[STATE_MY_ITFS] = TEST_DISPLACEMENT * 2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + // reference_interfaces is directly applied + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); + // message is not touched in chained mode + EXPECT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_slow) +{ + 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()); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + // When slow mode is enabled command ends up being half of the value + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_cmd_.writeFromNonRT(msg); + controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); + // this is input source in chained mode + controller_->reference_interfaces_[STATE_MY_ITFS] = TEST_DISPLACEMENT * 4; + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + ASSERT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // reference_interfaces is directly applied + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); + // message is not touched in chained mode + EXPECT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(PidControllerTest, publish_status_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.set_point, 101.101); +} + +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.set_point, 101.101); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[CMD_MY_ITFS], 0.45); + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 0.45); +} + +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..1bc05fe62a --- /dev/null +++ b/pid_controller/test/test_pid_controller.hpp @@ -0,0 +1,275 @@ +// Copyright (c) 2022, 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 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" + +// TODO(anyone): replace the state and command message types +using ControllerStateMsg = pid_controller::PidController::ControllerStateMsg; +using ControllerCommandMsg = pid_controller::PidController::ControllerCommandMsg; +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_setting_slow_mode_service); + FRIEND_TEST(PidControllerTest, test_update_logic_fast); + FRIEND_TEST(PidControllerTest, test_update_logic_slow); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_fast); + FRIEND_TEST(PidControllerTest, test_update_logic_chainable_slow); + +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) + { + cmd_subscriber_wait_set_.add_subscription(cmd_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, cmd_subscriber_wait_set_, timeout); + } + + // TODO(anyone): add implementation of any methods of your controller is needed + +private: + rclcpp::WaitSet cmd_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() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_pid_controller/commands", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + slow_control_service_client_ = service_caller_node_->create_client( + "/test_pid_controller/set_slow_control_mode"); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_pid_controller") + { + ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (size_t i = 0; i < joint_command_values_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], interface_name_, &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + // TODO(anyone): Add other command interfaces, if any + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + for (size_t i = 0; i < joint_state_values_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], interface_name_, &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + // TODO(anyone): Add other state interfaces, if any + + 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/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)); + } + + // TODO(anyone): add/remove arguments as it suites your command message type + void publish_commands( + const std::vector & displacements = {0.45}, + const std::vector & velocities = {0.0}, const double duration = 1.25) + { + 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.joint_names = joint_names_; + msg.displacements = displacements; + msg.velocities = velocities; + msg.duration = duration; + + command_publisher_->publish(msg); + } + + std::shared_ptr call_service( + const bool slow_control, rclcpp::Executor & executor) + { + auto request = std::make_shared(); + request->data = slow_control; + + bool wait_for_service_ret = + slow_control_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Services is not available!"); + } + auto result = slow_control_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 joint_names_ = {"joint1"}; + std::vector state_joint_names_ = {"joint1state"}; + std::string interface_name_ = "acceleration"; + std::array joint_state_values_ = {1.1}; + std::array joint_command_values_ = {101.101}; + + 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 slow_control_service_client_; +}; + +#endif // TEST_PID_CONTROLLER_HPP_ diff --git a/pid_controller/test/test_pid_controller_preceeding.cpp b/pid_controller/test/test_pid_controller_preceeding.cpp new file mode 100644 index 0000000000..a89a3ab924 --- /dev/null +++ b/pid_controller/test/test_pid_controller_preceeding.cpp @@ -0,0 +1,89 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2022, 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 "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::CMD_MY_ITFS; +using pid_controller::control_mode_type; +using pid_controller::STATE_MY_ITFS; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.joints.empty()); + ASSERT_TRUE(controller_->params_.state_joints.empty()); + ASSERT_TRUE(controller_->params_.interface_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); + ASSERT_THAT(controller_->params_.state_joints, testing::ElementsAreArray(state_joint_names_)); + ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(state_joint_names_)); + ASSERT_EQ(controller_->params_.interface_name, interface_name_); +} + +TEST_F(PidControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); + } + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + + state_joint_names_[i] + "/" + interface_name_; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[i].get_interface_name(), state_joint_names_[i] + "/" + interface_name_); + } +} + +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 d34a5b1375..3b77b7cc69 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -20,6 +20,7 @@ imu_sensor_broadcaster joint_state_broadcaster joint_trajectory_controller + pid_controller position_controllers range_sensor_broadcaster steering_controllers_library From e9a1fc911a9ea9dcb7fe0702905193cf9af51e8b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 15 Sep 2022 15:51:09 +0200 Subject: [PATCH 02/37] Updated PID controller with functionality. --- pid_controller/CMakeLists.txt | 50 +-- .../include/pid_controller/pid_controller.hpp | 61 ++-- .../pid_controller/visibility_control.h | 33 +- pid_controller/src/pid_controller.cpp | 335 ++++++++++++++---- pid_controller/src/pid_controller.yaml | 79 ++++- .../test/pid_controller_params.yaml | 2 +- .../pid_controller_preceeding_params.yaml | 4 +- .../test/test_load_pid_controller.cpp | 1 - pid_controller/test/test_pid_controller.cpp | 89 ++--- pid_controller/test/test_pid_controller.hpp | 43 +-- .../test/test_pid_controller_preceeding.cpp | 37 +- 11 files changed, 511 insertions(+), 223 deletions(-) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 31d20a0f66..4c58376fb6 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -86,31 +86,31 @@ if(BUILD_TESTING) ros2_control_test_assets ) - 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_preceeding - test/test_pid_controller_preceeding.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceeding_params.yaml - ) - target_include_directories(test_pid_controller_preceeding PRIVATE include) - target_link_libraries(test_pid_controller_preceeding pid_controller) - ament_target_dependencies( - test_pid_controller_preceeding - controller_interface - hardware_interface - ) + #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_preceeding + #test/test_pid_controller_preceeding.cpp + #${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceeding_params.yaml + #) + #target_include_directories(test_pid_controller_preceeding PRIVATE include) + #target_link_libraries(test_pid_controller_preceeding pid_controller) + #ament_target_dependencies( + #test_pid_controller_preceeding + #controller_interface + #hardware_interface + #) endif() ament_export_include_directories( diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index f5151d1a0f..7871630c91 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -1,5 +1,4 @@ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2022, 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. @@ -20,6 +19,9 @@ #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" @@ -33,6 +35,9 @@ #include "control_msgs/msg/joint_controller_state.hpp" #include "control_msgs/msg/joint_jog.hpp" +#include "control_msgs/msg/pid_state.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + namespace pid_controller { // name constants for state interfaces @@ -41,64 +46,68 @@ static constexpr size_t STATE_MY_ITFS = 0; // name constants for command interfaces static constexpr size_t CMD_MY_ITFS = 0; -// TODO(anyone: xample setup for control mode (usually you will use some enums defined in messages) -enum class control_mode_type : std::uint8_t +enum class feedforward_mode_type : std::uint8_t { - FAST = 0, - SLOW = 1, + OFF = 0, + ON = 1, }; class PidController : public controller_interface::ChainableControllerInterface { public: - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + PID_CONTROLLER__VISIBILITY_PUBLIC PidController(); - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_reference_from_subscribers() override; - TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; // TODO(anyone): replace the state and command message types - using ControllerCommandMsg = control_msgs::msg::JointJog; + using ControllerReferenceMsg = control_msgs::msg::MultiDOFCommand; + using ControllerMeasuredStateMsg = control_msgs::msg::MultiDOFCommand; using ControllerModeSrvType = std_srvs::srv::SetBool; - using ControllerStateMsg = control_msgs::msg::JointControllerState; + using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped; protected: std::shared_ptr param_listener_; pid_controller::Params params_; - std::vector state_joints_; + std::vector reference_and_state_dof_names_; + size_t dof_; // Command subscribers and Controller State publisher - rclcpp::Subscription::SharedPtr cmd_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> input_cmd_; + 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_slow_control_mode_service_; - realtime_tools::RealtimeBuffer control_mode_; + rclcpp::Service::SharedPtr set_feedforward_control_service_; + realtime_tools::RealtimeBuffer control_mode_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; @@ -109,6 +118,16 @@ class PidController : public controller_interface::ChainableControllerInterface std::vector on_export_reference_interfaces() override; bool on_set_chained_mode(bool chained_mode) override; + + 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_; + +private: + // callback for topic interface + PID_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); }; } // namespace pid_controller diff --git a/pid_controller/include/pid_controller/visibility_control.h b/pid_controller/include/pid_controller/visibility_control.h index 89fa6ad28c..7c9198a2b4 100644 --- a/pid_controller/include/pid_controller/visibility_control.h +++ b/pid_controller/include/pid_controller/visibility_control.h @@ -1,5 +1,4 @@ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2022, 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. @@ -21,30 +20,30 @@ #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define PID_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#define PID_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define PID_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) #endif -#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#ifdef PID_CONTROLLER__VISIBILITY_BUILDING_DLL +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_EXPORT #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#define PID_CONTROLLER__VISIBILITY_PUBLIC PID_CONTROLLER__VISIBILITY_IMPORT #endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#define PID_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_IMPORT #if __GNUC__ >= 4 -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#define PID_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define PID_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) #else -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#define PID_CONTROLLER__VISIBILITY_PUBLIC +#define PID_CONTROLLER__VISIBILITY_LOCAL #endif -#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#define PID_CONTROLLER__VISIBILITY_PUBLIC_TYPE #endif #endif // PID_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index ee29da64a9..6be5b9a5ab 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2022, 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. @@ -20,8 +19,25 @@ #include #include +#include "control_msgs/msg/single_dof_state.hpp" #include "controller_interface/helpers.hpp" +// TODO(destogl): should we add some knowledge from the control there here? +// The main knowledge could be with regard to the input and output interfaces and their phyical +// meaning, for example: +// +// input POSITION; output VELOCITY --> PI CONTROLLER +// input VELOCITY; output ACCELERATION --> PI CONTROLLER +// +// input VELOCITY; output POSITION --> PD CONTROLLER +// input ACCELERATION; output VELOCITY --> PD CONTROLLER +// +// input POSITION; output POSITION --> PID CONTROLLER +// input VELOCITY; output VELOCITY --> PID CONTROLLER +// input ACCELERATION; output ACCELERATION --> PID CONTROLLER +// input EFFORT; output EFFORT --> PID CONTROLLER +// + namespace { // utility @@ -38,16 +54,20 @@ static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, false}; -using ControllerCommandMsg = pid_controller::PidController::ControllerCommandMsg; +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; // called from RT control loop -void reset_controller_command_msg( - const std::shared_ptr & msg, const std::vector & joint_names) +void reset_controller_reference_msg( + const std::shared_ptr & msg, const std::vector & dof_names) { - msg->joint_names = joint_names; - msg->displacements.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); - msg->velocities.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); + msg->dof_names = dof_names; + msg->values.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 @@ -58,7 +78,7 @@ PidController::PidController() : controller_interface::ChainableControllerInterf controller_interface::CallbackReturn PidController::on_init() { - control_mode_.initRT(control_mode_type::FAST); + control_mode_.initRT(feedforward_mode_type::OFF); try { @@ -78,53 +98,92 @@ controller_interface::CallbackReturn PidController::on_configure( { params_ = param_listener_->get_params(); - if (!params_.state_joints.empty()) + if (!params_.reference_and_state_dof_names.empty()) { - state_joints_ = params_.state_joints; + reference_and_state_dof_names_ = params_.reference_and_state_dof_names; } else { - state_joints_ = params_.joints; + reference_and_state_dof_names_ = params_.dof_names; } - // Command Subscriber and callbacks - auto callback_cmd = [&](const std::shared_ptr msg) -> void { - if (msg->joint_names.size() == params_.joints.size()) - { - input_cmd_.writeFromNonRT(msg); - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received %zu , but expected %zu joints in command. Ignoring message.", - msg->joint_names.size(), params_.joints.size()); - } - }; - cmd_subscriber_ = get_node()->create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), callback_cmd); + 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) + { + pids_[i] = + std::make_shared(get_node(), "gains." + params_.dof_names[i]); + pids_[i]->initPid(); + } - std::shared_ptr msg = std::make_shared(); - reset_controller_command_msg(msg, params_.joints); - input_cmd_.writeFromNonRT(msg); + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); - auto set_slow_mode_service_callback = + // 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, params_.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 { + 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); + + auto set_feedforward_control_callback = [&]( const std::shared_ptr request, std::shared_ptr response) { if (request->data) { - control_mode_.writeFromNonRT(control_mode_type::SLOW); + control_mode_.writeFromNonRT(feedforward_mode_type::ON); } else { - control_mode_.writeFromNonRT(control_mode_type::FAST); + control_mode_.writeFromNonRT(feedforward_mode_type::OFF); } response->success = true; }; - set_slow_control_mode_service_ = get_node()->create_service( - "~/set_slow_control_mode", set_slow_mode_service_callback, + set_feedforward_control_service_ = get_node()->create_service( + "~/set_feedforward_control", set_feedforward_control_callback, rmw_qos_profile_services_hist_keep_all); try @@ -142,24 +201,85 @@ controller_interface::CallbackReturn PidController::on_configure( return controller_interface::CallbackReturn::ERROR; } - // TODO(anyone): Reserve memory in state publisher depending on the message type + // Reserve memory in state publisher state_publisher_->lock(); - state_publisher_->msg_.header.frame_id = params_.joints[0]; + state_publisher_->msg_.dof_states.reserve(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] = control_msgs::msg::SingleDOFState(); + state_publisher_->msg_.dof_states[i].name = reference_and_state_dof_names_[i]; + } state_publisher_->unlock(); + // TODO(destogl): Add separate timer-callback for the controller status publisher + 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 + ref_msg->dof_names = reference_and_state_dof_names_; + ref_msg->values.assign(ref_msg->values.size(), std::numeric_limits::quiet_NaN()); + + 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]; + } + + 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_.joints.size()); - for (const auto & joint : params_.joints) + command_interfaces_config.names.reserve(params_.dof_names.size()); + for (const auto & joint : params_.dof_names) { - command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + command_interfaces_config.names.push_back(joint + "/" + params_.command_interface); } return command_interfaces_config; @@ -168,12 +288,23 @@ controller_interface::InterfaceConfiguration PidController::command_interface_co controller_interface::InterfaceConfiguration PidController::state_interface_configuration() const { controller_interface::InterfaceConfiguration state_interfaces_config; - state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(state_joints_.size()); - for (const auto & joint : state_joints_) + if (params_.use_external_measured_states) { - state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + 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 & joint : reference_and_state_dof_names_) + { + state_interfaces_config.names.push_back(joint + "/" + interface); + } + } } return state_interfaces_config; @@ -181,16 +312,21 @@ controller_interface::InterfaceConfiguration PidController::state_interface_conf std::vector PidController::on_export_reference_interfaces() { - reference_interfaces_.resize(state_joints_.size(), std::numeric_limits::quiet_NaN()); + 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()); - for (size_t i = 0; i < reference_interfaces_.size(); ++i) + size_t index = 0; + for (const auto & interface : params_.reference_and_state_interfaces) { - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), state_joints_[i] + "/" + params_.interface_name, - &reference_interfaces_[i])); + for (const auto & joint : reference_and_state_dof_names_) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), joint + "/" + interface, &reference_interfaces_[index])); + ++index; + } } return reference_interfaces; @@ -205,8 +341,17 @@ bool PidController::on_set_chained_mode(bool chained_mode) controller_interface::CallbackReturn PidController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // Set default value in command - reset_controller_command_msg(*(input_cmd_.readFromRT()), state_joints_); + // 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_); + + for (size_t i = 0; i < dof_; ++i) + { + reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); + } + + // TODO(destogl): make here parameter update return controller_interface::CallbackReturn::SUCCESS; } @@ -216,7 +361,7 @@ controller_interface::CallbackReturn PidController::on_deactivate( { // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, // instead of a loop - for (size_t i = 0; i < command_interfaces_.size(); ++i) + for (size_t i = 0; i < dof_; ++i) { command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); } @@ -225,46 +370,96 @@ controller_interface::CallbackReturn PidController::on_deactivate( controller_interface::return_type PidController::update_reference_from_subscribers() { - auto current_cmd = input_cmd_.readFromRT(); + auto current_ref = input_ref_.readFromRT(); - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop for (size_t i = 0; i < reference_interfaces_.size(); ++i) { - if (!std::isnan((*current_cmd)->displacements[i])) + if (!std::isnan((*current_ref)->values[i])) { - reference_interfaces_[i] = (*current_cmd)->displacements[i]; + reference_interfaces_[i] = (*current_ref)->values[i]; - (*current_cmd)->displacements[i] = std::numeric_limits::quiet_NaN(); + (*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*/) + const rclcpp::Time & time, const rclcpp::Duration & period) { - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop - for (size_t i = 0; i < command_interfaces_.size(); ++i) + // TODO(destogl): make here runtime parameter update if allowed + + auto measured_state = measured_state_.readFromRT(); + + for (size_t i = 0; i < dof_; ++i) { - if (!std::isnan(reference_interfaces_[i])) + double tmp_command = std::numeric_limits::quiet_NaN(); + + if ( + *(control_mode_.readFromRT()) == feedforward_mode_type::ON && + !std::isnan(reference_interfaces_[i]) && !std::isnan((*measured_state)->values[i])) { - if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) + // calculate feed-forward + if (!std::isnan(reference_interfaces_[dof_ + i])) { - reference_interfaces_[i] /= 2; + tmp_command = reference_interfaces_[dof_ + i] * + params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; + } + else + { + tmp_command = 0.0; } - command_interfaces_[i].set_value(reference_interfaces_[i]); - reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); + if ( + reference_interfaces_.size() > dof_ && + (*measured_state)->values.size()) // TODO(denis): make this Always sufficiently big + { + if ( + !std::isnan(reference_interfaces_[dof_ + i]) && + !std::isnan((*measured_state)->values[dof_ + i])) + { + // use calculation with 'error' and 'error_dot' + tmp_command += pids_[i]->computeCommand( + reference_interfaces_[i] - (*measured_state)->values[i], + reference_interfaces_[dof_ + i] - (*measured_state)->values[dof_ + i], period); + } + else + { + // Fallback to calculation with 'error' only + tmp_command += pids_[i]->computeCommand( + reference_interfaces_[i] - (*measured_state)->values[i], period); + } + } + else + { + // use calculation with 'error' only + tmp_command += + pids_[i]->computeCommand(reference_interfaces_[i] - (*measured_state)->values[i], period); + } } + + command_interfaces_[i].set_value(tmp_command); } if (state_publisher_ && state_publisher_->trylock()) { state_publisher_->msg_.header.stamp = time; - state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].get_value(); - + for (size_t i = 0; i < dof_; ++i) + { + state_publisher_->msg_.dof_states[i].set_point = reference_interfaces_[i]; + state_publisher_->msg_.dof_states[i].process_value = (*measured_state)->values[i]; + state_publisher_->msg_.dof_states[i].error = + reference_interfaces_[i] - (*measured_state)->values[i]; + state_publisher_->msg_.dof_states[i].time_step = period.nanoseconds(); + state_publisher_->msg_.dof_states[i].command = command_interfaces_[i].get_value(); + + auto gains = pids_[i]->getGains(); + state_publisher_->msg_.dof_states[i].p = gains.p_gain_; + state_publisher_->msg_.dof_states[i].i = gains.i_gain_; + state_publisher_->msg_.dof_states[i].d = gains.d_gain_; + state_publisher_->msg_.dof_states[i].i_clamp = gains.i_max_; + state_publisher_->msg_.dof_states[i].antiwindup = gains.antiwindup_; + } state_publisher_->unlockAndPublish(); } diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 03069bc73c..abf3fd80b0 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -1,31 +1,98 @@ pid_controller: - joints: { + dof_names: { type: string_array, default_value: [], - description: "Specifies joints used by the controller. If state joints parameter is defined, then only command joints are defined with this parameter.", + description: "Specifies dof_names or axis used by the controller. If '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, } } - state_joints: { + reference_and_state_dof_names: { type: string_array, default_value: [], - description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + description: "(optional) Specifies dof_names or axis 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, } } - interface_name: { + command_interface: { type: string, default_value: "", - description: "Name of the interface used by the controller on joints and command_joints.", + description: "Name of the interface used by the controller for writing commands to the hardware.", read_only: true, validation: { not_empty<>: null, one_of<>: [["position", "velocity", "acceleration", "effort",]], + } + } + reference_and_state_interfaces: { + type: string_array, + default_value: [], + description: "Name of the interfaces used by the controller getting hardware states and reference commands.", + read_only: true, + validation: { + not_empty<>: null, + size_lt<>: 3, + subset_of<>: [["position", "velocity", "acceleration", "effort",]], forbidden_interface_name_prefix: null } } + use_external_measured_states: { + type: bool, + default_value: false, + description: "Use external states from a topic instead form state interfaces." + } + controller_state_publish_rate: { + type: double, + default_value: 50.0, + description: "Rate controller state is published", + validation: { + lower_bounds: [0.1] + } + } + 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 antiwindeup is activated." + } + i_clamp_min: { + type: double, + default_value: 0.0, + description: "Lower integral clamp. Only used if antiwindeup is activated." + } + feedforward_gain: { + type: double, + default_value: 0.0, + description: "Scale for the feed-forward." + } + + runtime_param_update: { + type: bool, + default_value: false, + description: "Enable parameters update when controller is active on each control step. If the parameter is 'false' then reactivation of controller is needed to update parameters." + } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index 8a72de7e8e..51faf63cf6 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -1,7 +1,7 @@ test_pid_controller: ros__parameters: - joints: + dof_names: - joint1 interface_name: acceleration diff --git a/pid_controller/test/pid_controller_preceeding_params.yaml b/pid_controller/test/pid_controller_preceeding_params.yaml index b81fdf9a5d..12339aa195 100644 --- a/pid_controller/test/pid_controller_preceeding_params.yaml +++ b/pid_controller/test/pid_controller_preceeding_params.yaml @@ -1,9 +1,9 @@ test_pid_controller: ros__parameters: - joints: + dof_names: - joint1 interface_name: acceleration - state_joints: + state_dof_names: - joint1state diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp index 5ac338cd74..022fab834b 100644 --- a/pid_controller/test/test_load_pid_controller.cpp +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2022, 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. diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index c4448d6f31..da75922c8e 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -21,7 +21,7 @@ #include using pid_controller::CMD_MY_ITFS; -using pid_controller::control_mode_type; +using pid_controller::feedforward_mode_type; using pid_controller::STATE_MY_ITFS; class PidControllerTest : public PidControllerFixture @@ -32,15 +32,17 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) { SetUpController(); - ASSERT_TRUE(controller_->params_.joints.empty()); - ASSERT_TRUE(controller_->params_.state_joints.empty()); + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); ASSERT_TRUE(controller_->params_.interface_name.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); - ASSERT_TRUE(controller_->params_.state_joints.empty()); - ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(joint_names_)); + 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_reference_and_state_dof_names_, + testing::ElementsAreArray(dof_names_)); ASSERT_EQ(controller_->params_.interface_name, interface_name_); } @@ -51,30 +53,29 @@ TEST_F(PidControllerTest, check_exported_intefaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + ASSERT_EQ(command_intefaces.names.size(), dof_command_values_.size()); for (size_t i = 0; i < command_intefaces.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + interface_name_); } auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); for (size_t i = 0; i < state_intefaces.names.size(); ++i) { - EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + EXPECT_EQ(state_intefaces.names[i], dof_names_[i] + "/" + interface_name_); } // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_names_.size()); - for (size_t i = 0; i < joint_names_.size(); ++i) + ASSERT_EQ(reference_interfaces.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - joint_names_[i] + "/" + interface_name_; + dof_names_[i] + "/" + interface_name_; EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[i].get_interface_name(), joint_names_[i] + "/" + interface_name_); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), dof_names_[i] + "/" + interface_name_); } } @@ -87,12 +88,12 @@ TEST_F(PidControllerTest, activate_success) // check that the message is reset auto msg = controller_->input_cmd_.readFromNonRT(); - EXPECT_EQ((*msg)->displacements.size(), joint_names_.size()); + EXPECT_EQ((*msg)->displacements.size(), dof_names_.size()); for (const auto & cmd : (*msg)->displacements) { EXPECT_TRUE(std::isnan(cmd)); } - EXPECT_EQ((*msg)->velocities.size(), joint_names_.size()); + EXPECT_EQ((*msg)->velocities.size(), dof_names_.size()); for (const auto & cmd : (*msg)->velocities) { EXPECT_TRUE(std::isnan(cmd)); @@ -100,7 +101,7 @@ TEST_F(PidControllerTest, activate_success) ASSERT_TRUE(std::isnan((*msg)->duration)); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -153,22 +154,22 @@ TEST_F(PidControllerTest, test_setting_slow_mode_service) executor.add_node(controller_->get_node()->get_node_base_interface()); executor.add_node(service_caller_node_->get_node_base_interface()); - // initially set to false - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + // 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()), control_mode_type::FAST); + 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()), control_mode_type::SLOW); + 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()), control_mode_type::FAST); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); } TEST_F(PidControllerTest, test_update_logic_fast) @@ -186,9 +187,9 @@ TEST_F(PidControllerTest, test_update_logic_fast) // set command statically static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->joint_names = dof_names_; + msg->displacements.resize(dof_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); msg->duration = std::numeric_limits::quiet_NaN(); controller_->input_cmd_.writeFromNonRT(msg); @@ -197,10 +198,10 @@ TEST_F(PidControllerTest, test_update_logic_fast) controller_interface::return_type::OK); EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); + EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); EXPECT_TRUE(std::isnan((*(controller_->input_cmd_.readFromRT()))->displacements[0])); EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -223,9 +224,9 @@ TEST_F(PidControllerTest, test_update_logic_slow) static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); // When slow mode is enabled command ends up being half of the value - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->joint_names = dof_names_; + msg->displacements.resize(dof_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); msg->duration = std::numeric_limits::quiet_NaN(); controller_->input_cmd_.writeFromNonRT(msg); controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); @@ -236,9 +237,9 @@ TEST_F(PidControllerTest, test_update_logic_slow) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); + EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); EXPECT_TRUE(std::isnan((*(controller_->input_cmd_.readFromRT()))->displacements[0])); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -260,9 +261,9 @@ TEST_F(PidControllerTest, test_update_logic_chainable_fast) // set command statically static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->joint_names = dof_names_; + msg->displacements.resize(dof_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); msg->duration = std::numeric_limits::quiet_NaN(); controller_->input_cmd_.writeFromNonRT(msg); // this is input source in chained mode @@ -274,11 +275,11 @@ TEST_F(PidControllerTest, test_update_logic_chainable_fast) EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); // reference_interfaces is directly applied - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); + EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); // message is not touched in chained mode EXPECT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -301,9 +302,9 @@ TEST_F(PidControllerTest, test_update_logic_chainable_slow) static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); // When slow mode is enabled command ends up being half of the value - msg->joint_names = joint_names_; - msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->joint_names = dof_names_; + msg->displacements.resize(dof_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); msg->duration = std::numeric_limits::quiet_NaN(); controller_->input_cmd_.writeFromNonRT(msg); controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); @@ -317,10 +318,10 @@ TEST_F(PidControllerTest, test_update_logic_chainable_slow) controller_interface::return_type::OK); // reference_interfaces is directly applied - EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); + EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); // message is not touched in chained mode EXPECT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_names_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -369,7 +370,7 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(joint_command_values_[CMD_MY_ITFS], 0.45); + EXPECT_EQ(dof_command_values_[CMD_MY_ITFS], 0.45); subscribe_and_get_messages(msg); diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 1bc05fe62a..f4ec9596f0 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -35,7 +35,7 @@ // TODO(anyone): replace the state and command message types using ControllerStateMsg = pid_controller::PidController::ControllerStateMsg; -using ControllerCommandMsg = pid_controller::PidController::ControllerCommandMsg; +using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; using ControllerModeSrvType = pid_controller::PidController::ControllerModeSrvType; namespace @@ -65,7 +65,7 @@ class TestablePidController : public pid_controller::PidController // Only if on_configure is successful create subscription if (ret == CallbackReturn::SUCCESS) { - cmd_subscriber_wait_set_.add_subscription(cmd_subscriber_); + ref_subscriber_wait_set_.add_subscription(ref_subscriber_); } return ret; } @@ -100,13 +100,13 @@ class TestablePidController : public pid_controller::PidController rclcpp::Executor & executor, const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) { - return wait_for_command(executor, cmd_subscriber_wait_set_, timeout); + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); } // TODO(anyone): add implementation of any methods of your controller is needed private: - rclcpp::WaitSet cmd_subscriber_wait_set_; + rclcpp::WaitSet ref_subscriber_wait_set_; }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -140,25 +140,25 @@ class PidControllerFixture : public ::testing::Test ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); std::vector command_ifs; - command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); + command_itfs_.reserve(dof_command_values_.size()); + command_ifs.reserve(dof_command_values_.size()); - for (size_t i = 0; i < joint_command_values_.size(); ++i) + for (size_t i = 0; i < dof_command_values_.size(); ++i) { command_itfs_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], interface_name_, &joint_command_values_[i])); + dof_names_[i], interface_name_, &dof_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); } // TODO(anyone): Add other command interfaces, if any std::vector state_ifs; - state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); + state_itfs_.reserve(dof_state_values_.size()); + state_ifs.reserve(dof_state_values_.size()); - for (size_t i = 0; i < joint_state_values_.size(); ++i) + for (size_t i = 0; i < dof_state_values_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], interface_name_, &joint_state_values_[i])); + state_itfs_.emplace_back( + hardware_interface::StateInterface(dof_names_[i], interface_name_, &dof_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } // TODO(anyone): Add other state interfaces, if any @@ -224,10 +224,11 @@ class PidControllerFixture : public ::testing::Test wait_for_topic(command_publisher_->get_topic_name()); ControllerCommandMsg msg; - msg.joint_names = joint_names_; - msg.displacements = displacements; - msg.velocities = velocities; - msg.duration = duration; + msg.dof_names = dof_names_; + // TODO(destogl): Update name of the arguments and remove unnecessary ones + msg.values = displacements; + // msg.velocities = velocities; + // msg.duration = duration; command_publisher_->publish(msg); } @@ -255,11 +256,11 @@ class PidControllerFixture : public ::testing::Test // TODO(anyone): adjust the members as needed // Controller-related parameters - std::vector joint_names_ = {"joint1"}; - std::vector state_joint_names_ = {"joint1state"}; + std::vector dof_names_ = {"joint1"}; + std::vector state_dof_names_ = {"joint1state"}; std::string interface_name_ = "acceleration"; - std::array joint_state_values_ = {1.1}; - std::array joint_command_values_ = {101.101}; + std::array dof_state_values_ = {1.1}; + std::array dof_command_values_ = {101.101}; std::vector state_itfs_; std::vector command_itfs_; diff --git a/pid_controller/test/test_pid_controller_preceeding.cpp b/pid_controller/test/test_pid_controller_preceeding.cpp index a89a3ab924..dba2624d35 100644 --- a/pid_controller/test/test_pid_controller_preceeding.cpp +++ b/pid_controller/test/test_pid_controller_preceeding.cpp @@ -1,5 +1,4 @@ // Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) -// Copyright (c) 2022, 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. @@ -22,7 +21,7 @@ #include using pid_controller::CMD_MY_ITFS; -using pid_controller::control_mode_type; +using pid_controller::feedforward_mode_type; using pid_controller::STATE_MY_ITFS; class PidControllerTest : public PidControllerFixture @@ -33,15 +32,19 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) { SetUpController(); - ASSERT_TRUE(controller_->params_.joints.empty()); - ASSERT_TRUE(controller_->params_.state_joints.empty()); + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); ASSERT_TRUE(controller_->params_.interface_name.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); - ASSERT_THAT(controller_->params_.state_joints, testing::ElementsAreArray(state_joint_names_)); - ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(state_joint_names_)); + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_THAT( + controller_->params_.reference_and_state_dof_names, + testing::ElementsAreArray(reference_and_reference_and_state_dof_names_)); + ASSERT_THAT( + controller_->reference_and_reference_and_state_dof_names_, + testing::ElementsAreArray(reference_and_reference_and_state_dof_names_)); ASSERT_EQ(controller_->params_.interface_name, interface_name_); } @@ -52,30 +55,34 @@ TEST_F(PidControllerTest, check_exported_intefaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + ASSERT_EQ(command_intefaces.names.size(), dof_command_values_.size()); for (size_t i = 0; i < command_intefaces.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + interface_name_); } auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); for (size_t i = 0; i < state_intefaces.names.size(); ++i) { - EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); + EXPECT_EQ( + state_intefaces.names[i], + reference_and_reference_and_state_dof_names_[i] + "/" + interface_name_); } // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_names_.size()); - for (size_t i = 0; i < joint_names_.size(); ++i) + ASSERT_EQ(reference_interfaces.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - state_joint_names_[i] + "/" + interface_name_; + reference_and_reference_and_state_dof_names_[i] + "/" + + interface_name_; EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); EXPECT_EQ( - reference_interfaces[i].get_interface_name(), state_joint_names_[i] + "/" + interface_name_); + reference_interfaces[i].get_interface_name(), + reference_and_reference_and_state_dof_names_[i] + "/" + interface_name_); } } From f21ec62a7c548a9bb47168f3faba854f44ddac6d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 15 Sep 2022 23:19:45 +0200 Subject: [PATCH 03/37] Move parameter read and check into separate method for easier inheritance. --- .../include/pid_controller/pid_controller.hpp | 11 +++++++---- pid_controller/src/pid_controller.cpp | 9 +++++++-- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 7871630c91..3142c0e960 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -99,6 +99,11 @@ class PidController : public controller_interface::ChainableControllerInterface std::vector reference_and_state_dof_names_; size_t dof_; + 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_; @@ -119,10 +124,8 @@ class PidController : public controller_interface::ChainableControllerInterface bool on_set_chained_mode(bool chained_mode) override; - 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_; + // internal methods + controller_interface::CallbackReturn configure_parameters(); private: // callback for topic interface diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 6be5b9a5ab..f5193653c0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -93,8 +93,7 @@ controller_interface::CallbackReturn PidController::on_init() return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn PidController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +controller_interface::CallbackReturn PidController::configure_parameters() { params_ = param_listener_->get_params(); @@ -137,6 +136,12 @@ controller_interface::CallbackReturn PidController::on_configure( std::make_shared(get_node(), "gains." + params_.dof_names[i]); pids_[i]->initPid(); } +} + +controller_interface::CallbackReturn PidController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + configure_parameters(); // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); From 64cd572bfbbfd88752e593b9bb9a7e10b94d6ff3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 29 Sep 2022 20:10:49 +0200 Subject: [PATCH 04/37] Small fixes of pid_controller for compilation. --- pid_controller/CMakeLists.txt | 3 ++- pid_controller/package.xml | 9 +-------- pid_controller/src/pid_controller.cpp | 8 +++++++- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 4c58376fb6..419640eff0 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs controller_interface hardware_interface + parameter_traits pluginlib rclcpp rclcpp_lifecycle @@ -117,7 +118,7 @@ ament_export_include_directories( include ) ament_export_dependencies( - + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_export_libraries( pid_controller diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 176d2cf52d..a66a010ce2 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -12,22 +12,15 @@ generate_parameter_library control_msgs - controller_interface - hardware_interface - + parameter_traits pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - std_srvs - ament_lint_auto ament_cmake_gmock controller_manager hardware_interface diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index f5193653c0..df9aad702a 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -136,12 +136,18 @@ controller_interface::CallbackReturn PidController::configure_parameters() std::make_shared(get_node(), "gains." + params_.dof_names[i]); pids_[i]->initPid(); } + + return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn PidController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - configure_parameters(); + auto ret = configure_parameters(); + if (ret != CallbackReturn::SUCCESS) + { + return ret; + } // topics QoS auto subscribers_qos = rclcpp::SystemDefaultsQoS(); From 6b92bc2daea07f82e4ec65a00223cebea6538681 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 30 Nov 2022 15:49:12 +0100 Subject: [PATCH 05/37] Correct dependencies of PID controller. --- pid_controller/CMakeLists.txt | 3 +-- pid_controller/package.xml | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 419640eff0..5111cb13e6 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -14,6 +14,7 @@ endif() # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs + control_toolbox controller_interface hardware_interface parameter_traits @@ -75,7 +76,6 @@ if(BUILD_TESTING) set(ament_cmake_cpplint_FOUND TRUE) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) - find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp) @@ -83,7 +83,6 @@ if(BUILD_TESTING) ament_target_dependencies( test_load_pid_controller controller_manager - hardware_interface ros2_control_test_assets ) diff --git a/pid_controller/package.xml b/pid_controller/package.xml index a66a010ce2..ccfe28aba3 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -12,6 +12,7 @@ generate_parameter_library control_msgs + control_toolbox controller_interface hardware_interface parameter_traits @@ -23,7 +24,6 @@ ament_cmake_gmock controller_manager - hardware_interface ros2_control_test_assets From ea7941bb71d6064ee661bc7fcd5e00739fe42306 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 30 Nov 2022 23:57:11 +0100 Subject: [PATCH 06/37] Debug PID controller to actually work. --- .../include/pid_controller/pid_controller.hpp | 1 + pid_controller/src/pid_controller.cpp | 106 ++++++++++++------ pid_controller/src/pid_controller.yaml | 34 +++--- 3 files changed, 87 insertions(+), 54 deletions(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 3142c0e960..27f437ef65 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -98,6 +98,7 @@ class PidController : public controller_interface::ChainableControllerInterface std::vector reference_and_state_dof_names_; size_t dof_; + std::vector measured_state_values_; using PidPtr = std::shared_ptr; std::vector pids_; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index df9aad702a..f41c9e9f53 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -23,19 +23,19 @@ #include "controller_interface/helpers.hpp" // TODO(destogl): should we add some knowledge from the control there here? -// The main knowledge could be with regard to the input and output interfaces and their phyical +// The main knowledge could be with regard to the input (reference/state) and output (command) interfaces and their physical // meaning, for example: // -// input POSITION; output VELOCITY --> PI CONTROLLER -// input VELOCITY; output ACCELERATION --> PI CONTROLLER +// reference/state POSITION; command VELOCITY --> PI CONTROLLER +// reference/state VELOCITY; command ACCELERATION --> PI CONTROLLER // -// input VELOCITY; output POSITION --> PD CONTROLLER -// input ACCELERATION; output VELOCITY --> PD CONTROLLER +// reference/state VELOCITY; command POSITION --> PD CONTROLLER +// reference/state ACCELERATION; command VELOCITY --> PD CONTROLLER // -// input POSITION; output POSITION --> PID CONTROLLER -// input VELOCITY; output VELOCITY --> PID CONTROLLER -// input ACCELERATION; output ACCELERATION --> PID CONTROLLER -// input EFFORT; output EFFORT --> PID 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 // namespace @@ -62,6 +62,7 @@ void reset_controller_reference_msg( { 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( @@ -160,7 +161,7 @@ controller_interface::CallbackReturn PidController::on_configure( std::bind(&PidController::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); - reset_controller_reference_msg(msg, params_.dof_names); + reset_controller_reference_msg(msg, reference_and_state_dof_names_); input_ref_.writeFromNonRT(msg); // input state Subscriber and callback @@ -168,6 +169,7 @@ controller_interface::CallbackReturn PidController::on_configure( { 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( @@ -178,6 +180,9 @@ controller_interface::CallbackReturn PidController::on_configure( 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, @@ -214,10 +219,9 @@ controller_interface::CallbackReturn PidController::on_configure( // Reserve memory in state publisher state_publisher_->lock(); - state_publisher_->msg_.dof_states.reserve(reference_and_state_dof_names_.size()); + 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] = control_msgs::msg::SingleDOFState(); state_publisher_->msg_.dof_states[i].name = reference_and_state_dof_names_[i]; } state_publisher_->unlock(); @@ -247,8 +251,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names = reference_and_state_dof_names_; - ref_msg->values.assign(ref_msg->values.size(), std::numeric_limits::quiet_NaN()); + 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) @@ -266,6 +269,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names.begin(), found_it); ref_msg->values[position] = msg->values[i]; + ref_msg->values_dot[position] = msg->values_dot[i]; } if (all_found) @@ -357,10 +361,8 @@ controller_interface::CallbackReturn PidController::on_activate( reset_controller_measured_state_msg( *(measured_state_.readFromRT()), reference_and_state_dof_names_); - for (size_t i = 0; i < dof_; ++i) - { - reference_interfaces_[i] = std::numeric_limits::quiet_NaN(); - } + reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + measured_state_values_.assign(measured_state_values_.size(), std::numeric_limits::quiet_NaN()); // TODO(destogl): make here parameter update @@ -383,11 +385,15 @@ controller_interface::return_type PidController::update_reference_from_subscribe { auto current_ref = input_ref_.readFromRT(); - for (size_t i = 0; i < reference_interfaces_.size(); ++i) + 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(); } @@ -400,18 +406,34 @@ controller_interface::return_type PidController::update_and_write_commands( { // TODO(destogl): make here runtime parameter update if allowed - auto measured_state = measured_state_.readFromRT(); + if (params_.use_external_measured_states) + { + for (size_t i = 0; i < dof_; ++i) + { + measured_state_values_[i] = (*(measured_state_.readFromRT()))->values[i]; + if (measured_state_values_.size() == 2 * dof_) + { + measured_state_values_[dof_ + i] = (*(measured_state_.readFromRT()))->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(); - if ( - *(control_mode_.readFromRT()) == feedforward_mode_type::ON && - !std::isnan(reference_interfaces_[i]) && !std::isnan((*measured_state)->values[i])) + // Using feedforward + if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i])) { // calculate feed-forward - if (!std::isnan(reference_interfaces_[dof_ + i])) + 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; @@ -421,35 +443,34 @@ controller_interface::return_type PidController::update_and_write_commands( tmp_command = 0.0; } - if ( - reference_interfaces_.size() > dof_ && - (*measured_state)->values.size()) // TODO(denis): make this Always sufficiently big + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) { if ( !std::isnan(reference_interfaces_[dof_ + i]) && - !std::isnan((*measured_state)->values[dof_ + i])) + !std::isnan(measured_state_values_[dof_ + i])) { // use calculation with 'error' and 'error_dot' tmp_command += pids_[i]->computeCommand( - reference_interfaces_[i] - (*measured_state)->values[i], - reference_interfaces_[dof_ + i] - (*measured_state)->values[dof_ + i], period); + reference_interfaces_[i] - measured_state_values_[i], + reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); } else { // Fallback to calculation with 'error' only tmp_command += pids_[i]->computeCommand( - reference_interfaces_[i] - (*measured_state)->values[i], period); + reference_interfaces_[i] - measured_state_values_[i], period); } } else { // use calculation with 'error' only tmp_command += - pids_[i]->computeCommand(reference_interfaces_[i] - (*measured_state)->values[i], period); + pids_[i]->computeCommand(reference_interfaces_[i] - measured_state_values_[i], period); } - } - command_interfaces_[i].set_value(tmp_command); + // write calculated values + command_interfaces_[i].set_value(tmp_command); + } } if (state_publisher_ && state_publisher_->trylock()) @@ -458,10 +479,21 @@ controller_interface::return_type PidController::update_and_write_commands( for (size_t i = 0; i < dof_; ++i) { state_publisher_->msg_.dof_states[i].set_point = reference_interfaces_[i]; - state_publisher_->msg_.dof_states[i].process_value = (*measured_state)->values[i]; + state_publisher_->msg_.dof_states[i].process_value = measured_state_values_[i]; + if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + { + state_publisher_->msg_.dof_states[i].process_value = measured_state_values_[i]; + } state_publisher_->msg_.dof_states[i].error = - reference_interfaces_[i] - (*measured_state)->values[i]; - state_publisher_->msg_.dof_states[i].time_step = period.nanoseconds(); + reference_interfaces_[i] - measured_state_values_[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].command = command_interfaces_[i].get_value(); auto gains = pids_[i]->getGains(); diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index abf3fd80b0..116e6fa334 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -4,41 +4,41 @@ pid_controller: default_value: [], description: "Specifies dof_names or axis used by the controller. If '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, - } + #validation: { + #unique<>: null, + #not_empty<>: null, + #} } reference_and_state_dof_names: { type: string_array, default_value: [], description: "(optional) Specifies dof_names or axis 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, - } + #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, - one_of<>: [["position", "velocity", "acceleration", "effort",]], - } + #validation: { + #not_empty<>: null, + #one_of<>: [["position", "velocity", "acceleration", "effort",]], + #} } reference_and_state_interfaces: { type: string_array, default_value: [], description: "Name of the interfaces used by the controller getting hardware states and reference commands.", read_only: true, - validation: { - not_empty<>: null, - size_lt<>: 3, - subset_of<>: [["position", "velocity", "acceleration", "effort",]], - forbidden_interface_name_prefix: null - } + #validation: { + #not_empty<>: null, + #size_lt<>: 3, + #subset_of<>: [["position", "velocity", "acceleration", "effort",]], + #forbidden_interface_name_prefix: null + #} } use_external_measured_states: { type: bool, From 19c585959c55a28c03071224718767106c26c550 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 1 Dec 2022 15:50:38 +0100 Subject: [PATCH 07/37] Remove validation file since it is not needed. --- pid_controller/CMakeLists.txt | 1 - .../validate_pid_controller_parameters.hpp | 38 ------------------- pid_controller/src/pid_controller.cpp | 36 ++++++++++-------- pid_controller/test/test_pid_controller.hpp | 3 +- 4 files changed, 22 insertions(+), 56 deletions(-) delete mode 100644 pid_controller/include/pid_controller/validate_pid_controller_parameters.hpp diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 5111cb13e6..bc6222f445 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -37,7 +37,6 @@ find_package(ament_cmake REQUIRED) # Add pid_controller library related compile commands generate_parameter_library(pid_controller_parameters src/pid_controller.yaml - include/pid_controller/validate_pid_controller_parameters.hpp ) add_library( pid_controller diff --git a/pid_controller/include/pid_controller/validate_pid_controller_parameters.hpp b/pid_controller/include/pid_controller/validate_pid_controller_parameters.hpp deleted file mode 100644 index 9f64c4e3b7..0000000000 --- a/pid_controller/include/pid_controller/validate_pid_controller_parameters.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) 2022, 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__VALIDATE_PID_CONTROLLER_PARAMETERS_HPP_ -#define PID_CONTROLLER__VALIDATE_PID_CONTROLLER_PARAMETERS_HPP_ - -#include - -#include "parameter_traits/parameter_traits.hpp" - -namespace parameter_traits -{ -Result forbidden_interface_name_prefix(rclcpp::Parameter const & parameter) -{ - auto const & interface_name = parameter.as_string(); - - if (interface_name.rfind("blup_", 0) == 0) - { - return ERROR("'interface_name' parameter can not start with 'blup_'"); - } - - return OK; -} - -} // namespace parameter_traits - -#endif // PID_CONTROLLER__VALIDATE_PID_CONTROLLER_PARAMETERS_HPP_ diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index f41c9e9f53..7742335e74 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -168,7 +168,8 @@ controller_interface::CallbackReturn PidController::on_configure( if (params_.use_external_measured_states) { auto measured_state_callback = - [&](const std::shared_ptr msg) -> void { + [&](const std::shared_ptr msg) -> void + { // TODO(destogl): Sort the input values based on joint and interface names measured_state_.writeFromNonRT(msg); }; @@ -186,17 +187,18 @@ controller_interface::CallbackReturn PidController::on_configure( 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; - }; + 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, @@ -361,8 +363,10 @@ controller_interface::CallbackReturn PidController::on_activate( 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()); + reference_interfaces_.assign( + reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + measured_state_values_.assign( + measured_state_values_.size(), std::numeric_limits::quiet_NaN()); // TODO(destogl): make here parameter update @@ -457,8 +461,8 @@ controller_interface::return_type PidController::update_and_write_commands( else { // Fallback to calculation with 'error' only - tmp_command += pids_[i]->computeCommand( - reference_interfaces_[i] - measured_state_values_[i], period); + tmp_command += + pids_[i]->computeCommand(reference_interfaces_[i] - measured_state_values_[i], period); } } else diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index f4ec9596f0..63e0e3eb27 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -206,7 +206,8 @@ class PidControllerFixture : public ::testing::Test const std::vector & displacements = {0.45}, const std::vector & velocities = {0.0}, const double duration = 1.25) { - auto wait_for_topic = [&](const auto topic_name) { + auto wait_for_topic = [&](const auto topic_name) + { size_t wait_count = 0; while (command_publisher_node_->count_subscribers(topic_name) == 0) { From 229946a6b35b60f95c65784748dfb078b808cd56 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 7 Feb 2023 14:03:13 +0100 Subject: [PATCH 08/37] Delete LICENSE --- pid_controller/LICENSE | 202 ----------------------------------------- 1 file changed, 202 deletions(-) delete mode 100644 pid_controller/LICENSE diff --git a/pid_controller/LICENSE b/pid_controller/LICENSE deleted file mode 100644 index d645695673..0000000000 --- a/pid_controller/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. From 446d909da2dc4272c65a2f89e2effac0bf35eb9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 16 Dec 2022 10:05:17 +0100 Subject: [PATCH 09/37] Simplify things in the PID controller and add some error management. --- pid_controller/src/pid_controller.cpp | 43 ++++++++++++++------------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 7742335e74..e5a699f6d2 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -23,8 +23,8 @@ #include "controller_interface/helpers.hpp" // TODO(destogl): should we add some knowledge from the control there here? -// The main knowledge could be with regard to the input (reference/state) and output (command) interfaces and their physical -// meaning, for example: +// The main knowledge could be with regard to the input (reference/state) and output (command) +// interfaces and their physical meaning, for example: // // reference/state POSITION; command VELOCITY --> PI CONTROLLER // reference/state VELOCITY; command ACCELERATION --> PI CONTROLLER @@ -133,9 +133,13 @@ controller_interface::CallbackReturn PidController::configure_parameters() 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]); - pids_[i]->initPid(); + std::make_shared(get_node(), "gains." + params_.dof_names[i], true); + if (!pids_[i]->initPid()) + { + return CallbackReturn::FAILURE; + } } return CallbackReturn::SUCCESS; @@ -168,8 +172,7 @@ controller_interface::CallbackReturn PidController::on_configure( if (params_.use_external_measured_states) { auto measured_state_callback = - [&](const std::shared_ptr msg) -> void - { + [&](const std::shared_ptr msg) -> void { // TODO(destogl): Sort the input values based on joint and interface names measured_state_.writeFromNonRT(msg); }; @@ -187,18 +190,17 @@ controller_interface::CallbackReturn PidController::on_configure( 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; - }; + 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, @@ -412,12 +414,13 @@ controller_interface::return_type PidController::update_and_write_commands( 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_.readFromRT()))->values[i]; + measured_state_values_[i] = measured_state->values[i]; if (measured_state_values_.size() == 2 * dof_) { - measured_state_values_[dof_ + i] = (*(measured_state_.readFromRT()))->values_dot[i]; + measured_state_values_[dof_ + i] = measured_state->values_dot[i]; } } } From 3fab74451ca12068df077272358b2199743b8634 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 17 Mar 2023 21:18:36 -0600 Subject: [PATCH 10/37] Update CMake export in pid-controller --- pid_controller/CMakeLists.txt | 23 +++++++++-------------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index bc6222f445..5bbd92eceb 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -43,7 +43,10 @@ add_library( SHARED src/pid_controller.cpp ) -target_include_directories(pid_controller PRIVATE include) +target_include_directories(pid_controller PUBLIC + $ + $ +) target_link_libraries(pid_controller pid_controller_parameters) ament_target_dependencies(pid_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL") @@ -52,8 +55,8 @@ pluginlib_export_plugin_description_file( controller_interface pid_controller.xml) install( - TARGETS - pid_controller + TARGETS pid_controller pid_controller_parameters + EXPORT export_pid_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -61,7 +64,7 @@ install( install( DIRECTORY include/ - DESTINATION include + DESTINATION include/pid_controller ) if(BUILD_TESTING) @@ -112,14 +115,6 @@ if(BUILD_TESTING) #) endif() -ament_export_include_directories( - include -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -ament_export_libraries( - pid_controller -) - +ament_export_targets(export_pid_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() From bcc9f3f175825bcb89afbac6ffcfede90878807a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 20 Mar 2023 18:12:02 +0100 Subject: [PATCH 11/37] Cleaning CMakeLists and deprecation from PID controller and starting working on the tests. --- pid_controller/CMakeLists.txt | 113 ++++++++---------- .../include/pid_controller/pid_controller.hpp | 1 - pid_controller/src/pid_controller.cpp | 46 +++---- pid_controller/src/pid_controller.yaml | 8 -- .../test/pid_controller_params.yaml | 3 + pid_controller/test/test_pid_controller.cpp | 29 ++++- pid_controller/test/test_pid_controller.hpp | 2 +- 7 files changed, 96 insertions(+), 106 deletions(-) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 5bbd92eceb..e306bb374a 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -1,21 +1,15 @@ -cmake_minimum_required(VERSION 3.8) -project(pid_controller) +cmake_minimum_required(VERSION 3.16) +project(pid_controller LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) endif() -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -# find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs control_toolbox controller_interface + generate_parameter_library hardware_interface parameter_traits pluginlib @@ -26,81 +20,52 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) find_package(ament_cmake REQUIRED) -find_package(generate_parameter_library REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -# find dependencies -find_package(ament_cmake REQUIRED) - -# Add pid_controller library related compile commands generate_parameter_library(pid_controller_parameters src/pid_controller.yaml ) -add_library( - pid_controller - SHARED + +add_library(pid_controller SHARED src/pid_controller.cpp ) -target_include_directories(pid_controller PUBLIC +target_compile_features(pid_controller PUBLIC cxx_std_17) +target_include_directories(pid_controller PUBLIC $ $ ) -target_link_libraries(pid_controller pid_controller_parameters) -ament_target_dependencies(pid_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL") - -pluginlib_export_plugin_description_file( - controller_interface pid_controller.xml) - -install( - TARGETS pid_controller pid_controller_parameters - EXPORT export_pid_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib +target_link_libraries(pid_controller PUBLIC + pid_controller_parameters ) +ament_target_dependencies(pid_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -install( - DIRECTORY include/ - DESTINATION include/pid_controller -) +# 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_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp) - target_include_directories(test_load_pid_controller PRIVATE include) + 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_load_pid_controller - controller_manager - ros2_control_test_assets + test_pid_controller + controller_interface + hardware_interface ) - #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_preceeding #test/test_pid_controller_preceeding.cpp @@ -113,8 +78,30 @@ if(BUILD_TESTING) #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/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 27f437ef65..dc56bc7758 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -31,7 +31,6 @@ #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" -// TODO(anyone): Replace with controller specific messages #include "control_msgs/msg/joint_controller_state.hpp" #include "control_msgs/msg/joint_jog.hpp" diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index e5a699f6d2..9b5ae44188 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -41,18 +41,11 @@ namespace { // utility -// TODO(destogl): remove this when merged upstream // Changed services history QoS to keep all so we don't lose any client service calls -static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1, // message queue depth - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; @@ -172,7 +165,8 @@ controller_interface::CallbackReturn PidController::on_configure( if (params_.use_external_measured_states) { auto measured_state_callback = - [&](const std::shared_ptr msg) -> void { + [&](const std::shared_ptr msg) -> void + { // TODO(destogl): Sort the input values based on joint and interface names measured_state_.writeFromNonRT(msg); }; @@ -190,21 +184,21 @@ controller_interface::CallbackReturn PidController::on_configure( 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; - }; + 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, - rmw_qos_profile_services_hist_keep_all); + "~/set_feedforward_control", set_feedforward_control_callback, qos_services); try { @@ -230,8 +224,6 @@ controller_interface::CallbackReturn PidController::on_configure( } state_publisher_->unlock(); - // TODO(destogl): Add separate timer-callback for the controller status publisher - RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 116e6fa334..627a4998ef 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -45,14 +45,6 @@ pid_controller: default_value: false, description: "Use external states from a topic instead form state interfaces." } - controller_state_publish_rate: { - type: double, - default_value: 50.0, - description: "Rate controller state is published", - validation: { - lower_bounds: [0.1] - } - } gains: __map_dof_names: p: { diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index 51faf63cf6..b297b3a482 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -5,3 +5,6 @@ test_pid_controller: - joint1 interface_name: acceleration + + gains: + joint1: {p: 1.0, i: 2.0, d: 10, i_clamp_max: 5.0, i_clamp_min: -5.0} diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index da75922c8e..bb40fea1bd 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -34,7 +34,10 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_TRUE(controller_->params_.dof_names.empty()); ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); - ASSERT_TRUE(controller_->params_.interface_name.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_FALSE(controller_->params_.runtime_param_update); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -43,7 +46,20 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_THAT( controller_->reference_and_reference_and_state_dof_names_, testing::ElementsAreArray(dof_names_)); - ASSERT_EQ(controller_->params_.interface_name, interface_name_); + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains[dof_name].p, 1.0); + ASSERT_EQ(controller_->params_.gains[dof_name].i, 2.0); + ASSERT_EQ(controller_->params_.gains[dof_name].d, 10.0); + ASSERT_FALSE(controller_->params_.gains[dof_name].antiwindup); + ASSERT_EQ(controller_->params_.gains[dof_name].i_clamp_max, 5.0); + ASSERT_EQ(controller_->params_.gains[dof_name].i_clamp_min, -5.0); + ASSERT_EQ(controller_->params_.gains[dof_name].feedforward_gain, 0.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + ASSERT_FALSE(controller_->params_.runtime_param_update); } TEST_F(PidControllerTest, check_exported_intefaces) @@ -56,14 +72,14 @@ TEST_F(PidControllerTest, check_exported_intefaces) ASSERT_EQ(command_intefaces.names.size(), dof_command_values_.size()); for (size_t i = 0; i < command_intefaces.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + interface_name_); + EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + command_interface_); } auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); for (size_t i = 0; i < state_intefaces.names.size(); ++i) { - EXPECT_EQ(state_intefaces.names[i], dof_names_[i] + "/" + interface_name_); + EXPECT_EQ(state_intefaces.names[i], dof_names_[i] + "/" + command_interface_); } // check ref itfs @@ -72,10 +88,11 @@ TEST_F(PidControllerTest, check_exported_intefaces) for (size_t i = 0; i < dof_names_.size(); ++i) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - dof_names_[i] + "/" + interface_name_; + dof_names_[i] + "/" + command_interface_; EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), dof_names_[i] + "/" + interface_name_); + EXPECT_EQ( + reference_interfaces[i].get_interface_name(), dof_names_[i] + "/" + command_interface_); } } diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 63e0e3eb27..c511fdccde 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -259,7 +259,7 @@ class PidControllerFixture : public ::testing::Test // Controller-related parameters std::vector dof_names_ = {"joint1"}; std::vector state_dof_names_ = {"joint1state"}; - std::string interface_name_ = "acceleration"; + std::string command_interface_ = "acceleration"; std::array dof_state_values_ = {1.1}; std::array dof_command_values_ = {101.101}; From e80197e317b8510c26e82d9c811dfc04499038fc Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Tue, 21 Mar 2023 21:22:16 +0100 Subject: [PATCH 12/37] pid_controller compiles, tests need adjustments --- pid_controller/src/pid_controller.cpp | 9 +- .../test/pid_controller_params.yaml | 5 +- pid_controller/test/test_pid_controller.cpp | 105 +++++++++--------- pid_controller/test/test_pid_controller.hpp | 17 +-- 4 files changed, 59 insertions(+), 77 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 9b5ae44188..adf6b03ccd 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -481,7 +481,7 @@ controller_interface::return_type PidController::update_and_write_commands( state_publisher_->msg_.dof_states[i].process_value = measured_state_values_[i]; if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) { - state_publisher_->msg_.dof_states[i].process_value = measured_state_values_[i]; + state_publisher_->msg_.dof_states[i].process_value_dot = measured_state_values_[dof_ + i]; } state_publisher_->msg_.dof_states[i].error = reference_interfaces_[i] - measured_state_values_[i]; @@ -494,13 +494,6 @@ controller_interface::return_type PidController::update_and_write_commands( // 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].command = command_interfaces_[i].get_value(); - - auto gains = pids_[i]->getGains(); - state_publisher_->msg_.dof_states[i].p = gains.p_gain_; - state_publisher_->msg_.dof_states[i].i = gains.i_gain_; - state_publisher_->msg_.dof_states[i].d = gains.d_gain_; - state_publisher_->msg_.dof_states[i].i_clamp = gains.i_max_; - state_publisher_->msg_.dof_states[i].antiwindup = gains.antiwindup_; } state_publisher_->unlockAndPublish(); } diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index b297b3a482..5d690fd138 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -1,10 +1,9 @@ test_pid_controller: ros__parameters: - dof_names: - joint1 interface_name: acceleration - gains: - joint1: {p: 1.0, i: 2.0, d: 10, i_clamp_max: 5.0, i_clamp_min: -5.0} + 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/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index bb40fea1bd..1ad4dc3b07 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -43,18 +43,16 @@ TEST_F(PidControllerTest, all_parameters_set_configure_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_reference_and_state_dof_names_, - testing::ElementsAreArray(dof_names_)); + 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_name].p, 1.0); - ASSERT_EQ(controller_->params_.gains[dof_name].i, 2.0); - ASSERT_EQ(controller_->params_.gains[dof_name].d, 10.0); - ASSERT_FALSE(controller_->params_.gains[dof_name].antiwindup); - ASSERT_EQ(controller_->params_.gains[dof_name].i_clamp_max, 5.0); - ASSERT_EQ(controller_->params_.gains[dof_name].i_clamp_min, -5.0); - ASSERT_EQ(controller_->params_.gains[dof_name].feedforward_gain, 0.0); + 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_); ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); @@ -104,20 +102,18 @@ TEST_F(PidControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_cmd_.readFromNonRT(); - EXPECT_EQ((*msg)->displacements.size(), dof_names_.size()); - for (const auto & cmd : (*msg)->displacements) + 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)->velocities.size(), dof_names_.size()); - for (const auto & cmd : (*msg)->velocities) + EXPECT_EQ((*msg)->values_dot.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values_dot) { EXPECT_TRUE(std::isnan(cmd)); } - ASSERT_TRUE(std::isnan((*msg)->duration)); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -204,20 +200,19 @@ TEST_F(PidControllerTest, test_update_logic_fast) // set command statically static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); - msg->joint_names = dof_names_; - msg->displacements.resize(dof_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); - controller_->input_cmd_.writeFromNonRT(msg); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), TEST_DISPLACEMENT); + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); - EXPECT_TRUE(std::isnan((*(controller_->input_cmd_.readFromRT()))->displacements[0])); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -241,21 +236,20 @@ TEST_F(PidControllerTest, test_update_logic_slow) static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); // When slow mode is enabled command ends up being half of the value - msg->joint_names = dof_names_; - msg->displacements.resize(dof_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); - controller_->input_cmd_.writeFromNonRT(msg); - controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - ASSERT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), TEST_DISPLACEMENT); + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::OFF); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->values[0], TEST_DISPLACEMENT); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); - EXPECT_TRUE(std::isnan((*(controller_->input_cmd_.readFromRT()))->displacements[0])); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -278,11 +272,10 @@ TEST_F(PidControllerTest, test_update_logic_chainable_fast) // set command statically static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); - msg->joint_names = dof_names_; - msg->displacements.resize(dof_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); - controller_->input_cmd_.writeFromNonRT(msg); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), TEST_DISPLACEMENT); + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); // this is input source in chained mode controller_->reference_interfaces_[STATE_MY_ITFS] = TEST_DISPLACEMENT * 2; @@ -290,12 +283,12 @@ TEST_F(PidControllerTest, test_update_logic_chainable_fast) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); // reference_interfaces is directly applied EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); // message is not touched in chained mode - EXPECT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[0], TEST_DISPLACEMENT); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -319,17 +312,16 @@ TEST_F(PidControllerTest, test_update_logic_chainable_slow) static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); // When slow mode is enabled command ends up being half of the value - msg->joint_names = dof_names_; - msg->displacements.resize(dof_names_.size(), TEST_DISPLACEMENT); - msg->velocities.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - msg->duration = std::numeric_limits::quiet_NaN(); - controller_->input_cmd_.writeFromNonRT(msg); - controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), TEST_DISPLACEMENT); + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::OFF); // this is input source in chained mode controller_->reference_interfaces_[STATE_MY_ITFS] = TEST_DISPLACEMENT * 4; - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); - ASSERT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->values[0], TEST_DISPLACEMENT); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -337,7 +329,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_slow) // reference_interfaces is directly applied EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); // message is not touched in chained mode - EXPECT_EQ((*(controller_->input_cmd_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[0], TEST_DISPLACEMENT); EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -359,7 +351,8 @@ TEST_F(PidControllerTest, publish_status_success) ControllerStateMsg msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.set_point, 101.101); + ASSERT_EQ(msg.dof_states.size(), 1); + ASSERT_EQ(msg.dof_states[0].set_point, 101.101); } TEST_F(PidControllerTest, receive_message_and_publish_updated_status) @@ -378,7 +371,8 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) ControllerStateMsg msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.set_point, 101.101); + ASSERT_EQ(msg.dof_states.size(), 1); + ASSERT_EQ(msg.dof_states[0].set_point, 101.101); publish_commands(); ASSERT_TRUE(controller_->wait_for_commands(executor)); @@ -391,7 +385,8 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) subscribe_and_get_messages(msg); - ASSERT_EQ(msg.set_point, 0.45); + ASSERT_EQ(msg.dof_states.size(), 1); + ASSERT_EQ(msg.dof_states[0].set_point, 0.45); } int main(int argc, char ** argv) diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index c511fdccde..593b1b6585 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -33,7 +33,6 @@ #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -// TODO(anyone): replace the state and command message types using ControllerStateMsg = pid_controller::PidController::ControllerStateMsg; using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; using ControllerModeSrvType = pid_controller::PidController::ControllerModeSrvType; @@ -146,7 +145,7 @@ class PidControllerFixture : public ::testing::Test for (size_t i = 0; i < dof_command_values_.size(); ++i) { command_itfs_.emplace_back(hardware_interface::CommandInterface( - dof_names_[i], interface_name_, &dof_command_values_[i])); + dof_names_[i], command_interface_, &dof_command_values_[i])); command_ifs.emplace_back(command_itfs_.back()); } // TODO(anyone): Add other command interfaces, if any @@ -157,8 +156,8 @@ class PidControllerFixture : public ::testing::Test for (size_t i = 0; i < dof_state_values_.size(); ++i) { - state_itfs_.emplace_back( - hardware_interface::StateInterface(dof_names_[i], interface_name_, &dof_state_values_[i])); + state_itfs_.emplace_back(hardware_interface::StateInterface( + dof_names_[i], command_interface_, &dof_state_values_[i])); state_ifs.emplace_back(state_itfs_.back()); } // TODO(anyone): Add other state interfaces, if any @@ -201,10 +200,8 @@ class PidControllerFixture : public ::testing::Test ASSERT_TRUE(subscription->take(msg, msg_info)); } - // TODO(anyone): add/remove arguments as it suites your command message type void publish_commands( - const std::vector & displacements = {0.45}, - const std::vector & velocities = {0.0}, const double duration = 1.25) + const std::vector & values = {0.45}, const std::vector & values_dot = {0.0}) { auto wait_for_topic = [&](const auto topic_name) { @@ -226,10 +223,8 @@ class PidControllerFixture : public ::testing::Test ControllerCommandMsg msg; msg.dof_names = dof_names_; - // TODO(destogl): Update name of the arguments and remove unnecessary ones - msg.values = displacements; - // msg.velocities = velocities; - // msg.duration = duration; + msg.values = values; + msg.values_dot = values_dot; command_publisher_->publish(msg); } From 8acea6f7b7d7ceedef059dbdf8bb8394ecadae8b Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Fri, 24 Mar 2023 20:53:34 +0100 Subject: [PATCH 13/37] all tests in test_pid_controller are green --- pid_controller/src/pid_controller.cpp | 1 + pid_controller/src/pid_controller.yaml | 2 +- .../test/pid_controller_params.yaml | 4 +- pid_controller/test/test_pid_controller.cpp | 245 ++++++++++++------ pid_controller/test/test_pid_controller.hpp | 74 +++--- 5 files changed, 211 insertions(+), 115 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index adf6b03ccd..6417eb4010 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -442,6 +442,7 @@ controller_interface::return_type PidController::update_and_write_commands( tmp_command = 0.0; } + // checking if there are two interfaces if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) { if ( diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 627a4998ef..ba2ba1f53c 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -31,7 +31,7 @@ pid_controller: reference_and_state_interfaces: { type: string_array, default_value: [], - description: "Name of the interfaces used by the controller getting hardware states and reference commands.", + description: "Name of the interfaces used by the controller getting hardware states and reference commands. The second interface should be the derivation of the first.", read_only: true, #validation: { #not_empty<>: null, diff --git a/pid_controller/test/pid_controller_params.yaml b/pid_controller/test/pid_controller_params.yaml index 5d690fd138..7555cfc156 100644 --- a/pid_controller/test/pid_controller_params.yaml +++ b/pid_controller/test/pid_controller_params.yaml @@ -3,7 +3,9 @@ test_pid_controller: dof_names: - joint1 - interface_name: acceleration + 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/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 1ad4dc3b07..3443687df2 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -55,7 +55,9 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0); } ASSERT_EQ(controller_->params_.command_interface, command_interface_); - ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); ASSERT_FALSE(controller_->params_.use_external_measured_states); ASSERT_FALSE(controller_->params_.runtime_param_update); } @@ -66,31 +68,41 @@ TEST_F(PidControllerTest, check_exported_intefaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), dof_command_values_.size()); - for (size_t i = 0; i < command_intefaces.names.size(); ++i) + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), dof_command_values_.size()); + for (size_t i = 0; i < command_interfaces.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + command_interface_); + EXPECT_EQ(command_interfaces.names[i], dof_names_[i] + "/" + command_interface_); } auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); - for (size_t i = 0; i < state_intefaces.names.size(); ++i) + size_t si_index = 0; + for (const auto & interface : state_interfaces_) { - EXPECT_EQ(state_intefaces.names[i], dof_names_[i] + "/" + command_interface_); + for (const auto & dof_name : dof_names_) + { + EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); + ++si_index; + } } // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), dof_names_.size()); - for (size_t i = 0; i < dof_names_.size(); ++i) + ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - dof_names_[i] + "/" + command_interface_; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[i].get_interface_name(), dof_names_[i] + "/" + command_interface_); + 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(reference_interfaces[ri_index].get_name(), ref_itf_name); + EXPECT_EQ( + reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } } } @@ -114,7 +126,7 @@ TEST_F(PidControllerTest, activate_success) EXPECT_TRUE(std::isnan(cmd)); } - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); for (const auto & interface : controller_->reference_interfaces_) { EXPECT_TRUE(std::isnan(interface)); @@ -159,7 +171,7 @@ TEST_F(PidControllerTest, reactivate_success) controller_interface::return_type::OK); } -TEST_F(PidControllerTest, test_setting_slow_mode_service) +TEST_F(PidControllerTest, test_feedforward_mode_service) { SetUpController(); @@ -185,7 +197,7 @@ TEST_F(PidControllerTest, test_setting_slow_mode_service) ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); } -TEST_F(PidControllerTest, test_update_logic_fast) +TEST_F(PidControllerTest, test_update_logic_feedforward_off) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -196,31 +208,45 @@ TEST_F(PidControllerTest, test_update_logic_fast) 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)); + } - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), TEST_DISPLACEMENT); + 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::ON); - EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); - for (const auto & interface : controller_->reference_interfaces_) + 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(interface)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); } } -TEST_F(PidControllerTest, test_update_logic_slow) +TEST_F(PidControllerTest, test_update_logic_feedforward_on) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -231,33 +257,48 @@ TEST_F(PidControllerTest, test_update_logic_slow) 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)); + } - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); - // When slow mode is enabled command ends up being half of the value msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), TEST_DISPLACEMENT); + 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::OFF); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->values[0], TEST_DISPLACEMENT); + 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(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); - for (const auto & interface : controller_->reference_interfaces_) + 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(interface)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); } } -TEST_F(PidControllerTest, test_update_logic_chainable_fast) +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -269,34 +310,40 @@ TEST_F(PidControllerTest, test_update_logic_chainable_fast) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(controller_->is_in_chained_mode()); - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), TEST_DISPLACEMENT); + 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); - // this is input source in chained mode - controller_->reference_interfaces_[STATE_MY_ITFS] = TEST_DISPLACEMENT * 2; + + 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); - // reference_interfaces is directly applied - EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); - // message is not touched in chained mode - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[0], TEST_DISPLACEMENT); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); - for (const auto & interface : controller_->reference_interfaces_) + 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_TRUE(std::isnan(interface)); + 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_slow) +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) { SetUpController(); rclcpp::executors::MultiThreadedExecutor executor; @@ -307,37 +354,45 @@ TEST_F(PidControllerTest, test_update_logic_chainable_slow) 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); - // set command statically - static constexpr double TEST_DISPLACEMENT = 23.24; std::shared_ptr msg = std::make_shared(); - // When slow mode is enabled command ends up being half of the value msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), TEST_DISPLACEMENT); + 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::OFF); - // this is input source in chained mode - controller_->reference_interfaces_[STATE_MY_ITFS] = TEST_DISPLACEMENT * 4; - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->values[0], TEST_DISPLACEMENT); + 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); - // reference_interfaces is directly applied - EXPECT_EQ(dof_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT * 2); - // message is not touched in chained mode - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[0], TEST_DISPLACEMENT); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_names_.size()); - for (const auto & interface : controller_->reference_interfaces_) + 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_TRUE(std::isnan(interface)); + 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, publish_status_success) +TEST_F(PidControllerTest, subscribe_and_get_messages_success) { SetUpController(); @@ -351,8 +406,13 @@ TEST_F(PidControllerTest, publish_status_success) ControllerStateMsg msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.dof_states.size(), 1); - ASSERT_EQ(msg.dof_states[0].set_point, 101.101); + 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].set_point)); + ASSERT_EQ(msg.dof_states[i].command, dof_command_values_[i]); + } } TEST_F(PidControllerTest, receive_message_and_publish_updated_status) @@ -371,22 +431,45 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) ControllerStateMsg msg; subscribe_and_get_messages(msg); - ASSERT_EQ(msg.dof_states.size(), 1); - ASSERT_EQ(msg.dof_states[0].set_point, 101.101); + 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].set_point)); + ASSERT_EQ(msg.dof_states[i].command, 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); - EXPECT_EQ(dof_command_values_[CMD_MY_ITFS], 0.45); + 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(), 1); - ASSERT_EQ(msg.dof_states[0].set_point, 0.45); + 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].set_point, 0.45); + ASSERT_NE(msg.dof_states[i].command, dof_command_values_[i]); + } } int main(int argc, char ** argv) diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 593b1b6585..14a902e3da 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -50,11 +50,13 @@ 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_setting_slow_mode_service); - FRIEND_TEST(PidControllerTest, test_update_logic_fast); - FRIEND_TEST(PidControllerTest, test_update_logic_slow); - FRIEND_TEST(PidControllerTest, test_update_logic_chainable_fast); - FRIEND_TEST(PidControllerTest, test_update_logic_chainable_slow); + 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( @@ -117,19 +119,25 @@ class PidControllerFixture : public ::testing::Test void SetUp() { + dof_names_ = {"joint1"}; + command_interface_ = "position"; + state_interfaces_ = {"position"}; + dof_state_values_ = {1.1}; + dof_command_values_ = {101.101}; + // initialize controller controller_ = std::make_unique(); command_publisher_node_ = std::make_shared("command_publisher"); command_publisher_ = command_publisher_node_->create_publisher( - "/test_pid_controller/commands", rclcpp::SystemDefaultsQoS()); + "/test_pid_controller/reference", rclcpp::SystemDefaultsQoS()); service_caller_node_ = std::make_shared("service_caller"); - slow_control_service_client_ = service_caller_node_->create_client( - "/test_pid_controller/set_slow_control_mode"); + feedforward_service_client_ = service_caller_node_->create_client( + "/test_pid_controller/set_feedforward_control"); } - static void TearDownTestCase() {} + static void TearDownTestCase() { rclcpp::shutdown(); } void TearDown() { controller_.reset(nullptr); } @@ -139,28 +147,30 @@ class PidControllerFixture : public ::testing::Test ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); std::vector command_ifs; - command_itfs_.reserve(dof_command_values_.size()); - command_ifs.reserve(dof_command_values_.size()); + command_itfs_.reserve(dof_names_.size()); + command_ifs.reserve(dof_names_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + 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()); } - // TODO(anyone): Add other command interfaces, if any std::vector state_ifs; - state_itfs_.reserve(dof_state_values_.size()); - state_ifs.reserve(dof_state_values_.size()); - - for (size_t i = 0; i < dof_state_values_.size(); ++i) + 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_) { - state_itfs_.emplace_back(hardware_interface::StateInterface( - dof_names_[i], command_interface_, &dof_state_values_[i])); - state_ifs.emplace_back(state_itfs_.back()); + 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; + } } - // TODO(anyone): Add other state interfaces, if any controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } @@ -230,19 +240,19 @@ class PidControllerFixture : public ::testing::Test } std::shared_ptr call_service( - const bool slow_control, rclcpp::Executor & executor) + const bool feedforward, rclcpp::Executor & executor) { auto request = std::make_shared(); - request->data = slow_control; + request->data = feedforward; bool wait_for_service_ret = - slow_control_service_client_->wait_for_service(std::chrono::milliseconds(500)); + 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("Services is not available!"); + throw std::runtime_error("Service is not available!"); } - auto result = slow_control_service_client_->async_send_request(request); + auto result = feedforward_service_client_->async_send_request(request); EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); return result.get(); @@ -252,11 +262,11 @@ class PidControllerFixture : public ::testing::Test // TODO(anyone): adjust the members as needed // Controller-related parameters - std::vector dof_names_ = {"joint1"}; - std::vector state_dof_names_ = {"joint1state"}; - std::string command_interface_ = "acceleration"; - std::array dof_state_values_ = {1.1}; - std::array dof_command_values_ = {101.101}; + std::vector dof_names_; + std::string command_interface_; + std::vector state_interfaces_; + std::vector dof_state_values_; + std::vector dof_command_values_; std::vector state_itfs_; std::vector command_itfs_; @@ -266,7 +276,7 @@ class PidControllerFixture : public ::testing::Test rclcpp::Node::SharedPtr command_publisher_node_; rclcpp::Publisher::SharedPtr command_publisher_; rclcpp::Node::SharedPtr service_caller_node_; - rclcpp::Client::SharedPtr slow_control_service_client_; + rclcpp::Client::SharedPtr feedforward_service_client_; }; #endif // TEST_PID_CONTROLLER_HPP_ From b47d9610caa54b5e6381d9f0086e5e2e3de7f827 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Mon, 27 Mar 2023 17:10:04 +0200 Subject: [PATCH 14/37] test_pid_controller_preceeding: all green --- pid_controller/CMakeLists.txt | 24 ++++----- .../pid_controller_preceeding_params.yaml | 6 ++- pid_controller/test/test_pid_controller.hpp | 2 + .../test/test_pid_controller_preceeding.cpp | 50 +++++++++++-------- 4 files changed, 48 insertions(+), 34 deletions(-) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index e306bb374a..8beb31d7cd 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -66,18 +66,18 @@ if(BUILD_TESTING) hardware_interface ) - #add_rostest_with_parameters_gmock( - #test_pid_controller_preceeding - #test/test_pid_controller_preceeding.cpp - #${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceeding_params.yaml - #) - #target_include_directories(test_pid_controller_preceeding PRIVATE include) - #target_link_libraries(test_pid_controller_preceeding pid_controller) - #ament_target_dependencies( - #test_pid_controller_preceeding - #controller_interface - #hardware_interface - #) + add_rostest_with_parameters_gmock( + test_pid_controller_preceeding + test/test_pid_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceeding_params.yaml + ) + target_include_directories(test_pid_controller_preceeding PRIVATE include) + target_link_libraries(test_pid_controller_preceeding pid_controller) + ament_target_dependencies( + test_pid_controller_preceeding + 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) diff --git a/pid_controller/test/pid_controller_preceeding_params.yaml b/pid_controller/test/pid_controller_preceeding_params.yaml index 12339aa195..673abfe08e 100644 --- a/pid_controller/test/pid_controller_preceeding_params.yaml +++ b/pid_controller/test/pid_controller_preceeding_params.yaml @@ -3,7 +3,9 @@ test_pid_controller: dof_names: - joint1 - interface_name: acceleration + command_interface: position - state_dof_names: + reference_and_state_interfaces: ["position"] + + reference_and_state_dof_names: - joint1state diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 14a902e3da..e9a2cb1c81 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -124,6 +124,7 @@ class PidControllerFixture : public ::testing::Test 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(); @@ -267,6 +268,7 @@ class PidControllerFixture : public ::testing::Test 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_; diff --git a/pid_controller/test/test_pid_controller_preceeding.cpp b/pid_controller/test/test_pid_controller_preceeding.cpp index dba2624d35..8b1ddc064a 100644 --- a/pid_controller/test/test_pid_controller_preceeding.cpp +++ b/pid_controller/test/test_pid_controller_preceeding.cpp @@ -34,18 +34,18 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) ASSERT_TRUE(controller_->params_.dof_names.empty()); ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); - ASSERT_TRUE(controller_->params_.interface_name.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_reference_and_state_dof_names_)); + testing::ElementsAreArray(reference_and_state_dof_names_)); ASSERT_THAT( - controller_->reference_and_reference_and_state_dof_names_, - testing::ElementsAreArray(reference_and_reference_and_state_dof_names_)); - ASSERT_EQ(controller_->params_.interface_name, interface_name_); + 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_intefaces) @@ -58,31 +58,41 @@ TEST_F(PidControllerTest, check_exported_intefaces) ASSERT_EQ(command_intefaces.names.size(), dof_command_values_.size()); for (size_t i = 0; i < command_intefaces.names.size(); ++i) { - EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + interface_name_); + EXPECT_EQ(command_intefaces.names[i], dof_names_[i] + "/" + command_interface_); } auto state_intefaces = controller_->state_interface_configuration(); ASSERT_EQ(state_intefaces.names.size(), dof_state_values_.size()); - for (size_t i = 0; i < state_intefaces.names.size(); ++i) + size_t si_index = 0; + for (const auto & interface : state_interfaces_) { - EXPECT_EQ( - state_intefaces.names[i], - reference_and_reference_and_state_dof_names_[i] + "/" + interface_name_); + for (const auto & dof_name : dof_names_) + { + EXPECT_EQ( + state_intefaces.names[si_index], + reference_and_state_dof_names_[si_index] + "/" + interface); + ++si_index; + } } // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), dof_names_.size()); - for (size_t i = 0; i < dof_names_.size(); ++i) + ASSERT_EQ(reference_interfaces.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - reference_and_reference_and_state_dof_names_[i] + "/" + - interface_name_; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[i].get_interface_name(), - reference_and_reference_and_state_dof_names_[i] + "/" + interface_name_); + for (const auto & dof_name : dof_names_) + { + const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + + reference_and_state_dof_names_[ri_index] + "/" + interface; + EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); + EXPECT_EQ( + reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ( + reference_interfaces[ri_index].get_interface_name(), + reference_and_state_dof_names_[ri_index] + "/" + interface); + ++ri_index; + } } } From e511393fea5bd237db45505a0830a4291c8ed04d Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 28 Mar 2023 19:58:31 +0200 Subject: [PATCH 15/37] Update pid_controller/include/pid_controller/pid_controller.hpp --- pid_controller/include/pid_controller/pid_controller.hpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index dc56bc7758..73c3c042d5 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -39,11 +39,6 @@ namespace pid_controller { -// name constants for state interfaces -static constexpr size_t STATE_MY_ITFS = 0; - -// name constants for command interfaces -static constexpr size_t CMD_MY_ITFS = 0; enum class feedforward_mode_type : std::uint8_t { From 6fae4c929a7294fcc66c858ad31a1e628b4efec8 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 28 Mar 2023 20:11:52 +0200 Subject: [PATCH 16/37] Apply suggestions from code review --- .../include/pid_controller/pid_controller.hpp | 9 ++++++--- .../include/pid_controller/visibility_control.h | 2 +- pid_controller/src/pid_controller.cpp | 12 ++++++------ pid_controller/src/pid_controller.yaml | 8 ++++---- pid_controller/test/test_load_pid_controller.cpp | 5 ++++- pid_controller/test/test_pid_controller.cpp | 3 +++ pid_controller/test/test_pid_controller.hpp | 5 ++++- .../test/test_pid_controller_preceeding.cpp | 7 ++++--- 8 files changed, 32 insertions(+), 19 deletions(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 73c3c042d5..8e254c7f5b 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. @@ -11,6 +11,9 @@ // 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_ @@ -74,13 +77,13 @@ class PidController : public controller_interface::ChainableControllerInterface const rclcpp_lifecycle::State & previous_state) override; PID_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::return_type update_reference_from_subscribers() override; + 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; - // TODO(anyone): replace the state and command message types using ControllerReferenceMsg = control_msgs::msg::MultiDOFCommand; using ControllerMeasuredStateMsg = control_msgs::msg::MultiDOFCommand; using ControllerModeSrvType = std_srvs::srv::SetBool; diff --git a/pid_controller/include/pid_controller/visibility_control.h b/pid_controller/include/pid_controller/visibility_control.h index 7c9198a2b4..1509364b5a 100644 --- a/pid_controller/include/pid_controller/visibility_control.h +++ b/pid_controller/include/pid_controller/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 6417eb4010..90b628e059 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. @@ -11,6 +11,9 @@ // 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" @@ -22,7 +25,6 @@ #include "control_msgs/msg/single_dof_state.hpp" #include "controller_interface/helpers.hpp" -// TODO(destogl): should we add some knowledge from the control there here? // The main knowledge could be with regard to the input (reference/state) and output (command) // interfaces and their physical meaning, for example: // @@ -370,7 +372,6 @@ controller_interface::CallbackReturn PidController::on_activate( controller_interface::CallbackReturn PidController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, // instead of a loop for (size_t i = 0; i < dof_; ++i) { @@ -379,7 +380,8 @@ controller_interface::CallbackReturn PidController::on_deactivate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type PidController::update_reference_from_subscribers() +controller_interface::return_type PidController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { auto current_ref = input_ref_.readFromRT(); @@ -402,8 +404,6 @@ controller_interface::return_type PidController::update_reference_from_subscribe controller_interface::return_type PidController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) { - // TODO(destogl): make here runtime parameter update if allowed - if (params_.use_external_measured_states) { const auto measured_state = *(measured_state_.readFromRT()); diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index ba2ba1f53c..9dfda59684 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -4,10 +4,10 @@ pid_controller: default_value: [], description: "Specifies dof_names or axis used by the controller. If '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, - #} + validation: { + unique<>: null, + not_empty<>: null, + } } reference_and_state_dof_names: { type: string_array, diff --git a/pid_controller/test/test_load_pid_controller.cpp b/pid_controller/test/test_load_pid_controller.cpp index 022fab834b..3a75f6e170 100644 --- a/pid_controller/test/test_load_pid_controller.cpp +++ b/pid_controller/test/test_load_pid_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. @@ -11,6 +11,9 @@ // 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 diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 3443687df2..3d5813f42c 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -11,6 +11,9 @@ // 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" diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index e9a2cb1c81..d533821583 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. @@ -11,6 +11,9 @@ // 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_ diff --git a/pid_controller/test/test_pid_controller_preceeding.cpp b/pid_controller/test/test_pid_controller_preceeding.cpp index 8b1ddc064a..be144565fd 100644 --- a/pid_controller/test/test_pid_controller_preceeding.cpp +++ b/pid_controller/test/test_pid_controller_preceeding.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. @@ -11,6 +11,9 @@ // 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" @@ -20,9 +23,7 @@ #include #include -using pid_controller::CMD_MY_ITFS; using pid_controller::feedforward_mode_type; -using pid_controller::STATE_MY_ITFS; class PidControllerTest : public PidControllerFixture { From cd1203419a2e9817b444ea696b2be3959de6e85b Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 3 Apr 2023 16:33:25 +0200 Subject: [PATCH 17/37] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- pid_controller/README.md | 2 +- pid_controller/src/pid_controller.cpp | 8 +++--- pid_controller/src/pid_controller.yaml | 29 +++++++++------------ pid_controller/test/test_pid_controller.cpp | 8 +++--- 4 files changed, 21 insertions(+), 26 deletions(-) diff --git a/pid_controller/README.md b/pid_controller/README.md index 2544278e5f..01e8fddac8 100644 --- a/pid_controller/README.md +++ b/pid_controller/README.md @@ -1,7 +1,7 @@ pid_controller ========================================== -Controller based on PID implememenation from control_toolbox package. +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/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 90b628e059..d4a8b24596 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -478,11 +478,11 @@ controller_interface::return_type PidController::update_and_write_commands( state_publisher_->msg_.header.stamp = time; for (size_t i = 0; i < dof_; ++i) { - state_publisher_->msg_.dof_states[i].set_point = reference_interfaces_[i]; - state_publisher_->msg_.dof_states[i].process_value = measured_state_values_[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].process_value_dot = measured_state_values_[dof_ + i]; + 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]; @@ -494,7 +494,7 @@ controller_interface::return_type PidController::update_and_write_commands( 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].command = command_interfaces_[i].get_value(); + state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); } state_publisher_->unlockAndPublish(); } diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 9dfda59684..db15d43ac2 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -2,7 +2,7 @@ pid_controller: dof_names: { type: string_array, default_value: [], - description: "Specifies dof_names or axis used by the controller. If 'state_dof_names' parameter is defined, then only command dof names are defined with this parameter.", + description: "Specifies dof_names or axes used by the controller. If 'state_dof_names' parameter is defined, then only command dof names are defined with this parameter.", read_only: true, validation: { unique<>: null, @@ -12,33 +12,30 @@ pid_controller: reference_and_state_dof_names: { type: string_array, default_value: [], - description: "(optional) Specifies dof_names or axis 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.", + 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, - #} + 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, - #one_of<>: [["position", "velocity", "acceleration", "effort",]], - #} + 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 derivation of the first.", + 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, - #subset_of<>: [["position", "velocity", "acceleration", "effort",]], - #forbidden_interface_name_prefix: null - #} + validation: { + not_empty<>: null, + size_lt<>: 3, + } } use_external_measured_states: { type: bool, diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 3d5813f42c..a673a27daa 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -23,9 +23,7 @@ #include #include -using pid_controller::CMD_MY_ITFS; using pid_controller::feedforward_mode_type; -using pid_controller::STATE_MY_ITFS; class PidControllerTest : public PidControllerFixture { @@ -163,11 +161,11 @@ TEST_F(PidControllerTest, reactivate_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->command_interfaces_[CMD_MY_ITFS].get_value(), 101.101); + 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_->command_interfaces_[CMD_MY_ITFS].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), From e46219172b9fad77665cb774d30c608f759cba56 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 3 Apr 2023 16:33:50 +0200 Subject: [PATCH 18/37] Rename test_pid_controller_preceeding.cpp to test_pid_controller_preceding.cpp --- ...ontroller_preceeding.cpp => test_pid_controller_preceding.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename pid_controller/test/{test_pid_controller_preceeding.cpp => test_pid_controller_preceding.cpp} (100%) diff --git a/pid_controller/test/test_pid_controller_preceeding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp similarity index 100% rename from pid_controller/test/test_pid_controller_preceeding.cpp rename to pid_controller/test/test_pid_controller_preceding.cpp From 223e1b141e04c77eca241e4fcbd64723d0234797 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 3 Apr 2023 16:34:06 +0200 Subject: [PATCH 19/37] Rename pid_controller_preceeding_params.yaml to pid_controller_preceding_params.yaml --- ...receeding_params.yaml => pid_controller_preceding_params.yaml} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename pid_controller/test/{pid_controller_preceeding_params.yaml => pid_controller_preceding_params.yaml} (100%) diff --git a/pid_controller/test/pid_controller_preceeding_params.yaml b/pid_controller/test/pid_controller_preceding_params.yaml similarity index 100% rename from pid_controller/test/pid_controller_preceeding_params.yaml rename to pid_controller/test/pid_controller_preceding_params.yaml From 5baa48471f7c1ba55e38a40bdaf18d69283ca0bc Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 3 Apr 2023 16:36:56 +0200 Subject: [PATCH 20/37] Update pid_controller/CMakeLists.txt --- pid_controller/CMakeLists.txt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 8beb31d7cd..d13bda3751 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -67,14 +67,14 @@ if(BUILD_TESTING) ) add_rostest_with_parameters_gmock( - test_pid_controller_preceeding - test/test_pid_controller_preceeding.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceeding_params.yaml + 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_preceeding PRIVATE include) - target_link_libraries(test_pid_controller_preceeding pid_controller) + target_include_directories(test_pid_controller_preceding PRIVATE include) + target_link_libraries(test_pid_controller_preceding pid_controller) ament_target_dependencies( - test_pid_controller_preceeding + test_pid_controller_preceding controller_interface hardware_interface ) From 9f2eb74066d4d217627143ed64a1a854dc141def Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 4 Apr 2023 15:15:00 +0200 Subject: [PATCH 21/37] Update pid_controller/test/test_pid_controller.cpp --- pid_controller/test/test_pid_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index a673a27daa..ce384baf32 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// 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. From 7e73ef79eb26ad4f3eddb316c7d77e67bd9f0ff0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 4 Apr 2023 21:51:26 +0200 Subject: [PATCH 22/37] More fixups for PID after renaming stuff in the message. --- .../include/pid_controller/pid_controller.hpp | 2 +- pid_controller/test/test_pid_controller.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 8e254c7f5b..93a2152fe4 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -78,7 +78,7 @@ class PidController : public controller_interface::ChainableControllerInterface PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_reference_from_subscribers( - const rclcpp::Time & /*time*/, const rclcpp::Duration & period) override; + const rclcpp::Time & time, const rclcpp::Duration & period) override; PID_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands( diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index ce384baf32..eb7bd0b701 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -411,8 +411,8 @@ TEST_F(PidControllerTest, subscribe_and_get_messages_success) 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].set_point)); - ASSERT_EQ(msg.dof_states[i].command, dof_command_values_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); } } @@ -436,8 +436,8 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) 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].set_point)); - ASSERT_EQ(msg.dof_states[i].command, dof_command_values_[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) @@ -468,8 +468,8 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) 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].set_point, 0.45); - ASSERT_NE(msg.dof_states[i].command, dof_command_values_[i]); + ASSERT_EQ(msg.dof_states[i].reference, 0.45); + ASSERT_NE(msg.dof_states[i].output, dof_command_values_[i]); } } From f8d8667d722012b26ed0d6e5ece7838680ff0d57 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 4 Apr 2023 22:31:45 +0200 Subject: [PATCH 23/37] Apply suggestions from code review --- pid_controller/src/pid_controller.cpp | 14 +++++++------- pid_controller/test/test_pid_controller.hpp | 4 +--- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index d4a8b24596..9c8159acb4 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -206,7 +206,7 @@ controller_interface::CallbackReturn PidController::on_configure( { // State publisher s_publisher_ = - get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + get_node()->create_publisher("~/controller_state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(s_publisher_); } catch (const std::exception & e) @@ -290,9 +290,9 @@ controller_interface::InterfaceConfiguration PidController::command_interface_co command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(params_.dof_names.size()); - for (const auto & joint : params_.dof_names) + for (const auto & dof_name : params_.dof_names) { - command_interfaces_config.names.push_back(joint + "/" + params_.command_interface); + command_interfaces_config.names.push_back(dof_name + "/" + params_.command_interface); } return command_interfaces_config; @@ -313,9 +313,9 @@ controller_interface::InterfaceConfiguration PidController::state_interface_conf state_interfaces_config.names.reserve(dof_ * params_.reference_and_state_interfaces.size()); for (const auto & interface : params_.reference_and_state_interfaces) { - for (const auto & joint : reference_and_state_dof_names_) + for (const auto & dof_name : reference_and_state_dof_names_) { - state_interfaces_config.names.push_back(joint + "/" + interface); + state_interfaces_config.names.push_back(dof_name + "/" + interface); } } } @@ -334,10 +334,10 @@ std::vector PidController::on_export_refer size_t index = 0; for (const auto & interface : params_.reference_and_state_interfaces) { - for (const auto & joint : reference_and_state_dof_names_) + for (const auto & dof_name : reference_and_state_dof_names_) { reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), joint + "/" + interface, &reference_interfaces_[index])); + get_node()->get_name(), dof_name + "/" + interface, &reference_interfaces_[index])); ++index; } } diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index d533821583..48ffccd737 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -107,8 +107,6 @@ class TestablePidController : public pid_controller::PidController return wait_for_command(executor, ref_subscriber_wait_set_, timeout); } - // TODO(anyone): add implementation of any methods of your controller is needed - private: rclcpp::WaitSet ref_subscriber_wait_set_; }; @@ -185,7 +183,7 @@ class PidControllerFixture : public ::testing::Test rclcpp::Node test_subscription_node("test_subscription_node"); auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; auto subscription = test_subscription_node.create_subscription( - "/test_pid_controller/state", 10, subs_callback); + "/test_pid_controller/controller_state", 10, subs_callback); // call update to publish the test value ASSERT_EQ( From c60a8559945cafb6352f52d534295b66d87ccbac Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 4 Apr 2023 22:32:00 +0200 Subject: [PATCH 24/37] Add user doc --- pid_controller/doc/userdoc.rst | 79 ++++++++++++++++++++++++++++++++++ 1 file changed, 79 insertions(+) create mode 100644 pid_controller/doc/userdoc.rst diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst new file mode 100644 index 0000000000..c7a3ce7963 --- /dev/null +++ b/pid_controller/doc/userdoc.rst @@ -0,0 +1,79 @@ +.. _pid_controller_userdoc: + +pid_controller +========================= + +PID Controller implementation that uses PidROS implemenation 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 first derivation of the reference and its feedback to have second-order PID control. + +Depending on the referece/state and command interface of hardware differnt parameters 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:: + + Theretically one can missuse :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 use if you need to interpolate between trajectory points using linear, cubic or quintic interplation. PID Controller doesn't to that. + PID term of JTC has differnet purpose - it enable 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 considerd to be first derivative of the first type. +For example a valid combination would be ``position`` and ``velocity`` interface types. + + +Description of controller's interfaces +-------------------------------------- + +References (from a preceding controller) +,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names`` can be ``reference_and_state_dof_names`` parameter of if empty ``dof_names``. + +Commands +,,,,,,,,, +- / [double] + +States +,,,,,,, +- / [double] + **NOTE**: ``reference_and_state_dof_names`` can be ``reference_and_state_dof_names`` parameter of if empty ``dof_names``. + + +Subscribers +,,,,,,,,,,,, +Used when controller is not in chained mode (``in_chained_mode == false``). + +- /reference [control_msgs/msg/MultiDOFCommand] + +Used when controller parameter ``use_external_measured_states`` is used. + +- /measured_state [control_msgs/msg/MultiDOFCommand] + +Services +````````` + +- /set_feedforward_control [std_srvs/srv/SetBool] + +Publishers +,,,,,,,,,,, +- /controller_state [control_msgs/msg/MultiDOFStateStamped] + +Parameters +,,,,,,,,,,, + +For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. + +For an exameplary parameterization see the ``test`` folder of the controller's package. From cd1c875289f58808460b53e5fd7597a24974ac22 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 4 Apr 2023 22:32:53 +0200 Subject: [PATCH 25/37] Update controllers_index.rst --- doc/controllers_index.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 5f8780d961..d042fe79dd 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -55,6 +55,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> From ef6a20600649c23e35d38d57603f42b3ffcf1a17 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 5 Apr 2023 13:51:50 +0200 Subject: [PATCH 26/37] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- pid_controller/doc/userdoc.rst | 29 +++++++++++++++----------- pid_controller/src/pid_controller.yaml | 8 +++---- 2 files changed, 21 insertions(+), 16 deletions(-) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index c7a3ce7963..e10fbef6fd 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -3,11 +3,11 @@ pid_controller ========================= -PID Controller implementation that uses PidROS implemenation from `control_toolbox `_ package. +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 first derivation of the reference and its feedback to have second-order PID control. +It also enables to use the first derivative of the reference and its feedback to have second-order PID control. -Depending on the referece/state and command interface of hardware differnt parameters setup of PidROS should be used as for example: +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 @@ -22,17 +22,22 @@ Depending on the referece/state and command interface of hardware differnt param .. note:: - Theretically one can missuse :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 use if you need to interpolate between trajectory points using linear, cubic or quintic interplation. PID Controller doesn't to that. + 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 differnet purpose - it enable 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 considerd to be first derivative of the first type. +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 -------------------------------------- @@ -40,7 +45,7 @@ Description of controller's interfaces References (from a preceding controller) ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, - / [double] - **NOTE**: ``reference_and_state_dof_names`` can be ``reference_and_state_dof_names`` parameter of if empty ``dof_names``. + **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 ,,,,,,,,, @@ -49,16 +54,16 @@ Commands States ,,,,,,, - / [double] - **NOTE**: ``reference_and_state_dof_names`` can be ``reference_and_state_dof_names`` parameter of if empty ``dof_names``. + **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 ,,,,,,,,,,,, -Used when controller is not in chained mode (``in_chained_mode == false``). +If controller is not in chained mode (``in_chained_mode == false``): - /reference [control_msgs/msg/MultiDOFCommand] -Used when controller parameter ``use_external_measured_states`` is used. +If controller parameter ``use_external_measured_states`` is true: - /measured_state [control_msgs/msg/MultiDOFCommand] @@ -74,6 +79,6 @@ Publishers Parameters ,,,,,,,,,,, -For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package. +For a list of parameters and their meaning see the YAML file in the ``src`` folder of the controller's package. -For an exameplary parameterization see the ``test`` folder of the controller's package. +For an exemplary parameterization see the ``test`` folder of the controller's package. diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index db15d43ac2..6e5fb89abd 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -40,7 +40,7 @@ pid_controller: use_external_measured_states: { type: bool, default_value: false, - description: "Use external states from a topic instead form state interfaces." + description: "Use external states from a topic instead from state interfaces." } gains: __map_dof_names: @@ -67,17 +67,17 @@ pid_controller: i_clamp_max: { type: double, default_value: 0.0, - description: "Upper integral clamp. Only used if antiwindeup is activated." + 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 antiwindeup is activated." + description: "Lower integral clamp. Only used if antiwindup is activated." } feedforward_gain: { type: double, default_value: 0.0, - description: "Scale for the feed-forward." + description: "Gain for the feed-forward part." } runtime_param_update: { From 0a34441e84bf3229901cd4d36be185bf301bf2ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 5 Apr 2023 13:54:18 +0200 Subject: [PATCH 27/37] Fixup formatting. --- pid_controller/doc/userdoc.rst | 2 +- pid_controller/src/pid_controller.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index e10fbef6fd..47bfc76a7c 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -24,7 +24,7 @@ Depending on the reference/state and command interface of the hardware a differe 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 differnet purpose - it enable commanding only ``velocity`` or ``effort`` interfaces to hardware. + PID term of JTC has different purpose - it enables commanding only ``velocity`` or ``effort`` interfaces to hardware. Execution logic of the controller ---------------------------------- diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 9c8159acb4..53799782ed 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -205,8 +205,8 @@ controller_interface::CallbackReturn PidController::on_configure( try { // State publisher - s_publisher_ = - get_node()->create_publisher("~/controller_state", rclcpp::SystemDefaultsQoS()); + s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(s_publisher_); } catch (const std::exception & e) From d29ef49b477fbc4fb1a3d8d1f9939d5611adb86c Mon Sep 17 00:00:00 2001 From: Bijou Date: Fri, 7 Apr 2023 11:42:56 -0700 Subject: [PATCH 28/37] Fix unused variable compiler warning in pid_controller tests --- .../test/test_pid_controller_preceding.cpp | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index be144565fd..e0d3051226 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -67,11 +67,9 @@ TEST_F(PidControllerTest, check_exported_intefaces) size_t si_index = 0; for (const auto & interface : state_interfaces_) { - for (const auto & dof_name : dof_names_) + for (const auto & dof_name : reference_and_state_dof_names_) { - EXPECT_EQ( - state_intefaces.names[si_index], - reference_and_state_dof_names_[si_index] + "/" + interface); + EXPECT_EQ(state_intefaces.names[si_index], dof_name + "/" + interface); ++si_index; } } @@ -82,16 +80,14 @@ TEST_F(PidControllerTest, check_exported_intefaces) size_t ri_index = 0; for (const auto & interface : state_interfaces_) { - for (const auto & dof_name : dof_names_) + for (const auto & dof_name : reference_and_state_dof_names_) { - const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + - reference_and_state_dof_names_[ri_index] + "/" + interface; + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; EXPECT_EQ(reference_interfaces[ri_index].get_name(), ref_itf_name); EXPECT_EQ( reference_interfaces[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ( - reference_interfaces[ri_index].get_interface_name(), - reference_and_state_dof_names_[ri_index] + "/" + interface); + EXPECT_EQ(reference_interfaces[ri_index].get_interface_name(), dof_name + "/" + interface); ++ri_index; } } From 0a414ee1f64a91f1762d9b51303e00b635d39a76 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 12 Apr 2023 19:15:53 +0200 Subject: [PATCH 29/37] Update pid_controller/src/pid_controller.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- pid_controller/src/pid_controller.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index 6e5fb89abd..fa4e2c678e 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -2,7 +2,7 @@ pid_controller: dof_names: { type: string_array, default_value: [], - description: "Specifies dof_names or axes used by the controller. If 'state_dof_names' parameter is defined, then only command dof names are defined with this parameter.", + 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, From 9ec3c02c58acc6ea183db20bdbfdecd7bd1a5524 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 19 Apr 2023 09:59:29 +0200 Subject: [PATCH 30/37] Update pid_controller/doc/userdoc.rst MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- pid_controller/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index 47bfc76a7c..91592183c9 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -68,7 +68,7 @@ If controller parameter ``use_external_measured_states`` is true: - /measured_state [control_msgs/msg/MultiDOFCommand] Services -````````` +,,,,,,,,,,, - /set_feedforward_control [std_srvs/srv/SetBool] From ce5ac8cb7f60175cdb2d273cad6ad3726095c99a Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 4 May 2023 21:14:07 +0200 Subject: [PATCH 31/37] Remove misleading docs. --- pid_controller/src/pid_controller.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 53799782ed..d7ac7fb5ee 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -25,21 +25,6 @@ #include "control_msgs/msg/single_dof_state.hpp" #include "controller_interface/helpers.hpp" -// The main knowledge could be with regard to the input (reference/state) and output (command) -// interfaces and their physical meaning, 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 -// - namespace { // utility From cc8d2c09995881e4d28992778270c8f06a63405a Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 19 Jun 2023 18:30:04 +0200 Subject: [PATCH 32/37] Don't reset commands in `on_deactivate`. --- pid_controller/src/pid_controller.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index d7ac7fb5ee..0a90db18c9 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -357,11 +357,6 @@ controller_interface::CallbackReturn PidController::on_activate( controller_interface::CallbackReturn PidController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - // instead of a loop - for (size_t i = 0; i < dof_; ++i) - { - command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); - } return controller_interface::CallbackReturn::SUCCESS; } From 2f7bf48419b71eab5bb6123c8d0f2bf8fdf6492f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 11 Sep 2023 18:54:43 +0200 Subject: [PATCH 33/37] Update PIDController to enable parameter updates. --- .../include/pid_controller/pid_controller.hpp | 5 ++++ pid_controller/src/pid_controller.cpp | 23 ++++++++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 93a2152fe4..a34dc9f87f 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -64,6 +64,10 @@ class PidController : public controller_interface::ChainableControllerInterface 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; @@ -123,6 +127,7 @@ class PidController : public controller_interface::ChainableControllerInterface bool on_set_chained_mode(bool chained_mode) override; // internal methods + void update_parameters(); controller_interface::CallbackReturn configure_parameters(); private: diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 0a90db18c9..0c2ffc2127 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -74,9 +74,18 @@ controller_interface::CallbackReturn PidController::on_init() return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn PidController::configure_parameters() +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()) { @@ -125,6 +134,15 @@ controller_interface::CallbackReturn PidController::configure_parameters() 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*/) { @@ -384,6 +402,9 @@ controller_interface::return_type PidController::update_reference_from_subscribe 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()); From fe7f0a1e1d74a0e5cf370973e473f033c8efdd4d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Sun, 3 Dec 2023 19:26:55 +0100 Subject: [PATCH 34/37] Remove option for enabling/disabling runtime param updates - they can be always updated. Enalbe option for Angle wraparound and add templates for the tests. --- pid_controller/src/pid_controller.cpp | 22 ++++++----- pid_controller/src/pid_controller.yaml | 12 +++--- pid_controller/test/test_pid_controller.cpp | 44 +++++++++++++++++++++ 3 files changed, 62 insertions(+), 16 deletions(-) diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 0c2ffc2127..c46e9dbab8 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -367,8 +367,6 @@ controller_interface::CallbackReturn PidController::on_activate( measured_state_values_.assign( measured_state_values_.size(), std::numeric_limits::quiet_NaN()); - // TODO(destogl): make here parameter update - return controller_interface::CallbackReturn::SUCCESS; } @@ -443,6 +441,14 @@ controller_interface::return_type PidController::update_and_write_commands( 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( - reference_interfaces_[i] - measured_state_values_[i], - reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); + error, reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); } else { // Fallback to calculation with 'error' only - tmp_command += - pids_[i]->computeCommand(reference_interfaces_[i] - measured_state_values_[i], period); + tmp_command += pids_[i]->computeCommand(error, period); } } else { // use calculation with 'error' only - tmp_command += - pids_[i]->computeCommand(reference_interfaces_[i] - measured_state_values_[i], period); + tmp_command += pids_[i]->computeCommand(error, period); } // write calculated values @@ -485,8 +488,7 @@ controller_interface::return_type PidController::update_and_write_commands( { 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]; + state_publisher_->msg_.dof_states[i].error = error; if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) { state_publisher_->msg_.dof_states[i].error_dot = diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index fa4e2c678e..f645738862 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -79,9 +79,9 @@ pid_controller: default_value: 0.0, description: "Gain for the feed-forward part." } - - runtime_param_update: { - type: bool, - default_value: false, - description: "Enable parameters update when controller is active on each control step. If the parameter is 'false' then reactivation of controller is needed to update parameters." - } + 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/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index eb7bd0b701..bbb669dd78 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -393,6 +393,50 @@ TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) } } +/** + * @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(); From 166a9f5e182130507fa04b94f6f7b276584be9be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Sun, 3 Dec 2023 20:03:36 +0100 Subject: [PATCH 35/37] Correct compilation and tests. --- pid_controller/CMakeLists.txt | 1 + pid_controller/package.xml | 1 + pid_controller/src/pid_controller.cpp | 12 ++++++++++-- pid_controller/test/test_pid_controller.cpp | 12 ++++++++---- pid_controller/test/test_pid_controller.hpp | 2 +- 5 files changed, 21 insertions(+), 7 deletions(-) diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index d13bda3751..81cbe6f006 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -6,6 +6,7 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + angles control_msgs control_toolbox controller_interface diff --git a/pid_controller/package.xml b/pid_controller/package.xml index ccfe28aba3..ccbb4b2feb 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -11,6 +11,7 @@ generate_parameter_library + angles control_msgs control_toolbox controller_interface diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index c46e9dbab8..373e941d96 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -22,6 +22,7 @@ #include #include +#include "angles/angles.h" #include "control_msgs/msg/single_dof_state.hpp" #include "controller_interface/helpers.hpp" @@ -446,7 +447,7 @@ controller_interface::return_type PidController::update_and_write_commands( { // for continuous angles the error is normalized between -pimsg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i]; } - state_publisher_->msg_.dof_states[i].error = error; + 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 = diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index bbb669dd78..11f703a1a4 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -38,7 +38,6 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) 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_FALSE(controller_->params_.runtime_param_update); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -60,7 +59,6 @@ TEST_F(PidControllerTest, all_parameters_set_configure_success) controller_->params_.reference_and_state_interfaces, testing::ElementsAreArray(state_interfaces_)); ASSERT_FALSE(controller_->params_.use_external_measured_states); - ASSERT_FALSE(controller_->params_.runtime_param_update); } TEST_F(PidControllerTest, check_exported_intefaces) @@ -161,11 +159,17 @@ TEST_F(PidControllerTest, reactivate_success) 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_->command_interfaces_[0].get_value())); + 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_->command_interfaces_[0].get_value())); + 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)), diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 48ffccd737..ab32f5cb48 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -146,7 +146,7 @@ class PidControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_pid_controller") { - ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); + ASSERT_EQ(controller_->init(controller_name, "", 0), controller_interface::return_type::OK); std::vector command_ifs; command_itfs_.reserve(dof_names_.size()); From d0cb6e2e42a048c8e67b7d4993a20aaf2669dd87 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 4 Dec 2023 17:06:48 +0100 Subject: [PATCH 36/37] Update doc. --- pid_controller/doc/userdoc.rst | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/pid_controller/doc/userdoc.rst b/pid_controller/doc/userdoc.rst index 91592183c9..5570f523fe 100644 --- a/pid_controller/doc/userdoc.rst +++ b/pid_controller/doc/userdoc.rst @@ -1,7 +1,9 @@ +: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 +-------------------------------- 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. @@ -27,20 +29,20 @@ Depending on the reference/state and command interface of the hardware a differe 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) ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, @@ -79,6 +81,6 @@ Publishers Parameters ,,,,,,,,,,, -For a list of parameters and their meaning see the YAML file in the ``src`` folder of the controller's package. +The PID controller uses the `generate_parameter_library `_ to handle its parameters. -For an exemplary parameterization see the ``test`` folder of the controller's package. +.. generate_parameter_library_details:: ../src/pid_controller.yaml From 40b93607f8b33e3080cc1ca33afd890966d1e3cb Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 4 Dec 2023 18:58:01 +0000 Subject: [PATCH 37/37] update maintainers --- pid_controller/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pid_controller/package.xml b/pid_controller/package.xml index ccbb4b2feb..7d52121582 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -4,7 +4,9 @@ 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