diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index b339b45e66..1d506f3438 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -3,6 +3,9 @@ on: workflow_dispatch: branches: - master + push: + branches: + - master pull_request: branches: - master @@ -19,7 +22,7 @@ jobs: - uses: ros-tooling/setup-ros@0.7.0 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-tooling/action-ros-ci@0.3.3 with: target-ros2-distro: ${{ env.ROS_DISTRO }} @@ -49,7 +52,7 @@ jobs: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.2 + - uses: actions/upload-artifact@v3.1.3 with: name: colcon-logs-ubuntu-22.04-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 9b91657851..d1ea84222c 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,7 +12,7 @@ jobs: name: Format runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: actions/setup-python@v4.7.0 with: python-version: '3.10' diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 6428cd18a7..3efd557adc 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -5,13 +5,15 @@ on: jobs: ament_lint: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-latest strategy: fail-fast: false matrix: linter: [cppcheck, copyright, lint_cmake] + env: + AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@0.7.0 - uses: ros-tooling/action-ros-lint@v0.1 with: @@ -29,13 +31,13 @@ jobs: ament_lint_100: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-latest strategy: fail-fast: false matrix: linter: [cpplint] steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@0.7.0 - uses: ros-tooling/action-ros-lint@v0.1 with: diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 9f3858d59f..f5015d8a90 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -19,13 +19,13 @@ jobs: steps: - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 + uses: docker/setup-buildx-action@v3 - name: Checkout repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Log in to the Container registry - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: registry: ${{ env.REGISTRY }} username: ${{ github.actor }} @@ -33,7 +33,7 @@ jobs: - name: Docker meta id: meta - uses: docker/metadata-action@v4 + uses: docker/metadata-action@v5 with: images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}_release tags: | @@ -42,7 +42,7 @@ jobs: type=semver,pattern={{major}}.{{minor}} - name: Build and push Docker image - uses: docker/build-push-action@v4 + uses: docker/build-push-action@v5 with: context: . push: true @@ -59,13 +59,13 @@ jobs: steps: - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v2 + uses: docker/setup-buildx-action@v3 - name: Checkout repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Log in to the Container registry - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: registry: ${{ env.REGISTRY }} username: ${{ github.actor }} @@ -73,7 +73,7 @@ jobs: - name: Docker meta id: meta - uses: docker/metadata-action@v4 + uses: docker/metadata-action@v5 with: images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}_source tags: | @@ -82,7 +82,7 @@ jobs: type=semver,pattern={{major}}.{{minor}} - name: Build and push Docker image - uses: docker/build-push-action@v4 + uses: docker/build-push-action@v5 with: context: . push: true diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index 3f38d5b6ce..708ea5c1f4 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -11,7 +11,7 @@ jobs: abi_check: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: humble diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index 589eda663f..0267849692 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -21,7 +21,7 @@ jobs: ROS_DISTRO: humble container: ghcr.io/ros-controls/ros:humble-rhel steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: src/ros2_control - run: | diff --git a/.github/workflows/iron-abi-compatibility.yml b/.github/workflows/iron-abi-compatibility.yml index f92166b0c1..5bce340193 100644 --- a/.github/workflows/iron-abi-compatibility.yml +++ b/.github/workflows/iron-abi-compatibility.yml @@ -11,7 +11,7 @@ jobs: abi_check: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: ros-industrial/industrial_ci@master env: ROS_DISTRO: iron diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index afcdf332d4..9db28cf987 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -21,7 +21,7 @@ jobs: ROS_DISTRO: iron container: ghcr.io/ros-controls/ros:iron-rhel steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: src/ros2_control - run: | diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index eefb15ae87..182df6e22b 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -26,7 +26,7 @@ jobs: pre_release: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ${{ github.event.inputs.branch }} - name: industrial_ci diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index bca08ce5d2..acefeebfac 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -58,10 +58,10 @@ jobs: steps: - name: Checkout ${{ inputs.ref }} when build is not scheduled if: ${{ github.event_name != 'schedule' }} - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Checkout ${{ inputs.ref }} on scheduled build if: ${{ github.event_name == 'schedule' }} - uses: actions/checkout@v3 + uses: actions/checkout@v4 with: ref: ${{ inputs.ref_for_scheduled_build }} - name: cache target_ws diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 6cd59ce5f9..d7484dee0a 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -29,7 +29,7 @@ jobs: - uses: ros-tooling/setup-ros@0.7.0 with: required-ros-distributions: ${{ inputs.ros_distro }} - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: ref: ${{ inputs.ref }} - uses: ros-tooling/action-ros-ci@0.3.3 @@ -49,7 +49,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_control.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v3.1.2 + - uses: actions/upload-artifact@v3.1.3 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index 772809825f..2edbc9b59e 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -7,7 +7,7 @@ jobs: test: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: uesteibar/reviewer-lottery@v3 with: repo-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index a4faa537af..0768fbfd68 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -17,7 +17,7 @@ jobs: ROS_DISTRO: rolling container: ghcr.io/ros-controls/ros:rolling-rhel steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 with: path: src/ros2_control - run: | diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index c0325027a4..aaa2667a06 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- +* add a broadcaster for range sensor (`#1091 `_) +* Contributors: flochre + +3.17.0 (2023-08-07) +------------------- + 3.16.0 (2023-07-09) ------------------- diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index a9755b95ed..ca5ab5e57b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -114,8 +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 & urdf, - const std::string & namespace_ = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()); + 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 /* diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp new file mode 100644 index 0000000000..baa9022c75 --- /dev/null +++ b/controller_interface/include/semantic_components/range_sensor.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 flochre +// +// 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 SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ +#define SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ + +#include +#include +#include + +#include "semantic_components/semantic_component_interface.hpp" +#include "sensor_msgs/msg/range.hpp" + +namespace semantic_components +{ +class RangeSensor : public SemanticComponentInterface +{ +public: + explicit RangeSensor(const std::string & name) : SemanticComponentInterface(name, 1) + { + interface_names_.emplace_back(name_ + "/" + "range"); + } + + virtual ~RangeSensor() = default; + + /** + * Return Range reported by a sensor + * + * \return value of the range in meters + */ + float get_range() { return state_interfaces_[0].get().get_value(); } + + /// Return Range message with range in meters + /** + * Constructs and return a Range message from the current values. + * \return Range message from values; + */ + bool get_values_as_message(sensor_msgs::msg::Range & message) + { + // update the message values + message.range = get_range(); + + return true; + } +}; + +} // namespace semantic_components + +#endif // SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_ diff --git a/controller_interface/package.xml b/controller_interface/package.xml index a060a01054..d960bdf28a 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.16.0 + 3.18.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index d78eaec6a7..2a9e348b8e 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- +* Controller sorting and proper execution in a chain (Fixes `#853 `_) (`#1063 `_) +* Contributors: Sai Kishor Kothakota, Christoph Fröhlich, Dr Denis, Bence Magyar + +3.17.0 (2023-08-07) +------------------- +* [CM] Fixes the issue with individual controller's update rate (`#1082 `_) +* Fix deprecation warning (`#1093 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 3.16.0 (2023-07-09) ------------------- * added controller manager runner to have update cycles (`#1075 `_) diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst index 437cb70509..d79e730d3f 100644 --- a/controller_manager/doc/controller_chaining.rst +++ b/controller_manager/doc/controller_chaining.rst @@ -63,48 +63,14 @@ After configuring a chainable controller, controller manager calls ``input_inter This is the same process as done by ``ResourceManager`` and hardware interfaces. Controller manager maintains "claimed" status of interface in a vector (the same as done in ``ResourceManager``). + Activation and Deactivation Chained Controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Controller Manager has an additional parameter that describes how controllers are chained. -In the first version, the parameter-structure would have some semantic meaning embedded into it, as follows: - - -.. code-block:: yaml - - controller_manager: - ros__parameters: - chained_controllers: - - - parallel_group_1: - - controller1_1 - - controller1_2 - - - parallel_group_2: - - controller2_1 - - - parallel_group_3: - - controller3_1 - - controller3_2 - - controller3_3 - - ... - - - parallel_group_N: - - controllerN_1 - - ... - - controllerN_M - - - -This structure is motivated by ``filter_chain`` structure from `ros/filters repository `__, see `this file for implementation `__. - -This structure is stored internally by controller manager into an ordered map (``std::map>``) with group name as key. -When a controller should be deactivated, the controller manager deactivates all the controllers in the preceding groups first. -All other controllers from the group stay active, as well as all controllers in the following groups. -NOTE: In the future this could be done more intelligently, i.e., deactivate only controllers in the preceding groups that actually precede the controller that should be deactivated. - -On the other hand, the controller should be manually activated in the reverse order, i.e., from the those closer to the hardware toward those preceding them. +Chained controllers must be activated and deactivated together or in the proper order. +This means you must first activate all following controllers to have the preceding one activated. +For the deactivation there is the inverse rule - all preceding controllers have to be deactivated before the following controller is deactivated. +One can also think of it as an actual chain, you can not add a chain link or break the chain in the middle. Debugging outputs diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 631d5173aa..2850b2be80 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -389,6 +389,29 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /// A method to be used in the std::sort method to sort the controllers to be able to + /// execute them in a proper order + /** + * Compares the controllers ctrl_a and ctrl_b and then returns which comes first in the sequence + * + * @note The following conditions needs to be handled while ordering the controller list + * 1. The controllers that do not use any state or command interfaces are updated first + * 2. The controllers that use only the state system interfaces only are updated next + * 3. The controllers that use any of an another controller's reference interface are updated + * before the preceding controller + * 4. The controllers that use the controller's estimated interfaces are updated after the + * preceding controller + * 5. The controllers that only use the hardware command interfaces are updated last + * 6. All inactive controllers go at the end of the list + * + * \param[in] controllers list of controllers to compare their names to interface's prefix. + * + * @return true, if ctrl_a needs to execute first, else false + */ + bool controller_sorting( + const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, + const std::vector & controllers); + void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); diagnostic_updater::Updater diagnostics_updater_; diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 33255fc700..adb6769d9c 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.16.0 + 3.18.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 c4887db15a..c13eb74a80 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -118,6 +118,123 @@ bool command_interface_is_reference_interface_of_controller( return true; } +/** + * A method to retrieve the names of all it's following controllers given a controller name + * For instance, for the following case + * A -> B -> C -> D + * When called with B, returns C and D + * 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 + * + * @param controller_name - Name of the controller for checking the tree + * \param[in] controllers list of controllers to compare their names to interface's prefix. + * @return list of controllers that are following the given controller in a chain. If none, return + * empty. + */ +std::vector get_following_controller_names( + const std::string controller_name, + const std::vector & controllers) +{ + std::vector following_controllers; + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (controller_it == controllers.end()) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "Required controller : '%s' is not found in the controller list ", controller_name.c_str()); + + 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) + { + controller_manager::ControllersListIterator ctrl_it; + if (command_interface_is_reference_interface_of_controller(itf, controllers, ctrl_it)) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "The interface is a reference interface of controller : %s", ctrl_it->info.name.c_str()); + following_controllers.push_back(ctrl_it->info.name); + const std::vector ctrl_names = + get_following_controller_names(ctrl_it->info.name, controllers); + for (const std::string & controller : ctrl_names) + { + if ( + std::find(following_controllers.begin(), following_controllers.end(), controller) == + following_controllers.end()) + { + // Only add to the list if it doesn't exist + following_controllers.push_back(controller); + } + } + } + } + return following_controllers; +} + +/** + * A method to retrieve the names of all it's preceding controllers given a controller name + * For instance, for the following case + * A -> B -> C -> D + * When called with C, returns A and B + * 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 + * + * @param controller_name - Name of the controller for checking the tree + * \param[in] controllers list of controllers to compare their names to interface's prefix. + * @return list of controllers that are preceding the given controller in a chain. If none, return + * empty. + */ +std::vector get_preceding_controller_names( + const std::string controller_name, + const std::vector & controllers) +{ + std::vector preceding_controllers; + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (controller_it == controllers.end()) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "Required controller : '%s' is not found in the controller list ", controller_name.c_str()); + return preceding_controllers; + } + 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) + { + preceding_controllers.push_back(ctrl.info.name); + auto ctrl_names = get_preceding_controller_names(ctrl.info.name, controllers); + for (const std::string & controller : ctrl_names) + { + if ( + std::find(preceding_controllers.begin(), preceding_controllers.end(), controller) == + preceding_controllers.end()) + { + // Only add to the list if it doesn't exist + preceding_controllers.push_back(controller); + } + } + } + } + } + return preceding_controllers; +} + } // namespace namespace controller_manager @@ -295,9 +412,9 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript RCLCPP_WARN( get_logger(), "Parameter 'configure_components_on_start' is deprecated. " - "Use 'hardware_interface_state_after_start.inactive' instead, to set component's initial " + "Use 'hardware_components_initial_state.inactive' instead, to set component's initial " "state to 'inactive'. Don't use this parameters in combination with the new " - "'hardware_interface_state_after_start' parameter structure."); + "'hardware_components_initial_state' parameter structure."); set_components_to_state( "configure_components_on_start", rclcpp_lifecycle::State( @@ -323,7 +440,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript get_logger(), "Parameter 'activate_components_on_start' is deprecated. " "Components are activated per default. Don't use this parameters in combination with the new " - "'hardware_interface_state_after_start' parameter structure."); + "'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) @@ -621,6 +738,19 @@ 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) + { + // NOTE: The following computation is done to compute the approx controller update that can be + // achieved w.r.t to the CM's update rate. This is done this way to take into account the + // unsigned integer division. + const auto act_ctrl_update_rate = cm_update_rate / (cm_update_rate / controller_update_rate); + RCLCPP_WARN( + get_logger(), + "The controller : %s update rate : %d Hz is not a perfect divisor of the controller " + "manager's update rate : %d Hz!. The controller will be updated with nearest divisor's " + "update rate which is : %d Hz.", + controller_name.c_str(), controller_update_rate, cm_update_rate, act_ctrl_update_rate); + } // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers if (controller->is_chainable()) @@ -1103,6 +1233,20 @@ controller_interface::return_type ControllerManager::switch_controller( controller.info.claimed_interfaces.clear(); } } + + // 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 @@ -1495,11 +1639,17 @@ void ControllerManager::list_controllers_srv_cb( } } // check reference interfaces only if controller is inactive or active - auto references = controllers[i].c->export_reference_interfaces(); - controller_state.reference_interfaces.reserve(references.size()); - for (const auto & reference : references) + if (controllers[i].c->is_chainable()) { - controller_state.reference_interfaces.push_back(reference.get_interface_name()); + auto references = + resource_manager_->get_controller_reference_interface_names(controllers[i].info.name); + controller_state.reference_interfaces.reserve(references.size()); + for (const auto & reference : references) + { + const std::string prefix_name = controllers[i].c->get_node()->get_name(); + const std::string interface_name = reference.substr(prefix_name.size() + 1); + controller_state.reference_interfaces.push_back(interface_name); + } } } response->controller.push_back(controller_state); @@ -1887,10 +2037,12 @@ 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 auto controller_update_factor = + (controller_update_rate == 0) || (controller_update_rate >= update_rate_) + ? 1u + : update_rate_ / controller_update_rate; - bool controller_go = controller_update_rate == 0 || - ((update_loop_counter_ % controller_update_rate) == 0) || - (controller_update_rate >= update_rate_); + bool controller_go = ((update_loop_counter_ % controller_update_factor) == 0); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", update_loop_counter_, controller_go ? "True" : "False", @@ -1899,7 +2051,7 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { auto controller_ret = loaded_controller.c->update( - time, (controller_update_rate != update_rate_ && controller_update_rate != 0) + time, (controller_update_factor != 1u) ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate) : period); @@ -2265,6 +2417,99 @@ controller_interface::return_type ControllerManager::check_preceeding_controller } } return controller_interface::return_type::OK; +} + +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()) + { + // The case of the controllers that don't have any command interfaces. For instance, + // joint_state_broadcaster + return true; + } + else + { + auto following_ctrls = get_following_controller_names(ctrl_a.info.name, controllers); + 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); + // This is to check that the ctrl_b is in the preceding controllers list of ctrl_a - This + // check is useful when there is a chained controller branching, but they belong to same + // branch + if ( + std::find(ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end(), ctrl_b.info.name) != + ctrl_a_preceding_ctrls.end()) + { + return false; + } + + // This is to handle the cases where, the parsed ctrl_a and ctrl_b are not directly related + // but might have a common parent - happens in branched chained controller + auto ctrl_b_preceding_ctrls = get_preceding_controller_names(ctrl_b.info.name, controllers); + std::sort(ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end()); + std::sort(ctrl_b_preceding_ctrls.begin(), ctrl_b_preceding_ctrls.end()); + std::list intersection; + std::set_intersection( + ctrl_a_preceding_ctrls.begin(), ctrl_a_preceding_ctrls.end(), + ctrl_b_preceding_ctrls.begin(), ctrl_b_preceding_ctrls.end(), + std::back_inserter(intersection)); + if (!intersection.empty()) + { + // If there is an intersection, then there is a common parent controller for both ctrl_a and + // ctrl_b + return true; + } + + // If there is no common parent, then they belong to 2 different sets + auto following_ctrls_b = get_following_controller_names(ctrl_b.info.name, controllers); + if (following_ctrls_b.empty()) return true; + auto find_first_element = [&](const auto & controllers_list) + { + auto 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; + } + }; + 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; + } + + // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be + // infront of ctrl_a + // 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); }); + if (state_it != state_itfs.end()) + { + return false; + } + + // The rest of the cases, basically end up at the end of the list + return false; + } }; void ControllerManager::controller_activity_diagnostic_callback( diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 5a452bb0b1..6db4cfd1b2 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -246,9 +246,13 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); - EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + // As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle + for (size_t i = 0; i < 25; i++) + { + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + } EXPECT_GE(test_controller->internal_counter, 1u); EXPECT_EQ(test_controller->get_update_rate(), 4u); } @@ -364,3 +368,82 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd INSTANTIATE_TEST_SUITE_P( per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates, testing::Values(100, 232, 400)); + +class TestControllerUpdateRates +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +}; + +TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) +{ + const unsigned int ctrl_update_rate = GetParam(); + auto test_controller = std::make_shared(); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + + test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); + // configure controller + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + + // Start controller, will take effect at the end of the update function + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + + EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); + const auto cm_update_rate = cm_->get_update_rate(); + const auto controller_update_rate = test_controller->get_update_rate(); + const auto controller_factor = (cm_update_rate / controller_update_rate); + const auto expected_controller_update_rate = + static_cast(std::round(cm_update_rate / static_cast(controller_factor))); + + const auto initial_counter = test_controller->internal_counter; + for (size_t update_counter = 1; update_counter <= 10 * cm_update_rate; ++update_counter) + { + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + + if (update_counter % cm_update_rate == 0) + { + const auto no_of_secs_passed = update_counter / cm_update_rate; + EXPECT_EQ( + test_controller->internal_counter - initial_counter, + (expected_controller_update_rate * no_of_secs_passed)); + } + } +} + +INSTANTIATE_TEST_SUITE_P( + per_controller_update_rate_check, TestControllerUpdateRates, + testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98)); diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index adb227d6fe..7873adaacf 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -269,7 +269,9 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) ASSERT_EQ(result->controller[0].is_chainable, true); 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].chain_connections.size(), 0u); // check test controller ASSERT_EQ(result->controller[1].state, "inactive"); @@ -289,19 +291,20 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) 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); - // check chainable controller - ASSERT_EQ(result->controller[0].state, "active"); - ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 1u); - ASSERT_EQ(result->controller[0].is_chained, true); // check test controller + ASSERT_EQ(result->controller[0].state, "active"); + ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 3u); + // check chainable controller ASSERT_EQ(result->controller[1].state, "active"); - ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 3u); + ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 1u); + ASSERT_EQ(result->controller[1].is_chained, true); ASSERT_EQ( test_chainable_controller::TEST_CONTROLLER_NAME, - result->controller[1].chain_connections[0].name); - ASSERT_EQ(2u, result->controller[1].chain_connections[0].reference_interfaces.size()); - ASSERT_EQ("joint1/position", result->controller[1].chain_connections[0].reference_interfaces[0]); - ASSERT_EQ("joint1/velocity", result->controller[1].chain_connections[0].reference_interfaces[1]); + result->controller[0].chain_connections[0].name); + ASSERT_EQ(2u, result->controller[0].chain_connections[0].reference_interfaces.size()); + ASSERT_EQ("test_chainable_controller_name", result->controller[0].chain_connections[0].name); + ASSERT_EQ("joint1/position", result->controller[0].chain_connections[0].reference_interfaces[0]); + ASSERT_EQ("joint1/velocity", result->controller[0].chain_connections[0].reference_interfaces[1]); } TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) @@ -480,3 +483,586 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, cm_->get_loaded_controllers()[0].c->get_state().id()); } + +TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers) +{ + /// The simulated controller chaining is: + /// test_controller_name -> chain_ctrl_5 -> chain_ctrl_4 -> chain_ctrl_3 -> chain_ctrl_2 -> + /// chain_ctrl_1 + /// + /// 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"; + 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_chained_controller_4 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_3) + "/joint1/position"}}; + 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({"joint1/position", "joint1/velocity"}); + + auto test_chained_controller_5 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_4) + "/joint1/position"}}; + 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({"joint1/position", "joint1/velocity"}); + + // create non-chained controller + auto test_controller = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_5) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_5) + "/joint1/velocity", "joint2/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller->set_command_interface_configuration(cmd_cfg); + test_controller->set_state_interface_configuration(state_cfg); + // add controllers + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + 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_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_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_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_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(6u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_4); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_1); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_3); + ASSERT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_5); + ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_2); + // check test controller + ASSERT_EQ(result->controller[5].name, "test_controller_name"); + + // configure controllers + for (const auto & controller : + {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); + 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); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_4); + ASSERT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_3); + ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_2); + ASSERT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_1); + RCLCPP_ERROR(srv_node->get_logger(), "Check successful!"); +} + +TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) +{ + /// The simulated controller chain branching is: + /// test_controller_name -> chain_ctrl_7 -> chain_ctrl_6 -> chain_ctrl_2 -> chain_ctrl_1 + /// & + /// chain_ctrl_6 -> chain_ctrl_5 -> chain_ctrl_4 -> chain_ctrl_3 + /// + /// 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"; + 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", "joint2/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, {"joint2/velocity"}}; + 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({"joint2/velocity"}); + + auto test_chained_controller_4 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_3) + "/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_2) + "/joint1/position", + 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({"joint1/position", "joint2/velocity"}); + + auto test_chained_controller_7 = std::make_shared(); + chained_cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_6) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_6) + "/joint2/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({"joint1/position", "joint2/velocity"}); + + // create non-chained controller + auto test_controller = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(TEST_CHAINED_CONTROLLER_7) + "/joint1/position", + std::string(TEST_CHAINED_CONTROLLER_7) + "/joint2/velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint2/velocity"}}; + test_controller->set_command_interface_configuration(cmd_cfg); + test_controller->set_state_interface_configuration(state_cfg); + // add controllers + cm_->add_controller( + test_chained_controller_4, TEST_CHAINED_CONTROLLER_4, + 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_3, TEST_CHAINED_CONTROLLER_3, + test_chainable_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_6, TEST_CHAINED_CONTROLLER_6, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_2, TEST_CHAINED_CONTROLLER_2, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_chained_controller_7, TEST_CHAINED_CONTROLLER_7, + 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(8u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CHAINED_CONTROLLER_4); + ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_1); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_3); + ASSERT_EQ(result->controller[3].name, TEST_CHAINED_CONTROLLER_5); + ASSERT_EQ(result->controller[4].name, TEST_CHAINED_CONTROLLER_6); + ASSERT_EQ(result->controller[5].name, TEST_CHAINED_CONTROLLER_2); + // check test controller + ASSERT_EQ(result->controller[6].name, "test_controller_name"); + ASSERT_EQ(result->controller[7].name, TEST_CHAINED_CONTROLLER_7); + + // configure controllers + for (const auto & controller : + {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); + 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); + ASSERT_EQ(result->controller[2].name, TEST_CHAINED_CONTROLLER_6); + + auto get_ctrl_pos = [result](const std::string & controller_name) -> int + { + 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_1_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_1); + auto ctrl_2_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_2); + auto ctrl_3_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_3); + auto ctrl_4_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_4); + auto ctrl_5_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_5); + auto ctrl_6_pos = get_ctrl_pos(TEST_CHAINED_CONTROLLER_6); + + // Extra check to see that they are index only after their parent controller (ctrl_6) + ASSERT_GT(ctrl_3_pos, ctrl_6_pos); + ASSERT_GT(ctrl_1_pos, ctrl_6_pos); + + // first branch + ASSERT_GT(ctrl_2_pos, ctrl_6_pos); + ASSERT_GT(ctrl_1_pos, ctrl_2_pos); + + // second branch + ASSERT_GT(ctrl_5_pos, ctrl_6_pos); + ASSERT_GT(ctrl_4_pos, ctrl_5_pos); + ASSERT_GT(ctrl_3_pos, ctrl_4_pos); +} + +TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) +{ + /// 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 + /// + /// 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_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"}); + + // 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); + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + + // check chainable controller + ASSERT_EQ(10u, 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_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}; + 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()); + + // 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( + 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); +} diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 62babe998d..16ba39d953 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -52,6 +52,8 @@ class TestableTestChainableController : public test_chainable_controller::TestCh FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling); + FRIEND_TEST( + TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order); }; class TestableControllerManager : public controller_manager::ControllerManager @@ -84,6 +86,8 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers_deactivation_error_handling); + FRIEND_TEST( + TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order); public: TestableControllerManager( @@ -975,10 +979,132 @@ TEST_P( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); } -// TODO(destogl): Add test case with controllers added in "random" order -// -// new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain -// +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) +{ + SetupControllers(); + + // add all controllers in random order to test the sorting + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); + + EXPECT_THROW( + cm_->resource_manager_->make_controller_reference_interfaces_available( + POSITION_TRACKING_CONTROLLER), + std::out_of_range); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + // activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER + // (otherwise, interface will be missing) + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 3u); + + // Diff-Drive Controller claims the reference interfaces of PID controllers + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 3u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 5u); + + // Position-Tracking Controller uses reference interfaces of Diff-Drive Controller + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + // 'rot_z' reference interface is not claimed + for (const auto & interface : {"diff_drive_controller/rot_z"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + ASSERT_EQ(diff_drive_controller->internal_counter, 3u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); + + // update controllers + std::vector reference = {32.0, 128.0}; + + // update 'Position Tracking' controller + for (auto & value : diff_drive_controller->reference_interfaces_) + { + ASSERT_EQ(value, 0.0); // default reference values are 0.0 + } + position_tracking_controller->external_commands_for_testing_[0] = reference[0]; + position_tracking_controller->external_commands_for_testing_[1] = reference[1]; + position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(position_tracking_controller->internal_counter, 2u); + + ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller + ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through + + // update 'Diff Drive' Controller + diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(diff_drive_controller->internal_counter, 4u); + // default reference values are 0.0 - they should be changed now + EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 + EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 + ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); + ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + + // update PID controllers that are writing to hardware + pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 8u); + pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 6u); + + // update hardware ('read' is sufficient for test hardware) + cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // 32 - 0 + EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); + // 32 / 2 + EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + + // 128 - 0 + EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); + // 128 / 2 + EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + + // update all controllers at once and see that all have expected values --> also checks the order + // of controller execution + + reference = {1024.0, 4096.0}; + UpdateAllControllerAndCheck(reference, 3u); +} INSTANTIATE_TEST_SUITE_P( test_strict_best_effort, TestControllerChainingWithControllerManager, diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 3c481827d2..8072652c55 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + 3.16.0 (2023-07-09) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 93af7dc706..f7b9eb5642 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.16.0 + 3.18.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 14dc8b8744..0428cf4f8e 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- +* Add checks if hardware is initialized. (`#1054 `_) +* Contributors: Dr. Denis + 3.16.0 (2023-07-09) ------------------- diff --git a/hardware_interface/doc/hardware_components_userdoc.rst b/hardware_interface/doc/hardware_components_userdoc.rst index cd93f472a5..1a8c5e611b 100644 --- a/hardware_interface/doc/hardware_components_userdoc.rst +++ b/hardware_interface/doc/hardware_components_userdoc.rst @@ -14,20 +14,21 @@ Guidelines and Best Practices .. toctree:: :titlesonly: - Writing a Hardware Interface + Hardware Interface Types + Writing a Hardware Component Handling of errors that happen during read() and write() calls ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``write()`` methods of a hardware interface, ``on_error(previous_state)`` method will be called to handle any error that happened. +If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``write()`` methods of a hardware_interface class, ``on_error(previous_state)`` method will be called to handle any error that happened. Error handling follows the `node lifecycle `_. If successful ``CallbackReturn::SUCCESS`` is returned and hardware is again in ``UNCONFIGURED`` state, if any ``ERROR`` or ``FAILURE`` happens the hardware ends in ``FINALIZED`` state and can not be recovered. The only option is to reload the complete plugin, but there is currently no service for this in the Controller Manager. -Migration from Foxy to Galactic -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Migration from Foxy to newer versions +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle. The following list shows a set of quick changes to port existing hardware components to Galactic: @@ -36,20 +37,29 @@ The following list shows a set of quick changes to port existing hardware compon 2. If using BaseInterface as base class then you should remove it. Specifically, change: -hardware_interface::BaseInterface to hardware_interface::[Actuator|Sensor|System]Interface + .. code-block:: c++ + + hardware_interface::BaseInterface + + to + + .. code-block:: c++ + + hardware_interface::[Actuator|Sensor|System]Interface 3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp`` 4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary -5. replace first three lines in ``on_init`` to: +5. replace first three lines in ``on_init`` to + + .. code-block:: c++ -.. code-block:: c++ + if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } - if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) - { - return CallbackReturn::ERROR; - } 6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;`` diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst new file mode 100644 index 0000000000..54b2003568 --- /dev/null +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -0,0 +1,150 @@ +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/hardware_interface_types_userdoc.rst + +.. _hardware_interface_types_userdoc: + +``ros2_control`` hardware interface types +--------------------------------------------------------- + +The ``ros2_control`` framework provides a set of hardware interface types that can be used to implement +a hardware component for a specific robot or device. +The following sections describe the different hardware interface types and their usage. + +Joints +***************************** +````-tag groups the interfaces associated with the joints of physical robots and actuators. +They have command and state interfaces to set the goal values for hardware and read its current state. + +State interfaces of joints can be published as a ROS topic by means of the :ref:`joint_state_broadcaster ` + +Sensors +***************************** +````-tag groups multiple state interfaces describing, e.g., internal states of hardware. + +Depending on the type of sensor, there exist a couple of specific semantic components with broadcasters shipped with ros2_controllers, e.g. + +- :ref:`Imu Sensor Broadcaster ` +- :ref:`Force Torque Sensor Broadcaster ` + +GPIOs +***************************** +The ````-tag is used for describing input and output ports of a robotic device that cannot be associated with any joint or sensor. +Parsing of ````-tag is similar to this of a ````-tag having command and state interfaces. +The tag must have at least one ````- or ````-tag as a child. + +The keyword "gpio" is chosen for its generality. +Although strictly used for digital signals, it describes any electrical analog, digital signal, or physical value. + +The ```` tag can be used as a child of all three types of hardware components, i.e., system, sensor, or actuator. + +Because ports implemented as ````-tag are typically very application-specific, there exists no generic publisher +within the ros2_control framework. A custom gpio-controller has to be implemented for each application. As an example, see :ref:`the GPIO controller example ` as part of the demo repository. + +Examples +***************************** +The following examples show how to use the different hardware interface types in a ``ros2_control`` URDF. +They can be combined together within the different hardware component types (system, actuator, sensor) (:ref:`see detailed documentation `) as follows + +1. Robot with multiple GPIO interfaces + + - RRBot System + - Digital: 4 inputs and 2 outputs + - Analog: 2 inputs and 1 output + - Vacuum valve at the flange (on/off) + + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + + + + + + + + + + + + + +2. Gripper with electrical and suction grasping possibilities + + - Multimodal gripper + - 1-DoF parallel gripper + - suction on/off + + .. code:: xml + + + + ros2_control_demo_hardware/MultimodalGripper + + + + 0 + 100 + + + + + + + + + +3. Force-Torque-Sensor with temperature feedback and adjustable calibration + + - 2D FTS + - Temperature feedback in °C + - Choice between 3 calibration matrices, i.e., calibration ranges + + .. code:: xml + + + + ros2_control_demo_hardware/ForceTorqueSensor2DHardware + 0.43 + + + + + kuka_tcp + 100 + 100 + + + + + + + + + diff --git a/hardware_interface/doc/writing_new_hardware_interface.rst b/hardware_interface/doc/writing_new_hardware_component.rst similarity index 98% rename from hardware_interface/doc/writing_new_hardware_interface.rst rename to hardware_interface/doc/writing_new_hardware_component.rst index 1ff4dc4420..698f6cf6e2 100644 --- a/hardware_interface/doc/writing_new_hardware_interface.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -1,9 +1,9 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/writing_new_hardware_interface.rst +:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/doc/writing_new_hardware_component.rst -.. _writing_new_hardware_interface: +.. _writing_new_hardware_component: -Writing a new hardware interface -================================= +Writing a Hardware Component +============================ In ros2_control hardware system components are libraries, dynamically loaded by the controller manager using the `pluginlib `_ interface. The following is a step-by-step guide to create source files, basic tests, and compile rules for a new hardware interface. diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index a329215218..e3a2f4f71e 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.16.0 + 3.18.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index d89b2981e9..46acee3f8d 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + 3.16.0 (2023-07-09) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index d72273844a..f1c0365cfd 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.16.0 + 3.18.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 7befead368..272c2d3fe4 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + 3.16.0 (2023-07-09) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index a445ed27ff..15a379933a 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.16.0 + 3.18.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 bcd91a1b55..d633c415d1 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + 3.16.0 (2023-07-09) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 8c8edff772..c77b828276 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.16.0 + 3.18.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index bbb52f9821..a5a153faf1 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- +* Add info where the pdf is saved to view_controller_chains (`#1094 `_) +* Contributors: Christoph Fröhlich + 3.16.0 (2023-07-09) ------------------- diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index dba173cfbf..5880c054cb 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -268,4 +268,14 @@ view_controller_chains $ ros2 control view_controller_chains -h usage: ros2 view_controller_chains - Creates a graphviz image from loaded controllers. + Generates a diagram of the loaded chained controllers into /tmp/controller_diagram.gv.pdf + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node + --include-hidden-nodes + Consider hidden nodes as well diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index c72388c597..92eba51b62 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.16.0 + 3.18.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/ros2controlcli/verb/view_controller_chains.py b/ros2controlcli/ros2controlcli/verb/view_controller_chains.py index fbd999192e..f838a96e1c 100644 --- a/ros2controlcli/ros2controlcli/verb/view_controller_chains.py +++ b/ros2controlcli/ros2controlcli/verb/view_controller_chains.py @@ -195,7 +195,7 @@ def parse_response(list_controllers_response, list_hardware_response, visualize= class ViewControllerChainsVerb(VerbExtension): - """Generates a diagram of the loaded chained controllers.""" + """Generates a diagram of the loaded chained controllers into /tmp/controller_diagram.gv.pdf.""" def add_arguments(self, parser, cli_name): add_arguments(parser) diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 5eb29039cc..aa2b19b830 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.16.0", + version="3.18.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 660dedfeb0..9996ff5a85 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + 3.16.0 (2023-07-09) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 3dac59a834..4888459d49 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.16.0 + 3.18.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 30117e7fe2..318cf556d2 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.16.0", + version="3.18.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 a124333b85..c45b040c1b 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.18.0 (2023-08-17) +------------------- + +3.17.0 (2023-08-07) +------------------- + 3.16.0 (2023-07-09) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 9d2839198c..9e2b2fded2 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.16.0 + 3.18.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