From ec2c0b9ff05d6cd7915e07ecdf0af6111cab055f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 19 Nov 2025 20:50:23 +0000 Subject: [PATCH] Remove deprecated packages --- doc/controllers_index.rst | 3 - effort_controllers/CHANGELOG.rst | 356 ----------------- effort_controllers/CMakeLists.txt | 74 ---- effort_controllers/doc/userdoc.rst | 51 --- .../effort_controllers_plugins.xml | 7 - .../joint_group_effort_controller.hpp | 45 --- effort_controllers/package.xml | 43 --- .../src/joint_group_effort_controller.cpp | 74 ---- .../test_joint_group_effort_controller.yaml | 3 - .../test_joint_group_effort_controller.cpp | 207 ---------- .../test_joint_group_effort_controller.hpp | 67 ---- ...est_load_joint_group_effort_controller.cpp | 45 --- position_controllers/CHANGELOG.rst | 357 ------------------ position_controllers/CMakeLists.txt | 73 ---- position_controllers/doc/userdoc.rst | 51 --- .../joint_group_position_controller.hpp | 42 --- position_controllers/package.xml | 43 --- .../position_controllers_plugins.xml | 7 - .../src/joint_group_position_controller.cpp | 56 --- .../test_joint_group_position_controller.yaml | 3 - .../test_joint_group_position_controller.cpp | 180 --------- .../test_joint_group_position_controller.hpp | 68 ---- ...t_load_joint_group_position_controller.cpp | 46 --- ros2_controllers/doc/index.rst | 12 - ros2_controllers/package.xml | 3 - velocity_controllers/CHANGELOG.rst | 354 ----------------- velocity_controllers/CMakeLists.txt | 74 ---- velocity_controllers/doc/userdoc.rst | 51 --- .../joint_group_velocity_controller.hpp | 45 --- velocity_controllers/package.xml | 43 --- .../src/joint_group_velocity_controller.cpp | 75 ---- .../test_joint_group_velocity_controller.yaml | 3 - .../test_joint_group_velocity_controller.cpp | 204 ---------- .../test_joint_group_velocity_controller.hpp | 68 ---- ...t_load_joint_group_velocity_controller.cpp | 46 --- .../velocity_controllers_plugins.xml | 7 - 36 files changed, 2886 deletions(-) delete mode 100644 effort_controllers/CHANGELOG.rst delete mode 100644 effort_controllers/CMakeLists.txt delete mode 100644 effort_controllers/doc/userdoc.rst delete mode 100644 effort_controllers/effort_controllers_plugins.xml delete mode 100644 effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp delete mode 100644 effort_controllers/package.xml delete mode 100644 effort_controllers/src/joint_group_effort_controller.cpp delete mode 100644 effort_controllers/test/config/test_joint_group_effort_controller.yaml delete mode 100644 effort_controllers/test/test_joint_group_effort_controller.cpp delete mode 100644 effort_controllers/test/test_joint_group_effort_controller.hpp delete mode 100644 effort_controllers/test/test_load_joint_group_effort_controller.cpp delete mode 100644 position_controllers/CHANGELOG.rst delete mode 100644 position_controllers/CMakeLists.txt delete mode 100644 position_controllers/doc/userdoc.rst delete mode 100644 position_controllers/include/position_controllers/joint_group_position_controller.hpp delete mode 100644 position_controllers/package.xml delete mode 100644 position_controllers/position_controllers_plugins.xml delete mode 100644 position_controllers/src/joint_group_position_controller.cpp delete mode 100644 position_controllers/test/config/test_joint_group_position_controller.yaml delete mode 100644 position_controllers/test/test_joint_group_position_controller.cpp delete mode 100644 position_controllers/test/test_joint_group_position_controller.hpp delete mode 100644 position_controllers/test/test_load_joint_group_position_controller.cpp delete mode 100644 velocity_controllers/CHANGELOG.rst delete mode 100644 velocity_controllers/CMakeLists.txt delete mode 100644 velocity_controllers/doc/userdoc.rst delete mode 100644 velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp delete mode 100644 velocity_controllers/package.xml delete mode 100644 velocity_controllers/src/joint_group_velocity_controller.cpp delete mode 100644 velocity_controllers/test/config/test_joint_group_velocity_controller.yaml delete mode 100644 velocity_controllers/test/test_joint_group_velocity_controller.cpp delete mode 100644 velocity_controllers/test/test_joint_group_velocity_controller.hpp delete mode 100644 velocity_controllers/test/test_load_joint_group_velocity_controller.cpp delete mode 100644 velocity_controllers/velocity_controllers_plugins.xml diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 7b286a61d5..f0a08751ff 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -40,13 +40,10 @@ Controllers for Manipulators and Other Robots :titlesonly: Admittance Controller <../admittance_controller/doc/userdoc.rst> - Effort Controllers <../effort_controllers/doc/userdoc.rst> Forward Command Controller <../forward_command_controller/doc/userdoc.rst> Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst> Parallel Gripper Controller <../parallel_gripper_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> Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> Motion Primitive Controller <../motion_primitives_controllers/userdoc.rst> diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst deleted file mode 100644 index 2bfccc231e..0000000000 --- a/effort_controllers/CHANGELOG.rst +++ /dev/null @@ -1,356 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package effort_controllers -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -6.0.0 (2025-11-10) ------------------- -* Controller interface api update to ros2_controller packages (`#1973 `_) -* Contributors: Anand Vardhan - -5.8.0 (2025-10-02) ------------------- -* Remove deprecated methods from ros2_control (`#1936 `_) -* Contributors: Christoph Fröhlich - -5.7.0 (2025-09-12) ------------------- - -5.6.1 (2025-08-30) ------------------- - -5.6.0 (2025-08-29) ------------------- -* Remove usage of `get_ordered_interfaces` but update parameter validation instead (`#1816 `_) -* Contributors: Christoph Fröhlich - -5.5.0 (2025-07-31) ------------------- - -5.4.0 (2025-07-23) ------------------- -* Use new handles API in ros2_controllers to fix deprecation warnings (`#1566 `_) -* Contributors: Sanjeev Kumar - -5.3.0 (2025-07-14) ------------------- -* Update realtime containers (`#1721 `_) -* Contributors: Christoph Fröhlich - -5.2.0 (2025-06-23) ------------------- - -5.1.0 (2025-06-11) ------------------- - -5.0.2 (2025-05-26) ------------------- - -5.0.1 (2025-05-24) ------------------- -* Use target_link_libraries instead of ament_target_dependencies (`#1697 `_) -* Contributors: Sai Kishor Kothakota - -5.0.0 (2025-05-17) ------------------- - -4.24.0 (2025-04-27) -------------------- -* Call `configure()` of base class instead of node (`#1659 `_) -* Contributors: Christoph Fröhlich - -4.23.0 (2025-04-10) -------------------- -* Use global cmake macros and fix gcc-10 build (`#1527 `_) -* Contributors: Christoph Fröhlich - -4.22.0 (2025-03-17) -------------------- -* Fix the `ActivateWithWrongJointsNamesFails` test (`#1570 `_) -* Contributors: Christoph Fröhlich - -4.21.0 (2025-03-01) -------------------- - -4.20.0 (2025-01-29) -------------------- - -4.19.0 (2025-01-13) -------------------- -* Remove visibility macros (`#1451 `_) -* Contributors: Bence Magyar - -4.18.0 (2024-12-19) -------------------- - -4.17.0 (2024-12-07) -------------------- -* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) -* Update maintainers and add url tags (`#1363 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.16.0 (2024-11-08) -------------------- - -4.15.0 (2024-10-07) -------------------- - -4.14.0 (2024-09-11) -------------------- - -4.13.0 (2024-08-22) -------------------- - -4.12.1 (2024-08-14) -------------------- - -4.12.0 (2024-07-23) -------------------- -* Unused header cleanup (`#1199 `_) -* Fix WaitSet issue in tests (`#1206 `_) -* Fix parallel gripper controller CI (`#1202 `_) -* Contributors: Henry Moore, Sai Kishor Kothakota - -4.11.0 (2024-07-09) -------------------- -* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) -* Contributors: Sai Kishor Kothakota - -4.10.0 (2024-07-01) -------------------- - -4.9.0 (2024-06-05) ------------------- - -4.8.0 (2024-05-14) ------------------- - -4.7.0 (2024-03-22) ------------------- - -4.6.0 (2024-02-12) ------------------- -* Add test_depend on `hardware_interface_testing` (`#1018 `_) -* Fix tests for using new `get_node_options` API (`#840 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.5.0 (2024-01-31) ------------------- - -4.4.0 (2024-01-11) ------------------- - -4.3.0 (2024-01-08) ------------------- -* Add few warning flags to error (`#961 `_) -* Contributors: Sai Kishor Kothakota - -4.2.0 (2023-12-12) ------------------- - -4.1.0 (2023-12-01) ------------------- - -4.0.0 (2023-11-21) ------------------- -* fix tests for API break of passing controller manager update rate in init method (`#854 `_) -* Adjust tests after passing URDF to controllers (`#817 `_) -* Contributors: Bence Magyar, Sai Kishor Kothakota - -3.17.0 (2023-10-31) -------------------- - -3.16.0 (2023-09-20) -------------------- -* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) -* Contributors: Christoph Fröhlich - -3.15.0 (2023-09-11) -------------------- - -3.14.0 (2023-08-16) -------------------- - -3.13.0 (2023-08-04) -------------------- - -3.12.0 (2023-07-18) -------------------- - -3.11.0 (2023-06-24) -------------------- -* Added -Wconversion flag and fix warnings (`#667 `_) -* Contributors: gwalck - -3.10.1 (2023-06-06) -------------------- - -3.10.0 (2023-06-04) -------------------- - -3.9.0 (2023-05-28) ------------------- -* Use branch name substitution for all links (`#618 `_) -* Fix github links on control.ros.org (`#604 `_) -* Contributors: Christoph Fröhlich - -3.8.0 (2023-05-14) ------------------- - -3.7.0 (2023-05-02) ------------------- - -3.6.0 (2023-04-29) ------------------- -* Renovate load controller tests (`#569 `_) -* Contributors: Bence Magyar - -3.5.0 (2023-04-14) ------------------- - -3.4.0 (2023-04-02) ------------------- - -3.3.0 (2023-03-07) ------------------- - -3.2.0 (2023-02-10) ------------------- -* Fix overriding of install (`#510 `_) -* Contributors: Tyler Weaver, Chris Thrasher - -3.1.0 (2023-01-26) ------------------- - -3.0.0 (2023-01-19) ------------------- -* Add backward_ros to all controllers (`#489 `_) -* Contributors: Bence Magyar - -2.15.0 (2022-12-06) -------------------- - -2.14.0 (2022-11-18) -------------------- - -2.13.0 (2022-10-05) -------------------- - -2.12.0 (2022-09-01) -------------------- - -2.11.0 (2022-08-04) -------------------- - -2.10.0 (2022-08-01) -------------------- - -2.9.0 (2022-07-14) ------------------- - -2.8.0 (2022-07-09) ------------------- - -2.7.0 (2022-07-03) ------------------- - -2.6.0 (2022-06-18) ------------------- -* CMakeLists cleanup (`#362 `_) -* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) - * Default C++ version to 17 - * Replace explicit use of declare_paremeter with auto_declare -* Contributors: Andy Zelenak, Jafar Abdi - -2.5.0 (2022-05-13) ------------------- - -2.4.0 (2022-04-29) ------------------- -* Multi-interface Forward Controller (`#154 `_) -* Contributors: Denis Štogl - -2.3.0 (2022-04-21) ------------------- -* Use CallbackReturn from controller_interface namespace (`#333 `_) -* Contributors: Bence Magyar, Denis Štogl - -2.2.0 (2022-03-25) ------------------- -* Use lifecycle node as base for controllers (`#244 `_) -* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar - -2.1.0 (2022-02-23) ------------------- - -2.0.1 (2022-02-01) ------------------- - -2.0.0 (2022-01-28) ------------------- - -1.3.0 (2022-01-11) ------------------- - -1.2.0 (2021-12-29) ------------------- - -1.1.0 (2021-10-25) ------------------- - -1.0.0 (2021-09-29) ------------------- -* Add time and period to update function (`#241 `_) -* Unify style of controllers. (`#236 `_) -* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) -* Removing Boost from controllers. (`#235 `_) -* Contributors: Bence Magyar, Denis Štogl, bailaC - -0.5.0 (2021-08-30) ------------------- -* Bring precommit config up to speed with ros2_control (`#227 `_) -* Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) -* Add initial pre-commit setup. (`#220 `_) -* Reduce docs warnings and correct adding guidelines (`#219 `_) -* Contributors: Bence Magyar, Denis Štogl, Joseph Schornak - -0.4.1 (2021-07-08) ------------------- -* Fix test dependencies (`#213 `_) -* Contributors: Bence Magyar - -0.4.0 (2021-06-28) ------------------- -* Force torque sensor broadcaster (`#152 `_) - * Add rclcpp::shutdown(); to all standalone test functions -* Contributors: Denis Štogl - -0.3.1 (2021-05-23) ------------------- - -0.3.0 (2021-05-21) ------------------- - -0.2.1 (2021-05-03) ------------------- -* Migrate from deprecated controller_interface::return_type::SUCCESS -> OK (`#167 `_) -* Add basic user docs pages for each package (`#156 `_) -* Contributors: Bence Magyar - -0.2.0 (2021-02-06) ------------------- -* Use ros2 contol test assets (`#138 `_) - * Add description to test trajecotry_controller - * Use ros2_control_test_assets package - * Delete obsolete components plugin export -* Contributors: Denis Štogl - -0.1.2 (2021-01-07) ------------------- - -0.1.1 (2021-01-06) ------------------- -* Restore forward command derivatives (`#133 `_) -* Contributors: Bence Magyar - -0.1.0 (2020-12-23) ------------------- diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt deleted file mode 100644 index ddab54bb3f..0000000000 --- a/effort_controllers/CMakeLists.txt +++ /dev/null @@ -1,74 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(effort_controllers) - -find_package(ros2_control_cmake REQUIRED) -set_compiler_options() -export_windows_symbols() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp -) - -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(effort_controllers SHARED - src/joint_group_effort_controller.cpp -) -target_compile_features(effort_controllers PUBLIC cxx_std_17) -target_include_directories(effort_controllers PUBLIC - $ - $ -) -target_link_libraries(effort_controllers PUBLIC - forward_command_controller::forward_command_controller - pluginlib::pluginlib - rclcpp::rclcpp -) - -pluginlib_export_plugin_description_file(controller_interface effort_controllers_plugins.xml) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") - - ament_add_gmock(test_load_joint_group_effort_controller - test/test_load_joint_group_effort_controller.cpp - ) - target_link_libraries(test_load_joint_group_effort_controller - effort_controllers - controller_manager::controller_manager - ros2_control_test_assets::ros2_control_test_assets - ) - - ament_add_gmock(test_joint_group_effort_controller - test/test_joint_group_effort_controller.cpp - ) - target_link_libraries(test_joint_group_effort_controller - effort_controllers - ) -endif() - -install( - DIRECTORY include/ - DESTINATION include/effort_controllers -) -install( - TARGETS effort_controllers - EXPORT export_effort_controllers - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -ament_export_targets(export_effort_controllers HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/effort_controllers/doc/userdoc.rst b/effort_controllers/doc/userdoc.rst deleted file mode 100644 index d6189f0483..0000000000 --- a/effort_controllers/doc/userdoc.rst +++ /dev/null @@ -1,51 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/effort_controllers/doc/userdoc.rst - -.. _effort_controllers_userdoc: - -effort_controllers -================== - -This is a collection of controllers that work using the "effort" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the effort on a certain joint to achieve a set position. - -The package contains the following controllers: - -effort_controllers/JointGroupEffortController -------------------------------------------------- - -.. warning:: - - ``effort_controllers/JointGroupEffortController`` is deprecated. Use :ref:`forward_command_controller ` instead by adding the ``interface_name`` parameter and set it to ``effort``. - -This is specialization of the :ref:`forward_command_controller ` that works using the "effort" joint interface. - - -ROS 2 interface of the controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Topics -,,,,,,,,,,,,,,,,,, - -~/commands (input topic) [std_msgs::msg::Float64MultiArray] - Joints' effort commands - - -Parameters -,,,,,,,,,,,,,,,,,, -This controller overrides the interface parameter from :ref:`forward_command_controller `, and the -``joints`` parameter is the only one that is required. - -An example parameter file is given here - -.. code-block:: yaml - - controller_manager: - ros__parameters: - update_rate: 100 # Hz - - effort_controller: - type: effort_controllers/JointGroupEffortController - - effort_controller: - ros__parameters: - joints: - - slider_to_cart diff --git a/effort_controllers/effort_controllers_plugins.xml b/effort_controllers/effort_controllers_plugins.xml deleted file mode 100644 index 3f7d5853ae..0000000000 --- a/effort_controllers/effort_controllers_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The joint effort controller commands a group of joints through the effort interface - - - diff --git a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp b/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp deleted file mode 100644 index 2855570cfd..0000000000 --- a/effort_controllers/include/effort_controllers/joint_group_effort_controller.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ -#define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ - -#include "forward_command_controller/forward_command_controller.hpp" - -namespace effort_controllers -{ -/** - * \brief Forward command controller for a set of effort controlled joints (linear or angular). - * - * This class forwards the commanded efforts down to a set of joints. - * - * \param joints Names of the joints to control. - * - * Subscribes to: - * - \b command (std_msgs::msg::Float64MultiArray) : The effort commands to apply. - */ -class JointGroupEffortController : public forward_command_controller::ForwardCommandController -{ -public: - JointGroupEffortController(); - - controller_interface::CallbackReturn on_init() override; - - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; -}; - -} // namespace effort_controllers - -#endif // EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml deleted file mode 100644 index deee0cd3a0..0000000000 --- a/effort_controllers/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - effort_controllers - 6.0.0 - Generic controller for forwarding commands. - - Bence Magyar - Denis Štogl - Christoph Froehlich - Sai Kishor Kothakota - - Apache License 2.0 - - https://control.ros.org - https://github.com/ros-controls/ros2_controllers/issues - https://github.com/ros-controls/ros2_controllers/ - - Jordan Palacios - - ament_cmake - - ros2_control_cmake - - backward_ros - forward_command_controller - pluginlib - rclcpp - - ament_cmake_gmock - controller_manager - hardware_interface_testing - hardware_interface - ros2_control_test_assets - - - ament_cmake - - This package will be removed in ROS 2 Lyrical Luth. Instead, use - forward_command_controller, see migration guides for details: - https://control.ros.org/rolling/doc/ros2_controllers/doc/migration.html - - - diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp deleted file mode 100644 index e1dc67b57e..0000000000 --- a/effort_controllers/src/joint_group_effort_controller.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 "controller_interface/controller_interface.hpp" -#include "effort_controllers/joint_group_effort_controller.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/parameter.hpp" - -namespace effort_controllers -{ -JointGroupEffortController::JointGroupEffortController() -: forward_command_controller::ForwardCommandController() -{ - interface_name_ = hardware_interface::HW_IF_EFFORT; -} - -controller_interface::CallbackReturn JointGroupEffortController::on_init() -{ - RCLCPP_WARN( - get_node()->get_logger(), - "'effort_controllers/JointGroupEffortController' is deprecated. " - "Use 'forward_command_controller/ForwardCommandController' instead by adding the " - "'interface_name' parameter and set it to 'effort'."); - try - { - // Explicitly set the interface parameter declared by the forward_command_controller - // to match the value set in the JointGroupEffortController constructor. - auto_declare("interface_name", interface_name_); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return forward_command_controller::ForwardCommandController::on_init(); -} - -controller_interface::CallbackReturn JointGroupEffortController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) -{ - auto ret = ForwardCommandController::on_deactivate(previous_state); - // stop all joints - for (auto & command_interface : command_interfaces_) - { - if (!command_interface.set_value(0.0)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Unable to set command interface value to 0.0"); - return controller_interface::CallbackReturn::SUCCESS; - } - } - - return ret; -} - -} // namespace effort_controllers - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - effort_controllers::JointGroupEffortController, controller_interface::ControllerInterface) diff --git a/effort_controllers/test/config/test_joint_group_effort_controller.yaml b/effort_controllers/test/config/test_joint_group_effort_controller.yaml deleted file mode 100644 index 5cc99fec83..0000000000 --- a/effort_controllers/test/config/test_joint_group_effort_controller.yaml +++ /dev/null @@ -1,3 +0,0 @@ -test_joint_group_effort_controller: - ros__parameters: - joints: ["joint1"] diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp deleted file mode 100644 index 83a576a16d..0000000000 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ /dev/null @@ -1,207 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "hardware_interface/loaned_command_interface.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/utilities.hpp" -#include "test_joint_group_effort_controller.hpp" - -using CallbackReturn = controller_interface::CallbackReturn; -using hardware_interface::LoanedCommandInterface; - -void JointGroupEffortControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } - -void JointGroupEffortControllerTest::TearDownTestCase() { rclcpp::shutdown(); } - -void JointGroupEffortControllerTest::SetUp() -{ - controller_ = std::make_unique(); -} - -void JointGroupEffortControllerTest::TearDown() { controller_.reset(nullptr); } - -void JointGroupEffortControllerTest::SetUpController( - const std::vector & parameters) -{ - auto node_options = controller_->define_custom_node_options(); - node_options.parameter_overrides(parameters); - - controller_interface::ControllerInterfaceParams params; - params.controller_name = "test_joint_group_effort_controller"; - params.robot_description = ""; - params.update_rate = 0; - params.node_namespace = ""; - params.node_options = node_options; - const auto result = controller_->init(params); - ASSERT_EQ(result, controller_interface::return_type::OK); - - std::vector command_ifs; - command_ifs.emplace_back(joint_1_cmd_, nullptr); - command_ifs.emplace_back(joint_2_cmd_, nullptr); - command_ifs.emplace_back(joint_3_cmd_, nullptr); - controller_->assign_interfaces(std::move(command_ifs), {}); - executor.add_node(controller_->get_node()->get_node_base_interface()); -} - -TEST_F(JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); -} - -TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful though no command has been send yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - // send command - forward_command_controller::CmdType command; - command.data = {10.0, 20.0, 30.0}; - controller_->rt_command_.set(command); - - // update successful, command received - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); -} - -TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // send command with wrong number of joints - forward_command_controller::CmdType command; - command.data = {10.0, 20.0}; - controller_->rt_command_.set(command); - - // update failed, command size does not match number of joints - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::ERROR); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful, no command received yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // default values - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - auto node_state = controller_->configure(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - - node_state = controller_->get_node()->activate(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - - // send a new command - rclcpp::Node test_node("test_node"); - auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); - std_msgs::msg::Float64MultiArray command_msg; - command_msg.data = {10.0, 20.0, 30.0}; - command_pub->publish(command_msg); - - // wait for command message to be passed - const auto timeout = std::chrono::milliseconds{10}; - const auto until = controller_->get_node()->get_clock()->now() + timeout; - while (controller_->get_node()->get_clock()->now() < until) - { - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::microseconds(10)); - } - - // update successful - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check command in handle was set - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); -} - -TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - // stop the controller - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 0.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 0.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 0.0); -} diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp deleted file mode 100644 index 74562623ed..0000000000 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ -#define TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ - -#include - -#include -#include -#include - -#include "effort_controllers/joint_group_effort_controller.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" - -using hardware_interface::CommandInterface; -using hardware_interface::HW_IF_EFFORT; -// subclassing and friending so we can access member variables -class FriendJointGroupEffortController : public effort_controllers::JointGroupEffortController -{ - FRIEND_TEST(JointGroupEffortControllerTest, CommandSuccessTest); - FRIEND_TEST(JointGroupEffortControllerTest, WrongCommandCheckTest); - FRIEND_TEST(JointGroupEffortControllerTest, CommandCallbackTest); - FRIEND_TEST(JointGroupEffortControllerTest, StopJointsOnDeactivateTest); -}; - -class JointGroupEffortControllerTest : public ::testing::Test -{ -public: - static void SetUpTestCase(); - static void TearDownTestCase(); - - void SetUp(); - void TearDown(); - - void SetUpController(const std::vector & parameters = {}); - -protected: - std::unique_ptr controller_; - - // dummy joint state values used for tests - const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; - std::vector joint_commands_ = {1.1, 2.1, 3.1}; - - CommandInterface::SharedPtr joint_1_cmd_ = - std::make_shared(joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]); - CommandInterface::SharedPtr joint_2_cmd_ = - std::make_shared(joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]); - CommandInterface::SharedPtr joint_3_cmd_ = - std::make_shared(joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]); - rclcpp::executors::SingleThreadedExecutor executor; -}; - -#endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_ diff --git a/effort_controllers/test/test_load_joint_group_effort_controller.cpp b/effort_controllers/test/test_load_joint_group_effort_controller.cpp deleted file mode 100644 index ed6fb53838..0000000000 --- a/effort_controllers/test/test_load_joint_group_effort_controller.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadJointGroupVelocityController, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - - const std::string test_file_path = - std::string(TEST_FILES_DIRECTORY) + "/config/test_joint_group_effort_controller.yaml"; - - cm.set_parameter({"test_joint_group_effort_controller.params_file", test_file_path}); - cm.set_parameter( - {"test_joint_group_effort_controller.type", "effort_controllers/JointGroupEffortController"}); - - ASSERT_NE(cm.load_controller("test_joint_group_effort_controller"), nullptr); - - rclcpp::shutdown(); -} diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst deleted file mode 100644 index 1115cf77dd..0000000000 --- a/position_controllers/CHANGELOG.rst +++ /dev/null @@ -1,357 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package position_controllers -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -6.0.0 (2025-11-10) ------------------- -* Controller interface api update to ros2_controller packages (`#1973 `_) -* Contributors: Anand Vardhan - -5.8.0 (2025-10-02) ------------------- -* Remove deprecated methods from ros2_control (`#1936 `_) -* Contributors: Christoph Fröhlich - -5.7.0 (2025-09-12) ------------------- - -5.6.1 (2025-08-30) ------------------- - -5.6.0 (2025-08-29) ------------------- -* Remove usage of `get_ordered_interfaces` but update parameter validation instead (`#1816 `_) -* Contributors: Christoph Fröhlich - -5.5.0 (2025-07-31) ------------------- - -5.4.0 (2025-07-23) ------------------- -* Use new handles API in ros2_controllers to fix deprecation warnings (`#1566 `_) -* Contributors: Sanjeev Kumar - -5.3.0 (2025-07-14) ------------------- -* Update realtime containers (`#1721 `_) -* Contributors: Christoph Fröhlich - -5.2.0 (2025-06-23) ------------------- - -5.1.0 (2025-06-11) ------------------- - -5.0.2 (2025-05-26) ------------------- - -5.0.1 (2025-05-24) ------------------- -* Use target_link_libraries instead of ament_target_dependencies (`#1697 `_) -* Contributors: Sai Kishor Kothakota - -5.0.0 (2025-05-17) ------------------- - -4.24.0 (2025-04-27) -------------------- -* Call `configure()` of base class instead of node (`#1659 `_) -* Contributors: Christoph Fröhlich - -4.23.0 (2025-04-10) -------------------- -* Use gmock instead of gtest (`#1625 `_) -* Use global cmake macros and fix gcc-10 build (`#1527 `_) -* Contributors: Christoph Fröhlich, Yassine Cherni - -4.22.0 (2025-03-17) -------------------- -* Fix the `ActivateWithWrongJointsNamesFails` test (`#1570 `_) -* Contributors: Christoph Fröhlich - -4.21.0 (2025-03-01) -------------------- - -4.20.0 (2025-01-29) -------------------- - -4.19.0 (2025-01-13) -------------------- -* Remove visibility macros (`#1451 `_) -* Contributors: Bence Magyar - -4.18.0 (2024-12-19) -------------------- -* Update position controller package.xml (`#1431 `_) -* Contributors: Jakub "Deli" Delicat - -4.17.0 (2024-12-07) -------------------- -* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) -* Update maintainers and add url tags (`#1363 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.16.0 (2024-11-08) -------------------- - -4.15.0 (2024-10-07) -------------------- - -4.14.0 (2024-09-11) -------------------- - -4.13.0 (2024-08-22) -------------------- - -4.12.1 (2024-08-14) -------------------- - -4.12.0 (2024-07-23) -------------------- -* Unused header cleanup (`#1199 `_) -* Fix WaitSet issue in tests (`#1206 `_) -* Fix parallel gripper controller CI (`#1202 `_) -* Contributors: Henry Moore, Sai Kishor Kothakota - -4.11.0 (2024-07-09) -------------------- -* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) -* Contributors: Sai Kishor Kothakota - -4.10.0 (2024-07-01) -------------------- - -4.9.0 (2024-06-05) ------------------- - -4.8.0 (2024-05-14) ------------------- - -4.7.0 (2024-03-22) ------------------- - -4.6.0 (2024-02-12) ------------------- -* Add test_depend on `hardware_interface_testing` (`#1018 `_) -* Fix tests for using new `get_node_options` API (`#840 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.5.0 (2024-01-31) ------------------- - -4.4.0 (2024-01-11) ------------------- - -4.3.0 (2024-01-08) ------------------- -* Add few warning flags to error (`#961 `_) -* Contributors: Sai Kishor Kothakota - -4.2.0 (2023-12-12) ------------------- - -4.1.0 (2023-12-01) ------------------- - -4.0.0 (2023-11-21) ------------------- -* fix tests for API break of passing controller manager update rate in init method (`#854 `_) -* Adjust tests after passing URDF to controllers (`#817 `_) -* Contributors: Bence Magyar, Sai Kishor Kothakota - -3.17.0 (2023-10-31) -------------------- - -3.16.0 (2023-09-20) -------------------- -* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) -* Contributors: Christoph Fröhlich - -3.15.0 (2023-09-11) -------------------- - -3.14.0 (2023-08-16) -------------------- - -3.13.0 (2023-08-04) -------------------- - -3.12.0 (2023-07-18) -------------------- - -3.11.0 (2023-06-24) -------------------- -* Added -Wconversion flag and fix warnings (`#667 `_) -* Contributors: gwalck - -3.10.1 (2023-06-06) -------------------- - -3.10.0 (2023-06-04) -------------------- - -3.9.0 (2023-05-28) ------------------- -* Use branch name substitution for all links (`#618 `_) -* Fix github links on control.ros.org (`#604 `_) -* Contributors: Christoph Fröhlich - -3.8.0 (2023-05-14) ------------------- - -3.7.0 (2023-05-02) ------------------- - -3.6.0 (2023-04-29) ------------------- -* Renovate load controller tests (`#569 `_) -* Contributors: Bence Magyar - -3.5.0 (2023-04-14) ------------------- - -3.4.0 (2023-04-02) ------------------- - -3.3.0 (2023-03-07) ------------------- - -3.2.0 (2023-02-10) ------------------- -* Fix overriding of install (`#510 `_) -* Contributors: Tyler Weaver, Chris Thrasher - -3.1.0 (2023-01-26) ------------------- - -3.0.0 (2023-01-19) ------------------- -* Add backward_ros to all controllers (`#489 `_) -* Contributors: Bence Magyar - -2.15.0 (2022-12-06) -------------------- - -2.14.0 (2022-11-18) -------------------- - -2.13.0 (2022-10-05) -------------------- - -2.12.0 (2022-09-01) -------------------- - -2.11.0 (2022-08-04) -------------------- - -2.10.0 (2022-08-01) -------------------- - -2.9.0 (2022-07-14) ------------------- - -2.8.0 (2022-07-09) ------------------- - -2.7.0 (2022-07-03) ------------------- - -2.6.0 (2022-06-18) ------------------- -* CMakeLists cleanup (`#362 `_) -* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) - * Default C++ version to 17 - * Replace explicit use of declare_paremeter with auto_declare -* Contributors: Andy Zelenak, Jafar Abdi - -2.5.0 (2022-05-13) ------------------- - -2.4.0 (2022-04-29) ------------------- -* Multi-interface Forward Controller (`#154 `_) -* Contributors: Denis Štogl - -2.3.0 (2022-04-21) ------------------- -* Use CallbackReturn from controller_interface namespace (`#333 `_) -* Contributors: Bence Magyar, Denis Štogl - -2.2.0 (2022-03-25) ------------------- -* Use lifecycle node as base for controllers (`#244 `_) -* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar - -2.1.0 (2022-02-23) ------------------- - -2.0.1 (2022-02-01) ------------------- - -2.0.0 (2022-01-28) ------------------- - -1.3.0 (2022-01-11) ------------------- - -1.2.0 (2021-12-29) ------------------- - -1.1.0 (2021-10-25) ------------------- - -1.0.0 (2021-09-29) ------------------- -* Add time and period to update function (`#241 `_) -* Unify style of controllers. (`#236 `_) -* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) -* Removing Boost from controllers. (`#235 `_) -* Contributors: Bence Magyar, Denis Štogl, bailaC - -0.5.0 (2021-08-30) ------------------- -* Bring precommit config up to speed with ros2_control (`#227 `_) -* Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) -* Add initial pre-commit setup. (`#220 `_) -* Reduce docs warnings and correct adding guidelines (`#219 `_) -* Contributors: Bence Magyar, Denis Štogl, Joseph Schornak - -0.4.1 (2021-07-08) ------------------- - -0.4.0 (2021-06-28) ------------------- -* Force torque sensor broadcaster (`#152 `_) - * Add rclcpp::shutdown(); to all standalone test functions -* Contributors: Denis Štogl - -0.3.1 (2021-05-23) ------------------- - -0.3.0 (2021-05-21) ------------------- - -0.2.1 (2021-05-03) ------------------- -* Migrate from deprecated controller_interface::return_type::SUCCESS -> OK (`#167 `_) -* Add basic user docs pages for each package (`#156 `_) -* Contributors: Bence Magyar - -0.2.0 (2021-02-06) ------------------- -* Use ros2 contol test assets (`#138 `_) - * Add description to test trajecotry_controller - * Use ros2_control_test_assets package - * Delete obsolete components plugin export -* Contributors: Denis Štogl - -0.1.2 (2021-01-07) ------------------- - -0.1.1 (2021-01-06) ------------------- -* Restore forward command derivatives (`#133 `_) -* Contributors: Bence Magyar - -0.1.0 (2020-12-23) ------------------- diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt deleted file mode 100644 index 48e56d6b0d..0000000000 --- a/position_controllers/CMakeLists.txt +++ /dev/null @@ -1,73 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(position_controllers) - -find_package(ros2_control_cmake REQUIRED) -set_compiler_options() -export_windows_symbols() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp -) - -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(position_controllers SHARED - src/joint_group_position_controller.cpp -) -target_compile_features(position_controllers PUBLIC cxx_std_17) -target_include_directories(position_controllers PUBLIC - $ - $ -) -target_link_libraries(position_controllers PUBLIC - forward_command_controller::forward_command_controller - pluginlib::pluginlib - rclcpp::rclcpp) - -pluginlib_export_plugin_description_file(controller_interface position_controllers_plugins.xml) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") - - ament_add_gmock(test_load_joint_group_position_controller - test/test_load_joint_group_position_controller.cpp - ) - target_link_libraries(test_load_joint_group_position_controller - position_controllers - controller_manager::controller_manager - ros2_control_test_assets::ros2_control_test_assets - ) - - ament_add_gmock(test_joint_group_position_controller - test/test_joint_group_position_controller.cpp - ) - target_link_libraries(test_joint_group_position_controller - position_controllers - ) -endif() - -install( - DIRECTORY include/ - DESTINATION include/position_controllers -) -install( - TARGETS position_controllers - EXPORT export_position_controllers - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -ament_export_targets(export_position_controllers HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/position_controllers/doc/userdoc.rst b/position_controllers/doc/userdoc.rst deleted file mode 100644 index 7e54747ae0..0000000000 --- a/position_controllers/doc/userdoc.rst +++ /dev/null @@ -1,51 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/position_controllers/doc/userdoc.rst - -.. _position_controllers_userdoc: - -position_controllers -===================== - -This is a collection of controllers that work using the "position" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the position on a certain joint to achieve a set velocity. - -The package contains the following controllers: - -position_controllers/JointGroupPositionController -------------------------------------------------- - -.. warning:: - - ``position_controllers/JointGroupPositionController`` is deprecated. Use :ref:`forward_command_controller ` instead by adding the ``interface_name`` parameter and set it to ``position``. - -This is specialization of the :ref:`forward_command_controller ` that works using the "position" joint interface. - - -ROS 2 interface of the controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Topics -,,,,,,,,,,,,,,,,,, - -~/commands (input topic) [std_msgs::msg::Float64MultiArray] - Joints' position commands - - -Parameters -,,,,,,,,,,,,,,,,,, -This controller overrides the interface parameter from :ref:`forward_command_controller `, and the -``joints`` parameter is the only one that is required. - -An example parameter file is given here - -.. code-block:: yaml - - controller_manager: - ros__parameters: - update_rate: 100 # Hz - - position_controller: - type: position_controllers/JointGroupPositionController - - position_controller: - ros__parameters: - joints: - - slider_to_cart diff --git a/position_controllers/include/position_controllers/joint_group_position_controller.hpp b/position_controllers/include/position_controllers/joint_group_position_controller.hpp deleted file mode 100644 index ceaf7639ed..0000000000 --- a/position_controllers/include/position_controllers/joint_group_position_controller.hpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ -#define POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ - -#include "forward_command_controller/forward_command_controller.hpp" - -namespace position_controllers -{ -/** - * \brief Forward command controller for a set of position controlled joints (linear or angular). - * - * This class forwards the commanded positions down to a set of joints. - * - * \param joints Names of the joints to control. - * - * Subscribes to: - * - \b commands (std_msgs::msg::Float64MultiArray) : The position commands to apply. - */ -class JointGroupPositionController : public forward_command_controller::ForwardCommandController -{ -public: - JointGroupPositionController(); - - controller_interface::CallbackReturn on_init() override; -}; - -} // namespace position_controllers - -#endif // POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/position_controllers/package.xml b/position_controllers/package.xml deleted file mode 100644 index 5f63464946..0000000000 --- a/position_controllers/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - position_controllers - 6.0.0 - Generic position controller for forwarding position commands. - - Bence Magyar - Denis Štogl - Christoph Froehlich - Sai Kishor Kothakota - - Apache License 2.0 - - https://control.ros.org - https://github.com/ros-controls/ros2_controllers/issues - https://github.com/ros-controls/ros2_controllers/ - - Jordan Palacios - - ament_cmake - - ros2_control_cmake - - backward_ros - forward_command_controller - pluginlib - rclcpp - - ament_cmake_gmock - controller_manager - hardware_interface_testing - hardware_interface - ros2_control_test_assets - - - ament_cmake - - This package will be removed in ROS 2 Lyrical Luth. Instead, use - forward_command_controller, see migration guides for details: - https://control.ros.org/rolling/doc/ros2_controllers/doc/migration.html - - - diff --git a/position_controllers/position_controllers_plugins.xml b/position_controllers/position_controllers_plugins.xml deleted file mode 100644 index 5f8331c324..0000000000 --- a/position_controllers/position_controllers_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The joint position controller commands a group of joints through the position interface - - - diff --git a/position_controllers/src/joint_group_position_controller.cpp b/position_controllers/src/joint_group_position_controller.cpp deleted file mode 100644 index 510d8e337d..0000000000 --- a/position_controllers/src/joint_group_position_controller.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 "controller_interface/controller_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "position_controllers/joint_group_position_controller.hpp" -#include "rclcpp/parameter.hpp" - -namespace position_controllers -{ -JointGroupPositionController::JointGroupPositionController() -: forward_command_controller::ForwardCommandController() -{ - interface_name_ = hardware_interface::HW_IF_POSITION; -} - -controller_interface::CallbackReturn JointGroupPositionController::on_init() -{ - RCLCPP_WARN( - get_node()->get_logger(), - "'position_controllers/JointGroupPositionController' is deprecated. " - "Use 'forward_command_controller/ForwardCommandController' instead by adding the " - "'interface_name' parameter and set it to 'position'."); - try - { - // Explicitly set the interface parameter declared by the forward_command_controller - // to match the value set in the JointGroupPositionController constructor. - auto_declare("interface_name", interface_name_); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return forward_command_controller::ForwardCommandController::on_init(); -} -} // namespace position_controllers - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - position_controllers::JointGroupPositionController, controller_interface::ControllerInterface) diff --git a/position_controllers/test/config/test_joint_group_position_controller.yaml b/position_controllers/test/config/test_joint_group_position_controller.yaml deleted file mode 100644 index 6e4b7bee08..0000000000 --- a/position_controllers/test/config/test_joint_group_position_controller.yaml +++ /dev/null @@ -1,3 +0,0 @@ -test_joint_group_position_controller: - ros__parameters: - joints: ["joint1"] diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp deleted file mode 100644 index d0125c7e14..0000000000 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ /dev/null @@ -1,180 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "hardware_interface/loaned_command_interface.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/utilities.hpp" -#include "test_joint_group_position_controller.hpp" - -using CallbackReturn = controller_interface::CallbackReturn; -using hardware_interface::LoanedCommandInterface; - -void JointGroupPositionControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } - -void JointGroupPositionControllerTest::TearDownTestCase() { rclcpp::shutdown(); } - -void JointGroupPositionControllerTest::SetUp() -{ - controller_ = std::make_unique(); -} - -void JointGroupPositionControllerTest::TearDown() { controller_.reset(nullptr); } - -void JointGroupPositionControllerTest::SetUpController( - const std::vector & parameters) -{ - auto node_options = controller_->define_custom_node_options(); - node_options.parameter_overrides(parameters); - - controller_interface::ControllerInterfaceParams params; - params.controller_name = "test_joint_group_position_controller"; - params.robot_description = ""; - params.update_rate = 0; - params.node_namespace = ""; - params.node_options = node_options; - const auto result = controller_->init(params); - ASSERT_EQ(result, controller_interface::return_type::OK); - - std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_, nullptr); - command_ifs.emplace_back(joint_2_pos_cmd_, nullptr); - command_ifs.emplace_back(joint_3_pos_cmd_, nullptr); - controller_->assign_interfaces(std::move(command_ifs), {}); - executor.add_node(controller_->get_node()->get_node_base_interface()); -} - -TEST_F(JointGroupPositionControllerTest, ConfigureAndActivateParamsSuccess) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); -} - -TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful though no command has been send yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); - - // send command - forward_command_controller::CmdType command; - command.data = {10.0, 20.0, 30.0}; - controller_->rt_command_.set(command); - - // update successful, command received - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 30.0); -} - -TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // send command with wrong number of joints - forward_command_controller::CmdType command; - command.data = {10.0, 20.0}; - controller_->rt_command_.set(command); - - // update failed, command size does not match number of joints - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::ERROR); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful, no command received yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // default values - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 3.1); - - auto node_state = controller_->configure(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - - node_state = controller_->get_node()->activate(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - - // send a new command - rclcpp::Node test_node("test_node"); - auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); - std_msgs::msg::Float64MultiArray command_msg; - command_msg.data = {10.0, 20.0, 30.0}; - command_pub->publish(command_msg); - - // wait for command message to be passed - const auto timeout = std::chrono::milliseconds{10}; - const auto until = controller_->get_node()->get_clock()->now() + timeout; - while (controller_->get_node()->get_clock()->now() < until) - { - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::microseconds(10)); - } - - // update successful - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_->get_optional().value(), 30.0); -} diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp deleted file mode 100644 index 404f60b0ed..0000000000 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ -#define TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ - -#include - -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "position_controllers/joint_group_position_controller.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" - -using hardware_interface::CommandInterface; -using hardware_interface::HW_IF_POSITION; - -// subclassing and friending so we can access member variables -class FriendJointGroupPositionController : public position_controllers::JointGroupPositionController -{ - FRIEND_TEST(JointGroupPositionControllerTest, CommandSuccessTest); - FRIEND_TEST(JointGroupPositionControllerTest, WrongCommandCheckTest); - FRIEND_TEST(JointGroupPositionControllerTest, CommandCallbackTest); -}; - -class JointGroupPositionControllerTest : public ::testing::Test -{ -public: - static void SetUpTestCase(); - static void TearDownTestCase(); - - void SetUp(); - void TearDown(); - - void SetUpController(const std::vector & parameters = {}); - void SetUpHandles(); - -protected: - std::unique_ptr controller_; - - // dummy joint state values used for tests - const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; - std::vector joint_commands_ = {1.1, 2.1, 3.1}; - - CommandInterface::SharedPtr joint_1_pos_cmd_ = - std::make_shared(joint_names_[0], HW_IF_POSITION, &joint_commands_[0]); - CommandInterface::SharedPtr joint_2_pos_cmd_ = - std::make_shared(joint_names_[1], HW_IF_POSITION, &joint_commands_[1]); - CommandInterface::SharedPtr joint_3_pos_cmd_ = - std::make_shared(joint_names_[2], HW_IF_POSITION, &joint_commands_[2]); - rclcpp::executors::SingleThreadedExecutor executor; -}; - -#endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_ diff --git a/position_controllers/test/test_load_joint_group_position_controller.cpp b/position_controllers/test/test_load_joint_group_position_controller.cpp deleted file mode 100644 index cd3fa9b945..0000000000 --- a/position_controllers/test/test_load_joint_group_position_controller.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadJointGroupPositionController, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - - const std::string test_file_path = - std::string(TEST_FILES_DIRECTORY) + "/config/test_joint_group_position_controller.yaml"; - - cm.set_parameter({"test_joint_group_position_controller.params_file", test_file_path}); - cm.set_parameter( - {"test_joint_group_position_controller.type", - "position_controllers/JointGroupPositionController"}); - - ASSERT_NE(cm.load_controller("test_joint_group_position_controller"), nullptr); - - rclcpp::shutdown(); -} diff --git a/ros2_controllers/doc/index.rst b/ros2_controllers/doc/index.rst index f92d17702d..fd0f9acf2d 100644 --- a/ros2_controllers/doc/index.rst +++ b/ros2_controllers/doc/index.rst @@ -26,10 +26,6 @@ For more information of the ros2_control framework see `control.ros.org `__ - `API `__ - `ROS Index `__ - * - effort_controllers - - `Documentation `__ - - `API `__ - - `ROS Index `__ * - force_torque_sensor_broadcaster - `Documentation `__ - `API `__ @@ -58,10 +54,6 @@ For more information of the ros2_control framework see `control.ros.org `__ - `API `__ - `ROS Index `__ - * - position_controllers - - `Documentation `__ - - `API `__ - - `ROS Index `__ * - range_sensor_broadcaster - `Documentation `__ - `API `__ @@ -78,7 +70,3 @@ For more information of the ros2_control framework see `control.ros.org `__ - `API `__ - `ROS Index `__ - * - velocity_controllers - - `Documentation `__ - - `API `__ - - `ROS Index `__ diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index b41c116658..a7fa17a85f 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -22,7 +22,6 @@ bicycle_steering_controller chained_filter_controller diff_drive_controller - effort_controllers force_torque_sensor_broadcaster forward_command_controller gpio_controllers @@ -36,12 +35,10 @@ parallel_gripper_controller pid_controller pose_broadcaster - position_controllers range_sensor_broadcaster steering_controllers_library tricycle_controller tricycle_steering_controller - velocity_controllers ament_cmake diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst deleted file mode 100644 index b43ec3980b..0000000000 --- a/velocity_controllers/CHANGELOG.rst +++ /dev/null @@ -1,354 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package velocity_controllers -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -6.0.0 (2025-11-10) ------------------- -* Controller interface api update to ros2_controller packages (`#1973 `_) -* Contributors: Anand Vardhan - -5.8.0 (2025-10-02) ------------------- -* Remove deprecated methods from ros2_control (`#1936 `_) -* Contributors: Christoph Fröhlich - -5.7.0 (2025-09-12) ------------------- - -5.6.1 (2025-08-30) ------------------- - -5.6.0 (2025-08-29) ------------------- -* Remove usage of `get_ordered_interfaces` but update parameter validation instead (`#1816 `_) -* Contributors: Christoph Fröhlich - -5.5.0 (2025-07-31) ------------------- - -5.4.0 (2025-07-23) ------------------- -* Use new handles API in ros2_controllers to fix deprecation warnings (`#1566 `_) -* Contributors: Sanjeev Kumar - -5.3.0 (2025-07-14) ------------------- -* Update realtime containers (`#1721 `_) -* Contributors: Christoph Fröhlich - -5.2.0 (2025-06-23) ------------------- - -5.1.0 (2025-06-11) ------------------- - -5.0.2 (2025-05-26) ------------------- - -5.0.1 (2025-05-24) ------------------- -* Use target_link_libraries instead of ament_target_dependencies (`#1697 `_) -* Contributors: Sai Kishor Kothakota - -5.0.0 (2025-05-17) ------------------- - -4.24.0 (2025-04-27) -------------------- -* Call `configure()` of base class instead of node (`#1659 `_) -* Contributors: Christoph Fröhlich - -4.23.0 (2025-04-10) -------------------- -* Use global cmake macros and fix gcc-10 build (`#1527 `_) -* Contributors: Christoph Fröhlich - -4.22.0 (2025-03-17) -------------------- -* Fix the `ActivateWithWrongJointsNamesFails` test (`#1570 `_) -* Contributors: Christoph Fröhlich - -4.21.0 (2025-03-01) -------------------- - -4.20.0 (2025-01-29) -------------------- - -4.19.0 (2025-01-13) -------------------- -* Remove visibility macros (`#1451 `_) -* Contributors: Bence Magyar - -4.18.0 (2024-12-19) -------------------- - -4.17.0 (2024-12-07) -------------------- -* Add few warning flags to error in all ros2_controllers packages and fix tests (`#1370 `_) -* Update maintainers and add url tags (`#1363 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.16.0 (2024-11-08) -------------------- - -4.15.0 (2024-10-07) -------------------- - -4.14.0 (2024-09-11) -------------------- - -4.13.0 (2024-08-22) -------------------- - -4.12.1 (2024-08-14) -------------------- - -4.12.0 (2024-07-23) -------------------- -* Unused header cleanup (`#1199 `_) -* Fix WaitSet issue in tests (`#1206 `_) -* Fix parallel gripper controller CI (`#1202 `_) -* Contributors: Henry Moore, Sai Kishor Kothakota - -4.11.0 (2024-07-09) -------------------- -* added changes corresponding to the logger and clock propagation in ResourceManager (`#1184 `_) -* Contributors: Sai Kishor Kothakota - -4.10.0 (2024-07-01) -------------------- - -4.9.0 (2024-06-05) ------------------- - -4.8.0 (2024-05-14) ------------------- - -4.7.0 (2024-03-22) ------------------- - -4.6.0 (2024-02-12) ------------------- -* Add test_depend on `hardware_interface_testing` (`#1018 `_) -* Fix tests for using new `get_node_options` API (`#840 `_) -* Contributors: Christoph Fröhlich, Sai Kishor Kothakota - -4.5.0 (2024-01-31) ------------------- - -4.4.0 (2024-01-11) ------------------- - -4.3.0 (2024-01-08) ------------------- -* Add few warning flags to error (`#961 `_) -* Contributors: Sai Kishor Kothakota - -4.2.0 (2023-12-12) ------------------- - -4.1.0 (2023-12-01) ------------------- - -4.0.0 (2023-11-21) ------------------- -* fix tests for API break of passing controller manager update rate in init method (`#854 `_) -* Adjust tests after passing URDF to controllers (`#817 `_) -* Contributors: Bence Magyar, Sai Kishor Kothakota - -3.17.0 (2023-10-31) -------------------- - -3.16.0 (2023-09-20) -------------------- -* [Doc] Add specific documentation on the available fw cmd controllers (`#765 `_) -* Contributors: Christoph Fröhlich - -3.15.0 (2023-09-11) -------------------- - -3.14.0 (2023-08-16) -------------------- - -3.13.0 (2023-08-04) -------------------- - -3.12.0 (2023-07-18) -------------------- - -3.11.0 (2023-06-24) -------------------- -* Added -Wconversion flag and fix warnings (`#667 `_) -* Contributors: gwalck - -3.10.1 (2023-06-06) -------------------- - -3.10.0 (2023-06-04) -------------------- - -3.9.0 (2023-05-28) ------------------- -* Use branch name substitution for all links (`#618 `_) -* Fix github links on control.ros.org (`#604 `_) -* Contributors: Christoph Fröhlich - -3.8.0 (2023-05-14) ------------------- - -3.7.0 (2023-05-02) ------------------- - -3.6.0 (2023-04-29) ------------------- -* Renovate load controller tests (`#569 `_) -* Contributors: Bence Magyar - -3.5.0 (2023-04-14) ------------------- - -3.4.0 (2023-04-02) ------------------- - -3.3.0 (2023-03-07) ------------------- - -3.2.0 (2023-02-10) ------------------- -* Fix overriding of install (`#510 `_) -* Contributors: Tyler Weaver, Chris Thrasher - -3.1.0 (2023-01-26) ------------------- - -3.0.0 (2023-01-19) ------------------- -* Add backward_ros to all controllers (`#489 `_) -* Contributors: Bence Magyar - -2.15.0 (2022-12-06) -------------------- - -2.14.0 (2022-11-18) -------------------- - -2.13.0 (2022-10-05) -------------------- - -2.12.0 (2022-09-01) -------------------- - -2.11.0 (2022-08-04) -------------------- - -2.10.0 (2022-08-01) -------------------- - -2.9.0 (2022-07-14) ------------------- - -2.8.0 (2022-07-09) ------------------- - -2.7.0 (2022-07-03) ------------------- - -2.6.0 (2022-06-18) ------------------- -* CMakeLists cleanup (`#362 `_) -* Fix exception about parameter already been declared & Change default c++ version to 17 (`#360 `_) - * Default C++ version to 17 - * Replace explicit use of declare_paremeter with auto_declare -* Contributors: AndyZe, Jafar - -2.5.0 (2022-05-13) ------------------- - -2.4.0 (2022-04-29) ------------------- -* Multi-interface Forward Controller (`#154 `_) -* Contributors: Denis Štogl - -2.3.0 (2022-04-21) ------------------- -* Use CallbackReturn from controller_interface namespace (`#333 `_) -* Contributors: Bence Magyar, Denis Štogl - -2.2.0 (2022-03-25) ------------------- -* Use lifecycle node as base for controllers (`#244 `_) -* Contributors: Denis Štogl, Vatan Aksoy Tezer, Bence Magyar - -2.1.0 (2022-02-23) ------------------- - -2.0.1 (2022-02-01) ------------------- - -2.0.0 (2022-01-28) ------------------- - -1.3.0 (2022-01-11) ------------------- - -1.2.0 (2021-12-29) ------------------- - -1.1.0 (2021-10-25) ------------------- - -1.0.0 (2021-09-29) ------------------- -* Add time and period to update function (`#241 `_) -* Unify style of controllers. (`#236 `_) -* ros2_controllers code changes to support ros2_controls issue `#489 `_ (`#233 `_) -* Removing Boost from controllers. (`#235 `_) -* Contributors: Bence Magyar, Denis Štogl, bailaC - -0.5.0 (2021-08-30) ------------------- -* Bring precommit config up to speed with ros2_control (`#227 `_) -* Delete failing parameter undeclare in JointGroupPositionController (`#222 `_) -* Add initial pre-commit setup. (`#220 `_) -* Reduce docs warnings and correct adding guidelines (`#219 `_) -* Contributors: Bence Magyar, Denis Štogl, Joseph Schornak - -0.4.1 (2021-07-08) ------------------- - -0.4.0 (2021-06-28) ------------------- -* Force torque sensor broadcaster (`#152 `_) - * Add rclcpp::shutdown(); to all standalone test functions -* Contributors: Denis Štogl - -0.3.1 (2021-05-23) ------------------- - -0.3.0 (2021-05-21) ------------------- - -0.2.1 (2021-05-03) ------------------- -* Migrate from deprecated controller_interface::return_type::SUCCESS -> OK (`#167 `_) -* Add basic user docs pages for each package (`#156 `_) -* Contributors: Bence Magyar - -0.2.0 (2021-02-06) ------------------- -* Use ros2 contol test assets (`#138 `_) - * Add description to test trajecotry_controller - * Use ros2_control_test_assets package - * Delete obsolete components plugin export -* Contributors: Denis Štogl - -0.1.2 (2021-01-07) ------------------- - -0.1.1 (2021-01-06) ------------------- -* Restore forward command derivatives (`#133 `_) -* Contributors: Bence Magyar - -0.1.0 (2020-12-23) ------------------- diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt deleted file mode 100644 index 8507e81d89..0000000000 --- a/velocity_controllers/CMakeLists.txt +++ /dev/null @@ -1,74 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(velocity_controllers) - -find_package(ros2_control_cmake REQUIRED) -set_compiler_options() -export_windows_symbols() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - forward_command_controller - pluginlib - rclcpp -) - -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() - -add_library(velocity_controllers SHARED - src/joint_group_velocity_controller.cpp -) -target_compile_features(velocity_controllers PUBLIC cxx_std_17) -target_include_directories(velocity_controllers PUBLIC - $ - $ -) -target_link_libraries(velocity_controllers PUBLIC - forward_command_controller::forward_command_controller - pluginlib::pluginlib - rclcpp::rclcpp) - -pluginlib_export_plugin_description_file(controller_interface velocity_controllers_plugins.xml) - -if(BUILD_TESTING) - find_package(ament_cmake_gmock REQUIRED) - find_package(controller_manager REQUIRED) - find_package(ros2_control_test_assets REQUIRED) - - add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") - - ament_add_gmock(test_load_joint_group_velocity_controller - test/test_load_joint_group_velocity_controller.cpp - ) - target_link_libraries(test_load_joint_group_velocity_controller - velocity_controllers - controller_manager::controller_manager - ros2_control_test_assets::ros2_control_test_assets - ) - - ament_add_gmock(test_joint_group_velocity_controller - test/test_joint_group_velocity_controller.cpp - ) - target_link_libraries(test_joint_group_velocity_controller - velocity_controllers - ) -endif() - -install( - DIRECTORY include/ - DESTINATION include/velocity_controllers -) -install( - TARGETS - velocity_controllers - EXPORT export_velocity_controllers - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -ament_export_targets(export_velocity_controllers HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() diff --git a/velocity_controllers/doc/userdoc.rst b/velocity_controllers/doc/userdoc.rst deleted file mode 100644 index ce98a7fc45..0000000000 --- a/velocity_controllers/doc/userdoc.rst +++ /dev/null @@ -1,51 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/velocity_controllers/doc/userdoc.rst - -.. _velocity_controllers_userdoc: - -velocity_controllers -==================== - -This is a collection of controllers that work using the "velocity" joint command interface but may accept different joint-level commands at the controller level, e.g. controlling the velocity on a certain joint to achieve a set position. - -The package contains the following controllers: - -velocity_controllers/JointGroupVelocityController -------------------------------------------------- - -.. warning:: - - ``velocity_controllers/JointGroupVelocityController`` is deprecated. Use :ref:`forward_command_controller ` instead by adding the ``interface_name`` parameter and set it to ``velocity``. - -This is specialization of the :ref:`forward_command_controller ` that works using the "velocity" joint interface. - - -ROS 2 interface of the controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Topics -,,,,,,,,,,,,,,,,,, - -~/commands (input topic) [std_msgs::msg::Float64MultiArray] - Joints' velocity commands - - -Parameters -,,,,,,,,,,,,,,,,,, -This controller overrides the interface parameter from :ref:`forward_command_controller `, and the -``joints`` parameter is the only one that is required. - -An example parameter file is given here - -.. code-block:: yaml - - controller_manager: - ros__parameters: - update_rate: 100 # Hz - - velocity_controller: - type: velocity_controllers/JointGroupVelocityController - - velocity_controller: - ros__parameters: - joints: - - slider_to_cart diff --git a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp b/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp deleted file mode 100644 index 4cb0f338ea..0000000000 --- a/velocity_controllers/include/velocity_controllers/joint_group_velocity_controller.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ -#define VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ - -#include "forward_command_controller/forward_command_controller.hpp" - -namespace velocity_controllers -{ -/** - * \brief Forward command controller for a set of velocity controlled joints (linear or angular). - * - * This class forwards the commanded velocities down to a set of joints. - * - * \param joints Names of the joints to control. - * - * Subscribes to: - * - \b command (std_msgs::msg::Float64MultiArray) : The velocity commands to apply. - */ -class JointGroupVelocityController : public forward_command_controller::ForwardCommandController -{ -public: - JointGroupVelocityController(); - - controller_interface::CallbackReturn on_init() override; - - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; -}; - -} // namespace velocity_controllers - -#endif // VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml deleted file mode 100644 index 4b6d7a5902..0000000000 --- a/velocity_controllers/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - velocity_controllers - 6.0.0 - Generic controller for forwarding commands. - - Bence Magyar - Denis Štogl - Christoph Froehlich - Sai Kishor Kothakota - - Apache License 2.0 - - https://control.ros.org - https://github.com/ros-controls/ros2_controllers/issues - https://github.com/ros-controls/ros2_controllers/ - - Jordan Palacios - - ament_cmake - - ros2_control_cmake - - backward_ros - forward_command_controller - pluginlib - rclcpp - - ament_cmake_gmock - controller_manager - hardware_interface_testing - hardware_interface - ros2_control_test_assets - - - ament_cmake - - This package will be removed in ROS 2 Lyrical Luth. Instead, use - forward_command_controller, see migration guides for details: - https://control.ros.org/rolling/doc/ros2_controllers/doc/migration.html - - - diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp deleted file mode 100644 index fc67ead82b..0000000000 --- a/velocity_controllers/src/joint_group_velocity_controller.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 "controller_interface/controller_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/parameter.hpp" -#include "velocity_controllers/joint_group_velocity_controller.hpp" - -namespace velocity_controllers -{ -JointGroupVelocityController::JointGroupVelocityController() -: forward_command_controller::ForwardCommandController() -{ - interface_name_ = hardware_interface::HW_IF_VELOCITY; -} - -controller_interface::CallbackReturn JointGroupVelocityController::on_init() -{ - RCLCPP_WARN( - get_node()->get_logger(), - "'velocity_controllers/JointGroupVelocityController' is deprecated. " - "Use 'forward_command_controller/ForwardCommandController' instead by adding the " - "'interface_name' parameter and set it to 'velocity'."); - try - { - // Explicitly set the interface parameter declared by the forward_command_controller - // to match the value set in the JointGroupVelocityController constructor. - auto_declare("interface_name", interface_name_); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return CallbackReturn::ERROR; - } - - return forward_command_controller::ForwardCommandController::on_init(); -} - -controller_interface::CallbackReturn JointGroupVelocityController::on_deactivate( - const rclcpp_lifecycle::State & previous_state) -{ - auto ret = ForwardCommandController::on_deactivate(previous_state); - - // stop all joints - for (auto & command_interface : command_interfaces_) - { - if (!command_interface.set_value(0.0)) - { - RCLCPP_WARN(get_node()->get_logger(), "Unable to set the command interface to value 0.0"); - return controller_interface::CallbackReturn::SUCCESS; - } - } - - return ret; -} - -} // namespace velocity_controllers - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - velocity_controllers::JointGroupVelocityController, controller_interface::ControllerInterface) diff --git a/velocity_controllers/test/config/test_joint_group_velocity_controller.yaml b/velocity_controllers/test/config/test_joint_group_velocity_controller.yaml deleted file mode 100644 index 6de94c007c..0000000000 --- a/velocity_controllers/test/config/test_joint_group_velocity_controller.yaml +++ /dev/null @@ -1,3 +0,0 @@ -test_joint_group_velocity_controller: - ros__parameters: - joints: ["joint1"] diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp deleted file mode 100644 index 718de55dee..0000000000 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ /dev/null @@ -1,204 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "hardware_interface/loaned_command_interface.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/utilities.hpp" -#include "test_joint_group_velocity_controller.hpp" - -using CallbackReturn = controller_interface::CallbackReturn; -using hardware_interface::LoanedCommandInterface; - -void JointGroupVelocityControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } - -void JointGroupVelocityControllerTest::TearDownTestCase() { rclcpp::shutdown(); } - -void JointGroupVelocityControllerTest::SetUp() -{ - controller_ = std::make_unique(); -} - -void JointGroupVelocityControllerTest::TearDown() { controller_.reset(nullptr); } - -void JointGroupVelocityControllerTest::SetUpController( - const std::vector & parameters) -{ - auto node_options = controller_->define_custom_node_options(); - node_options.parameter_overrides(parameters); - - controller_interface::ControllerInterfaceParams params; - params.controller_name = "test_joint_group_velocity_controller"; - params.robot_description = ""; - params.update_rate = 0; - params.node_namespace = ""; - params.node_options = node_options; - const auto result = controller_->init(params); - ASSERT_EQ(result, controller_interface::return_type::OK); - - std::vector command_ifs; - command_ifs.emplace_back(joint_1_cmd_, nullptr); - command_ifs.emplace_back(joint_2_cmd_, nullptr); - command_ifs.emplace_back(joint_3_cmd_, nullptr); - controller_->assign_interfaces(std::move(command_ifs), {}); - executor.add_node(controller_->get_node()->get_node_base_interface()); -} - -TEST_F(JointGroupVelocityControllerTest, ConfigureAndActivateParamsSuccess) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); -} - -TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful though no command has been send yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - // send command - forward_command_controller::CmdType command; - command.data = {10.0, 20.0, 30.0}; - controller_->rt_command_.set(command); - - // update successful, command received - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); -} - -TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // send command with wrong number of joints - forward_command_controller::CmdType command; - command.data = {10.0, 20.0}; - controller_->rt_command_.set(command); - - // update failed, command size does not match number of joints - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::ERROR); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // update successful, no command received yet - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); -} - -TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // default values - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - auto node_state = controller_->configure(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - - node_state = controller_->get_node()->activate(); - ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - - // send a new command - rclcpp::Node test_node("test_node"); - auto command_pub = test_node.create_publisher( - std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); - std_msgs::msg::Float64MultiArray command_msg; - command_msg.data = {10.0, 20.0, 30.0}; - command_pub->publish(command_msg); - - // wait for command message to be passed - const auto timeout = std::chrono::milliseconds{10}; - const auto until = controller_->get_node()->get_clock()->now() + timeout; - while (controller_->get_node()->get_clock()->now() < until) - { - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::microseconds(10)); - } - - // update successful - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - // check command in handle was set - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 10.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 20.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 30.0); -} - -TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) -{ - SetUpController({rclcpp::Parameter("joints", joint_names_)}); - - // configure successful - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 1.1); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 2.1); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 3.1); - - // stop the controller - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - - // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_->get_optional().value(), 0.0); - ASSERT_EQ(joint_2_cmd_->get_optional().value(), 0.0); - ASSERT_EQ(joint_3_cmd_->get_optional().value(), 0.0); -} diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp deleted file mode 100644 index fff024bd2a..0000000000 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ -#define TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ - -#include - -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "velocity_controllers/joint_group_velocity_controller.hpp" - -using hardware_interface::CommandInterface; -using hardware_interface::HW_IF_VELOCITY; - -// subclassing and friending so we can access member variables -class FriendJointGroupVelocityController : public velocity_controllers::JointGroupVelocityController -{ - FRIEND_TEST(JointGroupVelocityControllerTest, CommandSuccessTest); - FRIEND_TEST(JointGroupVelocityControllerTest, WrongCommandCheckTest); - FRIEND_TEST(JointGroupVelocityControllerTest, CommandCallbackTest); - FRIEND_TEST(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest); -}; - -class JointGroupVelocityControllerTest : public ::testing::Test -{ -public: - static void SetUpTestCase(); - static void TearDownTestCase(); - - void SetUp(); - void TearDown(); - - void SetUpController(const std::vector & parameters = {}); - -protected: - std::unique_ptr controller_; - - // dummy joint state values used for tests - const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; - std::vector joint_commands_ = {1.1, 2.1, 3.1}; - - CommandInterface::SharedPtr joint_1_cmd_ = - std::make_shared(joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]); - CommandInterface::SharedPtr joint_2_cmd_ = - std::make_shared(joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]); - CommandInterface::SharedPtr joint_3_cmd_ = - std::make_shared(joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]); - rclcpp::executors::SingleThreadedExecutor executor; -}; - -#endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_ diff --git a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp deleted file mode 100644 index 377c5dd4da..0000000000 --- a/velocity_controllers/test/test_load_joint_group_velocity_controller.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2020 PAL Robotics SL. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "controller_manager/controller_manager.hpp" -#include "hardware_interface/resource_manager.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/utilities.hpp" -#include "ros2_control_test_assets/descriptions.hpp" - -TEST(TestLoadJointGroupVelocityController, load_controller) -{ - rclcpp::init(0, nullptr); - - std::shared_ptr executor = - std::make_shared(); - - controller_manager::ControllerManager cm( - executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - - const std::string test_file_path = - std::string(TEST_FILES_DIRECTORY) + "/config/test_joint_group_velocity_controller.yaml"; - - cm.set_parameter({"test_joint_group_velocity_controller.params_file", test_file_path}); - cm.set_parameter( - {"test_joint_group_velocity_controller.type", - "velocity_controllers/JointGroupVelocityController"}); - - ASSERT_NE(cm.load_controller("test_joint_group_velocity_controller"), nullptr); - - rclcpp::shutdown(); -} diff --git a/velocity_controllers/velocity_controllers_plugins.xml b/velocity_controllers/velocity_controllers_plugins.xml deleted file mode 100644 index 52d410bac8..0000000000 --- a/velocity_controllers/velocity_controllers_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The joint velocity controller commands a group of joints through the velocity interface - - -