diff --git a/.clang-format b/.clang-format index 698bda7da8..10dd2c7c7a 100644 --- a/.clang-format +++ b/.clang-format @@ -12,4 +12,5 @@ DerivePointerAlignment: false PointerAlignment: Middle ReflowComments: true IncludeBlocks: Preserve +InsertBraces: true ... diff --git a/.github/ISSUE_TEMPLATE/good-first-issue.md b/.github/ISSUE_TEMPLATE/good-first-issue.md index 57779881d2..ececdae62c 100644 --- a/.github/ISSUE_TEMPLATE/good-first-issue.md +++ b/.github/ISSUE_TEMPLATE/good-first-issue.md @@ -55,6 +55,6 @@ Don’t hesitate to ask questions or to get help if you feel like you are gettin Furthermore, you find helpful resources here: * [ROS2 Control Contribution Guide](https://control.ros.org/master/doc/contributing/contributing.html) * [ROS2 Tutorials](https://docs.ros.org/en/rolling/Tutorials.html) -* [ROS Answers](https://answers.ros.org/questions/) +* [Robotics Stack Exchange](https://robotics.stackexchange.com) **Good luck with your first issue!** diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 05a48fc654..aafd67c236 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -11,3 +11,17 @@ updates: directory: "/" schedule: interval: "weekly" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "humble" + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" + target-branch: "iron" diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index 84b156f5a1..c6580eacd4 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -13,28 +13,21 @@ groups: - name: reviewers reviewers: 5 usernames: - - rosterloh - - progtologist + - aprotyas - arne48 + - bijoua29 - christophfroehlich - DasRoteSkelett - - sgmurray - - harderthan - - jaron-l - - malapatiravi + - duringhof - erickisos - - sachinkum0009 - - qiayuanliao - - homalozoa - - anfemosa - - jackcenter - - VX792 - - mhubii + - fmauch + - jaron-l - livanov93 - - aprotyas + - mcbed + - moriarty + - olivier-stasse - peterdavidfagan - - duringhof + - progtologist + - saikishor - VanshGehlot - - bijoua29 - - LukasMacha97 - - mcbed + - VX792 diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 7bff105e56..203fc4b6fc 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -17,7 +17,7 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 2e1400f718..c7199b88c5 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4.7.1 + - uses: actions/setup-python@v5.0.0 with: python-version: '3.10' - name: Install system hooks diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 3efd557adc..84c68217f3 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -14,7 +14,7 @@ jobs: AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -38,7 +38,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index c170922683..879946fced 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,7 +26,7 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.7.0 + - uses: ros-tooling/setup-ros@0.7.1 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v4 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 0ad4d0aa6c..6e6d41cc9c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -62,7 +62,7 @@ repos: # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.6 + rev: v15.0.6 hooks: - id: clang-format diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 5aa2ad0647..27db0c351d 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ +* Add few warning compiler options to error (`#1181 `_) +* [ControllerInterface] Avoid warning about conversion from `int64_t` to `unsigned int` (`#1173 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +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 + 3.21.0 (2023-11-06) ------------------- diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 88a54d5c7a..d02c422451 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(controller_interface LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/controller_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/package.xml b/controller_interface/package.xml index 6a2a2513ee..2d0b13727b 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.21.0 + 4.2.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index fcbc28590a..84cd0d5e2b 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) @@ -85,7 +85,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() // Other solution is to add check into the LifecycleNode if a transition is valid to trigger if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - update_rate_ = get_node()->get_parameter("update_rate").as_int(); + update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); is_async_ = get_node()->get_parameter("is_async").as_bool(); } 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/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 0d021492d9..0ddc9cfe68 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.2.0 (2023-12-12) +------------------ +* [CM] Linting if/else statements (`#1193 `_) +* Reformat with braces added (`#1209 `_) +* Report inactive controllers as a diagnostics ok instead of an error (`#1184 `_) +* Fix controller sorting issue while loading large number of controllers (`#1180 `_) +* Contributors: Bence Magyar, Dr. Denis, Lennart Nachtigall, Sai Kishor Kothakota + +4.1.0 (2023-11-30) +------------------ +* Add few warning compiler options to error (`#1181 `_) +* [ControllerManager] Fix all warnings from the latets features. (`#1174 `_) +* Compute the actual update period for each controller (`#1140 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + +4.0.0 (2023-11-21) +------------------ +* Pass controller manager update rate on the init of the controller interface (`#1141 `_) +* 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/CMakeLists.txt b/controller_manager/CMakeLists.txt index 77ba823e1f..41a6b18306 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(controller_manager LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 5cef7c9577..7be9aac597 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.21.0 + 4.2.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e61e3c769b..da7815d51e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -150,7 +150,9 @@ std::vector get_following_controller_names( } // If the controller is not configured, return empty if (!(is_controller_active(controller_it->c) || is_controller_inactive(controller_it->c))) + { return following_controllers; + } const auto cmd_itfs = controller_it->c->command_interface_configuration().names; for (const auto & itf : cmd_itfs) { @@ -210,7 +212,10 @@ std::vector get_preceding_controller_names( for (const auto & ctrl : controllers) { // If the controller is not configured, then continue - if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) continue; + if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) + { + continue; + } auto cmd_itfs = ctrl.c->command_interface_configuration().names; for (const auto & itf : cmd_itfs) { @@ -442,8 +447,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript "Parameter 'activate_components_on_start' is deprecated. " "Components are activated per default. Don't use this parameters in combination with the new " "'hardware_components_initial_state' parameter structure."); - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); for (const auto & component : activate_components_on_start) { resource_manager_->set_component_state(component, active_state); @@ -455,8 +458,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript // activate all other components for (const auto & [component, state] : components_to_activate) { - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); resource_manager_->set_component_state(component, active_state); } } @@ -741,7 +742,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(), @@ -781,7 +782,7 @@ controller_interface::return_type ControllerManager::configure_controller( to = from; // Reordering the controllers - std::sort( + std::stable_sort( to.begin(), to.end(), std::bind( &ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2, @@ -940,7 +941,7 @@ controller_interface::return_type ControllerManager::switch_controller( auto controller_it = std::find_if( controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); - controller_interface::return_type ret = controller_interface::return_type::OK; + controller_interface::return_type status = controller_interface::return_type::OK; // if controller is not inactive then do not do any following-controllers checks if (!is_controller_inactive(controller_it->c)) @@ -950,14 +951,14 @@ controller_interface::return_type ControllerManager::switch_controller( "Controller with name '%s' is not inactive so its following" "controllers do not have to be checked, because it cannot be activated.", controller_it->info.name.c_str()); - ret = controller_interface::return_type::ERROR; + status = controller_interface::return_type::ERROR; } else { - ret = check_following_controllers_for_activate(controllers, strictness, controller_it); + status = check_following_controllers_for_activate(controllers, strictness, controller_it); } - if (ret != controller_interface::return_type::OK) + if (status != controller_interface::return_type::OK) { RCLCPP_WARN( get_logger(), @@ -991,7 +992,7 @@ controller_interface::return_type ControllerManager::switch_controller( auto controller_it = std::find_if( controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); - controller_interface::return_type ret = controller_interface::return_type::OK; + controller_interface::return_type status = controller_interface::return_type::OK; // if controller is not active then skip preceding-controllers checks if (!is_controller_active(controller_it->c)) @@ -999,14 +1000,14 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_WARN( get_logger(), "Controller with name '%s' can not be deactivated since it is not active.", controller_it->info.name.c_str()); - ret = controller_interface::return_type::ERROR; + status = controller_interface::return_type::ERROR; } else { - ret = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); + status = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); } - if (ret != controller_interface::return_type::OK) + if (status != controller_interface::return_type::OK) { RCLCPP_WARN( get_logger(), @@ -1087,11 +1088,11 @@ controller_interface::return_type ControllerManager::switch_controller( // check for double stop if (!is_active && in_deactivate_list) { - auto ret = handle_conflict( + auto conflict_status = handle_conflict( "Could not deactivate controller '" + controller.info.name + "' since it is not active"); - if (ret != controller_interface::return_type::OK) + if (conflict_status != controller_interface::return_type::OK) { - return ret; + return conflict_status; } in_deactivate_list = false; deactivate_request_.erase(deactivate_list_it); @@ -1100,11 +1101,11 @@ controller_interface::return_type ControllerManager::switch_controller( // check for doubled activation if (is_active && !in_deactivate_list && in_activate_list) { - auto ret = handle_conflict( + auto conflict_status = handle_conflict( "Could not activate controller '" + controller.info.name + "' since it is already active"); - if (ret != controller_interface::return_type::OK) + if (conflict_status != controller_interface::return_type::OK) { - return ret; + return conflict_status; } in_activate_list = false; activate_request_.erase(activate_list_it); @@ -1113,21 +1114,21 @@ controller_interface::return_type ControllerManager::switch_controller( // check for illegal activation of an unconfigured/finalized controller if (!is_inactive && !in_deactivate_list && in_activate_list) { - auto ret = handle_conflict( + auto conflict_status = handle_conflict( "Could not activate controller '" + controller.info.name + "' since it is not in inactive state"); - if (ret != controller_interface::return_type::OK) + if (conflict_status != controller_interface::return_type::OK) { - return ret; + return conflict_status; } in_activate_list = false; activate_request_.erase(activate_list_it); } const auto extract_interfaces_for_controller = - [this](const ControllerSpec controller, std::vector & request_interface_list) + [this](const ControllerSpec ctrl, std::vector & request_interface_list) { - auto command_interface_config = controller.c->command_interface_configuration(); + auto command_interface_config = ctrl.c->command_interface_configuration(); std::vector command_interface_names = {}; if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { @@ -1296,7 +1297,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(); @@ -1766,8 +1768,8 @@ void ControllerManager::reload_controller_libraries_service_cb( loaded_controllers = get_controller_names(); { // lock controllers - std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); - for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) + std::lock_guard ctrl_guard(rt_controllers_wrapper_.controllers_lock_); + for (const auto & controller : rt_controllers_wrapper_.get_updated_list(ctrl_guard)) { if (is_controller_active(*controller.c)) { @@ -2045,8 +2047,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)); @@ -2063,7 +2064,9 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - auto controller_ret = loaded_controller.c->update(time, controller_period); + const auto controller_actual_period = + (time - *loaded_controller.next_update_cycle_time) + controller_period; + auto controller_ret = loaded_controller.c->update(time, controller_actual_period); if ( *loaded_controller.next_update_cycle_time == @@ -2445,7 +2448,10 @@ bool ControllerManager::controller_sorting( if (!((is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) && (is_controller_active(ctrl_b.c) || is_controller_inactive(ctrl_b.c)))) { - if (is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) return true; + if (is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) + { + return true; + } return false; } @@ -2455,17 +2461,30 @@ bool ControllerManager::controller_sorting( { // The case of the controllers that don't have any command interfaces. For instance, // joint_state_broadcaster - return true; + // If the controller b is also under the same condition, then maintain their initial order + const auto command_interfaces_exist = + !ctrl_b.c->command_interface_configuration().names.empty(); + return ctrl_b.c->is_chainable() && command_interfaces_exist; + } + else if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable()) + { + // If only the controller b is a broadcaster or non chainable type , then swap the controllers + return false; } else { auto following_ctrls = get_following_controller_names(ctrl_a.info.name, controllers); - if (following_ctrls.empty()) return false; + if (following_ctrls.empty()) + { + return false; + } // If the ctrl_b is any of the following controllers of ctrl_a, then place ctrl_a before ctrl_b if ( std::find(following_ctrls.begin(), following_ctrls.end(), ctrl_b.info.name) != following_ctrls.end()) + { return true; + } else { auto ctrl_a_preceding_ctrls = get_preceding_controller_names(ctrl_a.info.name, controllers); @@ -2498,21 +2517,27 @@ bool ControllerManager::controller_sorting( // If there is no common parent, then they belong to 2 different sets auto following_ctrls_b = get_following_controller_names(ctrl_b.info.name, controllers); - if (following_ctrls_b.empty()) return true; - auto find_first_element = [&](const auto & controllers_list) + if (following_ctrls_b.empty()) + { + return true; + } + auto find_first_element = [&](const auto & controllers_list) -> size_t { auto it = std::find_if( controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, controllers_list.back())); if (it != controllers.end()) { - int dist = std::distance(controllers.begin(), it); - return dist; + return std::distance(controllers.begin(), it); } + return 0; }; - const int ctrl_a_chain_first_controller = find_first_element(following_ctrls); - const int ctrl_b_chain_first_controller = find_first_element(following_ctrls_b); - if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller) return true; + const auto ctrl_a_chain_first_controller = find_first_element(following_ctrls); + const auto ctrl_b_chain_first_controller = find_first_element(following_ctrls_b); + if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller) + { + return true; + } } // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be @@ -2557,7 +2582,7 @@ void ControllerManager::controller_activity_diagnostic_callback( } else { - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Not all controllers are active"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not all controllers are active"); } } diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 06f76bd044..7585ae36e5 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -60,8 +60,9 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con } controller_interface::return_type TestController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + update_period_ = period.seconds(); ++internal_counter; // set value to hardware to produce and test different behaviors there diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index d7b318c597..14ad753803 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -80,6 +80,7 @@ class TestController : public controller_interface::ControllerInterface // enables external setting of values to command interfaces - used for simulation of hardware // errors double set_first_command_interface_value_to; + double update_period_ = 0; }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp index ba762573c7..6a6f954e87 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp @@ -33,7 +33,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() diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 8769a24749..e88b41f222 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -351,6 +351,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; + time_ += rclcpp::Duration::from_seconds(0.01); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -365,11 +366,17 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd const auto pre_internal_counter = test_controller->internal_counter; rclcpp::Rate loop_rate(cm_->get_update_rate()); + const auto cm_update_rate = cm_->get_update_rate(); for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++) { + time_ += rclcpp::Duration::from_seconds(0.01); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + // In case of a non perfect divisor, the update period should respect the rule + // [controller_update_rate, 2*controller_update_rate) + ASSERT_GE(test_controller->update_period_, 1.0 / cm_update_rate); + ASSERT_LT(test_controller->update_period_, 2.0 / cm_update_rate); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -448,6 +455,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) << "switch_controller should be blocking until next update cycle"; + time_ += rclcpp::Duration::from_seconds(0.01); EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); @@ -470,20 +478,26 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + // In case of a non perfect divisor, the update period should respect the rule + // [controller_update_rate, 2*controller_update_rate) + ASSERT_GE(test_controller->update_period_, 1.0 / controller_update_rate); + ASSERT_LT(test_controller->update_period_, 2.0 / controller_update_rate); time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0) { - const auto no_of_secs_passed = update_counter / cm_update_rate; + const double no_of_secs_passed = static_cast(update_counter) / cm_update_rate; // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it // is clearly tracking, so adding 1 here won't affect the final count. // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 // cycles it will have 369 instead of 370. - EXPECT_NEAR( + EXPECT_THAT( test_controller->internal_counter - initial_counter, - (controller_update_rate * no_of_secs_passed), 1); + testing::AnyOf( + testing::Eq(controller_update_rate * no_of_secs_passed), + testing::Eq((controller_update_rate * no_of_secs_passed) - 1))); } } } diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 773fbd94d8..8882d0a8de 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -639,7 +639,9 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) {TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_1, test_controller::TEST_CONTROLLER_NAME}) + { cm_->configure_controller(controller); + } // get controller list after configure result = call_service_and_wait(*client, request, srv_executor); @@ -796,7 +798,9 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) {TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_1, TEST_CHAINED_CONTROLLER_6, test_controller::TEST_CONTROLLER_NAME}) + { cm_->configure_controller(controller); + } // get controller list after configure result = call_service_and_wait(*client, request, srv_executor); @@ -807,7 +811,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7); ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_6); - auto get_ctrl_pos = [result](const std::string & controller_name) -> int + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t { auto it = std::find_if( result->controller.begin(), result->controller.end(), @@ -1010,13 +1014,293 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) TEST_CHAINED_CONTROLLER_4, TEST_CONTROLLER_2, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_8}; - for (const auto & controller : ctrls_order) cm_->configure_controller(controller); + for (const auto & controller : ctrls_order) + { + cm_->configure_controller(controller); + } // get controller list after configure result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(10u, result->controller.size()); - auto get_ctrl_pos = [result](const std::string & controller_name) -> int + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t + { + auto it = std::find_if( + result->controller.begin(), result->controller.end(), + [controller_name](auto itf) + { return (itf.name.find(controller_name) != std::string::npos); }); + return std::distance(result->controller.begin(), it); + }; + auto ctrl_chain_1_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_1); + auto ctrl_chain_2_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_2); + auto ctrl_chain_3_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_3); + auto ctrl_chain_4_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_4); + auto ctrl_chain_5_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_5); + auto ctrl_chain_6_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_6); + auto ctrl_chain_7_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_7); + auto ctrl_chain_8_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_8); + + auto ctrl_1_pos = get_ctrl_pos(TEST_CONTROLLER_1); + auto ctrl_2_pos = get_ctrl_pos(TEST_CONTROLLER_2); + + // Extra check to see that they are indexed after their parent controller + // first chain + ASSERT_GT(ctrl_chain_1_pos, ctrl_chain_2_pos); + ASSERT_GT(ctrl_chain_2_pos, ctrl_chain_3_pos); + ASSERT_GT(ctrl_chain_3_pos, ctrl_1_pos); + + // second tree + ASSERT_GT(ctrl_chain_4_pos, ctrl_chain_5_pos); + ASSERT_GT(ctrl_chain_5_pos, ctrl_chain_6_pos); + ASSERT_GT(ctrl_chain_6_pos, ctrl_2_pos); + + // third tree + ASSERT_GT(ctrl_chain_7_pos, ctrl_chain_8_pos); +} + +TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) +{ + /// The simulated controller chaining is: + /// test_controller_name_1 -> chain_ctrl_3 -> chain_ctrl_2 -> chain_ctrl_1 + /// && + /// test_controller_name_2 -> chain_ctrl_6 -> chain_ctrl_5 -> chain_ctrl_4 + /// && + /// test_controller_name_7 -> test_controller_name_8 + /// && + /// There are 100 more other basic controllers and 100 more different broadcasters to check for + /// crashing + /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces + /// exported from the controller B (or) the controller B is utilizing the expected interfaces + /// exported from the controller A + + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of chained controllers + static constexpr char TEST_CHAINED_CONTROLLER_1[] = "test_chainable_controller_name_1"; + static constexpr char TEST_CHAINED_CONTROLLER_2[] = "test_chainable_controller_name_2"; + static constexpr char TEST_CHAINED_CONTROLLER_3[] = "test_chainable_controller_name_3"; + static constexpr char TEST_CHAINED_CONTROLLER_4[] = "test_chainable_controller_name_4"; + static constexpr char TEST_CHAINED_CONTROLLER_5[] = "test_chainable_controller_name_5"; + static constexpr char TEST_CHAINED_CONTROLLER_6[] = "test_chainable_controller_name_6"; + static constexpr char TEST_CHAINED_CONTROLLER_7[] = "test_chainable_controller_name_7"; + static constexpr char TEST_CHAINED_CONTROLLER_8[] = "test_chainable_controller_name_8"; + static constexpr char TEST_CHAINED_CONTROLLER_9[] = "test_chainable_controller_name_9"; + static constexpr char TEST_CONTROLLER_1[] = "test_controller_name_1"; + static constexpr char TEST_CONTROLLER_2[] = "test_controller_name_2"; + + // First chain + auto test_chained_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_chained_controller_1->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_1->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_1->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_2 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_1) + "/joint1/position"}}; + test_chained_controller_2->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_2->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_2->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_3 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_2) + "/joint1/position"}}; + test_chained_controller_3->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_3->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_3->set_reference_interface_names({"joint1/position", "joint1/velocity"}); + + auto test_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_1->set_command_interface_configuration(cmd_cfg); + test_controller_1->set_state_interface_configuration(state_cfg); + + // Second chain + auto test_chained_controller_4 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + test_chained_controller_4->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_4->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_4->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_5 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_4) + "/joint2/velocity"}}; + test_chained_controller_5->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_5->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_5->set_reference_interface_names({"joint2/velocity"}); + + auto test_chained_controller_6 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_5) + "/joint2/velocity"}}; + test_chained_controller_6->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_6->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_6->set_reference_interface_names({"joint2/velocity"}); + + auto test_controller_2 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_6) + "/joint2/velocity"}}; + state_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}}; + test_controller_2->set_command_interface_configuration(cmd_cfg); + test_controller_2->set_state_interface_configuration(state_cfg); + + // Third chain + auto test_chained_controller_7 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint3/velocity"}}; + test_chained_controller_7->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_7->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_7->set_reference_interface_names({"joint3/velocity"}); + + auto test_chained_controller_8 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_7) + "/joint3/velocity"}}; + test_chained_controller_8->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_8->set_state_interface_configuration(chained_state_cfg); + test_chained_controller_8->set_reference_interface_names({"joint3/velocity"}); + + auto test_chained_controller_9 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/max_acceleration"}}; + test_chained_controller_9->set_command_interface_configuration(chained_cmd_cfg); + test_chained_controller_9->set_state_interface_configuration(state_cfg); + + unsigned int num_of_random_broadcasters = 100; + unsigned int num_of_random_controllers = 100; + std::vector chained_ref_interfaces; + for (size_t i = 0; i < num_of_random_controllers; i++) + { + chained_ref_interfaces.push_back("ref_" + std::to_string(i) + "/joint_2/acceleration"); + } + test_chained_controller_9->set_reference_interface_names(chained_ref_interfaces); + std::unordered_map> random_controllers_list; + for (size_t i = 0; i < num_of_random_broadcasters; i++) + { + auto controller_name = "test_broadcaster_" + std::to_string(i); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + } + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_controllers_" + std::to_string(i); + RCLCPP_ERROR(srv_node->get_logger(), "Initializing controller : %s !", controller_name.c_str()); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + random_controllers_list[controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_9) + std::string("/ref_") + std::to_string(i) + + std::string("/joint_2/acceleration")}}); + } + + // add controllers + /// @todo add controllers in random order + /// For now, adding the ordered case to see that current sorting doesn't change order + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_1, TEST_CHAINED_CONTROLLER_1, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_5, TEST_CHAINED_CONTROLLER_5, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_8, TEST_CHAINED_CONTROLLER_8, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_9, TEST_CHAINED_CONTROLLER_9, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + { + ControllerManagerRunner cm_runner(this); + for (auto random_ctrl : random_controllers_list) + { + cm_->add_controller( + random_ctrl.second, random_ctrl.first, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + } + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + + // check chainable controller + ASSERT_EQ( + 11u + num_of_random_broadcasters + num_of_random_controllers, result->controller.size()); + EXPECT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_2); + EXPECT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_6); + EXPECT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_1); + EXPECT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_7); + EXPECT_EQ(result->controller[4].name, TEST_CONTROLLER_1); + + EXPECT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_5); + EXPECT_EQ(result->controller[6].name, TEST_CHAINED_CONTROLLER_3); + EXPECT_EQ(result->controller[7].name, TEST_CHAINED_CONTROLLER_4); + EXPECT_EQ(result->controller[8].name, TEST_CONTROLLER_2); + EXPECT_EQ(result->controller[9].name, TEST_CHAINED_CONTROLLER_8); + + // configure controllers + auto ctrls_order = { + TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_9, + TEST_CHAINED_CONTROLLER_1, TEST_CONTROLLER_1, TEST_CHAINED_CONTROLLER_4, + TEST_CONTROLLER_2, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_6, + TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_8}; + { + ControllerManagerRunner cm_runner(this); + for (const auto & controller : ctrls_order) + { + cm_->configure_controller(controller); + } + + for (auto random_ctrl : random_controllers_list) + { + cm_->configure_controller(random_ctrl.first); + } + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 11u + num_of_random_broadcasters + num_of_random_controllers, result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t { auto it = std::find_if( result->controller.begin(), result->controller.end(), @@ -1032,6 +1316,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) auto ctrl_chain_6_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_6); auto ctrl_chain_7_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_7); auto ctrl_chain_8_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_8); + auto ctrl_chain_9_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_9); auto ctrl_1_pos = get_ctrl_pos(TEST_CONTROLLER_1); auto ctrl_2_pos = get_ctrl_pos(TEST_CONTROLLER_2); @@ -1049,4 +1334,222 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) // third tree ASSERT_GT(ctrl_chain_7_pos, ctrl_chain_8_pos); + + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_controllers_" + std::to_string(i); + ASSERT_GT(ctrl_chain_9_pos, get_ctrl_pos(controller_name)); + } + RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); +} + +TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) +{ + /// The simulated controller chain is like every joint has its own controller exposing interfaces + /// and then a controller chain using those interfaces + /// + /// There are 20 more broadcasters + 20 more normal controllers for complexity + /// NOTE: A -> B signifies that the controller A is utilizing the reference interfaces exported + /// from the controller B (or) the controller B is utilizing the expected interfaces exported from + /// the controller A + + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + const unsigned int joints_count = 20; + + static constexpr char JOINT_CONTROLLER_PREFIX[] = "test_chainable_controller_name_joint_"; + static constexpr char FWD_CONTROLLER_PREFIX[] = "forward_controller_joint_"; + static constexpr char JOINT_SENSOR_BROADCASTER_PREFIX[] = "test_broadcaster_joint_"; + std::vector controllers_list; + std::vector fwd_joint_position_interfaces_list; + std::vector fwd_joint_velocity_interfaces_list; + std::vector fwd_joint_position_ref_interfaces_list; + std::vector fwd_joint_velocity_ref_interfaces_list; + std::unordered_map> + random_chainable_controllers_list; + std::unordered_map> random_controllers_list; + for (size_t i = 0; i < joints_count; i++) + { + controller_interface::InterfaceConfiguration chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/position"}}; + controller_interface::InterfaceConfiguration chained_state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/position", "joint" + std::to_string(i) + "/velocity"}}; + // Joint controller + const std::string controller_name = JOINT_CONTROLLER_PREFIX + std::to_string(i); + random_chainable_controllers_list[controller_name] = + std::make_shared(); + random_chainable_controllers_list[controller_name]->set_state_interface_configuration( + chained_state_cfg); + random_chainable_controllers_list[controller_name]->set_command_interface_configuration( + chained_cmd_cfg); + random_chainable_controllers_list[controller_name]->set_reference_interface_names( + chained_state_cfg.names); + controllers_list.push_back(controller_name); + + // Forward Joint interfaces controller + fwd_joint_position_interfaces_list.push_back( + std::string(controller_name) + "/joint" + std::to_string(i) + "/position"); + fwd_joint_velocity_interfaces_list.push_back( + std::string(controller_name) + "/joint" + std::to_string(i) + "/velocity"); + const std::string fwd_controller_name = FWD_CONTROLLER_PREFIX + std::to_string(i); + random_chainable_controllers_list[fwd_controller_name] = + std::make_shared(); + random_chainable_controllers_list[fwd_controller_name]->set_state_interface_configuration( + chained_state_cfg); + random_chainable_controllers_list[fwd_controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {fwd_joint_position_interfaces_list.back(), fwd_joint_velocity_interfaces_list.back()}}); + random_chainable_controllers_list[fwd_controller_name]->set_reference_interface_names( + chained_state_cfg.names); + fwd_joint_position_ref_interfaces_list.push_back( + std::string(fwd_controller_name) + "/" + fwd_joint_position_interfaces_list.back()); + fwd_joint_velocity_ref_interfaces_list.push_back( + std::string(fwd_controller_name) + "/" + fwd_joint_velocity_interfaces_list.back()); + controllers_list.push_back(fwd_controller_name); + + // Add a broadcaster for every joint assuming it as a sensor (just for the tests) + const std::string broadcaster_name = JOINT_SENSOR_BROADCASTER_PREFIX + std::to_string(i); + random_controllers_list[broadcaster_name] = std::make_shared(); + random_controllers_list[broadcaster_name]->set_state_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint" + std::to_string(i) + "/torque", "joint" + std::to_string(i) + "/torque"}}); + controllers_list.push_back(broadcaster_name); + } + + // create set of chained controllers + static constexpr char POSITION_REFERENCE_CONTROLLER[] = "position_reference_chainable_controller"; + static constexpr char VELOCITY_REFERENCE_CONTROLLER[] = "velocity_reference_chainable_controller"; + static constexpr char HIGHER_LEVEL_REFERENCE_CONTROLLER[] = "task_level_controller"; + + // Position reference controller + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER] = + std::make_shared(); + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER] + ->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + fwd_joint_position_ref_interfaces_list}); + random_chainable_controllers_list[POSITION_REFERENCE_CONTROLLER]->set_reference_interface_names( + {"joint/position"}); + controllers_list.push_back(POSITION_REFERENCE_CONTROLLER); + + // Velocity reference controller + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER] = + std::make_shared(); + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER] + ->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + fwd_joint_velocity_ref_interfaces_list}); + random_chainable_controllers_list[VELOCITY_REFERENCE_CONTROLLER]->set_reference_interface_names( + {"joint/velocity"}); + controllers_list.push_back(VELOCITY_REFERENCE_CONTROLLER); + + // Higher level task level controller + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER] = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(POSITION_REFERENCE_CONTROLLER) + "/joint/position", + std::string(VELOCITY_REFERENCE_CONTROLLER) + "/joint/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER]->set_command_interface_configuration( + cmd_cfg); + random_controllers_list[HIGHER_LEVEL_REFERENCE_CONTROLLER]->set_state_interface_configuration( + state_cfg); + controllers_list.push_back(HIGHER_LEVEL_REFERENCE_CONTROLLER); + + const unsigned int num_of_random_broadcasters = 20; + const unsigned int num_of_random_controllers = 20; + for (size_t i = 0; i < num_of_random_broadcasters; i++) + { + auto controller_name = "test_broadcaster_" + std::to_string(i); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + controllers_list.push_back(controller_name); + } + for (size_t i = 0; i < num_of_random_controllers; i++) + { + auto controller_name = "test_random_reference_controllers_" + std::to_string(i); + RCLCPP_ERROR(srv_node->get_logger(), "Initializing controller : %s !", controller_name.c_str()); + random_controllers_list[controller_name] = std::make_shared(); + random_controllers_list[controller_name]->set_state_interface_configuration(state_cfg); + random_controllers_list[controller_name]->set_command_interface_configuration( + {controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string("ref_") + std::to_string(i) + std::string("/joint_2/acceleration")}}); + controllers_list.push_back(controller_name); + } + + // Now shuffle the list to be able to configure controller later randomly + std::random_shuffle(controllers_list.begin(), controllers_list.end()); + + { + ControllerManagerRunner cm_runner(this); + for (auto random_chain_ctrl : random_chainable_controllers_list) + { + cm_->add_controller( + random_chain_ctrl.second, random_chain_ctrl.first, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + for (auto random_ctrl : random_controllers_list) + { + cm_->add_controller( + random_ctrl.second, random_ctrl.first, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + } + } + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + + // check chainable controller + ASSERT_EQ(controllers_list.size(), result->controller.size()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + for (auto random_ctrl : controllers_list) + { + cm_->configure_controller(random_ctrl); + } + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(controllers_list.size(), result->controller.size()); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t + { + auto it = std::find_if( + result->controller.begin(), result->controller.end(), + [controller_name](auto itf) + { return (itf.name.find(controller_name) != std::string::npos); }); + return std::distance(result->controller.begin(), it); + }; + + // Check the controller indexing + auto pos_ref_pos = get_ctrl_pos(POSITION_REFERENCE_CONTROLLER); + auto vel_ref_pos = get_ctrl_pos(VELOCITY_REFERENCE_CONTROLLER); + auto task_level_ctrl_pos = get_ctrl_pos(HIGHER_LEVEL_REFERENCE_CONTROLLER); + ASSERT_GT(vel_ref_pos, task_level_ctrl_pos); + ASSERT_GT(pos_ref_pos, task_level_ctrl_pos); + + for (size_t i = 0; i < joints_count; i++) + { + const std::string controller_name = JOINT_CONTROLLER_PREFIX + std::to_string(i); + const std::string fwd_controller_name = FWD_CONTROLLER_PREFIX + std::to_string(i); + + ASSERT_GT(get_ctrl_pos(fwd_controller_name), pos_ref_pos); + ASSERT_GT(get_ctrl_pos(fwd_controller_name), vel_ref_pos); + ASSERT_GT(get_ctrl_pos(controller_name), pos_ref_pos); + ASSERT_GT(get_ctrl_pos(controller_name), vel_ref_pos); + } + RCLCPP_INFO(srv_node->get_logger(), "Check successful!"); } diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index e6d5095d7e..dc07947f33 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ + +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..7ef4c5d9e8 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.21.0 + 4.2.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 4efd1a3686..04e3996d01 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ +* Add few warning compiler options to error (`#1181 `_) +* Contributors: Sai Kishor Kothakota + +4.0.0 (2023-11-21) +------------------ +* [MockHardware] Remove all deprecated options and deprecated plugins from the library. (`#1150 `_) +* 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/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 2108bab17c..bcd03a0a16 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(hardware_interface LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 7fd3c78b61..b3685a7ee2 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.21.0 + 4.2.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 7b62af105b..f4aee6c8a6 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -580,12 +580,12 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } else { - for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + for (size_t k = 0; k < joint_states_[POSITION_INTERFACE_INDEX].size(); ++k) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][k])) { - joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][j] + + joint_states_[POSITION_INTERFACE_INDEX][k] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][k] + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index c80cdbb5bf..8246cc207d 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -247,21 +247,23 @@ TEST_F(ResourceManagerTest, resource_claiming) // Activate components to get all interfaces available activate_components(rm); - const auto key = "joint1/position"; - EXPECT_TRUE(rm.command_interface_is_available(key)); - EXPECT_FALSE(rm.command_interface_is_claimed(key)); - { - auto position_command_interface = rm.claim_command_interface(key); + const auto key = "joint1/position"; EXPECT_TRUE(rm.command_interface_is_available(key)); - EXPECT_TRUE(rm.command_interface_is_claimed(key)); + EXPECT_FALSE(rm.command_interface_is_claimed(key)); + { - EXPECT_ANY_THROW(rm.claim_command_interface(key)); + auto position_command_interface = rm.claim_command_interface(key); EXPECT_TRUE(rm.command_interface_is_available(key)); + EXPECT_TRUE(rm.command_interface_is_claimed(key)); + { + EXPECT_ANY_THROW(rm.claim_command_interface(key)); + EXPECT_TRUE(rm.command_interface_is_available(key)); + } } + EXPECT_TRUE(rm.command_interface_is_available(key)); + EXPECT_FALSE(rm.command_interface_is_claimed(key)); } - EXPECT_TRUE(rm.command_interface_is_available(key)); - EXPECT_FALSE(rm.command_interface_is_claimed(key)); // command interfaces can only be claimed once for (const auto & key : diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index da8a09aae2..ab215f461d 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ +* Add few warning compiler options to error (`#1181 `_) +* Contributors: Sai Kishor Kothakota + +4.0.0 (2023-11-21) +------------------ + 3.21.0 (2023-11-06) ------------------- diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 82467514a3..a788b6de4f 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(joint_limits LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 928a6f26fd..e1832fa776 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.21.0 + 4.2.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 2ed4b9ec05..28a37f1134 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ + +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..e8413239c7 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.21.0 + 4.2.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 66d1983c54..c9200cf182 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ + +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..6d5e8ac9d9 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.2.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 2481e4b8c5..5c50a0b21a 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ + +4.0.0 (2023-11-21) +------------------ + 3.21.0 (2023-11-06) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 1a8aca9b51..8aa2b528a6 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.21.0 + 4.2.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 9a897fae0a..ff1df137ed 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.21.0", + version="4.2.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 23b78e1527..959f47c6f7 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ + +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..e08545b41f 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.21.0 + 4.2.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index d32300651e..9e9d9f763d 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.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 067c4353c8..6b0cd5173b 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.2.0 (2023-12-12) +------------------ + +4.1.0 (2023-11-30) +------------------ +* Add few warning compiler options to error (`#1181 `_) +* Contributors: Sai Kishor Kothakota + +4.0.0 (2023-11-21) +------------------ + 3.21.0 (2023-11-06) ------------------- diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index d4674366e9..efd8db1652 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(transmission_interface LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index b294e56d24..17a63cca0b 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.21.0 + 4.2.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl