From 2056d6c2152db3d03cb8af8daf739ca194302f11 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Nov 2023 19:05:02 +0100 Subject: [PATCH 01/21] Pass controller manager update rate on the init of the controller interface (#1141) --- .../controller_interface_base.hpp | 2 +- .../src/controller_interface_base.cpp | 6 +++--- .../test/test_chainable_controller_interface.cpp | 12 ++++++------ .../test/test_controller_interface.cpp | 14 ++++++++------ .../test/test_controller_with_options.cpp | 4 ++-- .../test/test_controller_with_options.hpp | 4 ++-- controller_manager/src/controller_manager.cpp | 8 ++++---- .../test_controller_failed_init.cpp | 3 ++- .../test_controller_failed_init.hpp | 2 +- 9 files changed, 29 insertions(+), 26 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 96141a034b..bbc45c8583 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -114,7 +114,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type init( - const std::string & controller_name, const std::string & urdf, + const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true)); diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index fcbc28590a..4594e7ac07 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -25,8 +25,8 @@ namespace controller_interface { return_type ControllerInterfaceBase::init( - const std::string & controller_name, const std::string & urdf, const std::string & namespace_, - const rclcpp::NodeOptions & node_options) + const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, + const std::string & namespace_, const rclcpp::NodeOptions & node_options) { node_ = std::make_shared( controller_name, namespace_, node_options, false); // disable LifecycleNode service interfaces @@ -34,7 +34,7 @@ return_type ControllerInterfaceBase::init( try { - auto_declare("update_rate", 0); + auto_declare("update_rate", cm_update_rate); auto_declare("is_async", false); } catch (const std::exception & e) diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 9c46287ba7..d81d2bfdad 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -21,7 +21,7 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); EXPECT_TRUE(controller.is_chainable()); @@ -33,7 +33,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); @@ -50,7 +50,7 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); // expect empty return because storage is not resized @@ -64,7 +64,7 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix"); @@ -79,7 +79,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); @@ -126,7 +126,7 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); EXPECT_FALSE(controller.is_in_chained_mode()); diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 9be0dba9f0..24e780fab3 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -38,15 +38,15 @@ TEST(TestableControllerInterface, init) ASSERT_THROW(controller.get_node(), std::runtime_error); // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 10.0), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); // update_rate is set to default 0 ASSERT_EQ(controller.get_update_rate(), 0u); - // Even after configure is 0 + // Even after configure is 10 controller.configure(); - ASSERT_EQ(controller.get_update_rate(), 0u); + ASSERT_EQ(controller.get_update_rate(), 10u); rclcpp::shutdown(); } @@ -60,7 +60,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) TestableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::OK); + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 1.0), controller_interface::return_type::OK); // initialize executor to be able to get parameter update auto executor = @@ -122,7 +122,8 @@ TEST(TestableControllerInterfaceInitError, init_with_error) TestableControllerInterfaceInitError controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 100.0), controller_interface::return_type::ERROR); rclcpp::shutdown(); } @@ -136,7 +137,8 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure) TestableControllerInterfaceInitFailure controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, ""), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::ERROR); rclcpp::shutdown(); } diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 55c9db65c9..51169e945c 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -42,7 +42,7 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ(controller.init("controller_name", ""), controller_interface::return_type::OK); + EXPECT_EQ(controller.init("controller_name", "", 50.0), controller_interface::return_type::OK); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); @@ -63,7 +63,7 @@ TEST(ControllerWithOption, init_without_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ(controller.init("controller_name", ""), controller_interface::return_type::ERROR); + EXPECT_EQ(controller.init("controller_name", "", 50.0), controller_interface::return_type::ERROR); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index 892ec56f9d..1221536784 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -40,14 +40,14 @@ class ControllerWithOptions : public controller_interface::ControllerInterface } controller_interface::return_type init( - const std::string & controller_name, const std::string & urdf, + const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true)) override { - ControllerInterface::init(controller_name, urdf, namespace_, node_options); + ControllerInterface::init(controller_name, urdf, cm_update_rate, namespace_, node_options); switch (on_init()) { diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e61e3c769b..640dac8f5d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -741,7 +741,7 @@ controller_interface::return_type ControllerManager::configure_controller( "update rate.", controller_name.c_str(), controller_update_rate, cm_update_rate); } - else if (controller_update_rate != 0 && cm_update_rate % controller_update_rate != 0) + else if (cm_update_rate % controller_update_rate != 0) { RCLCPP_WARN( get_logger(), @@ -1296,7 +1296,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co } if ( - controller.c->init(controller.info.name, robot_description_, get_namespace()) == + controller.c->init( + controller.info.name, robot_description_, get_update_rate(), get_namespace()) == controller_interface::return_type::ERROR) { to.clear(); @@ -2045,8 +2046,7 @@ controller_interface::return_type ControllerManager::update( if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c)) { const auto controller_update_rate = loaded_controller.c->get_update_rate(); - const bool run_controller_at_cm_rate = - (controller_update_rate == 0) || (controller_update_rate >= update_rate_); + const bool run_controller_at_cm_rate = (controller_update_rate >= update_rate_); const auto controller_period = run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp index ba762573c7..262ed55beb 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp @@ -33,7 +33,8 @@ TestControllerFailedInit::on_init() controller_interface::return_type TestControllerFailedInit::init( const std::string & /* controller_name */, const std::string & /* urdf */, - const std::string & /*namespace_*/, const rclcpp::NodeOptions & /*node_options*/) + unsigned int cm_update_rate, const std::string & /*namespace_*/, + const rclcpp::NodeOptions & /*node_options*/) { return controller_interface::return_type::ERROR; } diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp index 69530e40b0..28cb48ab6c 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp @@ -40,7 +40,7 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac CONTROLLER_INTERFACE_PUBLIC controller_interface::return_type init( - const std::string & controller_name, const std::string & urdf, + const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() From 58deb40951602200af318d5066fd95013550543a Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Nov 2023 11:50:47 +0000 Subject: [PATCH 02/21] Update changelogs --- controller_interface/CHANGELOG.rst | 6 ++++++ controller_manager/CHANGELOG.rst | 8 ++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 5 +++++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 10 files changed, 40 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 5aa2ad0647..a3f7d63bdc 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Pass controller manager update rate on the init of the controller interface (`#1141 `_) +* Pass URDF to controllers on init (`#1088 `_) +* Contributors: Bence Magyar, Sai Kishor Kothakota + 3.21.0 (2023-11-06) ------------------- diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 0d021492d9..46fb098203 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Pass controller manager update rate on the init of the controller interface (`#1141 `_) +* Fix the controller sorting bug when the interfaces are not configured (fixes `#1164 `_) (`#1165 `_) +* Pass URDF to controllers on init (`#1088 `_) +* Remove deprecation warning (`#1101 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Sai Kishor Kothakota + 3.21.0 (2023-11-06) ------------------- * Sort controllers while configuring instead of while activating (`#1107 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index e6d5095d7e..4e8c6b90e7 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.21.0 (2023-11-06) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 4efd1a3686..10a972b757 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [MockHardware] Remove all deprecated options and deprecated plugins from the library. (`#1150 `_) +* Contributors: Dr. Denis + 3.21.0 (2023-11-06) ------------------- * [MockHardware] Fix the issues where hardware with multiple interfaces can not be started because of a logical bug added when adding dynamics calculation functionality. (`#1151 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index da8a09aae2..e6b46a128a 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.21.0 (2023-11-06) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 2ed4b9ec05..e66afe9a3b 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.21.0 (2023-11-06) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 66d1983c54..4b68e8f902 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.21.0 (2023-11-06) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 2481e4b8c5..a1a96385a8 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.21.0 (2023-11-06) ------------------- diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 23b78e1527..c53d8a3fd6 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.21.0 (2023-11-06) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 067c4353c8..a1c3290c9d 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.21.0 (2023-11-06) ------------------- From d2ee8350dbccfb71014ba018f3e5239e9946dea4 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Nov 2023 11:51:09 +0000 Subject: [PATCH 03/21] 4.0.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 22 files changed, 32 insertions(+), 32 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index a3f7d63bdc..d91d1b86ba 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * Pass controller manager update rate on the init of the controller interface (`#1141 `_) * Pass URDF to controllers on init (`#1088 `_) * Contributors: Bence Magyar, Sai Kishor Kothakota diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 6a2a2513ee..ff45865798 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.21.0 + 4.0.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 46fb098203..2f3d0e1e5a 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * Pass controller manager update rate on the init of the controller interface (`#1141 `_) * Fix the controller sorting bug when the interfaces are not configured (fixes `#1164 `_) (`#1165 `_) * Pass URDF to controllers on init (`#1088 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 5cef7c9577..383f06f71b 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.21.0 + 4.0.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 4e8c6b90e7..41dda9063e 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.21.0 (2023-11-06) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 814f814da1..d229f0c270 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.21.0 + 4.0.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 10a972b757..041dcf2396 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ * [MockHardware] Remove all deprecated options and deprecated plugins from the library. (`#1150 `_) * Contributors: Dr. Denis diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 7fd3c78b61..383da7d7ea 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.21.0 + 4.0.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index e6b46a128a..e826013120 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.21.0 (2023-11-06) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 928a6f26fd..88d59bb9f5 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.21.0 + 4.0.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index e66afe9a3b..cb43899177 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.21.0 (2023-11-06) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 2a0b0e957b..2870a344eb 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.21.0 + 4.0.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 4b68e8f902..3efd4ef581 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.21.0 (2023-11-06) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 252233bc2e..d524900e8d 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.21.0 + 4.0.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index a1a96385a8..751c199228 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.21.0 (2023-11-06) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 1a8aca9b51..ce99a6bd13 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.21.0 + 4.0.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 9a897fae0a..2334d0f3c4 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.21.0", + version="4.0.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index c53d8a3fd6..f30df4dc48 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.21.0 (2023-11-06) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 56f7824962..b9a74d2b22 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.21.0 + 4.0.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index d32300651e..6aff809701 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.21.0", + version="4.0.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index a1c3290c9d..3f73567200 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2023-11-21) +------------------ 3.21.0 (2023-11-06) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index b294e56d24..b4560b7d81 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.21.0 + 4.0.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 2569b76692f7ce9527099a81c1077c73143ea5e2 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 21 Nov 2023 18:40:11 +0100 Subject: [PATCH 04/21] [ControllerInterface] Avoid warning about conversion from `int64_t` to `unsigned int` (#1173) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Without this there is a warning. For what I found the best way is to do static cast. ``` ros2_control/controller_interface/src/controller_interface_base.cpp:88:67: warning: conversion from ‘int64_t’{aka ‘long int’} to ‘unsigned int’ may change value [-Wconversion] 88 | update_rate_ = get_node()->get_parameter("update_rate").as_int(); ``` --- controller_interface/src/controller_interface_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 4594e7ac07..84cd0d5e2b 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -85,7 +85,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() // Other solution is to add check into the LifecycleNode if a transition is valid to trigger if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - update_rate_ = get_node()->get_parameter("update_rate").as_int(); + update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); is_async_ = get_node()->get_parameter("is_async").as_bool(); } From 99d2f61b53c3be1181c36557edd8b28b309bf86f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 21 Nov 2023 18:42:50 +0100 Subject: [PATCH 05/21] Compute the actual update period for each controller (#1140) --- controller_manager/src/controller_manager.cpp | 4 +++- .../test/test_controller/test_controller.cpp | 3 ++- .../test/test_controller/test_controller.hpp | 1 + controller_manager/test/test_controller_manager.cpp | 12 ++++++++++++ 4 files changed, 18 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 640dac8f5d..1c3433058b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2063,7 +2063,9 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - auto controller_ret = loaded_controller.c->update(time, controller_period); + const auto controller_actual_period = + (time - *loaded_controller.next_update_cycle_time) + controller_period; + auto controller_ret = loaded_controller.c->update(time, controller_actual_period); if ( *loaded_controller.next_update_cycle_time == diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 06f76bd044..7585ae36e5 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -60,8 +60,9 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con } controller_interface::return_type TestController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + update_period_ = period.seconds(); ++internal_counter; // set value to hardware to produce and test different behaviors there diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index d7b318c597..14ad753803 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -80,6 +80,7 @@ class TestController : public controller_interface::ControllerInterface // enables external setting of values to command interfaces - used for simulation of hardware // errors double set_first_command_interface_value_to; + double update_period_ = 0; }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 8769a24749..4c9603ab42 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -351,6 +351,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; + time_ += rclcpp::Duration::from_seconds(0.01); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -365,11 +366,17 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd const auto pre_internal_counter = test_controller->internal_counter; rclcpp::Rate loop_rate(cm_->get_update_rate()); + const auto cm_update_rate = cm_->get_update_rate(); for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++) { + time_ += rclcpp::Duration::from_seconds(0.01); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + // In case of a non perfect divisor, the update period should respect the rule + // [controller_update_rate, 2*controller_update_rate) + ASSERT_GE(test_controller->update_period_, 1.0 / cm_update_rate); + ASSERT_LT(test_controller->update_period_, 2.0 / cm_update_rate); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -448,6 +455,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; + time_ += rclcpp::Duration::from_seconds(0.01); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -470,6 +478,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + // In case of a non perfect divisor, the update period should respect the rule + // [controller_update_rate, 2*controller_update_rate) + ASSERT_GE(test_controller->update_period_, 1.0 / controller_update_rate); + ASSERT_LT(test_controller->update_period_, 2.0 / controller_update_rate); time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0) From 7d7946424be4b501476191e9ba6d5de956410f47 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 24 Nov 2023 14:11:07 +0100 Subject: [PATCH 06/21] [ControllerManager] Fix all warnings from the latets features. (#1174) --- controller_manager/src/controller_manager.cpp | 15 +++++++++------ .../test_controller_failed_init.cpp | 2 +- .../test/test_controller_manager.cpp | 8 +++++--- .../test/test_controller_manager_srvs.cpp | 4 ++-- 4 files changed, 17 insertions(+), 12 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1c3433058b..5ef8062593 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2501,20 +2501,23 @@ bool ControllerManager::controller_sorting( // If there is no common parent, then they belong to 2 different sets auto following_ctrls_b = get_following_controller_names(ctrl_b.info.name, controllers); if (following_ctrls_b.empty()) return true; - auto find_first_element = [&](const auto & controllers_list) + auto find_first_element = [&](const auto & controllers_list) -> size_t { auto it = std::find_if( controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, controllers_list.back())); if (it != controllers.end()) { - int dist = std::distance(controllers.begin(), it); - return dist; + return std::distance(controllers.begin(), it); } + return 0; }; - const int ctrl_a_chain_first_controller = find_first_element(following_ctrls); - const int ctrl_b_chain_first_controller = find_first_element(following_ctrls_b); - if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller) return true; + const auto ctrl_a_chain_first_controller = find_first_element(following_ctrls); + const auto ctrl_b_chain_first_controller = find_first_element(following_ctrls_b); + if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller) + { + return true; + } } // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp index 262ed55beb..6a6f954e87 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp @@ -33,7 +33,7 @@ TestControllerFailedInit::on_init() controller_interface::return_type TestControllerFailedInit::init( const std::string & /* controller_name */, const std::string & /* urdf */, - unsigned int cm_update_rate, const std::string & /*namespace_*/, + unsigned int /*cm_update_rate*/, const std::string & /*namespace_*/, const rclcpp::NodeOptions & /*node_options*/) { return controller_interface::return_type::ERROR; diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 4c9603ab42..e88b41f222 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -486,16 +486,18 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0) { - const auto no_of_secs_passed = update_counter / cm_update_rate; + const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it // is clearly tracking, so adding 1 here won't affect the final count. // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 // cycles it will have 369 instead of 370. - EXPECT_NEAR( + EXPECT_THAT( test_controller->internal_counter - initial_counter, - (controller_update_rate * no_of_secs_passed), 1); + testing::AnyOf( + testing::Eq(controller_update_rate * no_of_secs_passed), + testing::Eq((controller_update_rate * no_of_secs_passed) - 1))); } } } diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 773fbd94d8..ef1640a1d9 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -807,7 +807,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7); ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_6); - auto get_ctrl_pos = [result](const std::string & controller_name) -> int + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t { auto it = std::find_if( result->controller.begin(), result->controller.end(), @@ -1016,7 +1016,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(10u, result->controller.size()); - auto get_ctrl_pos = [result](const std::string & controller_name) -> int + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t { auto it = std::find_if( result->controller.begin(), result->controller.end(), From 65353ffecccec18186a45cc2d9615b53d5a375bc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 27 Nov 2023 13:28:53 +0100 Subject: [PATCH 07/21] Add few warning compiler options to error (#1181) * add conversion, unused-but-set-variable, and return-type warnings to error * add shadow variables to error and their fixes for code compilation * apply the same flags to controller interface package * apply the same flags and their fixes to hardware_interface package * apply the same compiler options to the rest of the packages --- controller_interface/CMakeLists.txt | 2 +- controller_manager/CMakeLists.txt | 2 +- controller_manager/src/controller_manager.cpp | 46 +++++++++---------- hardware_interface/CMakeLists.txt | 2 +- .../src/mock_components/generic_system.cpp | 8 ++-- .../test/test_resource_manager.cpp | 20 ++++---- joint_limits/CMakeLists.txt | 2 +- transmission_interface/CMakeLists.txt | 2 +- 8 files changed, 41 insertions(+), 43 deletions(-) diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 88a54d5c7a..d02c422451 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(controller_interface LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 77ba823e1f..41a6b18306 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(controller_manager LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5ef8062593..4ffaef24aa 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -442,8 +442,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript "Parameter 'activate_components_on_start' is deprecated. " "Components are activated per default. Don't use this parameters in combination with the new " "'hardware_components_initial_state' parameter structure."); - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); for (const auto & component : activate_components_on_start) { resource_manager_->set_component_state(component, active_state); @@ -455,8 +453,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript // activate all other components for (const auto & [component, state] : components_to_activate) { - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); resource_manager_->set_component_state(component, active_state); } } @@ -940,7 +936,7 @@ controller_interface::return_type ControllerManager::switch_controller( auto controller_it = std::find_if( controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); - controller_interface::return_type ret = controller_interface::return_type::OK; + controller_interface::return_type status = controller_interface::return_type::OK; // if controller is not inactive then do not do any following-controllers checks if (!is_controller_inactive(controller_it->c)) @@ -950,14 +946,14 @@ controller_interface::return_type ControllerManager::switch_controller( "Controller with name '%s' is not inactive so its following" "controllers do not have to be checked, because it cannot be activated.", controller_it->info.name.c_str()); - ret = controller_interface::return_type::ERROR; + status = controller_interface::return_type::ERROR; } else { - ret = check_following_controllers_for_activate(controllers, strictness, controller_it); + status = check_following_controllers_for_activate(controllers, strictness, controller_it); } - if (ret != controller_interface::return_type::OK) + if (status != controller_interface::return_type::OK) { RCLCPP_WARN( get_logger(), @@ -991,7 +987,7 @@ controller_interface::return_type ControllerManager::switch_controller( auto controller_it = std::find_if( controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); - controller_interface::return_type ret = controller_interface::return_type::OK; + controller_interface::return_type status = controller_interface::return_type::OK; // if controller is not active then skip preceding-controllers checks if (!is_controller_active(controller_it->c)) @@ -999,14 +995,14 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_WARN( get_logger(), "Controller with name '%s' can not be deactivated since it is not active.", controller_it->info.name.c_str()); - ret = controller_interface::return_type::ERROR; + status = controller_interface::return_type::ERROR; } else { - ret = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); + status = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); } - if (ret != controller_interface::return_type::OK) + if (status != controller_interface::return_type::OK) { RCLCPP_WARN( get_logger(), @@ -1087,11 +1083,11 @@ controller_interface::return_type ControllerManager::switch_controller( // check for double stop if (!is_active && in_deactivate_list) { - auto ret = handle_conflict( + auto conflict_status = handle_conflict( "Could not deactivate controller '" + controller.info.name + "' since it is not active"); - if (ret != controller_interface::return_type::OK) + if (conflict_status != controller_interface::return_type::OK) { - return ret; + return conflict_status; } in_deactivate_list = false; deactivate_request_.erase(deactivate_list_it); @@ -1100,11 +1096,11 @@ controller_interface::return_type ControllerManager::switch_controller( // check for doubled activation if (is_active && !in_deactivate_list && in_activate_list) { - auto ret = handle_conflict( + auto conflict_status = handle_conflict( "Could not activate controller '" + controller.info.name + "' since it is already active"); - if (ret != controller_interface::return_type::OK) + if (conflict_status != controller_interface::return_type::OK) { - return ret; + return conflict_status; } in_activate_list = false; activate_request_.erase(activate_list_it); @@ -1113,21 +1109,21 @@ controller_interface::return_type ControllerManager::switch_controller( // check for illegal activation of an unconfigured/finalized controller if (!is_inactive && !in_deactivate_list && in_activate_list) { - auto ret = handle_conflict( + auto conflict_status = handle_conflict( "Could not activate controller '" + controller.info.name + "' since it is not in inactive state"); - if (ret != controller_interface::return_type::OK) + if (conflict_status != controller_interface::return_type::OK) { - return ret; + return conflict_status; } in_activate_list = false; activate_request_.erase(activate_list_it); } const auto extract_interfaces_for_controller = - [this](const ControllerSpec controller, std::vector & request_interface_list) + [this](const ControllerSpec ctrl, std::vector & request_interface_list) { - auto command_interface_config = controller.c->command_interface_configuration(); + auto command_interface_config = ctrl.c->command_interface_configuration(); std::vector command_interface_names = {}; if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { @@ -1767,8 +1763,8 @@ void ControllerManager::reload_controller_libraries_service_cb( loaded_controllers = get_controller_names(); { // lock controllers - std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); - for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) + std::lock_guard ctrl_guard(rt_controllers_wrapper_.controllers_lock_); + for (const auto & controller : rt_controllers_wrapper_.get_updated_list(ctrl_guard)) { if (is_controller_active(*controller.c)) { diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 2108bab17c..bcd03a0a16 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(hardware_interface LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 7b62af105b..f4aee6c8a6 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -580,12 +580,12 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } else { - for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + for (size_t k = 0; k < joint_states_[POSITION_INTERFACE_INDEX].size(); ++k) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][k])) { - joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][j] + + joint_states_[POSITION_INTERFACE_INDEX][k] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][k] + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index c80cdbb5bf..8246cc207d 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -247,21 +247,23 @@ TEST_F(ResourceManagerTest, resource_claiming) // Activate components to get all interfaces available activate_components(rm); - const auto key = "joint1/position"; - EXPECT_TRUE(rm.command_interface_is_available(key)); - EXPECT_FALSE(rm.command_interface_is_claimed(key)); - { - auto position_command_interface = rm.claim_command_interface(key); + const auto key = "joint1/position"; EXPECT_TRUE(rm.command_interface_is_available(key)); - EXPECT_TRUE(rm.command_interface_is_claimed(key)); + EXPECT_FALSE(rm.command_interface_is_claimed(key)); + { - EXPECT_ANY_THROW(rm.claim_command_interface(key)); + auto position_command_interface = rm.claim_command_interface(key); EXPECT_TRUE(rm.command_interface_is_available(key)); + EXPECT_TRUE(rm.command_interface_is_claimed(key)); + { + EXPECT_ANY_THROW(rm.claim_command_interface(key)); + EXPECT_TRUE(rm.command_interface_is_available(key)); + } } + EXPECT_TRUE(rm.command_interface_is_available(key)); + EXPECT_FALSE(rm.command_interface_is_claimed(key)); } - EXPECT_TRUE(rm.command_interface_is_available(key)); - EXPECT_FALSE(rm.command_interface_is_claimed(key)); // command interfaces can only be claimed once for (const auto & key : diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 82467514a3..a788b6de4f 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(joint_limits LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index d4674366e9..efd8db1652 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(transmission_interface LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS From 553628beb6386a5e971301b217228af9d94128bc Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 30 Nov 2023 18:40:07 +0000 Subject: [PATCH 08/21] Update changelogs --- controller_interface/CHANGELOG.rst | 6 ++++++ controller_manager/CHANGELOG.rst | 7 +++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 5 +++++ joint_limits/CHANGELOG.rst | 5 +++++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 5 +++++ 10 files changed, 43 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index d91d1b86ba..f057e7f337 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning compiler options to error (`#1181 `_) +* [ControllerInterface] Avoid warning about conversion from `int64_t` to `unsigned int` (`#1173 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + 4.0.0 (2023-11-21) ------------------ * Pass controller manager update rate on the init of the controller interface (`#1141 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 2f3d0e1e5a..0c69e2560e 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning compiler options to error (`#1181 `_) +* [ControllerManager] Fix all warnings from the latets features. (`#1174 `_) +* Compute the actual update period for each controller (`#1140 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + 4.0.0 (2023-11-21) ------------------ * Pass controller manager update rate on the init of the controller interface (`#1141 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 41dda9063e..891b055664 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 041dcf2396..ebe0f34ebf 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning compiler options to error (`#1181 `_) +* Contributors: Sai Kishor Kothakota + 4.0.0 (2023-11-21) ------------------ * [MockHardware] Remove all deprecated options and deprecated plugins from the library. (`#1150 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index e826013120..2c6412ecb8 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning compiler options to error (`#1181 `_) +* Contributors: Sai Kishor Kothakota + 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index cb43899177..9c428b1b94 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 3efd4ef581..caa31cdd15 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 751c199228..0bffe499ec 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index f30df4dc48..76be3b75d5 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.0.0 (2023-11-21) ------------------ diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 3f73567200..e9d6d114eb 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add few warning compiler options to error (`#1181 `_) +* Contributors: Sai Kishor Kothakota + 4.0.0 (2023-11-21) ------------------ From 587dffbb67eec710478de238a9ad4a4cbf7c87f1 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 30 Nov 2023 18:40:33 +0000 Subject: [PATCH 09/21] 4.1.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 22 files changed, 32 insertions(+), 32 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index f057e7f337..92b1d91933 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-11-30) +------------------ * Add few warning compiler options to error (`#1181 `_) * [ControllerInterface] Avoid warning about conversion from `int64_t` to `unsigned int` (`#1173 `_) * Contributors: Dr. Denis, Sai Kishor Kothakota diff --git a/controller_interface/package.xml b/controller_interface/package.xml index ff45865798..c7750bdec0 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.0.0 + 4.1.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 0c69e2560e..c94433f8b5 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-11-30) +------------------ * Add few warning compiler options to error (`#1181 `_) * [ControllerManager] Fix all warnings from the latets features. (`#1174 `_) * Compute the actual update period for each controller (`#1140 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 383f06f71b..63c38d52ab 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.0.0 + 4.1.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 891b055664..899ae0d3fc 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-11-30) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index d229f0c270..e9e5c46123 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.0.0 + 4.1.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index ebe0f34ebf..44f4246ac8 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-11-30) +------------------ * Add few warning compiler options to error (`#1181 `_) * Contributors: Sai Kishor Kothakota diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 383da7d7ea..b97a4242bd 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.0.0 + 4.1.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 2c6412ecb8..0360d15653 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-11-30) +------------------ * Add few warning compiler options to error (`#1181 `_) * Contributors: Sai Kishor Kothakota diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 88d59bb9f5..2990fdfd7e 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.0.0 + 4.1.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 9c428b1b94..ade3289319 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-11-30) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 2870a344eb..71d996ce8f 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.0.0 + 4.1.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index caa31cdd15..5740f66f1b 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-11-30) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index d524900e8d..c55a1d5ca8 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.0.0 + 4.1.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 0bffe499ec..524a5a89ae 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-11-30) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index ce99a6bd13..e14f9d8982 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.0.0 + 4.1.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 2334d0f3c4..3bc1200b09 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.0.0", + version="4.1.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 76be3b75d5..5e0e9ba9b8 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-11-30) +------------------ 4.0.0 (2023-11-21) ------------------ diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index b9a74d2b22..ba51d7bd26 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.0.0 + 4.1.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 6aff809701..d9d01fa304 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="4.0.0", + version="4.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index e9d6d114eb..981e57572e 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.1.0 (2023-11-30) +------------------ * Add few warning compiler options to error (`#1181 `_) * Contributors: Sai Kishor Kothakota diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index b4560b7d81..f0c9e70ed5 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.0.0 + 4.1.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From d68cc22f47e336abd178ba31be559fb03d7b599a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 3 Dec 2023 18:47:44 +0100 Subject: [PATCH 10/21] Fix controller sorting issue while loading large number of controllers (#1180) --- controller_manager/src/controller_manager.cpp | 13 +- .../test/test_controller_manager_srvs.cpp | 496 ++++++++++++++++++ 2 files changed, 507 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 4ffaef24aa..b144cd68f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -777,7 +777,7 @@ controller_interface::return_type ControllerManager::configure_controller( to = from; // Reordering the controllers - std::sort( + std::stable_sort( to.begin(), to.end(), std::bind( &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, @@ -2453,7 +2453,16 @@ bool ControllerManager::controller_sorting( { // The case of the controllers that don't have any command interfaces. For instance, // joint_state_broadcaster - return true; + // If the controller b is also under the same condition, then maintain their initial order + if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) + return false; + else + return true; + } + else if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) + { + // If only the controller b is a broadcaster or non chainable type , then swap the controllers + return false; } else { diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index ef1640a1d9..bc20236306 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -1050,3 +1050,499 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) // third tree ASSERT_GT(ctrl_chain_7_pos, ctrl_chain_8_pos); } + +TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) +{ + /// The simulated controller chaining is: + /// test_controller_name_1 -> chain_ctrl_3 -> chain_ctrl_2 -> chain_ctrl_1 + /// && + /// test_controller_name_2 -> chain_ctrl_6 -> chain_ctrl_5 -> chain_ctrl_4 + /// && + /// test_controller_name_7 -> test_controller_name_8 + /// && + /// There are 100 more other basic controllers and 100 more different broadcasters to check for + /// crashing + /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces + /// exported from the controller B (or) the controller B is utilizing the expected interfaces + /// exported from the controller A + + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of chained controllers + static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1"; + static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2"; + static constexpr char TEST_CHAINED_CONTROLLER_3[] = "test_chainable_controller_name_3"; + static constexpr char TEST_CHAINED_CONTROLLER_4[] = "test_chainable_controller_name_4"; + static constexpr char TEST_CHAINED_CONTROLLER_5[] = "test_chainable_controller_name_5"; + static constexpr char TEST_CHAINED_CONTROLLER_6[] = "test_chainable_controller_name_6"; + static constexpr char TEST_CHAINED_CONTROLLER_7[] = "test_chainable_controller_name_7"; + static constexpr char TEST_CHAINED_CONTROLLER_8[] = "test_chainable_controller_name_8"; + static constexpr char TEST_CHAINED_CONTROLLER_9[] = "test_chainable_controller_name_9"; + static constexpr char TEST_CONTROLLER_1[] = "test_controller_name_1"; + static constexpr char TEST_CONTROLLER_2[] = "test_controller_name_2"; + + // First chain + auto test_chained_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_1->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_2 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}}; + test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_2->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_3 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position"}}; + test_chained_controller_3->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_3->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_3->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_1->set_command_interface_configuration(cmd_cfg); + test_controller_1->set_state_interface_configuration(state_cfg); + + // Second chain + auto test_chained_controller_4 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + test_chained_controller_4->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_4->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_4->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_5 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_4) + "/joint2/velocity"}}; + test_chained_controller_5->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_5->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_5->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_6 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_5) + "/joint2/velocity"}}; + test_chained_controller_6->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_6->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_6->set_reference_interface_names({"joint2/velocity"}); + + auto test_controller_2 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_6) + "/joint2/velocity"}}; + state_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + test_controller_2->set_command_interface_configuration(cmd_cfg); + test_controller_2->set_state_interface_configuration(state_cfg); + + // Third chain + auto test_chained_controller_7 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint3/velocity"}}; + test_chained_controller_7->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_7->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_7->set_reference_interface_names({"joint3/velocity"}); + + auto test_chained_controller_8 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_7) + "/joint3/velocity"}}; + test_chained_controller_8->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_8->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_8->set_reference_interface_names({"joint3/velocity"}); + + auto test_chained_controller_9 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/max_acceleration"}}; + test_chained_controller_9->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_9->set_state_interface_configuration(state_cfg); + + unsigned int num_of_random_broadcasters = 100; + unsigned int num_of_random_controllers = 100; + std::vector chained_ref_interfaces; + for (size_t i = 0; i < num_of_random_controllers; i++) + { + chained_ref_interfaces.push_back("ref_" + std::to_string(i) + "/joint_2/acceleration"); + } + test_chained_controller_9->set_reference_interface_names(chained_ref_interfaces); + std::unordered_map> random_controllers_list; + for (size_t i = 0; i < num_of_random_broadcasters; i++) + { + auto controller_name = "test_broadcaster_" + std::to_string(i); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + } + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_controllers_" + std::to_string(i); + RCLCPP_ERROR(srv_node->get_logger(), "Initializing controller : %s !", controller_name.c_str()); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + random_controllers_list[controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_9) + std::string("/ref_") + std::to_string(i) + + std::string("/joint_2/acceleration")}}); + } + + // add controllers + /// @todo add controllers in random order + /// For now, adding the ordered case to see that current sorting doesn't change order + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_9, TEST_CHAINED_CONTROLLER_9, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + { + ControllerManagerRunner cm_runner(this); + for (auto random_ctrl : random_controllers_list) + { + cm_->add_controller( + random_ctrl.second, random_ctrl.first, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + } + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + + // check chainable controller + ASSERT_EQ( + 11u + num_of_random_broadcasters + num_of_random_controllers, result->controller.size()); + EXPECT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_2); + EXPECT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_6); + EXPECT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1); + EXPECT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_7); + EXPECT_EQ(result->controller[4].name, TEST_CONTROLLER_1); + + EXPECT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_5); + EXPECT_EQ(result->controller[6].name, TEST_CHAINED_CONTROLLER_3); + EXPECT_EQ(result->controller[7].name, TEST_CHAINED_CONTROLLER_4); + EXPECT_EQ(result->controller[8].name, TEST_CONTROLLER_2); + EXPECT_EQ(result->controller[9].name, TEST_CHAINED_CONTROLLER_8); + + // configure controllers + auto ctrls_order = { + TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_9, + TEST_CHAINED_CONTROLLER_1, TEST_CONTROLLER_1, TEST_CHAINED_CONTROLLER_4, + TEST_CONTROLLER_2, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_6, + TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_8}; + { + ControllerManagerRunner cm_runner(this); + for (const auto & controller : ctrls_order) + { + cm_->configure_controller(controller); + } + + for (auto random_ctrl : random_controllers_list) + { + cm_->configure_controller(random_ctrl.first); + } + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 11u + num_of_random_broadcasters + num_of_random_controllers, result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t + { + auto it = std::find_if( + result->controller.begin(), result->controller.end(), + [controller_name](auto itf) + { return (itf.name.find(controller_name) != std::string::npos); }); + return std::distance(result->controller.begin(), it); + }; + auto ctrl_chain_1_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_1); + auto ctrl_chain_2_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_2); + auto ctrl_chain_3_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_3); + auto ctrl_chain_4_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_4); + auto ctrl_chain_5_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_5); + auto ctrl_chain_6_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_6); + auto ctrl_chain_7_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_7); + auto ctrl_chain_8_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_8); + auto ctrl_chain_9_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_9); + + auto ctrl_1_pos = get_ctrl_pos(TEST_CONTROLLER_1); + auto ctrl_2_pos = get_ctrl_pos(TEST_CONTROLLER_2); + + // Extra check to see that they are indexed after their parent controller + // first chain + ASSERT_GT(ctrl_chain_1_pos, ctrl_chain_2_pos); + ASSERT_GT(ctrl_chain_2_pos, ctrl_chain_3_pos); + ASSERT_GT(ctrl_chain_3_pos, ctrl_1_pos); + + // second tree + ASSERT_GT(ctrl_chain_4_pos, ctrl_chain_5_pos); + ASSERT_GT(ctrl_chain_5_pos, ctrl_chain_6_pos); + ASSERT_GT(ctrl_chain_6_pos, ctrl_2_pos); + + // third tree + ASSERT_GT(ctrl_chain_7_pos, ctrl_chain_8_pos); + + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_controllers_" + std::to_string(i); + ASSERT_GT(ctrl_chain_9_pos, get_ctrl_pos(controller_name)); + } + RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); +} + +TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) +{ + /// The simulated controller chain is like every joint has its own controller exposing interfaces + /// and then a controller chain using those interfaces + /// + /// There are 20 more broadcasters + 20 more normal controllers for complexity + /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported + /// from the controller B (or) the controller B is utilizing the expected interfaces exported from + /// the controller A + + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + const unsigned int joints_count = 20; + + static constexpr char JOINT_CONTROLLER_PREFIX[] = "test_chainable_controller_name_joint_"; + static constexpr char FWD_CONTROLLER_PREFIX[] = "forward_controller_joint_"; + static constexpr char JOINT_SENSOR_BROADCASTER_PREFIX[] = "test_broadcaster_joint_"; + std::vector controllers_list; + std::vector fwd_joint_position_interfaces_list; + std::vector fwd_joint_velocity_interfaces_list; + std::vector fwd_joint_position_ref_interfaces_list; + std::vector fwd_joint_velocity_ref_interfaces_list; + std::unordered_map> + random_chainable_controllers_list; + std::unordered_map> random_controllers_list; + for (size_t i = 0; i < joints_count; i++) + { + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/position", "joint" + std::to_string(i) + "/velocity"}}; + // Joint controller + const std::string controller_name = JOINT_CONTROLLER_PREFIX + std::to_string(i); + random_chainable_controllers_list[controller_name] = + std::make_shared(); + random_chainable_controllers_list[controller_name]->set_state_interface_configuration( + chained_state_cfg); + random_chainable_controllers_list[controller_name]->set_command_interface_configuration( + chained_cmd_cfg); + random_chainable_controllers_list[controller_name]->set_reference_interface_names( + chained_state_cfg.names); + controllers_list.push_back(controller_name); + + // Forward Joint interfaces controller + fwd_joint_position_interfaces_list.push_back( + std::string(controller_name) + "/joint" + std::to_string(i) + "/position"); + fwd_joint_velocity_interfaces_list.push_back( + std::string(controller_name) + "/joint" + std::to_string(i) + "/velocity"); + const std::string fwd_controller_name = FWD_CONTROLLER_PREFIX + std::to_string(i); + random_chainable_controllers_list[fwd_controller_name] = + std::make_shared(); + random_chainable_controllers_list[fwd_controller_name]->set_state_interface_configuration( + chained_state_cfg); + random_chainable_controllers_list[fwd_controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {fwd_joint_position_interfaces_list.back(), fwd_joint_velocity_interfaces_list.back()}}); + random_chainable_controllers_list[fwd_controller_name]->set_reference_interface_names( + chained_state_cfg.names); + fwd_joint_position_ref_interfaces_list.push_back( + std::string(fwd_controller_name) + "/" + fwd_joint_position_interfaces_list.back()); + fwd_joint_velocity_ref_interfaces_list.push_back( + std::string(fwd_controller_name) + "/" + fwd_joint_velocity_interfaces_list.back()); + controllers_list.push_back(fwd_controller_name); + + // Add a broadcaster for every joint assuming it as a sensor (just for the tests) + const std::string broadcaster_name = JOINT_SENSOR_BROADCASTER_PREFIX + std::to_string(i); + random_controllers_list[broadcaster_name] = std::make_shared(); + random_controllers_list[broadcaster_name]->set_state_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/torque", "joint" + std::to_string(i) + "/torque"}}); + controllers_list.push_back(broadcaster_name); + } + + // create set of chained controllers + static constexpr char POSITION_REFERENCE_CONTROLLER[] = "position_reference_chainable_controller"; + static constexpr char VELOCITY_REFERENCE_CONTROLLER[] = "velocity_reference_chainable_controller"; + static constexpr char HIGHER_LEVEL_REFERENCE_CONTROLLER[] = "task_level_controller"; + + // Position reference controller + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER] = + std::make_shared(); + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER] + ->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + fwd_joint_position_ref_interfaces_list}); + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER]->set_reference_interface_names( + {"joint/position"}); + controllers_list.push_back(POSITION_REFERENCE_CONTROLLER); + + // Velocity reference controller + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER] = + std::make_shared(); + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER] + ->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + fwd_joint_velocity_ref_interfaces_list}); + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER]->set_reference_interface_names( + {"joint/velocity"}); + controllers_list.push_back(VELOCITY_REFERENCE_CONTROLLER); + + // Higher level task level controller + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER] = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(POSITION_REFERENCE_CONTROLLER) + "/joint/position", + std::string(VELOCITY_REFERENCE_CONTROLLER) + "/joint/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER]->set_command_interface_configuration( + cmd_cfg); + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER]->set_state_interface_configuration( + state_cfg); + controllers_list.push_back(HIGHER_LEVEL_REFERENCE_CONTROLLER); + + const unsigned int num_of_random_broadcasters = 20; + const unsigned int num_of_random_controllers = 20; + for (size_t i = 0; i < num_of_random_broadcasters; i++) + { + auto controller_name = "test_broadcaster_" + std::to_string(i); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + controllers_list.push_back(controller_name); + } + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_reference_controllers_" + std::to_string(i); + RCLCPP_ERROR(srv_node->get_logger(), "Initializing controller : %s !", controller_name.c_str()); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + random_controllers_list[controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string("ref_") + std::to_string(i) + std::string("/joint_2/acceleration")}}); + controllers_list.push_back(controller_name); + } + + // Now shuffle the list to be able to configure controller later randomly + std::random_shuffle(controllers_list.begin(), controllers_list.end()); + + { + ControllerManagerRunner cm_runner(this); + for (auto random_chain_ctrl : random_chainable_controllers_list) + { + cm_->add_controller( + random_chain_ctrl.second, random_chain_ctrl.first, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + for (auto random_ctrl : random_controllers_list) + { + cm_->add_controller( + random_ctrl.second, random_ctrl.first, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + } + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + + // check chainable controller + ASSERT_EQ(controllers_list.size(), result->controller.size()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + for (auto random_ctrl : controllers_list) + { + cm_->configure_controller(random_ctrl); + } + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(controllers_list.size(), result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t + { + auto it = std::find_if( + result->controller.begin(), result->controller.end(), + [controller_name](auto itf) + { return (itf.name.find(controller_name) != std::string::npos); }); + return std::distance(result->controller.begin(), it); + }; + + // Check the controller indexing + auto pos_ref_pos = get_ctrl_pos(POSITION_REFERENCE_CONTROLLER); + auto vel_ref_pos = get_ctrl_pos(VELOCITY_REFERENCE_CONTROLLER); + auto task_level_ctrl_pos = get_ctrl_pos(HIGHER_LEVEL_REFERENCE_CONTROLLER); + ASSERT_GT(vel_ref_pos, task_level_ctrl_pos); + ASSERT_GT(pos_ref_pos, task_level_ctrl_pos); + + for (size_t i = 0; i < joints_count; i++) + { + const std::string controller_name = JOINT_CONTROLLER_PREFIX + std::to_string(i); + const std::string fwd_controller_name = FWD_CONTROLLER_PREFIX + std::to_string(i); + + ASSERT_GT(get_ctrl_pos(fwd_controller_name), pos_ref_pos); + ASSERT_GT(get_ctrl_pos(fwd_controller_name), vel_ref_pos); + ASSERT_GT(get_ctrl_pos(controller_name), pos_ref_pos); + ASSERT_GT(get_ctrl_pos(controller_name), vel_ref_pos); + } + RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); +} From c9bd3d81c3312f08c191163b4922762af6c023b2 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Mon, 4 Dec 2023 11:00:45 +0100 Subject: [PATCH 11/21] Report inactive controllers as a diagnostics ok instead of an error (#1184) --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b144cd68f3..2ff070f7d2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2567,7 +2567,7 @@ void ControllerManager::controller_activity_diagnostic_callback( } else { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Not all controllers are active"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not all controllers are active"); } } From b8ba5c0d0e13c322ad49d99c3d29367ed87714fe Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 4 Dec 2023 13:34:44 +0000 Subject: [PATCH 12/21] Bump ros-tooling/setup-ros from 0.7.0 to 0.7.1 (#1192) --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/ci-ros-lint.yml | 4 ++-- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 7bff105e56..203fc4b6fc 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 3efd557adc..84c68217f3 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -14,7 +14,7 @@ jobs: AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -38,7 +38,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index c170922683..879946fced 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v4 From 91e64549d1261ffedec86402544c68a24e352066 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Dec 2023 17:51:14 +0100 Subject: [PATCH 13/21] Update good-first-issue.md (#1198) --- .github/ISSUE_TEMPLATE/good-first-issue.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 57779881d2..ececdae62c 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -55,6 +55,6 @@ Don’t hesitate to ask questions or to get help if you feel like you are gettin Furthermore, you find helpful resources here: * [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html) * [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) -* [ROS Answers](https://answers.ros.org/questions/) +* [Robotics Stack Exchange](https://robotics.stackexchange.com) **Good luck with your first issue!** From e4203a5a0b30301c5fb19ec199a15c7b7208ccba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Dec 2023 17:51:44 +0100 Subject: [PATCH 14/21] Add configs for humble and iron (#1197) --- .github/dependabot.yml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 05a48fc654..aafd67c236 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -11,3 +11,17 @@ updates: directory: "/" schedule: interval: "weekly" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "humble" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "iron" From 90680f2c043058702771d27ae9b248fd283d375b Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 11 Dec 2023 16:52:41 +0000 Subject: [PATCH 15/21] Bump actions/setup-python from 4.7.1 to 5.0.0 (#1199) --- .github/workflows/ci-format.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 2e1400f718..c7199b88c5 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4.7.1 + - uses: actions/setup-python@v5.0.0 with: python-version: '3.10' - name: Install system hooks From 6ca76cde6b5c6a26adfdb371f59714c9d1f203ae Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 11 Dec 2023 17:53:54 +0100 Subject: [PATCH 16/21] added InsertBraces option of clang formatting and update the needed mirrors-clang-format version (#1194) --- .clang-format | 1 + .pre-commit-config.yaml | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/.clang-format b/.clang-format index 698bda7da8..10dd2c7c7a 100644 --- a/.clang-format +++ b/.clang-format @@ -12,4 +12,5 @@ DerivePointerAlignment: false PointerAlignment: Middle ReflowComments: true IncludeBlocks: Preserve +InsertBraces: true ... diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 0ad4d0aa6c..6e6d41cc9c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -62,7 +62,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.6 + rev: v15.0.6 hooks: - id: clang-format From 345e70f9a8d8cbc0ba46af8437ff6feb9627be6d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 11 Dec 2023 19:14:18 +0100 Subject: [PATCH 17/21] Update list of reviewers (#1195) --- .github/reviewer-lottery.yml | 29 +++++++++++------------------ 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 84b156f5a1..c6580eacd4 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -13,28 +13,21 @@ groups: - name: reviewers reviewers: 5 usernames: - - rosterloh - - progtologist + - aprotyas - arne48 + - bijoua29 - christophfroehlich - DasRoteSkelett - - sgmurray - - harderthan - - jaron-l - - malapatiravi + - duringhof - erickisos - - sachinkum0009 - - qiayuanliao - - homalozoa - - anfemosa - - jackcenter - - VX792 - - mhubii + - fmauch + - jaron-l - livanov93 - - aprotyas + - mcbed + - moriarty + - olivier-stasse - peterdavidfagan - - duringhof + - progtologist + - saikishor - VanshGehlot - - bijoua29 - - LukasMacha97 - - mcbed + - VX792 From 562f36ddb615a70b54c71365393a8dc69508f52f Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 11 Dec 2023 18:18:53 +0000 Subject: [PATCH 18/21] Reformat with braces added (#1209) --- controller_manager/src/controller_manager.cpp | 28 ++++++++++++++++--- .../test/test_controller_manager_srvs.cpp | 9 +++++- 2 files changed, 32 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2ff070f7d2..b79f122c6b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -150,7 +150,9 @@ std::vector get_following_controller_names( } // If the controller is not configured, return empty if (!(is_controller_active(controller_it->c) || is_controller_inactive(controller_it->c))) + { return following_controllers; + } const auto cmd_itfs = controller_it->c->command_interface_configuration().names; for (const auto & itf : cmd_itfs) { @@ -210,7 +212,10 @@ std::vector get_preceding_controller_names( for (const auto & ctrl : controllers) { // If the controller is not configured, then continue - if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) continue; + if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) + { + continue; + } auto cmd_itfs = ctrl.c->command_interface_configuration().names; for (const auto & itf : cmd_itfs) { @@ -2443,7 +2448,10 @@ bool ControllerManager::controller_sorting( if (!((is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) && (is_controller_active(ctrl_b.c) || is_controller_inactive(ctrl_b.c)))) { - if (is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) return true; + if (is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) + { + return true; + } return false; } @@ -2455,9 +2463,13 @@ bool ControllerManager::controller_sorting( // joint_state_broadcaster // If the controller b is also under the same condition, then maintain their initial order if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) + { return false; + } else + { return true; + } } else if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) { @@ -2467,12 +2479,17 @@ bool ControllerManager::controller_sorting( else { auto following_ctrls = get_following_controller_names(ctrl_a.info.name, controllers); - if (following_ctrls.empty()) return false; + if (following_ctrls.empty()) + { + return false; + } // If the ctrl_b is any of the following controllers of ctrl_a, then place ctrl_a before ctrl_b if ( std::find(following_ctrls.begin(), following_ctrls.end(), ctrl_b.info.name) != following_ctrls.end()) + { return true; + } else { auto ctrl_a_preceding_ctrls = get_preceding_controller_names(ctrl_a.info.name, controllers); @@ -2505,7 +2522,10 @@ bool ControllerManager::controller_sorting( // If there is no common parent, then they belong to 2 different sets auto following_ctrls_b = get_following_controller_names(ctrl_b.info.name, controllers); - if (following_ctrls_b.empty()) return true; + if (following_ctrls_b.empty()) + { + return true; + } auto find_first_element = [&](const auto & controllers_list) -> size_t { auto it = std::find_if( diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index bc20236306..8882d0a8de 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -639,7 +639,9 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) {TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME}) + { cm_->configure_controller(controller); + } // get controller list after configure result = call_service_and_wait(*client, request, srv_executor); @@ -796,7 +798,9 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) {TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_6, test_controller::TEST_CONTROLLER_NAME}) + { cm_->configure_controller(controller); + } // get controller list after configure result = call_service_and_wait(*client, request, srv_executor); @@ -1010,7 +1014,10 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) TEST_CHAINED_CONTROLLER_4, TEST_CONTROLLER_2, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_8}; - for (const auto & controller : ctrls_order) cm_->configure_controller(controller); + for (const auto & controller : ctrls_order) + { + cm_->configure_controller(controller); + } // get controller list after configure result = call_service_and_wait(*client, request, srv_executor); From 51b8985400b10adb3fdd9f1851f368713def54eb Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 12 Dec 2023 13:11:35 +0100 Subject: [PATCH 19/21] [CM] Linting if/else statements (#1193) --- controller_manager/src/controller_manager.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b79f122c6b..da7815d51e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2462,14 +2462,9 @@ bool ControllerManager::controller_sorting( // The case of the controllers that don't have any command interfaces. For instance, // joint_state_broadcaster // If the controller b is also under the same condition, then maintain their initial order - if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) - { - return false; - } - else - { - return true; - } + const auto command_interfaces_exist = + !ctrl_b.c->command_interface_configuration().names.empty(); + return ctrl_b.c->is_chainable() && command_interfaces_exist; } else if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) { From 1330a4c73280c5b315e1f56519e3d0be23f513a0 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 12 Dec 2023 16:34:56 +0000 Subject: [PATCH 20/21] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 8 ++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 10 files changed, 35 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 92b1d91933..34ba3e2539 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-11-30) ------------------ * Add few warning compiler options to error (`#1181 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index c94433f8b5..3343fa7007 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [CM] Linting if/else statements (`#1193 `_) +* Reformat with braces added (`#1209 `_) +* Report inactive controllers as a diagnostics ok instead of an error (`#1184 `_) +* Fix controller sorting issue while loading large number of controllers (`#1180 `_) +* Contributors: Bence Magyar, Dr. Denis, Lennart Nachtigall, Sai Kishor Kothakota + 4.1.0 (2023-11-30) ------------------ * Add few warning compiler options to error (`#1181 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 899ae0d3fc..2816fc99a1 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-11-30) ------------------ diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 44f4246ac8..909712f885 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-11-30) ------------------ * Add few warning compiler options to error (`#1181 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 0360d15653..a2dbf930da 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-11-30) ------------------ * Add few warning compiler options to error (`#1181 `_) diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index ade3289319..bf269706a4 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-11-30) ------------------ diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 5740f66f1b..d95b95c4f6 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-11-30) ------------------ diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 524a5a89ae..6b698a7368 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-11-30) ------------------ diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 5e0e9ba9b8..7b56a66f90 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-11-30) ------------------ diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 981e57572e..ddb4603f3e 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.1.0 (2023-11-30) ------------------ * Add few warning compiler options to error (`#1181 `_) From 331379665eea1b0538eb116f927421f441c3dc00 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 12 Dec 2023 16:35:25 +0000 Subject: [PATCH 21/21] 4.2.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 22 files changed, 32 insertions(+), 32 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 34ba3e2539..27db0c351d 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-11-30) ------------------ diff --git a/controller_interface/package.xml b/controller_interface/package.xml index c7750bdec0..2d0b13727b 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.1.0 + 4.2.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 3343fa7007..0ddc9cfe68 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ * [CM] Linting if/else statements (`#1193 `_) * Reformat with braces added (`#1209 `_) * Report inactive controllers as a diagnostics ok instead of an error (`#1184 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 63c38d52ab..7be9aac597 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.1.0 + 4.2.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 2816fc99a1..dc07947f33 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-11-30) ------------------ diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index e9e5c46123..7ef4c5d9e8 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.1.0 + 4.2.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 909712f885..04e3996d01 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-11-30) ------------------ diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index b97a4242bd..b3685a7ee2 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.1.0 + 4.2.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index a2dbf930da..ab215f461d 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-11-30) ------------------ diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 2990fdfd7e..e1832fa776 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.1.0 + 4.2.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index bf269706a4..28a37f1134 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-11-30) ------------------ diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 71d996ce8f..e8413239c7 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.1.0 + 4.2.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index d95b95c4f6..c9200cf182 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-11-30) ------------------ diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index c55a1d5ca8..6d5e8ac9d9 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.1.0 + 4.2.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 6b698a7368..5c50a0b21a 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-11-30) ------------------ diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index e14f9d8982..8aa2b528a6 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.1.0 + 4.2.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 3bc1200b09..ff1df137ed 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.1.0", + version="4.2.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 7b56a66f90..959f47c6f7 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-11-30) ------------------ diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index ba51d7bd26..e08545b41f 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.1.0 + 4.2.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index d9d01fa304..9e9d9f763d 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="4.1.0", + version="4.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index ddb4603f3e..6b0cd5173b 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.2.0 (2023-12-12) +------------------ 4.1.0 (2023-11-30) ------------------ diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index f0c9e70ed5..17a63cca0b 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.1.0 + 4.2.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl