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..2e1400f718 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -12,8 +12,8 @@ jobs: name: Format runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.7.0 + - uses: actions/checkout@v4 + - uses: actions/setup-python@v4.7.1 with: python-version: '3.10' - name: Install system hooks diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 37c371ce83..3efd557adc 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -13,7 +13,7 @@ jobs: 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: @@ -37,7 +37,7 @@ jobs: 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 aaa2667a06..76b09e75a3 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- +* Enable services for setting the log-level in controller per default (`#1102 `_) +* Contributors: Dr. Denis + 3.18.0 (2023-08-17) ------------------- * add a broadcaster for range sensor (`#1091 `_) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 05ee0c8830..52bc970500 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -115,7 +115,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type init( const std::string & controller_name, const std::string & namespace_ = "", - const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()); + 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/package.xml b/controller_interface/package.xml index d960bdf28a..b9c89c0bfb 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.18.0 + 3.19.1 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 2a9e348b8e..0da7427315 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- +* Fix next controller update cycle time clock (`#1127 `_) +* Contributors: Sai Kishor Kothakota + +3.19.0 (2023-10-03) +------------------- +* Proper controller update rate (`#1105 `_) +* Fix multiple calls to export reference interfaces (`#1108 `_) +* [Docs] Fix information about activation and deactivation of chainable controllers (`#1104 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + 3.18.0 (2023-08-17) ------------------- * Controller sorting and proper execution in a chain (Fixes `#853 `_) (`#1063 `_) 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 d5b32cf9f1..806e92f5e7 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -59,7 +59,7 @@ namespace controller_manager { using ControllersListIterator = std::vector::const_iterator; -rclcpp::NodeOptions get_cm_node_options(); +CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options(); class ControllerManager : public rclcpp::Node { @@ -123,6 +123,7 @@ class ControllerManager : public rclcpp::Node controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; + controller_spec.next_update_cycle_time = std::make_shared(0); return add_controller_impl(controller_spec); } diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 63823f8d33..6f7483f3ec 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -20,6 +20,7 @@ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #include +#include #include #include #include "controller_interface/controller_interface.hpp" @@ -37,6 +38,7 @@ struct ControllerSpec { hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; + std::shared_ptr next_update_cycle_time; }; } // namespace controller_manager diff --git a/controller_manager/package.xml b/controller_manager/package.xml index adb6769d9c..556c1f222e 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.18.0 + 3.19.1 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 31a53aebb7..e6117278c6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -565,6 +565,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; + controller_spec.next_update_cycle_time = std::make_shared( + 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); return add_controller_impl(controller_spec); } @@ -741,16 +743,13 @@ controller_interface::return_type ControllerManager::configure_controller( } 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); + "The controller : %s update cycles won't be triggered at a constant period : %f sec, as the " + "controller's update rate : %d Hz is not a perfect divisor of the controller manager's " + "update rate : %d Hz!.", + controller_name.c_str(), 1.0 / controller_update_rate, controller_update_rate, + cm_update_rate); } // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers @@ -1459,6 +1458,9 @@ void ControllerManager::activate_controllers( } auto controller = found_it->c; auto controller_name = found_it->info.name; + // reset the next update cycle time for newly activated controllers + *found_it->next_update_cycle_time = + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); bool assignment_successful = true; // assign command interfaces to the controller @@ -1640,11 +1642,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); @@ -2025,19 +2033,24 @@ controller_interface::return_type ControllerManager::update( ++update_loop_counter_; update_loop_counter_ %= update_rate_; - for (auto loaded_controller : rt_controller_list) + for (const auto & loaded_controller : rt_controller_list) { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 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; + const bool run_controller_at_cm_rate = + (controller_update_rate == 0) || (controller_update_rate >= update_rate_); + const auto controller_period = + run_controller_at_cm_rate ? period + : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); + + bool controller_go = + (time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || + (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); - 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", @@ -2045,10 +2058,15 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - auto controller_ret = loaded_controller.c->update( - time, (controller_update_factor != 1u) - ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate) - : period); + auto controller_ret = loaded_controller.c->update(time, controller_period); + + if ( + *loaded_controller.next_update_cycle_time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + *loaded_controller.next_update_cycle_time = time; + } + *loaded_controller.next_update_cycle_time += controller_period; if (controller_ret != controller_interface::return_type::OK) { diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 78c3fcb06b..7bf5cae628 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -41,7 +41,6 @@ namespace { -const auto TIME = rclcpp::Time(0); const auto PERIOD = rclcpp::Duration::from_seconds(0.01); const auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT; const auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; @@ -101,6 +100,7 @@ class ControllerManagerFixture : public ::testing::Test cm_->robot_description_callback(msg); } } + time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); } static void SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -119,7 +119,7 @@ class ControllerManagerFixture : public ::testing::Test { while (run_updater_) { - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } }); @@ -157,6 +157,7 @@ class ControllerManagerFixture : public ::testing::Test bool run_updater_; const std::string robot_description_; const bool pass_urdf_as_parameter_; + rclcpp::Time time_; }; class TestControllerManagerSrvs @@ -177,9 +178,9 @@ class TestControllerManagerSrvs std::chrono::milliseconds(10), [&]() { - cm_->read(TIME, PERIOD); - cm_->update(TIME, PERIOD); - cm_->write(TIME, PERIOD); + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); }); executor_->add_node(cm_); @@ -206,7 +207,7 @@ class TestControllerManagerSrvs while (service_executor.spin_until_future_complete(result, std::chrono::milliseconds(50)) != rclcpp::FutureReturnCode::SUCCESS) { - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); } } else diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 6db4cfd1b2..e317726585 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -96,7 +96,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -108,7 +108,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) cm_->configure_controller(TEST_CONTROLLER2_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; @@ -123,7 +123,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -132,7 +132,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter); // Start the real test controller, will take effect at the end of the update function @@ -147,7 +147,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, 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); @@ -157,7 +157,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller->internal_counter, 1u); size_t last_internal_counter = test_controller->internal_counter; @@ -173,7 +173,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter) << "Controller is stopped at the end of update, so it should have done one more update"; { @@ -208,7 +208,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -220,7 +220,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) 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))); + cm_->update(time_, 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()); @@ -237,7 +237,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, 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); @@ -251,7 +251,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); } EXPECT_GE(test_controller->internal_counter, 1u); EXPECT_EQ(test_controller->get_update_rate(), 4u); @@ -286,7 +286,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -299,7 +299,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd 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))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -316,7 +316,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is started at the end of update"; { @@ -332,7 +332,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -348,8 +348,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd << "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))) + controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))) << "Controller is stopped at the end of update, so it should have done one more update"; { ControllerManagerRunner cm_runner(this); @@ -393,7 +392,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) 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))); + cm_->update(time_, 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()); @@ -411,7 +410,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, 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); @@ -423,23 +422,28 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) 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) + rclcpp::Time time = time_; + for (size_t update_counter = 0; 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))); + cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0) { const auto no_of_secs_passed = update_counter / cm_update_rate; - EXPECT_EQ( + // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + EXPECT_NEAR( test_controller->internal_counter - initial_counter, - (expected_controller_update_rate * no_of_secs_passed)); + (controller_update_rate * no_of_secs_passed), 1); } } } diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 1ec725f8c5..f12ab9e96b 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -22,6 +22,7 @@ #include "controller_manager_test_common.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; @@ -138,7 +139,7 @@ class TestControllerManagerWithTestableCM cm_->configure_controller(TEST_BROADCASTER_ALL_NAME); cm_->configure_controller(TEST_BROADCASTER_SENSOR_NAME); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(0u, test_controller_actuator->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller_system->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_broadcaster_all->internal_counter) << "Controller is not started"; @@ -162,10 +163,6 @@ class TestControllerManagerWithTestableCM {}, strictness); } - // values to set to hardware to simulate failure on read and write - static constexpr double READ_FAIL_VALUE = 28282828.0; - static constexpr double WRITE_FAIL_VALUE = 23232323.0; - static constexpr char TEST_CONTROLLER_ACTUATOR_NAME[] = "test_controller_actuator"; static constexpr char TEST_CONTROLLER_SYSTEM_NAME[] = "test_controller_system"; static constexpr char TEST_BROADCASTER_ALL_NAME[] = "test_broadcaster_all"; @@ -226,7 +223,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er hardware_interface::lifecycle_state_names::ACTIVE); { - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_GE(test_controller_actuator->internal_counter, 1u) << "Controller is started at the end of update"; EXPECT_GE(test_controller_system->internal_counter, 1u) @@ -249,7 +246,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Execute first time without any errors { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; @@ -258,10 +255,10 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to // READ_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -278,7 +275,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -289,7 +286,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) << "Execute has read error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -327,13 +324,13 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME // by setting first command interface to READ_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; - test_controller_system->set_first_command_interface_value_to = READ_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; { auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute without errors to write value"; @@ -350,7 +347,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er auto previous_counter_higher = test_controller_system->internal_counter; auto new_counter = test_broadcaster_sensor->internal_counter + 1; - EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -361,7 +358,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute has read error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) @@ -418,7 +415,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e hardware_interface::lifecycle_state_names::ACTIVE); { - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_GE(test_controller_actuator->internal_counter, 1u) << "Controller is started at the end of update"; EXPECT_GE(test_controller_system->internal_counter, 1u) @@ -441,7 +438,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Execute first time without any errors { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; @@ -450,10 +447,10 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to // WRITE_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -470,7 +467,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -481,7 +478,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -519,13 +516,13 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME // by setting first command interface to WRITE_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; - test_controller_system->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; { auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute without errors to write value"; @@ -544,7 +541,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -555,7 +552,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute has write error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 583480d494..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"); @@ -300,6 +302,7 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv) test_chainable_controller::TEST_CONTROLLER_NAME, 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]); } 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 16ba39d953..a34d70ada7 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -378,7 +378,7 @@ class TestControllerChainingWithControllerManager position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // check if all controllers are updated diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index d67ae8a7d1..4137386f72 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -39,9 +39,9 @@ class TestLoadController : public ControllerManagerFixtureread(TIME, PERIOD); - cm_->update(TIME, PERIOD); - cm_->write(TIME, PERIOD); + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); }); update_executor_ = diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 8072652c55..ae3049ef44 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index f7b9eb5642..b384585b22 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.18.0 + 3.19.1 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 0428cf4f8e..7c14eba8a6 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- +* [MockHardware] Added dynamic simulation functionality. (`#1028 `_) +* Add GPIO tag description to docs (`#1109 `_) +* Contributors: Christoph Fröhlich, Dr. Denis + 3.18.0 (2023-08-17) ------------------- diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 2d6c72ffae..e486302c4c 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -101,7 +101,9 @@ if(BUILD_TESTING) test/test_components/test_system.cpp) target_link_libraries(test_components hardware_interface) ament_target_dependencies(test_components - pluginlib) + pluginlib + ros2_control_test_assets + ) install(TARGETS test_components DESTINATION lib ) 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/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index 5c3ea22ca0..d39dee2c64 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -23,6 +23,7 @@ enum class return_type : std::uint8_t { OK = 0, ERROR = 1, + DEACTIVATE = 2, }; } // namespace hardware_interface diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index c244ee1254..e9b38de65d 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -32,6 +32,10 @@ namespace mock_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +static constexpr size_t POSITION_INTERFACE_INDEX = 0; +static constexpr size_t VELOCITY_INTERFACE_INDEX = 1; +static constexpr size_t ACCELERATION_INTERFACE_INDEX = 2; + class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface { public: @@ -41,6 +45,14 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector export_command_interfaces() override; + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + + return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -60,8 +72,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - const size_t POSITION_INTERFACE_INDEX = 0; - struct MimicJoint { std::size_t joint_index; @@ -115,6 +125,9 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::string custom_interface_with_following_offset_; size_t index_custom_interface_with_following_offset_; + bool calculate_dynamics_; + std::vector joint_control_mode_; + bool command_propagation_disabled_; }; diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index e3a2f4f71e..c53c8523ec 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.18.0 + 3.19.1 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 13a5a8b679..3ae3568a9e 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -131,6 +131,17 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i command_propagation_disabled_ = false; } + // check if there is parameter that enables dynamic calculation + it = info_.hardware_parameters.find("calculate_dynamics"); + if (it != info.hardware_parameters.end()) + { + calculate_dynamics_ = hardware_interface::parse_bool(it->second); + } + else + { + calculate_dynamics_ = false; + } + // process parameters about state following position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; @@ -312,7 +323,7 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) + for (size_t i = 0; i < info_.joints.size(); ++i) { const auto & joint = info_.joints[i]; for (const auto & interface : joint.command_interfaces) @@ -333,6 +344,8 @@ std::vector GenericSystem::export_command_ } } } + // Set position control mode per default + joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) @@ -369,7 +382,135 @@ std::vector GenericSystem::export_command_ return command_interfaces; } -return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +return_type GenericSystem::prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & /*stop_interfaces*/) +{ + hardware_interface::return_type ret_val = hardware_interface::return_type::OK; + + const size_t FOUND_ONCE_FLAG = 1000000; + + std::vector joint_found_in_x_requests_; + joint_found_in_x_requests_.resize(info_.joints.size(), 0); + + for (const auto & key : start_interfaces) + { + // check if interface is joint + auto joint_it_found = std::find_if( + info_.joints.begin(), info_.joints.end(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info_.joints.end()) + { + const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + if (joint_found_in_x_requests_[joint_index] == 0) + { + joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; + } + + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + { + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + if (!calculate_dynamics_) + { + RCUTILS_LOG_WARN_NAMED( + "mock_generic_system", + "Requested velocity mode for joint '%s' without dynamics calculation enabled - this " + "might lead to wrong feedback and unexpected behavior.", + info_.joints[joint_index].name.c_str()); + } + joint_found_in_x_requests_[joint_index] += 1; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + { + if (!calculate_dynamics_) + { + RCUTILS_LOG_WARN_NAMED( + "mock_generic_system", + "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " + "this might lead to wrong feedback and unexpected behavior.", + info_.joints[joint_index].name.c_str()); + } + joint_found_in_x_requests_[joint_index] += 1; + } + } + else + { + RCUTILS_LOG_DEBUG_NAMED( + "mock_generic_system", "Got interface '%s' that is not joint - nothing to do!", + key.c_str()); + } + } + + for (size_t i = 0; i < info_.joints.size(); ++i) + { + // There has to always be at least one control mode from the above three set + if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) + { + RCUTILS_LOG_ERROR_NAMED( + "mock_generic_system", "Joint '%s' has to have '%s', '%s', or '%s' interface!", + info_.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); + ret_val = hardware_interface::return_type::ERROR; + } + + // Currently we don't support multiple interface request + if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) + { + RCUTILS_LOG_ERROR_NAMED( + "mock_generic_system", + "Got multiple (%zu) starting interfaces for joint '%s' - this is not " + "supported!", + joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); + ret_val = hardware_interface::return_type::ERROR; + } + } + + return ret_val; +} + +return_type GenericSystem::perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & /*stop_interfaces*/) +{ + if (!calculate_dynamics_) + { + return hardware_interface::return_type::OK; + } + + for (const auto & key : start_interfaces) + { + // check if interface is joint + auto joint_it_found = std::find_if( + info_.joints.begin(), info_.joints.end(), + [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); + + if (joint_it_found != info_.joints.end()) + { + const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + { + joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + { + joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; + } + if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + { + joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; + } + } + } + + return hardware_interface::return_type::OK; +} + +return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { if (command_propagation_disabled_) { @@ -392,19 +533,97 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } }; - // apply offset to positions only for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if (calculate_dynamics_) { - joint_states_[POSITION_INTERFACE_INDEX][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + switch (joint_control_mode_[j]) + { + case ACCELERATION_INTERFACE_INDEX: + { + // currently we do backward integration + joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only + joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + joint_states_[VELOCITY_INTERFACE_INDEX][j] += + joint_states_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); + + if (!std::isnan(joint_commands_[ACCELERATION_INTERFACE_INDEX][j])) + { + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + joint_commands_[ACCELERATION_INTERFACE_INDEX][j]; + } + break; + } + case VELOCITY_INTERFACE_INDEX: + { + // currently we do backward integration + joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only + joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + if (!std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j])) + { + const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + joint_commands_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + } + break; + } + case POSITION_INTERFACE_INDEX: + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + const double old_position = joint_states_[POSITION_INTERFACE_INDEX][j]; + const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; + + joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + + joint_states_[VELOCITY_INTERFACE_INDEX][j] = + (joint_states_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); + + joint_states_[ACCELERATION_INTERFACE_INDEX][j] = + (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); + } + break; + } + } + } + else + { + for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) + { + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + { + joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ + : 0.0); + } + } } } - // do loopback on all other interfaces - starts from 1 because 0 index is position interface - mirror_command_to_state(joint_states_, joint_commands_, 1); + // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, + // velocity, and acceleration interface + if (calculate_dynamics_) + { + mirror_command_to_state(joint_states_, joint_commands_, 3); + } + else + { + mirror_command_to_state(joint_states_, joint_commands_, 1); + } for (const auto & mimic_joint : mimic_joints_) { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 67b73ea0ae..fcb01198cb 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1268,12 +1268,24 @@ HardwareReadWriteStatus ResourceManager::read( { for (auto & component : components) { - if (component.read(time, period) != return_type::OK) + auto ret_val = component.read(time, period); + if (ret_val == return_type::ERROR) { read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); } + else if (ret_val == return_type::DEACTIVATE) + { + resource_storage_->deactivate_hardware(component); + } + // If desired: automatic re-activation. We could add a flag for this... + // else + // { + // using lifecycle_msgs::msg::State; + // rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + // set_component_state(component.get_name(), state); + // } } }; @@ -1296,12 +1308,17 @@ HardwareReadWriteStatus ResourceManager::write( { for (auto & component : components) { - if (component.write(time, period) != return_type::OK) + auto ret_val = component.write(time, period); + if (ret_val == return_type::ERROR) { read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); } + else if (ret_val == return_type::DEACTIVATE) + { + resource_storage_->deactivate_hardware(component); + } } }; diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 8e78ade7fb..cd344b600e 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -33,7 +33,8 @@ namespace { const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math +const auto COMPARE_DELTA = 0.0001; } // namespace class TestGenericSystem : public ::testing::Test @@ -484,6 +485,39 @@ class TestGenericSystem : public ::testing::Test )"; + hardware_system_2dof_standard_interfaces_with_different_control_modes_ = + R"( + + + mock_components/GenericSystem + true + + + + + + 3.45 + + + + + + + + + 2.78 + + + + + + + + + + +)"; + disabled_commands_ = R"( @@ -521,6 +555,7 @@ class TestGenericSystem : public ::testing::Test std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_; std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; + std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string disabled_commands_; }; @@ -1624,6 +1659,202 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) ASSERT_EQ(1, state.get_value()); } +TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) +{ + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_2dof_standard_interfaces_with_different_control_modes_ + + ros2_control_test_assets::urdf_tail; + + TestableResourceManager rm(urdf); + // Activate components to get all interfaces available + activate_components(rm); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); + ASSERT_EQ(7u, rm.state_interface_keys().size()); + EXPECT_TRUE(rm.state_interface_exists("joint1/position")); + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint1/acceleration")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint2/acceleration")); + EXPECT_TRUE(rm.state_interface_exists("flange_vacuum/vacuum")); + + ASSERT_EQ(5u, rm.command_interface_keys().size()); + EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); + EXPECT_TRUE(rm.command_interface_exists("joint2/acceleration")); + EXPECT_TRUE(rm.command_interface_exists("flange_vacuum/vacuum")); + + // Check initial values + hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j1a_s = rm.claim_state_interface("joint1/acceleration"); + hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface("joint2/velocity"); + hardware_interface::LoanedStateInterface j2a_s = rm.claim_state_interface("joint2/acceleration"); + hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j2a_c = + rm.claim_command_interface("joint2/acceleration"); + + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_TRUE(std::isnan(j1p_c.get_value())); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_TRUE(std::isnan(j2a_c.get_value())); + + // Test error management in prepare mode switch + ASSERT_EQ( // joint2 has non 'position', 'velocity', or 'acceleration' interface + rm.prepare_command_mode_switch({"joint1/position", "joint2/effort"}, {}), false); + ASSERT_EQ( // joint1 has two interfaces + rm.prepare_command_mode_switch({"joint1/position", "joint1/acceleration"}, {}), false); + + // switch controller mode as controller manager is doing - gpio itf 'vacuum' will be ignored + ASSERT_EQ( + rm.prepare_command_mode_switch( + {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), + true); + ASSERT_EQ( + rm.perform_command_mode_switch( + {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), + true); + + // set some new values in commands + j1p_c.set_value(0.11); + j2a_c.set_value(3.5); + + // State values should not be changed + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + EXPECT_EQ(3.45, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(-33.4, j1v_s.get_value()); + EXPECT_NEAR(-334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_EQ(0.0, j2v_s.get_value()); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_NEAR(334.0, j1a_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(2.78, j2p_s.get_value()); + EXPECT_NEAR(0.35, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_TRUE(std::isnan(j1v_c.get_value())); + ASSERT_TRUE(std::isnan(j2v_c.get_value())); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // switch controller mode as controller manager is doing + ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + ASSERT_EQ(rm.perform_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); + + // set some new values in commands + j1v_c.set_value(0.5); + j2v_c.set_value(2.0); + + // State values should not be changed + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // write() does not change values + rm.write(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.0, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(2.815, j2p_s.get_value()); + EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); + EXPECT_EQ(3.5, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics (both velocity mode) + rm.read(TIME, PERIOD); + EXPECT_EQ(0.11, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(5.0, j1a_s.get_value()); + EXPECT_EQ(2.885, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_NEAR(13.0, j2a_s.get_value(), COMPARE_DELTA); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); + + // read() mirrors commands to states and calculate dynamics (both velocity mode) + rm.read(TIME, PERIOD); + EXPECT_EQ(0.16, j1p_s.get_value()); + EXPECT_EQ(0.5, j1v_s.get_value()); + EXPECT_EQ(0.0, j1a_s.get_value()); + EXPECT_EQ(3.085, j2p_s.get_value()); + EXPECT_EQ(2.0, j2v_s.get_value()); + EXPECT_EQ(0.0, j2a_s.get_value()); + ASSERT_EQ(0.11, j1p_c.get_value()); + ASSERT_EQ(0.5, j1v_c.get_value()); + ASSERT_EQ(2.0, j2v_c.get_value()); + ASSERT_EQ(3.5, j2a_c.get_value()); +} + TEST_F(TestGenericSystem, disabled_commands_flag_is_active) { auto urdf = @@ -1641,7 +1872,6 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ASSERT_EQ(2u, rm.command_interface_keys().size()); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); - // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 5f9c09e95e..7cf55b50d3 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -16,6 +16,7 @@ #include #include "hardware_interface/actuator_interface.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::ActuatorInterface; using hardware_interface::CommandInterface; @@ -76,12 +77,17 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_ == 28282828.0) + if (velocity_command_ == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_ = 0.0; return return_type::ERROR; } + // simulate deactivate on read + if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } // The next line is for the testing purposes. We need value to be changed to be sure that // the feedback from hardware to controllers in the chain is working as it should. // This makes value checks clearer and confirms there is no "state = command" line or some @@ -93,12 +99,17 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_ == 23232323.0) + if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_ = 0.0; return return_type::ERROR; } + // simulate deactivate on write + if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } return return_type::OK; } diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 7477734cff..3326cb893b 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -18,6 +18,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::CommandInterface; using hardware_interface::return_type; @@ -81,24 +82,34 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_[0] == 28282828) + if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_[0] = 0.0; return return_type::ERROR; } + // simulate deactivate on read + if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_[0] == 23232323) + if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_[0] = 0.0; return return_type::ERROR; } + // simulate deactivate on write + if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } return return_type::OK; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index aedf2b862d..c80cdbb5bf 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -28,6 +28,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; @@ -1538,6 +1539,122 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } } + void check_read_or_write_deactivate( + FunctionT method_that_deactivates, FunctionT other_method, const double deactivate_value) + { + // define state to set components to + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + // deactivate for TEST_ACTUATOR_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value); + claimed_itfs[1].set_value(deactivate_value - 10.0); + { + // deactivate on error + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + + // reactivate + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // deactivate for TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value - 10.0); + claimed_itfs[1].set_value(deactivate_value); + { + // deactivate on flag + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + check_if_interface_available(true, true); + // re-activate + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value); + claimed_itfs[1].set_value(deactivate_value); + { + // deactivate on flag + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + check_if_interface_available(true, true); + // re-activate + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + } + public: std::shared_ptr rm; std::vector claimed_itfs; @@ -1546,8 +1663,6 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); // values to set to hardware to simulate failure on read and write - static constexpr double READ_FAIL_VALUE = 28282828.0; - static constexpr double WRITE_FAIL_VALUE = 23232323.0; }; TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) @@ -1558,7 +1673,7 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) // check read methods failures check_read_or_write_failure( std::bind(&TestableResourceManager::read, rm, _1, _2), - std::bind(&TestableResourceManager::write, rm, _1, _2), READ_FAIL_VALUE); + std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_FAIL_VALUE); } TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) @@ -1569,7 +1684,29 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) // check write methods failures check_read_or_write_failure( std::bind(&TestableResourceManager::write, rm, _1, _2), - std::bind(&TestableResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE); + std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_FAIL_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_read) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check read methods failures + check_read_or_write_deactivate( + std::bind(&TestableResourceManager::read, rm, _1, _2), + std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_DEACTIVATE_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check write methods failures + check_read_or_write_deactivate( + std::bind(&TestableResourceManager::write, rm, _1, _2), + std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_DEACTIVATE_VALUE); } TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 46acee3f8d..c994efe036 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index f1c0365cfd..fc2cd26f9e 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.18.0 + 3.19.1 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 272c2d3fe4..5032992acf 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 15a379933a..ab9112ad81 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.18.0 + 3.19.1 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 d633c415d1..d1305ad796 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.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp new file mode 100644 index 0000000000..eb2bbf24c7 --- /dev/null +++ b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp @@ -0,0 +1,28 @@ +// Copyright (c) 2023, FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Felix Exner + +#ifndef ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ +#define ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ +namespace test_constants +{ +/// Constants defining special values used inside tests to trigger things like deactivate or errors +/// on read/write. +constexpr double READ_FAIL_VALUE = 28282828.0; +constexpr double WRITE_FAIL_VALUE = 23232323.0; +constexpr double READ_DEACTIVATE_VALUE = 29292929.0; +constexpr double WRITE_DEACTIVATE_VALUE = 24242424.0; +} // namespace test_constants +#endif // ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index c77b828276..ccf21c5295 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.18.0 + 3.19.1 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index a5a153faf1..ba85f2a928 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 5880c054cb..5eada2acb6 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -148,7 +148,7 @@ load_controller .. code-block:: console $ ros2 control load_controller -h - usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set_state {configure,activate}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name + usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set-state {inactive,active}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name Load a controller in a controller manager @@ -159,7 +159,7 @@ load_controller -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) - --set_state {inactive,active} + --set-state {inactive,active} Set the state of the loaded controller -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 92eba51b62..d91f66d4ea 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.18.0 + 3.19.1 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index aa2b19b830..fb5c06e6f4 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.18.0", + version="3.19.1", 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 9996ff5a85..5fd421b0cb 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 4888459d49..3f56fd18b6 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.18.0 + 3.19.1 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 318cf556d2..001968714a 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.18.0", + version="3.19.1", 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 c45b040c1b..f507b8921f 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 9e2b2fded2..4d68170366 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.18.0 + 3.19.1 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