From 5e6b676f6a5764a4954ea72e0fc7682b0739eecf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 3 Nov 2023 21:13:00 +0100 Subject: [PATCH 01/15] [CI] codecov updates (#1147) * Fix coverage build and exclude test folders * Add codecov badge * Remove ignored package --------- Co-authored-by: Bence Magyar --- .github/workflows/ci-coverage-build.yml | 6 ------ README.md | 1 + codecov.yml | 4 ++-- 3 files changed, 3 insertions(+), 8 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 3dabed59cc..ca0068ad88 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -1,8 +1,6 @@ name: Coverage Build on: workflow_dispatch: - branches: - - master push: branches: - master @@ -31,11 +29,7 @@ jobs: package-name: controller_interface controller_manager - controller_manager_msgs hardware_interface - ros2controlcli - ros2_control - ros2_control_test_assets transmission_interface vcs-repo-file-url: | diff --git a/README.md b/README.md index b68c11705c..0231a1c0e7 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # ros2_control [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) +[![codecov](https://codecov.io/gh/ros-controls/ros2_control/graph/badge.svg?token=idvm1zJXOf)](https://codecov.io/gh/ros-controls/ros2_control) This package is a part of the ros2_control framework. For more, please check the [documentation](https://control.ros.org/). diff --git a/codecov.yml b/codecov.yml index 0db154faa5..d8a5fde3e0 100644 --- a/codecov.yml +++ b/codecov.yml @@ -14,12 +14,12 @@ fixes: comment: layout: "diff, flags, files" behavior: default +ignore: + - "**/test" flags: unittests: paths: - controller_interface - controller_manager - hardware_interface - - joint_limits_interface - - test_robot_hardware - transmission_interface From d4f464e04b645dddc6597388cba790b0b382cb3b Mon Sep 17 00:00:00 2001 From: Christopher Wecht Date: Sat, 4 Nov 2023 14:26:57 +0100 Subject: [PATCH 02/15] Fix potential deadlock in ResourceManager (#925) --- hardware_interface/src/resource_manager.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 36ace2b78f..5c5d5af3e6 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -767,8 +767,7 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac { if (individual_hardware_info.type == actuator_type) { - std::lock_guard guard(resource_interfaces_lock_); - std::lock_guard guard_claimed(claimed_command_interfaces_lock_); + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); resource_storage_->load_and_initialize_actuator(individual_hardware_info); } if (individual_hardware_info.type == sensor_type) @@ -778,8 +777,7 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac } if (individual_hardware_info.type == system_type) { - std::lock_guard guard(resource_interfaces_lock_); - std::lock_guard guard_claimed(claimed_command_interfaces_lock_); + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); resource_storage_->load_and_initialize_system(individual_hardware_info); } } @@ -843,8 +841,7 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, std::vector & interfaces) { - std::lock_guard guard(resource_interfaces_lock_); - std::lock_guard guard_claimed(claimed_command_interfaces_lock_); + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); auto interface_names = resource_storage_->add_command_interfaces(interfaces); resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names; } @@ -898,8 +895,7 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string & resource_storage_->controllers_reference_interfaces_map_.at(controller_name); resource_storage_->controllers_reference_interfaces_map_.erase(controller_name); - std::lock_guard guard(resource_interfaces_lock_); - std::lock_guard guard_claimed(claimed_command_interfaces_lock_); + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); resource_storage_->remove_command_interfaces(interface_names); } From 863f161539fd8f4815c001baa080ee3a9e164816 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 4 Nov 2023 15:19:20 +0100 Subject: [PATCH 03/15] Sort controllers while configuring instead of while activating (#1107) --- controller_manager/src/controller_manager.cpp | 62 ++++++++------- .../test_chainable_controller.cpp | 24 +----- .../test/test_controller/test_controller.cpp | 24 +----- .../test/test_controller_manager.cpp | 22 ++++-- .../test/test_controller_manager_srvs.cpp | 79 +++---------------- .../test/test_load_controller.cpp | 15 +++- 6 files changed, 77 insertions(+), 149 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a48069acfb..28c8c8d80a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -148,9 +148,6 @@ std::vector get_following_controller_names( return following_controllers; } - // 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) { @@ -209,13 +206,11 @@ std::vector get_preceding_controller_names( } for (const auto & ctrl : controllers) { - // If the controller is not configured, return empty - if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) - return preceding_controllers; auto cmd_itfs = ctrl.c->command_interface_configuration().names; for (const auto & itf : cmd_itfs) { - if (itf.find(controller_name) != std::string::npos) + auto split_pos = itf.find_first_of('/'); + if ((split_pos != std::string::npos) && (itf.substr(0, split_pos) == controller_name)) { preceding_controllers.push_back(ctrl.info.name); auto ctrl_names = get_preceding_controller_names(ctrl.info.name, controllers); @@ -768,10 +763,35 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); + } + + // Now let's reorder the controllers + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); + + // Copy all controllers from the 'from' list to the 'to' list + to = from; + + // Reordering the controllers + std::sort( + to.begin(), to.end(), + std::bind( + &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, + to)); - // TODO(destogl): check and resort controllers in the vector + RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); + for (const auto & ctrl : to) + { + RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str()); } + // switch lists + rt_controllers_wrapper_.switch_updated_list(guard); + // clear unused list + rt_controllers_wrapper_.get_unused_list(guard).clear(); + return controller_interface::return_type::OK; } @@ -1233,19 +1253,6 @@ controller_interface::return_type ControllerManager::switch_controller( } } - // Reordering the controllers - std::sort( - to.begin(), to.end(), - std::bind( - &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, - to)); - - RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:"); - for (const auto & ctrl : to) - { - RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str()); - } - // switch lists rt_controllers_wrapper_.switch_updated_list(guard); // clear unused list @@ -2435,13 +2442,6 @@ bool ControllerManager::controller_sorting( const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, const std::vector & controllers) { - // If the controllers are not active, then should be at the end of the list - if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c)) - { - if (is_controller_active(ctrl_a.c)) return true; - return false; - } - const std::vector cmd_itfs = ctrl_a.c->command_interface_configuration().names; const std::vector state_itfs = ctrl_a.c->state_interface_configuration().names; if (cmd_itfs.empty() || !ctrl_a.c->is_chainable()) @@ -2513,7 +2513,11 @@ bool ControllerManager::controller_sorting( // TODO(saikishor): deal with the state interface chaining in the sorting algorithm auto state_it = std::find_if( state_itfs.begin(), state_itfs.end(), - [ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); }); + [ctrl_b](auto itf) + { + auto index = itf.find_first_of('/'); + return ((index != std::string::npos) && (itf.substr(0, index) == ctrl_b.info.name)); + }); if (state_it != state_itfs.end()) { return false; diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index d21957a0b4..b02c2e65ef 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -32,33 +32,13 @@ TestChainableController::TestChainableController() controller_interface::InterfaceConfiguration TestChainableController::command_interface_configuration() const { - if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - return cmd_iface_cfg_; - } - else - { - throw std::runtime_error( - "Can not get command interface configuration until the controller is configured."); - } + return cmd_iface_cfg_; } controller_interface::InterfaceConfiguration TestChainableController::state_interface_configuration() const { - if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - return state_iface_cfg_; - } - else - { - throw std::runtime_error( - "Can not get state interface configuration until the controller is configured."); - } + return state_iface_cfg_; } controller_interface::return_type TestChainableController::update_reference_from_subscribers( diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 06f76bd044..97670e9a7a 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -31,32 +31,12 @@ TestController::TestController() controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { - if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - return cmd_iface_cfg_; - } - else - { - throw std::runtime_error( - "Can not get command interface configuration until the controller is configured."); - } + return cmd_iface_cfg_; } controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { - if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - return state_iface_cfg_; - } - else - { - throw std::runtime_error( - "Can not get state interface configuration until the controller is configured."); - } + return state_iface_cfg_; } controller_interface::return_type TestController::update( diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index e317726585..eb001ce70f 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -104,8 +104,11 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); - cm_->configure_controller(TEST_CONTROLLER2_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(TEST_CONTROLLER2_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -217,7 +220,10 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) test_controller->get_node()->set_parameter({"update_rate", 4}); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -296,7 +302,10 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); test_controller->get_node()->set_parameter(update_rate_parameter); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -389,7 +398,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); // configure controller - cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + } EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 7873adaacf..6429f82a44 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -261,27 +261,28 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) // get controller list after configure result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(2u, result->controller.size()); + // At this stage, the controllers are already reordered // check chainable controller ASSERT_EQ(result->controller[0].state, "inactive"); ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u); - ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 1u); + ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 3u); ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u); - ASSERT_EQ(result->controller[0].is_chainable, true); + ASSERT_EQ(result->controller[0].is_chainable, false); ASSERT_EQ(result->controller[0].is_chained, false); - ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u); - ASSERT_EQ("joint1/position", result->controller[0].reference_interfaces[0]); - ASSERT_EQ("joint1/velocity", result->controller[0].reference_interfaces[1]); + ASSERT_EQ(result->controller[0].reference_interfaces.size(), 0u); + ASSERT_EQ(result->controller[0].chain_connections.size(), 1u); - ASSERT_EQ(result->controller[0].chain_connections.size(), 0u); // check test controller ASSERT_EQ(result->controller[1].state, "inactive"); ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 0u); - ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 3u); + ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 1u); ASSERT_EQ(result->controller[1].required_state_interfaces.size(), 2u); - ASSERT_EQ(result->controller[1].is_chainable, false); + ASSERT_EQ(result->controller[1].is_chainable, true); ASSERT_EQ(result->controller[1].is_chained, false); - ASSERT_EQ(result->controller[1].reference_interfaces.size(), 0u); - ASSERT_EQ(result->controller[1].chain_connections.size(), 1u); + ASSERT_EQ(result->controller[1].reference_interfaces.size(), 2u); + ASSERT_EQ(result->controller[1].chain_connections.size(), 0u); + ASSERT_EQ("joint1/position", result->controller[1].reference_interfaces[0]); + ASSERT_EQ("joint1/velocity", result->controller[1].reference_interfaces[1]); // activate controllers cm_->switch_controller( {test_chainable_controller::TEST_CONTROLLER_NAME}, {}, @@ -603,22 +604,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(6u, result->controller.size()); - // activate controllers - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_1}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, - TEST_CHAINED_CONTROLLER_4}, - {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - cm_->switch_controller( - {test_controller::TEST_CONTROLLER_NAME}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - // get controller list after activate - result = call_service_and_wait(*client, request, srv_executor); - - ASSERT_EQ(6u, result->controller.size()); // reordered controllers ASSERT_EQ(result->controller[0].name, "test_controller_name"); ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_5); @@ -776,25 +761,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(8u, result->controller.size()); - // activate controllers - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_1}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_3}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, - TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_7}, - {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - cm_->switch_controller( - {test_controller::TEST_CONTROLLER_NAME}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - // get controller list after activate - result = call_service_and_wait(*client, request, srv_executor); - - ASSERT_EQ(8u, result->controller.size()); // reordered controllers ASSERT_EQ(result->controller[0].name, "test_controller_name"); ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7); @@ -1009,29 +975,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(10u, result->controller.size()); - // activate controllers - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_1}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_4}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_7}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, - TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_8}, - {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - cm_->switch_controller( - {TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - // get controller list after activate - 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 it = std::find_if( diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index ca2f7f6b1c..390a7365f0 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -162,7 +162,10 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); // Activate configured controller - cm_->configure_controller(controller_name1); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(controller_name1); + } start_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } @@ -246,7 +249,10 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure test_controller->cleanup_calls = &cleanup_calls; // Configure from inactive state test_controller->simulate_cleanup_failure = false; - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + } ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); EXPECT_EQ(1u, cleanup_calls); } @@ -421,7 +427,10 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); - cm_->configure_controller(controller_name2); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(controller_name2); + } // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1"); From 8bf1a8a1b3dd2533c93d3cc0591abf4560199b23 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 6 Nov 2023 19:18:20 +0100 Subject: [PATCH 04/15] [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) Co-authored-by: Bence Magyar --- .../src/mock_components/generic_system.cpp | 5 ++ .../mock_components/test_generic_system.cpp | 81 +++++++++++++++++++ 2 files changed, 86 insertions(+) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 3ae3568a9e..1b339c71ce 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -388,6 +388,11 @@ return_type GenericSystem::prepare_command_mode_switch( { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; + if (!calculate_dynamics_) + { + return ret_val; + } + const size_t FOUND_ONCE_FLAG = 1000000; std::vector joint_found_in_x_requests_; diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index cd344b600e..9556ed6f8b 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -518,6 +518,46 @@ class TestGenericSystem : public ::testing::Test )"; + valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_ = + R"( + + + mock_components/GenericSystem + true + + + + + 3.45 + + + + + + + + 2.78 + + + + + + + + + 2.78 + + + + + + + + + + +)"; + disabled_commands_ = R"( @@ -556,6 +596,7 @@ class TestGenericSystem : public ::testing::Test std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_; + std::string valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string disabled_commands_; }; @@ -1901,3 +1942,43 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ASSERT_EQ(0.0, j1v_s.get_value()); ASSERT_EQ(0.11, j1p_c.get_value()); } + +TEST_F(TestGenericSystem, prepare_command_mode_switch_works_with_all_example_tags) +{ + auto check_prepare_command_mode_switch = [&](const std::string & urdf) + { + TestableResourceManager rm( + ros2_control_test_assets::urdf_head + urdf + ros2_control_test_assets::urdf_tail); + auto start_interfaces = rm.command_interface_keys(); + std::vector stop_interfaces; + return rm.prepare_command_mode_switch(start_interfaces, stop_interfaces); + }; + + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_asymetric_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_other_interface_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_)); + ASSERT_TRUE( + check_prepare_command_mode_switch(hardware_system_2dof_with_sensor_mock_command_True_)); + ASSERT_TRUE(check_prepare_command_mode_switch(hardware_system_2dof_with_mimic_joint_)); + ASSERT_TRUE( + check_prepare_command_mode_switch(hardware_system_2dof_standard_interfaces_with_offset_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_)); + ASSERT_TRUE(check_prepare_command_mode_switch(valid_urdf_ros2_control_system_robot_with_gpio_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_)); + ASSERT_TRUE(check_prepare_command_mode_switch(sensor_with_initial_value_)); + ASSERT_TRUE(check_prepare_command_mode_switch(gpio_with_initial_value_)); + ASSERT_FALSE(check_prepare_command_mode_switch( + hardware_system_2dof_standard_interfaces_with_different_control_modes_)); + ASSERT_TRUE(check_prepare_command_mode_switch( + valid_hardware_system_2dof_standard_interfaces_with_different_control_modes_)); + ASSERT_TRUE(check_prepare_command_mode_switch(disabled_commands_)); +} From 9684061695bdd0e1ec116f0a61e63ff2b468f59b Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 6 Nov 2023 18:22:41 +0000 Subject: [PATCH 05/15] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 5 +++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 6 ++++++ 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 2a3e13be6f..251a4adeb7 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.20.0 (2023-10-31) ------------------- diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 7623eb981b..287bcfc0e3 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Sort controllers while configuring instead of while activating (`#1107 `_) +* Contributors: Sai Kishor Kothakota + 3.20.0 (2023-10-31) ------------------- * Update spawner to accept controllers list and start them in sequence (`#1139 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index ad75d60dcc..50675fe03b 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.20.0 (2023-10-31) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 5a2e3f8c38..edfabb0d89 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [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 `_) +* Fix potential deadlock in ResourceManager (`#925 `_) +* Contributors: Christopher Wecht, Dr. Denis + 3.20.0 (2023-10-31) ------------------- * [ResourceManager] deactivate hardware from read/write return value (`#884 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index f112ccd29f..da8cfe0aa7 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.20.0 (2023-10-31) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 44ec3fc07f..19a7284d2d 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.20.0 (2023-10-31) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 2f7dd6a3ef..2eebb36e01 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.20.0 (2023-10-31) ------------------- * [ResourceManager] deactivate hardware from read/write return value (`#884 `_) diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 17a6f24df2..9decc23cc0 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.20.0 (2023-10-31) ------------------- * Fix doc of load_controller (`#1132 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 25383774ec..f91245e2a4 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.20.0 (2023-10-31) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 8ce851312a..e1c308e04d 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 3.20.0 (2023-10-31) ------------------- From 845d3f54f4e2c37657ff69dd6485df5ee8898c90 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 6 Nov 2023 18:23:01 +0000 Subject: [PATCH 06/15] 3.21.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 251a4adeb7..5aa2ad0647 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.21.0 (2023-11-06) +------------------- 3.20.0 (2023-10-31) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 6c21808b4d..6a2a2513ee 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.20.0 + 3.21.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 287bcfc0e3..0d021492d9 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.21.0 (2023-11-06) +------------------- * Sort controllers while configuring instead of while activating (`#1107 `_) * Contributors: Sai Kishor Kothakota diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 0114eb0b19..5cef7c9577 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.20.0 + 3.21.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 50675fe03b..e6d5095d7e 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.21.0 (2023-11-06) +------------------- 3.20.0 (2023-10-31) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index e30483bdc9..814f814da1 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.20.0 + 3.21.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 edfabb0d89..4efd1a3686 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) * Fix potential deadlock in ResourceManager (`#925 `_) * Contributors: Christopher Wecht, Dr. Denis diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index d18f40171d..7fd3c78b61 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.20.0 + 3.21.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index da8cfe0aa7..da8a09aae2 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.21.0 (2023-11-06) +------------------- 3.20.0 (2023-10-31) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index ce7ea4efa4..928a6f26fd 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.20.0 + 3.21.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 19a7284d2d..2ed4b9ec05 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.21.0 (2023-11-06) +------------------- 3.20.0 (2023-10-31) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 6f0100d5fd..2a0b0e957b 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.20.0 + 3.21.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 2eebb36e01..66d1983c54 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 ------------ +3.21.0 (2023-11-06) +------------------- 3.20.0 (2023-10-31) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index d9deb94d33..252233bc2e 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.20.0 + 3.21.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 9decc23cc0..2481e4b8c5 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.21.0 (2023-11-06) +------------------- 3.20.0 (2023-10-31) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index fa73a66355..1a8aca9b51 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.20.0 + 3.21.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 33ae2db9e9..9a897fae0a 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.20.0", + version="3.21.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 f91245e2a4..23b78e1527 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.21.0 (2023-11-06) +------------------- 3.20.0 (2023-10-31) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index fef2160f5b..56f7824962 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.20.0 + 3.21.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 36c0c087c5..d32300651e 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.20.0", + version="3.21.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 e1c308e04d..067c4353c8 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +3.21.0 (2023-11-06) +------------------- 3.20.0 (2023-10-31) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 7aba978c3c..b294e56d24 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.20.0 + 3.21.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 6100c5c082a246d6a37d45ac6ce83a28996cbdd3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 6 Nov 2023 18:40:08 +0000 Subject: [PATCH 07/15] Add iron branch (#1155) --- .github/mergify.yml | 9 +++++++++ .github/workflows/iron-abi-compatibility.yml | 4 ++-- .github/workflows/iron-binary-build-main.yml | 8 ++++---- .github/workflows/iron-binary-build-testing.yml | 8 ++++---- .github/workflows/iron-semi-binary-build-main.yml | 8 ++++---- .github/workflows/iron-semi-binary-build-testing.yml | 8 ++++---- .github/workflows/iron-source-build.yml | 6 +++--- README.md | 2 +- 8 files changed, 31 insertions(+), 22 deletions(-) diff --git a/.github/mergify.yml b/.github/mergify.yml index 49d2acedfa..e1f7f21a1e 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -8,6 +8,15 @@ pull_request_rules: branches: - humble + - name: Backport to iron at reviewers discretion + conditions: + - base=master + - "label=backport-iron" + actions: + backport: + branches: + - iron + - name: Ask to resolve conflict conditions: - conflict diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml index 5bce340193..20d93f5af1 100644 --- a/.github/workflows/iron-abi-compatibility.yml +++ b/.github/workflows/iron-abi-compatibility.yml @@ -2,10 +2,10 @@ name: Iron - ABI Compatibility Check on: workflow_dispatch: branches: - - master + - iron pull_request: branches: - - master + - iron jobs: abi_check: diff --git a/.github/workflows/iron-binary-build-main.yml b/.github/workflows/iron-binary-build-main.yml index be225cc0e1..bc2dbd96cc 100644 --- a/.github/workflows/iron-binary-build-main.yml +++ b/.github/workflows/iron-binary-build-main.yml @@ -5,13 +5,13 @@ name: Iron Binary Build - main on: workflow_dispatch: branches: - - master + - iron pull_request: branches: - - master + - iron push: branches: - - master + - iron schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' @@ -23,4 +23,4 @@ jobs: ros_distro: iron ros_repo: main upstream_workspace: ros2_control-not-released.iron.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-binary-build-testing.yml b/.github/workflows/iron-binary-build-testing.yml index c44786c7e2..512c4b55e4 100644 --- a/.github/workflows/iron-binary-build-testing.yml +++ b/.github/workflows/iron-binary-build-testing.yml @@ -5,13 +5,13 @@ name: Iron Binary Build - testing on: workflow_dispatch: branches: - - master + - iron pull_request: branches: - - master + - iron push: branches: - - master + - iron schedule: # Run every morning to detect flakiness and broken dependencies - cron: '03 1 * * *' @@ -23,4 +23,4 @@ jobs: ros_distro: iron ros_repo: testing upstream_workspace: ros2_control-not-released.iron.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-semi-binary-build-main.yml b/.github/workflows/iron-semi-binary-build-main.yml index 1d72d06149..1399e8b32b 100644 --- a/.github/workflows/iron-semi-binary-build-main.yml +++ b/.github/workflows/iron-semi-binary-build-main.yml @@ -4,13 +4,13 @@ name: Iron Semi-Binary Build - main on: workflow_dispatch: branches: - - master + - iron pull_request: branches: - - master + - iron push: branches: - - master + - iron schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' @@ -22,4 +22,4 @@ jobs: ros_distro: iron ros_repo: main upstream_workspace: ros2_control.iron.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-semi-binary-build-testing.yml b/.github/workflows/iron-semi-binary-build-testing.yml index 48707cd914..b29f798931 100644 --- a/.github/workflows/iron-semi-binary-build-testing.yml +++ b/.github/workflows/iron-semi-binary-build-testing.yml @@ -4,13 +4,13 @@ name: Iron Semi-Binary Build - testing on: workflow_dispatch: branches: - - master + - iron pull_request: branches: - - master + - iron push: branches: - - master + - iron schedule: # Run every morning to detect flakiness and broken dependencies - cron: '33 1 * * *' @@ -22,4 +22,4 @@ jobs: ros_distro: iron ros_repo: testing upstream_workspace: ros2_control.iron.repos - ref_for_scheduled_build: master + ref_for_scheduled_build: iron diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index 31eccd1bef..1e9d865c49 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -2,10 +2,10 @@ name: Iron Source Build on: workflow_dispatch: branches: - - master + - iron push: branches: - - master + - iron schedule: # Run every day to detect flakiness and broken dependencies - cron: '03 3 * * *' @@ -15,5 +15,5 @@ jobs: uses: ./.github/workflows/reusable-ros-tooling-source-build.yml with: ros_distro: iron - ref: master + ref: iron ros2_repo_branch: iron diff --git a/README.md b/README.md index 0231a1c0e7..0c26afd184 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ For more, please check the [documentation](https://control.ros.org/). ROS2 Distro | Branch | Build status | Documentation | Released packages :---------: | :----: | :----------: | :-----------: | :---------------: **Rolling** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-binary-build-main.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/rolling-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#rolling) -**Iron** | [`master`](https://github.com/ros-controls/ros2_control/tree/master) | [![Iron Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build-main.yml?branch=master)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/master/index.html)
[API Reference](https://control.ros.org/master/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#iron) +**Iron** | [`iron`](https://github.com/ros-controls/ros2_control/tree/iron) | [![Iron Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build-main.yml/badge.svg?branch=iron)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-binary-build-main.yml?branch=iron)
[![Iron Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build-main.yml/badge.svg?branch=iron)](https://github.com/ros-controls/ros2_control/actions/workflows/iron-semi-binary-build-main.yml?branch=iron) | [Documentation](https://control.ros.org/iron/index.html)
[API Reference](https://control.ros.org/iron/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#iron) **Humble** | [`humble`](https://github.com/ros-controls/ros2_control/tree/humble) | [![Humble Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-binary-build-main.yml?branch=master)
[![Humble Semi-Binary Build](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control/actions/workflows/humble-semi-binary-build-main.yml?branch=master) | [Documentation](https://control.ros.org/humble/index.html)
[API Reference](https://control.ros.org/humble/doc/api/index.html) | [ros2_control](https://index.ros.org/p/ros2_control/#humble) [Detailed build status](.github/workflows/README.md) From 132dd28d973466896155f331973f6b96a721c177 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 6 Nov 2023 19:44:09 +0100 Subject: [PATCH 08/15] Remove deprecation warning (#1101) --- controller_manager/src/controller_manager.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 28c8c8d80a..b3a4dc8945 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1880,13 +1880,6 @@ void ControllerManager::list_hardware_components_srv_cb( auto component = controller_manager_msgs::msg::HardwareComponentState(); component.name = component_name; component.type = component_info.type; - component.class_type = - component_info.plugin_name; // TODO(bence): deprecated currently. Remove soon - RCLCPP_WARN( - get_logger(), - "The 'class_type' field in 'controller_manager_msgs/msg/HardwareComponentState.msg' is " - "deprecated and will be removed soon. Please switch over client code to use 'plugin_name' " - "instead."); component.plugin_name = component_info.plugin_name; component.state.id = component_info.state.id(); component.state.label = component_info.state.label(); From b33e579b24c324efdd24b8f9d947845e5090b1d3 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 6 Nov 2023 18:45:34 +0000 Subject: [PATCH 09/15] Pass URDF to controllers on init (#1088) * Pass URDF to controllers on init * store the robot_description in the controller_manager class variable * added minimal test to verify the update of robot description in controllers * fix code formatting * added test with loading and unloading the controller * added changes as per bence's review comments * added get_robot_description method to the base class --------- Co-authored-by: Sai Kishor Kothakota --- .../controller_interface_base.hpp | 7 +++- .../src/controller_interface_base.cpp | 5 ++- .../test_chainable_controller_interface.cpp | 12 +++--- .../test/test_controller_interface.cpp | 8 ++-- .../test/test_controller_with_options.cpp | 4 +- .../test/test_controller_with_options.hpp | 5 ++- .../controller_manager/controller_manager.hpp | 1 + controller_manager/src/controller_manager.cpp | 13 +++--- .../test/test_controller/test_controller.hpp | 3 ++ .../test_controller_failed_init.cpp | 4 +- .../test_controller_failed_init.hpp | 3 +- .../test/test_controller_manager.cpp | 28 +++++++++++++ .../test/test_controller_manager_srvs.cpp | 41 +++++++++++++++++++ 13 files changed, 109 insertions(+), 25 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 52bc970500..96141a034b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -114,7 +114,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type init( - const std::string & controller_name, const std::string & namespace_ = "", + const std::string & controller_name, const std::string & urdf, + const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true)); /// Custom configure method to read additional parameters for controller-nodes @@ -155,6 +156,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC bool is_async() const; + CONTROLLER_INTERFACE_PUBLIC + const std::string & get_robot_description() const; + /// Declare and initialize a parameter with a type. /** * @@ -223,6 +227,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::vector state_interfaces_; unsigned int update_rate_ = 0; bool is_async_ = false; + std::string urdf_ = ""; private: std::shared_ptr node_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 926449e201..fcbc28590a 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -25,11 +25,12 @@ namespace controller_interface { return_type ControllerInterfaceBase::init( - const std::string & controller_name, const std::string & namespace_, + const std::string & controller_name, const std::string & urdf, const std::string & namespace_, const rclcpp::NodeOptions & node_options) { node_ = std::make_shared( controller_name, namespace_, node_options, false); // disable LifecycleNode service interfaces + urdf_ = urdf; try { @@ -132,4 +133,6 @@ unsigned int ControllerInterfaceBase::get_update_rate() const { return update_ra bool ControllerInterfaceBase::is_async() const { return is_async_; } +const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } + } // namespace controller_interface diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index 6ae36b39ce..9c46287ba7 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, ""), 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, ""), 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, ""), 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, ""), 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, ""), 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, ""), 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 f27fc43283..9be0dba9f0 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -38,7 +38,7 @@ 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, ""), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); // update_rate is set to default 0 @@ -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, ""), controller_interface::return_type::OK); // initialize executor to be able to get parameter update auto executor = @@ -122,7 +122,7 @@ 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, ""), controller_interface::return_type::ERROR); rclcpp::shutdown(); } @@ -136,7 +136,7 @@ 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, ""), 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 080b8c4f8d..55c9db65c9 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", ""), 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", ""), 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 e19fc18d43..892ec56f9d 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -40,13 +40,14 @@ class ControllerWithOptions : public controller_interface::ControllerInterface } controller_interface::return_type init( - const std::string & controller_name, const std::string & namespace_ = "", + const std::string & controller_name, const std::string & urdf, + 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, namespace_, node_options); + ControllerInterface::init(controller_name, urdf, namespace_, node_options); switch (on_init()) { diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index cf1e959e1f..79a9861463 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -534,6 +534,7 @@ class ControllerManager : public rclcpp::Node std::vector activate_command_interface_request_, deactivate_command_interface_request_; + std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; struct SwitchParams diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b3a4dc8945..b6c277126e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -262,10 +262,10 @@ ControllerManager::ControllerManager( RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } - std::string robot_description = ""; + robot_description_ = ""; // TODO(destogl): remove support at the end of 2023 - get_parameter("robot_description", robot_description); - if (robot_description.empty()) + get_parameter("robot_description", robot_description_); + if (robot_description_.empty()) { subscribe_to_robot_description_topic(); } @@ -275,7 +275,7 @@ ControllerManager::ControllerManager( get_logger(), "[Deprecated] Passing the robot description parameter directly to the control_manager node " "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead."); - init_resource_manager(robot_description); + init_resource_manager(robot_description_); } diagnostics_updater_.setHardwareID("ros2_control"); @@ -332,6 +332,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & // to die if a non valid urdf is passed. However, should maybe be fine tuned. try { + robot_description_ = robot_description.data; if (resource_manager_->is_urdf_already_loaded()) { RCLCPP_WARN( @@ -340,7 +341,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & "description file."); return; } - init_resource_manager(robot_description.data.c_str()); + init_resource_manager(robot_description_); } catch (std::runtime_error & e) { @@ -1290,7 +1291,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co } if ( - controller.c->init(controller.info.name, get_namespace()) == + controller.c->init(controller.info.name, robot_description_, get_namespace()) == controller_interface::return_type::ERROR) { to.clear(); diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index f73f592037..d7b318c597 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -30,6 +30,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface // indicating the node name under which the controller node // is being loaded. constexpr char TEST_CONTROLLER_NAME[] = "test_controller_name"; +constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name"; // corresponds to the name listed within the pluginlib xml constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_controller"; class TestController : public controller_interface::ControllerInterface @@ -65,6 +66,8 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg); + const std::string & getRobotDescription() const; + unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller 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 57d253d03c..ba762573c7 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 @@ -32,8 +32,8 @@ TestControllerFailedInit::on_init() } controller_interface::return_type TestControllerFailedInit::init( - const std::string & /* controller_name */, const std::string & /*namespace_*/, - const rclcpp::NodeOptions & /*node_options*/) + const std::string & /* controller_name */, const std::string & /* urdf */, + 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 bfc2cd3cad..69530e40b0 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,8 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac CONTROLLER_INTERFACE_PUBLIC controller_interface::return_type init( - const std::string & controller_name, const std::string & namespace_ = "", + const std::string & controller_name, const std::string & urdf, + const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .allow_undeclared_parameters(true) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index eb001ce70f..8769a24749 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -33,6 +33,34 @@ class TestControllerManagerWithStrictness { }; +class TestControllerManagerRobotDescription +: public ControllerManagerFixture +{ +}; + +TEST_F(TestControllerManagerRobotDescription, controller_robot_description_update) +{ + auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // Now change the robot description and then load a new controller and see if the new controller + // gets the new description and the old controller still maintains the configuration + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf; + cm_->robot_description_callback(msg); + cm_->add_controller( + test_controller2, test_controller::TEST_CONTROLLER2_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + ASSERT_EQ( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf, + test_controller2->get_robot_description()); +} + TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) { const auto test_param = GetParam(); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 6429f82a44..773fbd94d8 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -457,6 +457,47 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } +TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controller) +{ + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr unload_client = + srv_node->create_client( + "test_controller_manager/unload_controller"); + + auto test_controller = std::make_shared(); + auto abstract_test_controller = cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + + // check the robot description + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // Now change the robot description and then see that the controller maintains the old URDF until + // it is unloaded and loaded again + auto msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf; + cm_->robot_description_callback(msg); + ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description()); + + // now unload and load the controller and see if the controller gets the new robot description + auto unload_request = std::make_shared(); + unload_request->name = test_controller::TEST_CONTROLLER_NAME; + auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true); + EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); + + // now load it and check if it got the new robot description + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + ASSERT_EQ( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf, + test_controller->get_robot_description()); +} + TEST_F(TestControllerManagerSrvs, configure_controller_srv) { rclcpp::executors::SingleThreadedExecutor srv_executor; From 0863acd6905af0eba4db1074f11edd60215758d5 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 7 Nov 2023 21:51:49 +0100 Subject: [PATCH 10/15] [MockHardware] Remove all deprecated options and deprecated plugins from the library. (#1150) * Rename HW for simpler debugging and remove deprecated initial value handling. * Remove fake from parameters. * Remove FakeSystem * Cleanup fake from tests. --- hardware_interface/CMakeLists.txt | 18 - .../fake_components_plugin_description.xml | 9 - .../fake_components/generic_system.hpp | 29 -- .../fake_components/visibility_control.h | 49 --- .../mock_components/fake_generic_system.cpp | 19 - .../src/mock_components/generic_system.cpp | 55 +-- .../mock_components/test_generic_system.cpp | 346 ++++++++---------- 7 files changed, 164 insertions(+), 361 deletions(-) delete mode 100644 hardware_interface/fake_components_plugin_description.xml delete mode 100644 hardware_interface/include/fake_components/generic_system.hpp delete mode 100644 hardware_interface/include/fake_components/visibility_control.h delete mode 100644 hardware_interface/src/mock_components/fake_generic_system.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index e486302c4c..2108bab17c 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -54,23 +54,6 @@ target_compile_definitions(mock_components PRIVATE "HARDWARE_INTERFACE_BUILDING_ pluginlib_export_plugin_description_file( hardware_interface mock_components_plugin_description.xml) -add_library(fake_components SHARED - src/mock_components/generic_system.cpp - src/mock_components/fake_generic_system.cpp -) -target_compile_features(fake_components PUBLIC cxx_std_17) -target_include_directories(fake_components PUBLIC - $ - $ -) -ament_target_dependencies(fake_components PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(fake_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") - -pluginlib_export_plugin_description_file( - hardware_interface fake_components_plugin_description.xml) - if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) @@ -145,7 +128,6 @@ install( ) install( TARGETS - fake_components mock_components hardware_interface EXPORT export_hardware_interface diff --git a/hardware_interface/fake_components_plugin_description.xml b/hardware_interface/fake_components_plugin_description.xml deleted file mode 100644 index 604170849a..0000000000 --- a/hardware_interface/fake_components_plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - Generic components for simple faking of system hardware. - - - - diff --git a/hardware_interface/include/fake_components/generic_system.hpp b/hardware_interface/include/fake_components/generic_system.hpp deleted file mode 100644 index 3dfb2b9b70..0000000000 --- a/hardware_interface/include/fake_components/generic_system.hpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright (c) 2021 PickNik, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: Jafar Abdi, Denis Stogl - -#ifndef FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ -#define FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ - -#include "mock_components/generic_system.hpp" - -namespace fake_components -{ -using GenericSystem [[deprecated]] = mock_components::GenericSystem; - -using GenericSystem [[deprecated]] = mock_components::GenericRobot; -} // namespace fake_components - -#endif // FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ diff --git a/hardware_interface/include/fake_components/visibility_control.h b/hardware_interface/include/fake_components/visibility_control.h deleted file mode 100644 index 1e60a9d990..0000000000 --- a/hardware_interface/include/fake_components/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2021 PickNik, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef FAKE_COMPONENTS__VISIBILITY_CONTROL_H_ -#define FAKE_COMPONENTS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define FAKE_COMPONENTS_EXPORT __attribute__((dllexport)) -#define FAKE_COMPONENTS_IMPORT __attribute__((dllimport)) -#else -#define FAKE_COMPONENTS_EXPORT __declspec(dllexport) -#define FAKE_COMPONENTS_IMPORT __declspec(dllimport) -#endif -#ifdef FAKE_COMPONENTS_BUILDING_DLL -#define FAKE_COMPONENTS_PUBLIC FAKE_COMPONENTS_EXPORT -#else -#define FAKE_COMPONENTS_PUBLIC FAKE_COMPONENTS_IMPORT -#endif -#define FAKE_COMPONENTS_PUBLIC_TYPE FAKE_COMPONENTS_PUBLIC -#define FAKE_COMPONENTS_LOCAL -#else -#define FAKE_COMPONENTS_EXPORT __attribute__((visibility("default"))) -#define FAKE_COMPONENTS_IMPORT -#if __GNUC__ >= 4 -#define FAKE_COMPONENTS_PUBLIC __attribute__((visibility("default"))) -#define FAKE_COMPONENTS_LOCAL __attribute__((visibility("hidden"))) -#else -#define FAKE_COMPONENTS_PUBLIC -#define FAKE_COMPONENTS_LOCAL -#endif -#define FAKE_COMPONENTS_PUBLIC_TYPE -#endif - -#endif // FAKE_COMPONENTS__VISIBILITY_CONTROL_H_ diff --git a/hardware_interface/src/mock_components/fake_generic_system.cpp b/hardware_interface/src/mock_components/fake_generic_system.cpp deleted file mode 100644 index 736575c8a6..0000000000 --- a/hardware_interface/src/mock_components/fake_generic_system.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright (c) 2022 PickNik, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: Jafar Abdi, Denis Stogl - -#include "fake_components/generic_system.hpp" -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(fake_components::GenericSystem, hardware_interface::SystemInterface) diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 1b339c71ce..7b62af105b 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -79,20 +79,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } else { - // check if fake_sensor_commands was set instead and issue warning. - it = info_.hardware_parameters.find("fake_sensor_commands"); - if (it != info_.hardware_parameters.end()) - { - use_mock_sensor_command_interfaces_ = hardware_interface::parse_bool(it->second); - RCUTILS_LOG_WARN_NAMED( - "mock_generic_system", - "Parameter 'fake_sensor_commands' has been deprecated from usage. Use" - "'mock_sensor_commands' instead."); - } - else - { - use_mock_sensor_command_interfaces_ = false; - } + use_mock_sensor_command_interfaces_ = false; } // check if to create mock command interface for gpio @@ -103,20 +90,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } else { - // check if fake_gpio_commands was set instead and issue warning - it = info_.hardware_parameters.find("fake_gpio_commands"); - if (it != info_.hardware_parameters.end()) - { - use_mock_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); - RCUTILS_LOG_WARN_NAMED( - "mock_generic_system", - "Parameter 'fake_gpio_commands' has been deprecated from usage. Use" - "'mock_gpio_commands' instead."); - } - else - { - use_mock_gpio_command_interfaces_ = false; - } + use_mock_gpio_command_interfaces_ = false; } // check if there is parameter that disables commands @@ -707,7 +681,6 @@ void GenericSystem::initialize_storage_vectors( } // Initialize with values from URDF - bool print_hint = false; for (auto i = 0u; i < component_infos.size(); i++) { const auto & component = component_infos[i]; @@ -725,33 +698,9 @@ void GenericSystem::initialize_storage_vectors( { states[index][i] = parse_double(interface.initial_value); } - else - { - // Initialize the value in old way with warning message - auto it2 = component.parameters.find("initial_" + interface.name); - if (it2 != component.parameters.end()) - { - states[index][i] = parse_double(it2->second); - print_hint = true; - } - else - { - print_hint = true; - } - } } } } - if (print_hint) - { - RCUTILS_LOG_WARN_ONCE_NAMED( - "mock_generic_system", - "Parsing of optional initial interface values failed or uses a deprecated format. Add " - "initial values for every state interface in the ros2_control.xacro. For example: \n" - " \n" - " 0.0 \n" - ""); - } } template diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 9556ed6f8b..57b5796d0d 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -40,128 +40,120 @@ const auto COMPARE_DELTA = 0.0001; class TestGenericSystem : public ::testing::Test { public: - void test_generic_system_with_mimic_joint(std::string & urdf); - void test_generic_system_with_mock_sensor_commands(std::string & urdf); - void test_generic_system_with_mock_gpio_commands(std::string & urdf); + void test_generic_system_with_mimic_joint(std::string & urdf, const std::string & component_name); + void test_generic_system_with_mock_sensor_commands( + std::string & urdf, const std::string & component_name); + void test_generic_system_with_mock_gpio_commands( + std::string & urdf, const std::string & component_name); protected: void SetUp() override { - // REMOVE THIS MEMBER ONCE FAKE COMPONENTS ARE REMOVED - hardware_fake_system_2dof_ = - R"( - - - fake_components/GenericSystem - - - - - 1.57 - - - - - 0.7854 - - -)"; - hardware_system_2dof_ = R"( - + mock_components/GenericSystem - - 1.57 + + 1.57 + - - 0.7854 + + 0.7854 + )"; hardware_system_2dof_asymetric_ = R"( - + mock_components/GenericSystem - - 1.57 + + 1.57 + - - 0.7854 - 0.8554 + + 0.7854 + )"; hardware_system_2dof_standard_interfaces_ = R"( - + mock_components/GenericSystem - + + 3.45 + - 3.45 - + + 2.78 + - 2.78 )"; hardware_system_2dof_with_other_interface_ = R"( - + mock_components/GenericSystem - - - 1.55 - 0.1 + + 1.55 + + + 0.1 + - - - 0.65 - 0.2 + + 0.65 + + + 0.2 + - - 0.5 + + 0.5 + )"; hardware_system_2dof_with_sensor_ = R"( - + mock_components/GenericSystem @@ -189,7 +181,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_sensor_mock_command_ = R"( - + mock_components/GenericSystem true @@ -218,10 +210,10 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_sensor_mock_command_True_ = R"( - + mock_components/GenericSystem - True + True @@ -247,16 +239,17 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_with_mimic_joint_ = R"( - + mock_components/GenericSystem - + + 1.57 + - 1.57 joint1 @@ -271,7 +264,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_offset_ = R"( - + mock_components/GenericSystem -3 @@ -297,7 +290,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_ = R"( - + mock_components/GenericSystem -3 @@ -306,23 +299,29 @@ class TestGenericSystem : public ::testing::Test - - - 3.45 + + 3.45 + + + 0.0 + - - - 2.78 + + 2.78 + + + 0.0 + )"; hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_ = R"( - + mock_components/GenericSystem -3 @@ -331,25 +330,29 @@ class TestGenericSystem : public ::testing::Test - + + 3.45 + - 3.45 - - + + 2.78 + + + 0.0 + - 2.78 )"; valid_urdf_ros2_control_system_robot_with_gpio_ = R"( - + mock_components/GenericSystem 2 @@ -366,9 +369,10 @@ class TestGenericSystem : public ::testing::Test - + + 2.78 + - 2.78 @@ -385,7 +389,7 @@ class TestGenericSystem : public ::testing::Test valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ = R"( - + mock_components/GenericSystem true @@ -393,16 +397,18 @@ class TestGenericSystem : public ::testing::Test - + + 3.45 + - 3.45 - + + 2.78 + - 2.78 @@ -419,24 +425,26 @@ class TestGenericSystem : public ::testing::Test valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ = R"( - + mock_components/GenericSystem - True + True - + + 3.45 + - 3.45 - + + 2.78 + - 2.78 @@ -453,9 +461,9 @@ class TestGenericSystem : public ::testing::Test sensor_with_initial_value_ = R"( - + - fake_components/GenericSystem + mock_components/GenericSystem @@ -473,9 +481,9 @@ class TestGenericSystem : public ::testing::Test gpio_with_initial_value_ = R"( - + - fake_components/GenericSystem + mock_components/GenericSystem @@ -487,7 +495,7 @@ class TestGenericSystem : public ::testing::Test hardware_system_2dof_standard_interfaces_with_different_control_modes_ = R"( - + mock_components/GenericSystem true @@ -560,9 +568,9 @@ class TestGenericSystem : public ::testing::Test disabled_commands_ = R"( - + - fake_components/GenericSystem + mock_components/GenericSystem True @@ -577,9 +585,7 @@ class TestGenericSystem : public ::testing::Test )"; } - std::string hardware_robot_2dof_; std::string hardware_system_2dof_; - std::string hardware_fake_system_2dof_; std::string hardware_system_2dof_asymetric_; std::string hardware_system_2dof_standard_interfaces_; std::string hardware_system_2dof_with_other_interface_; @@ -611,7 +617,6 @@ class TestableResourceManager : public hardware_interface::ResourceManager public: friend TestGenericSystem; - FRIEND_TEST(TestGenericSystem, generic_fake_system_2dof_symetric_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_symetric_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_asymetric_interfaces); FRIEND_TEST(TestGenericSystem, generic_system_2dof_other_interfaces); @@ -677,37 +682,6 @@ TEST_F(TestGenericSystem, load_generic_system_2dof) ASSERT_NO_THROW(TestableResourceManager rm(urdf)); } -// REMOVE THIS TEST ONCE FAKE COMPONENTS ARE REMOVED -TEST_F(TestGenericSystem, generic_fake_system_2dof_symetric_interfaces) -{ - auto urdf = ros2_control_test_assets::urdf_head + hardware_fake_system_2dof_ + - ros2_control_test_assets::urdf_tail; - TestableResourceManager rm(urdf); - // Activate components to get all interfaces available - activate_components(rm); - - // Check interfaces - EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_EQ(2u, rm.state_interface_keys().size()); - EXPECT_TRUE(rm.state_interface_exists("joint1/position")); - EXPECT_TRUE(rm.state_interface_exists("joint2/position")); - - ASSERT_EQ(2u, rm.command_interface_keys().size()); - EXPECT_TRUE(rm.command_interface_exists("joint1/position")); - EXPECT_TRUE(rm.command_interface_exists("joint2/position")); - - // Check initial values - hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); - hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); - hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); - hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface("joint2/position"); - - ASSERT_EQ(1.57, j1p_s.get_value()); - ASSERT_EQ(0.7854, j2p_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j2p_c.get_value())); -} - // Test inspired by hardware_interface/test_resource_manager.cpp TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) { @@ -715,7 +689,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm); + activate_components(rm, {"HardwareSystem2dof"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -746,7 +720,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm); + activate_components(rm, {"HardwareSystem2dofAsymetric"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -782,30 +756,30 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) hardware_interface::LoanedCommandInterface j2a_c = rm.claim_command_interface("joint2/acceleration"); - ASSERT_EQ(0.0, j1v_s.get_value()); + ASSERT_EQ(1.57, j1v_s.get_value()); ASSERT_EQ(0.7854, j2p_s.get_value()); ASSERT_TRUE(std::isnan(j1p_c.get_value())); ASSERT_TRUE(std::isnan(j2a_c.get_value())); } -void generic_system_functional_test(const std::string & urdf, const double offset = 0) +void generic_system_functional_test( + const std::string & urdf, const std::string component_name = "GenericSystem2dof", + const double offset = 0) { TestableResourceManager rm(urdf); // check is hardware is configured auto status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); - configure_components(rm); + configure_components(rm, {component_name}); status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); - activate_components(rm); + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + activate_components(rm, {component_name}); status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), - hardware_interface::lifecycle_state_names::ACTIVE); + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -881,11 +855,10 @@ void generic_system_functional_test(const std::string & urdf, const double offse ASSERT_EQ(0.77, j2p_c.get_value()); ASSERT_EQ(0.88, j2v_c.get_value()); - deactivate_components(rm); + deactivate_components(rm, {component_name}); status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); } TEST_F(TestGenericSystem, generic_system_2dof_functionality) @@ -893,7 +866,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_ + ros2_control_test_assets::urdf_tail; - generic_system_functional_test(urdf); + generic_system_functional_test(urdf, {"HardwareSystem2dofStandardInterfaces"}); } TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) @@ -902,7 +875,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm); + activate_components(rm, {"HardwareSystem2dofWithOtherInterface"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -985,7 +958,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm); + activate_components(rm, {"HardwareSystem2dofWithSensor"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1079,11 +1052,12 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ASSERT_EQ(0.33, j2p_c.get_value()); } -void TestGenericSystem::test_generic_system_with_mock_sensor_commands(std::string & urdf) +void TestGenericSystem::test_generic_system_with_mock_sensor_commands( + std::string & urdf, const std::string & component_name) { TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm); + activate_components(rm, {component_name}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1206,7 +1180,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_mock_command_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_sensor_commands(urdf); + test_generic_system_with_mock_sensor_commands(urdf, "HardwareSystem2dofWithSensorMockCommand"); } TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) @@ -1215,14 +1189,16 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) hardware_system_2dof_with_sensor_mock_command_True_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_sensor_commands(urdf); + test_generic_system_with_mock_sensor_commands( + urdf, "HardwareSystem2dofWithSensorMockCommandTrue"); } -void TestGenericSystem::test_generic_system_with_mimic_joint(std::string & urdf) +void TestGenericSystem::test_generic_system_with_mimic_joint( + std::string & urdf, const std::string & component_name) { TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm); + activate_components(rm, {component_name}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1289,7 +1265,7 @@ TEST_F(TestGenericSystem, hardware_system_2dof_with_mimic_joint) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_mimic_joint_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mimic_joint(urdf); + test_generic_system_with_mimic_joint(urdf, "HardwareSystem2dofWithMimicJoint"); } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset) @@ -1298,7 +1274,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset) hardware_system_2dof_standard_interfaces_with_offset_ + ros2_control_test_assets::urdf_tail; - generic_system_functional_test(urdf, -3); + generic_system_functional_test(urdf, "HardwareSystem2dofStandardInterfacesWithOffset", -3); } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_interface_missing) @@ -1308,7 +1284,8 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ros2_control_test_assets::urdf_tail; // custom interface is missing so offset will not be applied - generic_system_functional_test(urdf, 0.0); + generic_system_functional_test( + urdf, "HardwareSystem2dofStandardInterfacesWithCustomInterfaceForOffsetMissing", 0.0); } TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_interface) @@ -1321,22 +1298,23 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i TestableResourceManager rm(urdf); + const std::string hardware_name = + "HardwareSystem2dofStandardInterfacesWithCustomInterfaceForOffset"; + // check is hardware is configured auto status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); - configure_components(rm); + configure_components(rm, {hardware_name}); status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); - activate_components(rm); + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + activate_components(rm, {hardware_name}); status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), - hardware_interface::lifecycle_state_names::ACTIVE); + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); @@ -1422,11 +1400,10 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i ASSERT_EQ(0.77, j2p_c.get_value()); ASSERT_EQ(0.88, j2v_c.get_value()); - deactivate_components(rm); + deactivate_components(rm, {hardware_name}); status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); } TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) @@ -1435,21 +1412,21 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) valid_urdf_ros2_control_system_robot_with_gpio_ + ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); + const std::string hardware_name = "ValidURDFros2controlSystemRobotWithGPIO"; + // check is hardware is started auto status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); - configure_components(rm); + configure_components(rm, {hardware_name}); status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); - activate_components(rm); + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + activate_components(rm, {hardware_name}); status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), - hardware_interface::lifecycle_state_names::ACTIVE); + status_map[hardware_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); ASSERT_EQ(8u, rm.state_interface_keys().size()); ASSERT_EQ(6u, rm.command_interface_keys().size()); @@ -1524,28 +1501,27 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) ASSERT_EQ(0.444, gpio2_vac_c.get_value()); // check other functionalities are working well - generic_system_functional_test(urdf); + generic_system_functional_test(urdf, hardware_name); } -void TestGenericSystem::test_generic_system_with_mock_gpio_commands(std::string & urdf) +void TestGenericSystem::test_generic_system_with_mock_gpio_commands( + std::string & urdf, const std::string & component_name) { TestableResourceManager rm(urdf); // check is hardware is started auto status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::UNCONFIGURED); - configure_components(rm); + configure_components(rm, {component_name}); status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), - hardware_interface::lifecycle_state_names::INACTIVE); - activate_components(rm); + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::INACTIVE); + activate_components(rm, {component_name}); status_map = rm.get_components_status(); EXPECT_EQ( - status_map["GenericSystem2dof"].state.label(), - hardware_interface::lifecycle_state_names::ACTIVE); + status_map[component_name].state.label(), hardware_interface::lifecycle_state_names::ACTIVE); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1641,7 +1617,8 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co valid_urdf_ros2_control_system_robot_with_gpio_mock_command_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_gpio_commands(urdf); + test_generic_system_with_mock_gpio_commands( + urdf, "ValidURDFros2controlSystemRobotWithGPIOMockCommand"); } TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True) @@ -1650,7 +1627,8 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_mock_co valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_ + ros2_control_test_assets::urdf_tail; - test_generic_system_with_mock_gpio_commands(urdf); + test_generic_system_with_mock_gpio_commands( + urdf, "ValidURDFros2controlSystemRobotWithGPIOMockCommandTrue"); } TEST_F(TestGenericSystem, sensor_with_initial_value) @@ -1659,7 +1637,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm); + activate_components(rm, {"SensorWithInitialValue"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1687,7 +1665,7 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm); + activate_components(rm, {"GPIOWithInitialValue"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1708,7 +1686,7 @@ TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm); + activate_components(rm, {"HardwareSystem2dofStandardInterfacesWithDifferentControlModes"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); @@ -1902,7 +1880,7 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ros2_control_test_assets::urdf_head + disabled_commands_ + ros2_control_test_assets::urdf_tail; TestableResourceManager rm(urdf); // Activate components to get all interfaces available - activate_components(rm); + activate_components(rm, {"DisabledCommands"}); // Check interfaces EXPECT_EQ(1u, rm.system_components_size()); From 75e7efd1ae2ce5316114f73345b182f600717aa3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 14 Nov 2023 07:57:14 +0100 Subject: [PATCH 11/15] Fix the controller sorting bug when the interfaces are not configured (fixes #1164) (#1165) --- controller_manager/src/controller_manager.cpp | 13 ++++++++++ .../test_chainable_controller.cpp | 24 +++++++++++++++++-- .../test/test_controller/test_controller.cpp | 24 +++++++++++++++++-- 3 files changed, 57 insertions(+), 4 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b6c277126e..e61e3c769b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -148,6 +148,9 @@ std::vector get_following_controller_names( return following_controllers; } + // 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) { @@ -206,6 +209,8 @@ 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; auto cmd_itfs = ctrl.c->command_interface_configuration().names; for (const auto & itf : cmd_itfs) { @@ -2436,6 +2441,14 @@ bool ControllerManager::controller_sorting( const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, const std::vector & controllers) { + // If the neither of the controllers are configured, then return false + 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; + return false; + } + const std::vector cmd_itfs = ctrl_a.c->command_interface_configuration().names; const std::vector state_itfs = ctrl_a.c->state_interface_configuration().names; if (cmd_itfs.empty() || !ctrl_a.c->is_chainable()) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index b02c2e65ef..d21957a0b4 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -32,13 +32,33 @@ TestChainableController::TestChainableController() controller_interface::InterfaceConfiguration TestChainableController::command_interface_configuration() const { - return cmd_iface_cfg_; + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return cmd_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get command interface configuration until the controller is configured."); + } } controller_interface::InterfaceConfiguration TestChainableController::state_interface_configuration() const { - return state_iface_cfg_; + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return state_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get state interface configuration until the controller is configured."); + } } controller_interface::return_type TestChainableController::update_reference_from_subscribers( diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 97670e9a7a..06f76bd044 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -31,12 +31,32 @@ TestController::TestController() controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { - return cmd_iface_cfg_; + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return cmd_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get command interface configuration until the controller is configured."); + } } controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { - return state_iface_cfg_; + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return state_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get state interface configuration until the controller is configured."); + } } controller_interface::return_type TestController::update( From cf405bc3d3ad6c999a35a98faccf871a9b00c008 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 14 Nov 2023 06:58:11 +0000 Subject: [PATCH 12/15] Bump ros-tooling/action-ros-ci from 0.3.4 to 0.3.5 (#1163) Bumps [ros-tooling/action-ros-ci](https://github.com/ros-tooling/action-ros-ci) from 0.3.4 to 0.3.5. - [Release notes](https://github.com/ros-tooling/action-ros-ci/releases) - [Commits](https://github.com/ros-tooling/action-ros-ci/compare/0.3.4...0.3.5) --- updated-dependencies: - dependency-name: ros-tooling/action-ros-ci dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- .github/workflows/reusable-ros-tooling-source-build.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index ca0068ad88..7bff105e56 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 160bd3cde1..c170922683 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v4 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.4 + - uses: ros-tooling/action-ros-ci@0.3.5 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package From 2056d6c2152db3d03cb8af8daf739ca194302f11 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 20 Nov 2023 19:05:02 +0100 Subject: [PATCH 13/15] 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 14/15] 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 15/15] 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