diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index c26bfa1f34..92ce384b50 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -21,7 +21,7 @@ jobs: with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} @@ -31,6 +31,7 @@ jobs: controller_manager hardware_interface joint_limits + hardware_interface_testing transmission_interface vcs-repo-file-url: | @@ -42,12 +43,12 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.4 + - uses: codecov/codecov-action@v4.0.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v4.2.0 + - uses: actions/upload-artifact@v4.3.0 with: name: colcon-logs-ubuntu-22.04-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 16bed93a7a..6e8e9b5e71 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -2,6 +2,19 @@ name: ROS Lint on: pull_request: +env: + package-name: + controller_interface + controller_manager + controller_manager_msgs + hardware_interface + hardware_interface_testing + joint_limits + ros2controlcli + ros2_control + ros2_control_test_assets + transmission_interface + jobs: ament_lint: name: ament_${{ matrix.linter }} @@ -19,17 +32,7 @@ jobs: with: distribution: rolling linter: ${{ matrix.linter }} - package-name: - controller_interface - controller_manager - controller_manager_msgs - hardware_interface - joint_limits - ros2controlcli - ros2_control - ros2_control_test_assets - transmission_interface - + package-name: ${{ env.package-name }} ament_lint_100: name: ament_${{ matrix.linter }} runs-on: ubuntu-latest @@ -45,13 +48,4 @@ jobs: distribution: rolling linter: cpplint arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: - controller_interface - controller_manager - controller_manager_msgs - hardware_interface - joint_limits - ros2controlcli - ros2_control - ros2_control_test_assets - transmission_interface + package-name: ${{ env.package-name }} diff --git a/.github/workflows/humble-binary-build-main.yml b/.github/workflows/humble-binary-build-main.yml index a438d7a9e5..1fafd01c9e 100644 --- a/.github/workflows/humble-binary-build-main.yml +++ b/.github/workflows/humble-binary-build-main.yml @@ -4,8 +4,6 @@ name: Humble Binary Build - main on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-binary-build-testing.yml b/.github/workflows/humble-binary-build-testing.yml index 5a61534d4e..99ff6ccd4a 100644 --- a/.github/workflows/humble-binary-build-testing.yml +++ b/.github/workflows/humble-binary-build-testing.yml @@ -4,8 +4,6 @@ name: Humble Binary Build - testing on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index 0db1f58210..482504c59a 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -15,16 +15,24 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: humble + skip-packages-build: rqt_controller_manager + skip-packages-test: rqt_controller_manager controller_manager_msgs container: ghcr.io/ros-controls/ros:humble-debian steps: - uses: actions/checkout@v4 with: path: src/ros2_control - - name: Build and test + ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} + - name: Build workspace shell: bash run: | source /opt/ros2_ws/install/setup.bash vcs import src < src/ros2_control/ros2_control.${{ env.ROS_DISTRO }}.repos - colcon build --packages-skip rqt_controller_manager - colcon test --packages-skip rqt_controller_manager control_msgs controller_manager_msgs + colcon build --packages-up-to $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages-build }} + - name: Test workspace + shell: bash + continue-on-error: true + run: | + source /opt/ros2_ws/install/setup.bash + colcon test --packages-select $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages-test }} colcon test-result --verbose diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index ed37092520..9d3b095e39 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -14,18 +14,29 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: humble + skip-packages: rqt_controller_manager container: ghcr.io/ros-controls/ros:humble-rhel steps: - uses: actions/checkout@v4 with: path: src/ros2_control + ref: ${{ github.event_name == 'schedule' && 'humble' || '' }} - name: Install dependencies run: | + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash rosdep update rosdep install -iyr --from-path src/ros2_control || true - - name: Build and test + - name: Build workspace + run: | + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + colcon build --packages-up-to $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages }} + - name: Test workspace + shell: bash + continue-on-error: true run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build --packages-skip rqt_controller_manager - colcon test --packages-skip rqt_controller_manager ros2controlcli + source /opt/ros2_ws/install/local_setup.bash + colcon test --packages-select $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages }} colcon test-result --verbose diff --git a/.github/workflows/humble-semi-binary-build-main.yml b/.github/workflows/humble-semi-binary-build-main.yml index 539477a302..88591b69f7 100644 --- a/.github/workflows/humble-semi-binary-build-main.yml +++ b/.github/workflows/humble-semi-binary-build-main.yml @@ -3,8 +3,6 @@ name: Humble Semi-Binary Build - main on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-semi-binary-build-testing.yml b/.github/workflows/humble-semi-binary-build-testing.yml index 889822e7f2..3d6b2717e3 100644 --- a/.github/workflows/humble-semi-binary-build-testing.yml +++ b/.github/workflows/humble-semi-binary-build-testing.yml @@ -3,8 +3,6 @@ name: Humble Semi-Binary Build - testing on: workflow_dispatch: - branches: - - humble pull_request: branches: - humble diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index ff0fd62e05..a40d53f8e3 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -1,8 +1,6 @@ name: Humble Source Build on: workflow_dispatch: - branches: - - humble push: branches: - humble diff --git a/.github/workflows/iron-binary-build-main.yml b/.github/workflows/iron-binary-build-main.yml index bc2dbd96cc..440b573cb9 100644 --- a/.github/workflows/iron-binary-build-main.yml +++ b/.github/workflows/iron-binary-build-main.yml @@ -4,8 +4,6 @@ name: Iron Binary Build - main on: workflow_dispatch: - branches: - - iron pull_request: branches: - iron diff --git a/.github/workflows/iron-binary-build-testing.yml b/.github/workflows/iron-binary-build-testing.yml index 512c4b55e4..78e13f2528 100644 --- a/.github/workflows/iron-binary-build-testing.yml +++ b/.github/workflows/iron-binary-build-testing.yml @@ -4,8 +4,6 @@ name: Iron Binary Build - testing on: workflow_dispatch: - branches: - - iron pull_request: branches: - iron diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index 405e4f9135..35db1eb8d9 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -15,16 +15,24 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: iron + skip-packages-build: rqt_controller_manager + skip-packages-test: rqt_controller_manager controller_manager_msgs container: ghcr.io/ros-controls/ros:iron-debian steps: - uses: actions/checkout@v4 with: path: src/ros2_control - - name: Build and test + ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} + - name: Build workspace shell: bash run: | source /opt/ros2_ws/install/setup.bash vcs import src < src/ros2_control/ros2_control.${{ env.ROS_DISTRO }}.repos - colcon build --packages-skip rqt_controller_manager - colcon test --packages-skip rqt_controller_manager control_msgs controller_manager_msgs + colcon build --packages-up-to $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages-build }} + - name: Test workspace + shell: bash + continue-on-error: true + run: | + source /opt/ros2_ws/install/setup.bash + colcon test --packages-select $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages-test }} colcon test-result --verbose diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index fc48bd80ea..435c3e9316 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -8,25 +8,36 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' - jobs: iron_rhel_binary: name: Iron RHEL binary build runs-on: ubuntu-latest env: ROS_DISTRO: iron + skip-packages: rqt_controller_manager container: ghcr.io/ros-controls/ros:iron-rhel steps: - uses: actions/checkout@v4 with: path: src/ros2_control + ref: ${{ github.event_name == 'schedule' && 'iron' || '' }} - name: Install dependencies run: | + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash rosdep update rosdep install -iyr --from-path src/ros2_control || true - - name: Build and test + - name: Build workspace + # source also underlay workspace with generate_parameter_library on rhel9 + run: | + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + colcon build --packages-up-to $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages }} + - name: Test workspace + shell: bash + continue-on-error: true run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build --packages-skip rqt_controller_manager - colcon test --packages-skip rqt_controller_manager ros2controlcli + source /opt/ros2_ws/install/local_setup.bash + colcon test --packages-select $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages }} colcon test-result --verbose diff --git a/.github/workflows/iron-semi-binary-build-main.yml b/.github/workflows/iron-semi-binary-build-main.yml index 1399e8b32b..c5697d6e74 100644 --- a/.github/workflows/iron-semi-binary-build-main.yml +++ b/.github/workflows/iron-semi-binary-build-main.yml @@ -3,8 +3,6 @@ name: Iron Semi-Binary Build - main on: workflow_dispatch: - branches: - - iron pull_request: branches: - iron diff --git a/.github/workflows/iron-semi-binary-build-testing.yml b/.github/workflows/iron-semi-binary-build-testing.yml index b29f798931..0055e38cba 100644 --- a/.github/workflows/iron-semi-binary-build-testing.yml +++ b/.github/workflows/iron-semi-binary-build-testing.yml @@ -3,8 +3,6 @@ name: Iron Semi-Binary Build - testing on: workflow_dispatch: - branches: - - iron pull_request: branches: - iron diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index 1e9d865c49..34372a4178 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -1,8 +1,6 @@ name: Iron Source Build on: workflow_dispatch: - branches: - - iron push: branches: - iron diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 050005a648..fa96b7288c 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -32,7 +32,7 @@ jobs: - uses: actions/checkout@v4 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.3.5 + - uses: ros-tooling/action-ros-ci@0.3.6 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package @@ -50,7 +50,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@v4.2.0 + - uses: actions/upload-artifact@v4.3.0 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log diff --git a/.github/workflows/rolling-binary-build-main.yml b/.github/workflows/rolling-binary-build-main.yml index 05a9b9c0b2..1777810661 100644 --- a/.github/workflows/rolling-binary-build-main.yml +++ b/.github/workflows/rolling-binary-build-main.yml @@ -4,8 +4,6 @@ name: Rolling Binary Build - main on: workflow_dispatch: - branches: - - master pull_request: branches: - master diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index 098af45029..efe1422404 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -15,16 +15,24 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: rolling + skip-packages: rqt_controller_manager container: ghcr.io/ros-controls/ros:rolling-debian steps: - uses: actions/checkout@v4 with: path: src/ros2_control - - name: Build and test + # default behavior is correct on master branch + # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} + - name: Build workspace shell: bash run: | source /opt/ros2_ws/install/setup.bash vcs import src < src/ros2_control/ros2_control.${{ env.ROS_DISTRO }}.repos - colcon build --packages-skip rqt_controller_manager - colcon test --packages-skip rqt_controller_manager + colcon build --packages-up-to $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages }} + - name: Test workspace + shell: bash + continue-on-error: true + run: | + source /opt/ros2_ws/install/setup.bash + colcon test --packages-select $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages }} colcon test-result --verbose diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 06a5411c24..8f2638405b 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -8,25 +8,37 @@ on: # Run every day to detect flakiness and broken dependencies - cron: '03 1 * * *' - jobs: rolling_rhel_binary: name: Rolling RHEL binary build runs-on: ubuntu-latest env: ROS_DISTRO: rolling + skip-packages: rqt_controller_manager container: ghcr.io/ros-controls/ros:rolling-rhel steps: - uses: actions/checkout@v4 with: path: src/ros2_control + # default behavior is correct on master branch + # ref: ${{ github.event_name == 'schedule' && 'master' || '' }} - name: Install dependencies run: | + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash rosdep update rosdep install -iyr --from-path src/ros2_control || true - - name: Build and test + - name: Build workspace + # source also underlay workspace with generate_parameter_library on rhel9 + run: | + source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash + source /opt/ros2_ws/install/local_setup.bash + colcon build --packages-up-to $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages }} + - name: Test workspace + shell: bash + continue-on-error: true run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash - colcon build --packages-skip rqt_controller_manager - colcon test --packages-skip rqt_controller_manager ros2controlcli + source /opt/ros2_ws/install/local_setup.bash + colcon test --packages-select $(colcon list --paths src/ros2_control/* --names-only) --packages-skip ${{ env.skip-packages }} colcon test-result --verbose diff --git a/.github/workflows/rolling-semi-binary-build-testing.yml b/.github/workflows/rolling-semi-binary-build-testing.yml index 4ddfcf5057..17d290c45b 100644 --- a/.github/workflows/rolling-semi-binary-build-testing.yml +++ b/.github/workflows/rolling-semi-binary-build-testing.yml @@ -3,8 +3,6 @@ name: Rolling Semi-Binary Build - testing on: workflow_dispatch: - branches: - - master pull_request: branches: - master diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index b96ad0298e..475e509e0f 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -1,8 +1,6 @@ name: Rolling Source Build on: workflow_dispatch: - branches: - - master push: branches: - master diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index d2265d1442..c6471c90f6 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.4.0 (2024-01-31) +------------------ + 4.3.0 (2024-01-20) ------------------ * Issue 695: Changing 'namespace\_' variables to 'node_namespace' to make it more explicit (`#1239 `_) diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index d02c422451..19a501dc62 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -40,6 +40,8 @@ if(BUILD_TESTING) ) ament_add_gmock(test_controller_with_options test/test_controller_with_options.cpp) + install(FILES test/test_controller_node_options.yaml + DESTINATION test) target_link_libraries(test_controller_with_options controller_interface ) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2e953ee932..6ce483d1ff 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -113,10 +113,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy void release_interfaces(); CONTROLLER_INTERFACE_PUBLIC - virtual return_type init( + return_type init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & node_namespace = "", - const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true)); + const std::string & node_namespace, const rclcpp::NodeOptions & node_options); /// Custom configure method to read additional parameters for controller-nodes /* @@ -159,6 +158,23 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC const std::string & get_robot_description() const; + /** + * Method used by the controller_manager for base NodeOptions to instantiate the Lifecycle node + * of the controller upon loading the controller. + * + * \note The controller_manager will modify these NodeOptions in case a params file is passed + * by the spawner to load the controller parameters or when controllers are loaded in simulation + * (see ros2_control#1311, ros2_controllers#698 , ros2_controllers#795,ros2_controllers#966 for + * more details) + * + * @returns NodeOptions required for the configuration of the controller lifecycle node + */ + CONTROLLER_INTERFACE_PUBLIC + virtual rclcpp::NodeOptions define_custom_node_options() const + { + return rclcpp::NodeOptions().enable_logger_service(true); + } + /// Declare and initialize a parameter with a type. /** * diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 4d1911123a..919fdf8d20 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.3.0 + 4.4.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 560feff5f9..e48e1d21b3 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -28,10 +28,10 @@ return_type ControllerInterfaceBase::init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, const std::string & node_namespace, const rclcpp::NodeOptions & node_options) { + urdf_ = urdf; node_ = std::make_shared( controller_name, node_namespace, node_options, false); // disable LifecycleNode service interfaces - urdf_ = urdf; try { diff --git a/controller_interface/test/test_chainable_controller_interface.cpp b/controller_interface/test/test_chainable_controller_interface.cpp index d81d2bfdad..47487e019c 100644 --- a/controller_interface/test/test_chainable_controller_interface.cpp +++ b/controller_interface/test/test_chainable_controller_interface.cpp @@ -21,7 +21,10 @@ TEST_F(ChainableControllerInterfaceTest, default_returns) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); EXPECT_TRUE(controller.is_chainable()); @@ -33,7 +36,10 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); @@ -50,7 +56,10 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correc TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); // expect empty return because storage is not resized @@ -64,7 +73,10 @@ TEST_F(ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); controller.set_name_prefix_of_reference_interfaces("some_not_correct_interface_prefix"); @@ -79,7 +91,10 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); auto reference_interfaces = controller.export_reference_interfaces(); @@ -126,7 +141,10 @@ TEST_F(ChainableControllerInterfaceTest, test_update_logic) TestableChainableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); EXPECT_FALSE(controller.is_in_chained_mode()); diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 24e780fab3..3976b2a81e 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -38,7 +38,10 @@ TEST(TestableControllerInterface, init) ASSERT_THROW(controller.get_node(), std::runtime_error); // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 10.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options), + controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); // update_rate is set to default 0 @@ -60,7 +63,10 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) TestableControllerInterface controller; // initialize, create node - ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME, "", 1.0), controller_interface::return_type::OK); + const auto node_options = controller.define_custom_node_options(); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options), + controller_interface::return_type::OK); // initialize executor to be able to get parameter update auto executor = @@ -122,8 +128,10 @@ TEST(TestableControllerInterfaceInitError, init_with_error) TestableControllerInterfaceInitError controller; // initialize, create node + const auto node_options = controller.define_custom_node_options(); ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 100.0), controller_interface::return_type::ERROR); + controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options), + controller_interface::return_type::ERROR); rclcpp::shutdown(); } @@ -137,8 +145,10 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure) TestableControllerInterfaceInitFailure controller; // initialize, create node + const auto node_options = controller.define_custom_node_options(); ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 50.0), controller_interface::return_type::ERROR); + controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), + controller_interface::return_type::ERROR); rclcpp::shutdown(); } diff --git a/controller_interface/test/test_controller_node_options.yaml b/controller_interface/test/test_controller_node_options.yaml new file mode 100644 index 0000000000..0c23c7f9d8 --- /dev/null +++ b/controller_interface/test/test_controller_node_options.yaml @@ -0,0 +1,6 @@ +controller_name: + ros__parameters: + parameter_list: + parameter1: 20.0 + parameter2: 23.14 + parameter3: -52.323 diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 51169e945c..1e22239215 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -16,7 +16,7 @@ #include #include - +#include "ament_index_cpp/get_package_prefix.hpp" #include "rclcpp/rclcpp.hpp" class FriendControllerWithOptions : public controller_with_options::ControllerWithOptions @@ -42,11 +42,14 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ(controller.init("controller_name", "", 50.0), controller_interface::return_type::OK); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), + controller_interface::return_type::OK); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_TRUE(node_options.arguments().empty()); // checks that the parameters have been correctly processed EXPECT_EQ(controller.params.size(), 3u); EXPECT_EQ(controller.params["parameter1"], 1.); @@ -55,6 +58,98 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::shutdown(); } +TEST(ControllerWithOption, init_with_node_options_arguments_parameters) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + // creates the controller + FriendControllerWithOptions controller; + auto controller_node_options = controller.define_custom_node_options(); + controller_node_options.arguments( + {"--ros-args", "-p", "parameter_list.parameter1:=1.", "-p", "parameter_list.parameter2:=2.", + "-p", "parameter_list.parameter3:=3."}); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller_node_options), + controller_interface::return_type::OK); + // checks that the node options have been updated + const auto & node_options = controller.get_node()->get_node_options(); + EXPECT_TRUE(node_options.allow_undeclared_parameters()); + EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_EQ(7lu, node_options.arguments().size()); + // checks that the parameters have been correctly processed + EXPECT_EQ(controller.params.size(), 3u); + EXPECT_EQ(controller.params["parameter1"], 1.); + EXPECT_EQ(controller.params["parameter2"], 2.); + EXPECT_EQ(controller.params["parameter3"], 3.); + rclcpp::shutdown(); +} + +TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + // creates the controller + FriendControllerWithOptions controller; + const std::string params_file_path = ament_index_cpp::get_package_prefix("controller_interface") + + "/test/test_controller_node_options.yaml"; + std::cerr << params_file_path << std::endl; + auto controller_node_options = controller.define_custom_node_options(); + controller_node_options.arguments({"--ros-args", "--params-file", params_file_path}); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller_node_options), + controller_interface::return_type::OK); + // checks that the node options have been updated + const auto & node_options = controller.get_node()->get_node_options(); + EXPECT_TRUE(node_options.allow_undeclared_parameters()); + EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_EQ(3lu, node_options.arguments().size()); + // checks that the parameters have been correctly processed + EXPECT_EQ(controller.params.size(), 3u); + EXPECT_EQ(controller.params["parameter1"], 20.0); + EXPECT_EQ(controller.params["parameter2"], 23.14); + EXPECT_EQ(controller.params["parameter3"], -52.323); + bool use_sim_time(true); + controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false); + ASSERT_FALSE(use_sim_time); + rclcpp::shutdown(); +} + +TEST( + ControllerWithOption, init_with_node_options_arguments_parameters_file_and_override_command_line) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + // creates the controller + FriendControllerWithOptions controller; + const std::string params_file_path = ament_index_cpp::get_package_prefix("controller_interface") + + "/test/test_controller_node_options.yaml"; + std::cerr << params_file_path << std::endl; + auto controller_node_options = controller.define_custom_node_options(); + controller_node_options.arguments( + {"--ros-args", "--params-file", params_file_path, "-p", "parameter_list.parameter1:=562.785", + "-p", "use_sim_time:=true"}); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller_node_options), + controller_interface::return_type::OK); + // checks that the node options have been updated + const auto & node_options = controller.get_node()->get_node_options(); + EXPECT_TRUE(node_options.allow_undeclared_parameters()); + EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); + EXPECT_EQ(7lu, node_options.arguments().size()); + // checks that the parameters have been correctly processed + EXPECT_EQ(controller.params.size(), 3u); + EXPECT_EQ(controller.params["parameter1"], 562.785); + EXPECT_EQ(controller.params["parameter2"], 23.14); + EXPECT_EQ(controller.params["parameter3"], -52.323); + bool use_sim_time(false); + controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false); + ASSERT_TRUE(use_sim_time); + rclcpp::shutdown(); +} + TEST(ControllerWithOption, init_without_overrides) { // mocks the declaration of overrides parameters in a yaml file @@ -63,7 +158,9 @@ TEST(ControllerWithOption, init_without_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ(controller.init("controller_name", "", 50.0), controller_interface::return_type::ERROR); + EXPECT_EQ( + controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), + controller_interface::return_type::ERROR); // checks that the node options have been updated const auto & node_options = controller.get_node()->get_node_options(); EXPECT_TRUE(node_options.allow_undeclared_parameters()); diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index 1416a7d2df..fc909df361 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -36,36 +36,19 @@ class ControllerWithOptions : public controller_interface::ControllerInterface ControllerWithOptions() = default; LifecycleNodeInterface::CallbackReturn on_init() override { - return LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - controller_interface::return_type init( - const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & node_namespace = "", - const rclcpp::NodeOptions & node_options = - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)) override - { - ControllerInterface::init(controller_name, urdf, cm_update_rate, node_namespace, node_options); - - switch (on_init()) - { - case LifecycleNodeInterface::CallbackReturn::SUCCESS: - break; - case LifecycleNodeInterface::CallbackReturn::ERROR: - case LifecycleNodeInterface::CallbackReturn::FAILURE: - return controller_interface::return_type::ERROR; - } if (get_node()->get_parameters("parameter_list", params)) { RCLCPP_INFO_STREAM(get_node()->get_logger(), "I found " << params.size() << " parameters."); - return controller_interface::return_type::OK; - } - else - { - return controller_interface::return_type::ERROR; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } + return LifecycleNodeInterface::CallbackReturn::FAILURE; + } + + rclcpp::NodeOptions define_custom_node_options() const override + { + return rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); } controller_interface::InterfaceConfiguration command_interface_configuration() const override diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 3699d3ec94..f5bc17f0f3 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.4.0 (2024-01-31) +------------------ +* Move `test_components` to own package (`#1325 `_) +* Fix controller parameter loading issue in different cases (`#1293 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.3.0 (2024-01-20) ------------------ * [CM] Better readability and maintainability: rename variables, move code to more logical places 🔧 (`#1227 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 33e0004e78..e58ff84c3d 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.3.0 + 4.4.0 Description of controller_manager Bence Magyar Denis Štogl @@ -29,6 +29,7 @@ std_msgs ament_cmake_gmock + hardware_interface_testing ros2_control_test_assets diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ab57c5a196..4becabb0a4 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -877,14 +877,15 @@ controller_interface::return_type ControllerManager::switch_controller( strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } - RCLCPP_DEBUG(get_logger(), "Switching controllers:"); + RCLCPP_DEBUG(get_logger(), "Activating controllers:"); for (const auto & controller : activate_controllers) { - RCLCPP_DEBUG(get_logger(), "- Activating controller '%s'", controller.c_str()); + RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str()); } + RCLCPP_DEBUG(get_logger(), "Deactivating controllers:"); for (const auto & controller : deactivate_controllers) { - RCLCPP_DEBUG(get_logger(), "- Deactivating controller '%s'", controller.c_str()); + RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str()); } const auto list_controllers = [this, strictness]( @@ -1222,6 +1223,17 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::OK; } + RCLCPP_DEBUG(get_logger(), "Request for command interfaces from activating controllers:"); + for (const auto & interface : activate_command_interface_request_) + { + RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); + } + RCLCPP_DEBUG(get_logger(), "Release of command interfaces from deactivating controllers:"); + for (const auto & interface : deactivate_command_interface_request_) + { + RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); + } + if ( !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) { @@ -2606,7 +2618,7 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( auto check_for_element = [](const auto & list, const auto & element) { return std::find(list.begin(), list.end(), element) != list.end(); }; - rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true); + rclcpp::NodeOptions controller_node_options = controller.c->define_custom_node_options(); std::vector node_options_arguments = controller_node_options.arguments(); const std::string ros_args_arg = "--ros-args"; if (controller.info.parameters_file.has_value()) diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp index a106d5cbdf..1b2276bf3c 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp @@ -31,14 +31,6 @@ TestControllerFailedInit::on_init() return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } -controller_interface::return_type TestControllerFailedInit::init( - const std::string & /* controller_name */, const std::string & /* urdf */, - unsigned int /*cm_update_rate*/, const std::string & /*node_namespace*/, - const rclcpp::NodeOptions & /*node_options*/) -{ - return controller_interface::return_type::ERROR; -} - controller_interface::return_type TestControllerFailedInit::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp index 315b0745b0..3f24df4e29 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp @@ -38,21 +38,19 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac CONTROLLER_MANAGER_PUBLIC virtual ~TestControllerFailedInit() = default; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::return_type init( - const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & node_namespace = "", - const rclcpp::NodeOptions & node_options = - rclcpp::NodeOptions() - .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true)) override; - controller_interface::InterfaceConfiguration command_interface_configuration() const override { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE}; } + rclcpp::NodeOptions define_custom_node_options() const override + { + return rclcpp::NodeOptions() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + } + controller_interface::InterfaceConfiguration state_interface_configuration() const override { return controller_interface::InterfaceConfiguration{ diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 390a7365f0..ed443ea3d4 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -28,8 +28,8 @@ using test_controller::TEST_CONTROLLER_CLASS_NAME; using ::testing::_; using ::testing::Return; -const auto controller_name1 = "test_controller1"; -const auto controller_name2 = "test_controller2"; +const auto CONTROLLER_NAME_1 = "test_controller1"; +const auto CONTROLLER_NAME_2 = "test_controller2"; using strvec = std::vector; class TestLoadController : public ControllerManagerFixture @@ -53,7 +53,7 @@ TEST_F(TestLoadController, load_controller_failed_init) TEST_F(TestLoadController, configuring_non_loaded_controller_fails) { // try configure non-loaded controller - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); } TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) @@ -82,7 +82,7 @@ class TestLoadedController : public TestLoadController { TestLoadController::SetUp(); - controller_if = cm_->load_controller(controller_name1, TEST_CONTROLLER_CLASS_NAME); + controller_if = cm_->load_controller(CONTROLLER_NAME_1, TEST_CONTROLLER_CLASS_NAME); ASSERT_NE(controller_if, nullptr); } @@ -93,7 +93,7 @@ class TestLoadedController : public TestLoadController controller_interface::return_type::OK) { switch_test_controllers( - strvec{controller_name1}, strvec{}, strictness, expected_future_status, + strvec{CONTROLLER_NAME_1}, strvec{}, strictness, expected_future_status, expected_interface_status); } @@ -104,7 +104,7 @@ class TestLoadedController : public TestLoadController controller_interface::return_type::OK) { switch_test_controllers( - strvec{}, strvec{controller_name1}, strictness, expected_future_status, + strvec{}, strvec{CONTROLLER_NAME_1}, strictness, expected_future_status, expected_interface_status); } }; @@ -121,7 +121,7 @@ TEST_P(TestLoadedControllerParametrized, load_and_configure_one_known_controller EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); - cm_->configure_controller(controller_name1); + cm_->configure_controller(CONTROLLER_NAME_1); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); } @@ -129,7 +129,7 @@ TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) { const auto test_param = GetParam(); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } @@ -138,7 +138,7 @@ TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) { const auto test_param = GetParam(); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); @@ -164,7 +164,7 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) // Activate configured controller { ControllerManagerRunner cm_runner(this); - cm_->configure_controller(controller_name1); + cm_->configure_controller(CONTROLLER_NAME_1); } start_test_controller(test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); @@ -180,11 +180,11 @@ TEST_P(TestLoadedControllerParametrized, can_not_configure_active_controller) { const auto test_param = GetParam(); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); // Can not configure active controller - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); } @@ -205,7 +205,7 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) test_param.strictness, std::future_status::ready, test_param.expected_return); // Can not configure finalize controller - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_state().id()); } @@ -213,7 +213,7 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u { const auto test_param = GetParam(); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); @@ -227,7 +227,7 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u test_controller->cleanup_calls = &cleanup_calls; // Configure from inactive state: controller can no be cleaned-up test_controller->simulate_cleanup_failure = true; - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::ERROR); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); EXPECT_EQ(0u, cleanup_calls); } @@ -236,7 +236,7 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure { const auto test_param = GetParam(); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); @@ -251,7 +251,7 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure test_controller->simulate_cleanup_failure = false; { ControllerManagerRunner cm_runner(this); - EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK); + EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); } ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); EXPECT_EQ(1u, cleanup_calls); @@ -270,8 +270,8 @@ class SwitchTest const auto UNSPECIFIED = 0; const auto EMPTY_STR_VEC = strvec{}; const auto NONEXISTENT_CONTROLLER = strvec{"nonexistent_controller"}; -const auto VALID_CONTROLLER = strvec{controller_name1}; -const auto VALID_PLUS_NONEXISTENT_CONTROLLERS = strvec{controller_name1, "nonexistent_controller"}; +const auto VALID_CONTROLLER = strvec{CONTROLLER_NAME_1}; +const auto VALID_PLUS_NONEXISTENT_CONTROLLERS = strvec{CONTROLLER_NAME_1, "nonexistent_controller"}; TEST_P(SwitchTest, EmptyListOrNonExistentTest) { @@ -375,10 +375,10 @@ class TestTwoLoadedControllers : public TestLoadController, void SetUp() override { TestLoadController::SetUp(); - controller_if1 = cm_->load_controller(controller_name1, TEST_CONTROLLER_CLASS_NAME); + controller_if1 = cm_->load_controller(CONTROLLER_NAME_1, TEST_CONTROLLER_CLASS_NAME); ASSERT_NE(controller_if1, nullptr); EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); - controller_if2 = cm_->load_controller(controller_name2, TEST_CONTROLLER_CLASS_NAME); + controller_if2 = cm_->load_controller(CONTROLLER_NAME_2, TEST_CONTROLLER_CLASS_NAME); ASSERT_NE(controller_if2, nullptr); EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); ASSERT_EQ( @@ -390,10 +390,10 @@ class TestTwoLoadedControllers : public TestLoadController, TEST_F(TestTwoLoadedControllers, load_and_configure_two_known_controllers) { - cm_->configure_controller(controller_name1); + cm_->configure_controller(CONTROLLER_NAME_1); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); - cm_->configure_controller(controller_name2); + cm_->configure_controller(CONTROLLER_NAME_2); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); } @@ -401,11 +401,11 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) { const auto test_param = GetParam(); - cm_->configure_controller(controller_name1); + cm_->configure_controller(CONTROLLER_NAME_1); // Start controller #1 RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); - switch_test_controllers(strvec{controller_name1}, strvec{}, test_param.strictness); + switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); @@ -418,10 +418,10 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) // TODO(destogl): fix this test for BEST_EFFORT - probably related to: // https://github.com/ros-controls/ros2_control/pull/582#issuecomment-1029931561 // switch_test_controllers( - // strvec{controller_name2}, strvec{controller_name1}, test_param.strictness, + // strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, test_param.strictness, // std::future_status::ready, controller_interface::return_type::ERROR); switch_test_controllers( - strvec{controller_name2}, strvec{controller_name1}, STRICT, std::future_status::ready, + strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, STRICT, std::future_status::ready, controller_interface::return_type::ERROR); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( @@ -429,31 +429,31 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) { ControllerManagerRunner cm_runner(this); - cm_->configure_controller(controller_name2); + cm_->configure_controller(CONTROLLER_NAME_2); } // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1"); - switch_test_controllers(strvec{}, strvec{controller_name1}, test_param.strictness); + switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_1}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); // Start controller 1 again RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); - switch_test_controllers(strvec{controller_name1}, strvec{}, test_param.strictness); + switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); // Stop controller 1, start controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1, starting controller #2"); switch_test_controllers( - strvec{controller_name2}, strvec{controller_name1}, test_param.strictness); + strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_state().id()); // Stop controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #2"); - switch_test_controllers(strvec{}, strvec{controller_name2}, test_param.strictness); + switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_2}, test_param.strictness); ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); } diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 6ebfcfd842..36245deaad 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.4.0 (2024-01-31) +------------------ + 4.3.0 (2024-01-20) ------------------ diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 5f8aedc8cf..1beaf47741 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.3.0 + 4.4.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 820c2c793f..b5fb894e0c 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.4.0 (2024-01-31) +------------------ +* Move `test_components` to own package (`#1325 `_) +* Fix controller parameter loading issue in different cases (`#1293 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.3.0 (2024-01-20) ------------------ * [RM] Fix crash for missing urdf in resource manager (`#1301 `_) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 35823b3ce9..2613ba735a 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -79,21 +79,6 @@ if(BUILD_TESTING) target_link_libraries(test_component_parser hardware_interface) ament_target_dependencies(test_component_parser ros2_control_test_assets) - add_library(test_components SHARED - test/test_components/test_actuator.cpp - test/test_components/test_sensor.cpp - test/test_components/test_system.cpp) - target_link_libraries(test_components hardware_interface) - ament_target_dependencies(test_components - pluginlib - ros2_control_test_assets - ) - install(TARGETS test_components - DESTINATION lib - ) - pluginlib_export_plugin_description_file( - hardware_interface test/test_components/test_components.xml) - add_library(test_hardware_components SHARED test/test_hardware_components/test_single_joint_actuator.cpp test/test_hardware_components/test_force_torque_sensor.cpp @@ -110,14 +95,6 @@ if(BUILD_TESTING) hardware_interface test/test_hardware_components/test_hardware_components.xml ) - ament_add_gmock(test_resource_manager test/test_resource_manager.cpp) - target_link_libraries(test_resource_manager hardware_interface) - ament_target_dependencies(test_resource_manager ros2_control_test_assets) - - ament_add_gmock(test_resource_manager_prepare_perform_switch test/test_resource_manager_prepare_perform_switch.cpp) - target_link_libraries(test_resource_manager_prepare_perform_switch hardware_interface) - ament_target_dependencies(test_resource_manager_prepare_perform_switch ros2_control_test_assets) - ament_add_gmock(test_generic_system test/mock_components/test_generic_system.cpp) target_include_directories(test_generic_system PRIVATE include) target_link_libraries(test_generic_system hardware_interface) diff --git a/hardware_interface/doc/hardware_interface_types_userdoc.rst b/hardware_interface/doc/hardware_interface_types_userdoc.rst index 54b2003568..cd1e475b20 100644 --- a/hardware_interface/doc/hardware_interface_types_userdoc.rst +++ b/hardware_interface/doc/hardware_interface_types_userdoc.rst @@ -85,7 +85,9 @@ They can be combined together within the different hardware component types (sys - + + 3.1 + diff --git a/hardware_interface/doc/mock_components_userdoc.rst b/hardware_interface/doc/mock_components_userdoc.rst index 4b54d5fa00..c075fdeafc 100644 --- a/hardware_interface/doc/mock_components_userdoc.rst +++ b/hardware_interface/doc/mock_components_userdoc.rst @@ -53,3 +53,19 @@ mimic (optional; string) multiplier (optional; double; default: 1; used if mimic joint is defined) Multiplier of values for mimicking joint defined in ``mimic`` parameter. Example: ``-2``. + +Per-interface Parameters +,,,,,,,,,,,,,,,,,,,,,,,, + +initial_value (optional; double) + Initial value of certain state interface directly after startup. Example: + + .. code-block:: xml + + + 3.45 + + + Note: This parameter is shared with the gazebo and gazebo classic plugins for + joint interfaces. For Mock components it is also possible to set initial + values for gpio or sensor state interfaces. diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 13db76cbe4..e3a343c8ae 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.3.0 + 4.4.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst new file mode 100644 index 0000000000..1ad077aa5c --- /dev/null +++ b/hardware_interface_testing/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package hardware_interface_testing +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +4.4.0 (2024-01-31) +------------------ +* Fix version +* Move `test_components` to own package (`#1325 `_) +* Contributors: Bence Magyar, Christoph Fröhlich diff --git a/hardware_interface_testing/CMakeLists.txt b/hardware_interface_testing/CMakeLists.txt new file mode 100644 index 0000000000..8e88a677bf --- /dev/null +++ b/hardware_interface_testing/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.16) +project(hardware_interface_testing LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + hardware_interface + lifecycle_msgs + pluginlib + rclcpp_lifecycle + ros2_control_test_assets +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(test_components SHARED +test/test_components/test_actuator.cpp +test/test_components/test_sensor.cpp +test/test_components/test_system.cpp) +ament_target_dependencies(test_components hardware_interface pluginlib ros2_control_test_assets) +install(TARGETS test_components +DESTINATION lib +) +pluginlib_export_plugin_description_file( +hardware_interface test/test_components/test_components.xml) + +if(BUILD_TESTING) + + find_package(ament_cmake_gmock REQUIRED) + + ament_add_gmock(test_resource_manager test/test_resource_manager.cpp) + ament_target_dependencies(test_resource_manager hardware_interface ros2_control_test_assets) + + ament_add_gmock(test_resource_manager_prepare_perform_switch test/test_resource_manager_prepare_perform_switch.cpp) + ament_target_dependencies(test_resource_manager_prepare_perform_switch hardware_interface ros2_control_test_assets) + +endif() + +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/hardware_interface_testing/README.md b/hardware_interface_testing/README.md new file mode 100644 index 0000000000..cba28d74a5 --- /dev/null +++ b/hardware_interface_testing/README.md @@ -0,0 +1,4 @@ +# hardware_interface_testing + +This package contains a set of hardware interfaces and controllers that can be used for other +packages to test their functionality. diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml new file mode 100644 index 0000000000..de7d75b2f5 --- /dev/null +++ b/hardware_interface_testing/package.xml @@ -0,0 +1,25 @@ + + + hardware_interface_testing + 4.4.0 + ros2_control hardware interface testing + Bence Magyar + Denis Štogl + Christoph Froehlich + Apache License 2.0 + + ament_cmake + + control_msgs + hardware_interface + lifecycle_msgs + pluginlib + rclcpp_lifecycle + ros2_control_test_assets + + ament_cmake_gmock + + + ament_cmake + + diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp similarity index 100% rename from hardware_interface/test/test_components/test_actuator.cpp rename to hardware_interface_testing/test/test_components/test_actuator.cpp diff --git a/hardware_interface/test/test_components/test_components.xml b/hardware_interface_testing/test/test_components/test_components.xml similarity index 100% rename from hardware_interface/test/test_components/test_components.xml rename to hardware_interface_testing/test/test_components/test_components.xml diff --git a/hardware_interface/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp similarity index 100% rename from hardware_interface/test/test_components/test_sensor.cpp rename to hardware_interface_testing/test/test_components/test_sensor.cpp diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp similarity index 100% rename from hardware_interface/test/test_components/test_system.cpp rename to hardware_interface_testing/test/test_components/test_system.cpp diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp similarity index 100% rename from hardware_interface/test/test_resource_manager.cpp rename to hardware_interface_testing/test/test_resource_manager.cpp diff --git a/hardware_interface/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp similarity index 100% rename from hardware_interface/test/test_resource_manager.hpp rename to hardware_interface_testing/test/test_resource_manager.hpp diff --git a/hardware_interface/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp similarity index 100% rename from hardware_interface/test/test_resource_manager_prepare_perform_switch.cpp rename to hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 1df9702430..aa9d8db111 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.4.0 (2024-01-31) +------------------ +* [Format] Correct formatting of JointLimits URDF file. (`#1339 `_) +* Add header to import limits from standard URDF definition (`#1298 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + 4.3.0 (2024-01-20) ------------------ diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 4e5e7097dd..08aeb05920 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.3.0 + 4.4.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/joint_limits_interface/CMakeLists.txt b/joint_limits_interface/CMakeLists.txt deleted file mode 100644 index a5d4577343..0000000000 --- a/joint_limits_interface/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -cmake_minimum_required(VERSION 3.5.0) -project(joint_limits_interface) - -find_package(ament_cmake REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(rclcpp REQUIRED) -find_package(urdf REQUIRED) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) - - ament_add_gtest(joint_limits_interface_test test/joint_limits_interface_test.cpp) - target_include_directories(joint_limits_interface_test PUBLIC include) - ament_target_dependencies(joint_limits_interface_test hardware_interface rclcpp) - - add_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) - target_include_directories(joint_limits_rosparam_test PUBLIC include ${GTEST_INCLUDE_DIRS}) - target_link_libraries(joint_limits_rosparam_test ${GTEST_LIBRARIES}) - ament_target_dependencies(joint_limits_rosparam_test rclcpp) - add_launch_test(test/joint_limits_rosparam.launch.py) - install( - TARGETS - joint_limits_rosparam_test - DESTINATION lib/${PROJECT_NAME} - ) - install( - FILES - test/joint_limits_rosparam.yaml - DESTINATION share/${PROJECT_NAME}/test - ) - - ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp) - target_include_directories(joint_limits_urdf_test PUBLIC include) - ament_target_dependencies(joint_limits_urdf_test rclcpp urdf) -endif() - -# Install headers -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} -) - -ament_export_include_directories(include) -ament_export_dependencies(rclcpp urdf) - -ament_package() diff --git a/joint_limits_interface/COLCON_IGNORE b/joint_limits_interface/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/joint_limits_interface/README.md b/joint_limits_interface/README.md deleted file mode 100644 index c77ccc2b75..0000000000 --- a/joint_limits_interface/README.md +++ /dev/null @@ -1,30 +0,0 @@ -## Joint Limits Interface ## - -### Overview ### - -**joint_limits_interface** contains data structures for representing joint limits, methods for populating them from common -formats such as URDF and the ROS parameter server, and methods for enforcing limits on different kinds of hardware interfaces. - -The **joint_limits_interface** is *not* used by controllers themselves (it does not implement a `HardwareInterface`) but -instead operates after the controllers have updated, in the `write()` method (or equivalent) of the robot abstraction. -Enforcing limits will *overwrite* the commands set by the controllers, it does not operate on a separate raw data buffer. - -There are two main elements involved in setting up a joint limits interface: - - - **Joint limits representation** - - **JointLimits** Position, velocity, acceleration, jerk and effort. - - **SoftJointLimits** Soft position limits, `k_p`, `k_v` (as described [here](http://www.ros.org/wiki/pr2_controller_manager/safety_limits)). - - **Loading from URDF** There are convenience methods for loading joint limits information - (position, velocity and effort), as well as soft joint limits information from the URDF. - - **Loading from ROS params** There are convenience methods for loading joint limits from the ROS parameter server - (position, velocity, acceleration, jerk and effort). Parameter specification is the same used in MoveIt, - with the addition that we also parse jerk and effort limits. - - - **Joint limits interface** - - - For **effort-controlled** joints, the soft-limits implementation from the PR2 has been ported. - - For **position-controlled** joints, a modified version of the PR2 soft limits has been implemented. - - For **velocity-controlled** joints, simple saturation based on acceleration and velocity limits has been implemented. - -### Examples ### -Please refer to the [joint_limits_interface](https://github.com/ros-controls/ros_control/wiki/joint_limits_interface) wiki page. diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp deleted file mode 100644 index db553948d1..0000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ /dev/null @@ -1,718 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "joint_limits_interface/joint_limits.hpp" -#include "joint_limits_interface/joint_limits_interface_exception.hpp" - -#include -#include - -#include -#include - -namespace joint_limits_interface -{ -/** - * The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint. - */ -class JointLimitHandle -{ -public: - JointLimitHandle() - : prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0), - jposh_(hardware_interface::HW_IF_POSITION), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_("position_command") - { - } - - JointLimitHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits) - : jposh_(jposh), - jvelh_(hardware_interface::HW_IF_VELOCITY), - jcmdh_(jcmdh), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - { - } - - JointLimitHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, const JointLimits & limits) - : jposh_(jposh), - jvelh_(jvelh), - jcmdh_(jcmdh), - limits_(limits), - prev_pos_(std::numeric_limits::quiet_NaN()), - prev_vel_(0.0) - { - } - - /// \return Joint name. - std::string get_name() const - { - return jposh_ ? jposh_.get_name() - : jvelh_ ? jvelh_.get_name() - : jcmdh_ ? jcmdh_.get_name() - : std::string(); - } - - /// Sub-class implementation of limit enforcing policy. - virtual void enforce_limits(const rclcpp::Duration & period) = 0; - - /// Clear stored state, causing it to reset next iteration. - virtual void reset() - { - prev_pos_ = std::numeric_limits::quiet_NaN(); - prev_vel_ = 0.0; - } - -protected: - hardware_interface::JointHandle jposh_; - hardware_interface::JointHandle jvelh_; - hardware_interface::JointHandle jcmdh_; - joint_limits_interface::JointLimits limits_; - - // stored state - track position and velocity of last update - double prev_pos_; - double prev_vel_; - - /// Return velocity for limit calculations. - /** - * @param period Time since last measurement - * @return the velocity, from state if available, otherwise from previous position history. - */ - double get_velocity(const rclcpp::Duration & period) const - { - // if we have a handle to a velocity state we can directly return state velocity - // otherwise we will estimate velocity from previous position (command or state) - return jvelh_ ? jvelh_.get_value() : (jposh_.get_value() - prev_pos_) / period.seconds(); - } -}; - -/** The base class of limit handles for enforcing position, velocity, and effort limits of - * an effort-controlled joint that has soft-limits. - */ -class JointSoftLimitsHandle : public JointLimitHandle -{ -public: - JointSoftLimitsHandle() {} - - JointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits, const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jcmdh, limits), soft_limits_(soft_limits) - { - } - - JointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, const JointLimits & limits, - const SoftJointLimits & soft_limits) - : JointLimitHandle(jposh, jvelh, jcmdh, limits), soft_limits_(soft_limits) - { - } - -protected: - joint_limits_interface::SoftJointLimits soft_limits_; -}; - -/** A handle used to enforce position and velocity limits of a position-controlled joint that does - not have soft limits. */ -class PositionJointSaturationHandle : public JointLimitHandle -{ -public: - PositionJointSaturationHandle() {} - - PositionJointSaturationHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const JointLimits & limits) - : JointLimitHandle(jposh, jcmdh, limits) - { - if (limits_.has_position_limits) - { - min_pos_limit_ = limits_.min_position; - max_pos_limit_ = limits_.max_position; - } - else - { - min_pos_limit_ = -std::numeric_limits::max(); - max_pos_limit_ = std::numeric_limits::max(); - } - } - - /// Enforce position and velocity limits for a joint that is not subject to soft limits. - /** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - if (std::isnan(prev_pos_)) - { - prev_pos_ = jposh_.get_value(); - } - - double min_pos, max_pos; - if (limits_.has_velocity_limits) - { - // enforce velocity limits - // set constraints on where the position can be based on the - // max velocity times seconds since last update - const double delta_pos = limits_.max_velocity * period.seconds(); - min_pos = std::max(prev_pos_ - delta_pos, min_pos_limit_); - max_pos = std::min(prev_pos_ + delta_pos, max_pos_limit_); - } - else - { - // no velocity limit, so position is simply limited to set extents (our imposed soft limits) - min_pos = min_pos_limit_; - max_pos = max_pos_limit_; - } - - // clamp command position to our computed min/max position - const double cmd = std::clamp(jcmdh_.get_value(), min_pos, max_pos); - jcmdh_.set_value(cmd); - - prev_pos_ = cmd; - } - -private: - double min_pos_limit_, max_pos_limit_; -}; - -/// A handle used to enforce position and velocity limits of a position-controlled joint. -/** - * This class implements a very simple position and velocity limits enforcing policy, and tries to - * impose the least amount of requisites on the underlying hardware platform. This lowers - * considerably the entry barrier to use it, but also implies some limitations. - * - * Requisites - * - Position (for non-continuous joints) and velocity limits specification. - * - Soft limits specification. The \c k_velocity parameter is \e not used. - * - * Open loop nature - * - * Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is - * checked for validity without relying on the actual position/velocity values. - * - * - Actual position values are \e not used because in some platforms there might be a substantial - * lag between sending a command and executing it (propagate command to hardware, reach control - * objective, read from hardware). - * - * - Actual velocity values are \e not used because of the above reason, and because some platforms - * might not expose trustworthy velocity measurements, or none at all. - * - * The downside of the open loop behavior is that velocity limits will not be enforced when - * recovering from large position tracking errors. Only the command is guaranteed to comply with the - * limits specification. - * - * \note: This handle type is \e stateful, ie. it stores the previous position command to estimate - * the command velocity. - */ - -// TODO(anyone): Leverage %Reflexxes Type II library for acceleration limits handling? -class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - PositionJointSoftLimitsHandle() {} - - PositionJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jcmdh, limits, soft_limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce position and velocity limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity limits will be - * enforced. - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - assert(period.seconds() > 0.0); - - // Current position - if (std::isnan(prev_pos_)) - { - // Happens only once at initialization - prev_pos_ = jposh_.get_value(); - } - const double pos = prev_pos_; - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) - { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), -limits_.max_velocity, - limits_.max_velocity); - } - else - { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Position bounds - const double dt = period.seconds(); - double pos_low = pos + soft_min_vel * dt; - double pos_high = pos + soft_max_vel * dt; - - if (limits_.has_position_limits) - { - // This extra measure safeguards against pathological cases, like when the soft limit lies - // beyond the hard limit - pos_low = std::max(pos_low, limits_.min_position); - pos_high = std::min(pos_high, limits_.max_position); - } - - // Saturate position command according to bounds - const double pos_cmd = std::clamp(jcmdh_.get_value(), pos_low, pos_high); - jcmdh_.set_value(pos_cmd); - - // Cache variables - // todo: shouldn't this just be pos_cmd? why call into the command handle to - // get what we have in the above line? - prev_pos_ = jcmdh_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and effort limits of an effort-controlled - * joint that does not have soft limits. - */ -class EffortJointSaturationHandle : public JointLimitHandle -{ -public: - EffortJointSaturationHandle() {} - - EffortJointSaturationHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle(jposh, jvelh, jcmdh, limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no efforts limits specification."); - } - } - - EffortJointSaturationHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : EffortJointSaturationHandle( - jposh, hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits) - { - } - - /** - * Enforce position, velocity, and effort limits for a joint that is not subject - * to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - double min_eff = -limits_.max_effort; - double max_eff = limits_.max_effort; - - if (limits_.has_position_limits) - { - const double pos = jposh_.get_value(); - if (pos < limits_.min_position) - { - min_eff = 0.0; - } - else if (pos > limits_.max_position) - { - max_eff = 0.0; - } - } - - const double vel = get_velocity(period); - if (vel < -limits_.max_velocity) - { - min_eff = 0.0; - } - else if (vel > limits_.max_velocity) - { - max_eff = 0.0; - } - - double clamped = std::clamp(jcmdh_.get_value(), min_eff, max_eff); - jcmdh_.set_value(clamped); - } -}; - -/// A handle used to enforce position, velocity and effort limits of an effort-controlled joint. -// TODO(anyone): This class is untested!. Update unit tests accordingly. -class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - EffortJointSoftLimitsHandle() {} - - EffortJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - if (!limits.has_effort_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no effort limits specification."); - } - } - - EffortJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : EffortJointSoftLimitsHandle( - jposh, hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits, - soft_limits) - { - } - - /// Enforce position, velocity and effort limits for a joint subject to soft limits. - /** - * If the joint has no position limits (eg. a continuous joint), only velocity and effort limits - * will be enforced. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Current state - const double pos = jposh_.get_value(); - const double vel = get_velocity(period); - - // Velocity bounds - double soft_min_vel; - double soft_max_vel; - - if (limits_.has_position_limits) - { - // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), -limits_.max_velocity, - limits_.max_velocity); - - soft_max_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), -limits_.max_velocity, - limits_.max_velocity); - } - else - { - // No position limits, eg. continuous joints - soft_min_vel = -limits_.max_velocity; - soft_max_vel = limits_.max_velocity; - } - - // Effort bounds depend on the velocity and effort bounds - const double soft_min_eff = std::clamp( - -soft_limits_.k_velocity * (vel - soft_min_vel), -limits_.max_effort, limits_.max_effort); - - const double soft_max_eff = std::clamp( - -soft_limits_.k_velocity * (vel - soft_max_vel), -limits_.max_effort, limits_.max_effort); - - // Saturate effort command according to bounds - const double eff_cmd = std::clamp(jcmdh_.get_value(), soft_min_eff, soft_max_eff); - jcmdh_.set_value(eff_cmd); - } -}; - -/// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint. -class VelocityJointSaturationHandle : public JointLimitHandle -{ -public: - VelocityJointSaturationHandle() {} - - VelocityJointSaturationHandle( - const hardware_interface::JointHandle & jvelh, // currently unused - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle( - hardware_interface::JointHandle(hardware_interface::HW_IF_POSITION), jvelh, jcmdh, limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - VelocityJointSaturationHandle( - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits) - : JointLimitHandle( - hardware_interface::JointHandle(hardware_interface::HW_IF_POSITION), - hardware_interface::JointHandle(hardware_interface::HW_IF_VELOCITY), jcmdh, limits) - { - if (!limits.has_velocity_limits) - { - throw joint_limits_interface::JointLimitsInterfaceException( - "Cannot enforce limits for joint '" + get_name() + - "'. It has no velocity limits specification."); - } - } - - /// Enforce joint velocity and acceleration limits. - /** - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) override - { - // Velocity bounds - double vel_low; - double vel_high; - - if (limits_.has_acceleration_limits) - { - assert(period.seconds() > 0.0); - const double dt = period.seconds(); - - vel_low = std::max(prev_vel_ - limits_.max_acceleration * dt, -limits_.max_velocity); - vel_high = std::min(prev_vel_ + limits_.max_acceleration * dt, limits_.max_velocity); - } - else - { - vel_low = -limits_.max_velocity; - vel_high = limits_.max_velocity; - } - - // Saturate velocity command according to limits - const double vel_cmd = std::clamp(jcmdh_.get_value(), vel_low, vel_high); - jcmdh_.set_value(vel_cmd); - - // Cache variables - prev_vel_ = jcmdh_.get_value(); - } -}; - -/** - * A handle used to enforce position, velocity, and acceleration limits of a - * velocity-controlled joint. - */ -class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle -{ -public: - VelocityJointSoftLimitsHandle() {} - - VelocityJointSoftLimitsHandle( - const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh, - const hardware_interface::JointHandle & jcmdh, - const joint_limits_interface::JointLimits & limits, - const joint_limits_interface::SoftJointLimits & soft_limits) - : JointSoftLimitsHandle(jposh, jvelh, jcmdh, limits, soft_limits) - { - if (limits.has_velocity_limits) - { - max_vel_limit_ = limits.max_velocity; - } - else - { - max_vel_limit_ = std::numeric_limits::max(); - } - } - - /** - * Enforce position, velocity, and acceleration limits for a velocity-controlled joint - * subject to soft limits. - * - * \param[in] period Control period. - */ - void enforce_limits(const rclcpp::Duration & period) - { - double min_vel, max_vel; - if (limits_.has_position_limits) - { - // Velocity bounds depend on the velocity limit and the proximity to the position limit. - const double pos = jposh_.get_value(); - min_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.min_position), -max_vel_limit_, - max_vel_limit_); - max_vel = std::clamp( - -soft_limits_.k_position * (pos - soft_limits_.max_position), -max_vel_limit_, - max_vel_limit_); - } - else - { - min_vel = -max_vel_limit_; - max_vel = max_vel_limit_; - } - - if (limits_.has_acceleration_limits) - { - const double vel = get_velocity(period); - const double delta_t = period.seconds(); - min_vel = std::max(vel - limits_.max_acceleration * delta_t, min_vel); - max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); - } - - jcmdh_.set_value(std::clamp(jcmdh_.get_value(), min_vel, max_vel)); - } - -private: - double max_vel_limit_; -}; - -// TODO(anyone): Port this to ROS 2 -// //** -// * Interface for enforcing joint limits. -// * -// * \tparam HandleType %Handle type. Must implement the following methods: -// * \code -// * void enforce_limits(); -// * std::string get_name() const; -// * \endcode -// */ -// template -// class joint_limits_interface::JointLimitsInterface -// : public hardware_interface::ResourceManager -// { -// public: -// HandleType getHandle(const std::string & name) -// { -// // Rethrow exception with a meaningful type -// try { -// return this->hardware_interface::ResourceManager::getHandle(name); -// } catch (const std::logic_error & e) { -// throw joint_limits_interface::JointLimitsInterfaceException(e.what()); -// } -// } -// -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Enforce limits for all managed handles. */ -// void enforce_limits(const rclcpp::Duration & period) -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.enforce_limits(period); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint through saturation. */ -// class PositionJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on a position-controlled joint with soft position limits. */ -// class PositionJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// public: -// /** \name Real-Time Safe Functions -// *\{*/ -// /** Reset all managed handles. */ -// void reset() -// { -// for (auto && resource_name_and_handle : this->resource_map_) { -// resource_name_and_handle.second.reset(); -// } -// } -// /*\}*/ -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint through saturation. */ -// class EffortJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */ -// class EffortJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint through saturation. */ -// class VelocityJointSaturationInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -// -// /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */ -// class VelocityJointSoftLimitsInterface -// : public joint_limits_interface::JointLimitsInterface -// { -// }; -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp deleted file mode 100644 index 430f46b48e..0000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface_exception.hpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_ - -#include - -namespace joint_limits_interface -{ -/// An exception related to a \ref JointLimitsInterface -class JointLimitsInterfaceException : public std::exception -{ -public: - explicit JointLimitsInterfaceException(const std::string & message) : msg(message) {} - - const char * what() const noexcept override { return msg.c_str(); } - -private: - std::string msg; -}; - -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_ diff --git a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp b/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp deleted file mode 100644 index 80cf7f0ed4..0000000000 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_urdf.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 Adolfo Rodriguez Tsouroukdissian - -#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_ -#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_ - -#include -#include -#include -#include - -namespace joint_limits_interface -{ -/** - * Populate a JointLimits instance from URDF joint data. - * \param[in] urdf_joint URDF joint. - * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will - * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged. - * \return True if \e urdf_joint has a valid limits specification, false otherwise. - */ -inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) -{ - if (!urdf_joint || !urdf_joint->limits) - { - return false; - } - - limits.has_position_limits = - urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC; - if (limits.has_position_limits) - { - limits.min_position = urdf_joint->limits->lower; - limits.max_position = urdf_joint->limits->upper; - } - - if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS) - { - limits.angle_wraparound = true; - } - - limits.has_velocity_limits = true; - limits.max_velocity = urdf_joint->limits->velocity; - - limits.has_acceleration_limits = false; - - limits.has_effort_limits = true; - limits.max_effort = urdf_joint->limits->effort; - - return true; -} - -/** - * Populate a SoftJointLimits instance from URDF joint data. - * \param[in] urdf_joint URDF joint. - * \param[out] soft_limits Where URDF soft joint limit data gets written into. - * \return True if \e urdf_joint has a valid soft limits specification, false otherwise. - */ -inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits) -{ - if (!urdf_joint || !urdf_joint->safety) - { - return false; - } - - soft_limits.min_position = urdf_joint->safety->soft_lower_limit; - soft_limits.max_position = urdf_joint->safety->soft_upper_limit; - soft_limits.k_position = urdf_joint->safety->k_position; - soft_limits.k_velocity = urdf_joint->safety->k_velocity; - - return true; -} - -} // namespace joint_limits_interface - -#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_ diff --git a/joint_limits_interface/mainpage.dox b/joint_limits_interface/mainpage.dox deleted file mode 100644 index 9b23463bc3..0000000000 --- a/joint_limits_interface/mainpage.dox +++ /dev/null @@ -1,142 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -joint_limits_interface contains data structures for representing joint limits, methods for -populating them from common formats such as URDF and rosparam, and methods for enforcing limits on different kinds -of joint commands. - -The joint_limits_interface is \e not used by controllers themselves (it does not implement a \p HardwareInterface) but instead operates after the controllers have updated, in the \p write() method (or equivalent) of the robot abstraction. Enforcing limits will \e overwrite the commands set by the controllers, it does not operate on a separate raw data buffer. - -\section codeapi Code API - -There are two main elements involved in setting up a joint_limits_interface: - -\subsection limits_representation Joint limits representation - - - \ref joint_limits_interface::JointLimits "JointLimits" Position, velocity, acceleration, jerk and effort. - - \ref joint_limits_interface::SoftJointLimits "SoftJointLimits" Soft position limits, k_p, k_v (as described here ). - - \ref joint_limits_urdf.h "Convenience methods" for loading joint limits information (only position, velocity, effort), as well as soft joint limits information from the URDF. - - \ref joint_limits_rosparam.h "Convenience methods" for loading joint limits from ROS parameter server (all values). Parameter specification is the same used in MoveIt, with the addition that we also parse jerk and effort limits. - -\subsection limits_interface Joint limits interface - -For \b effort-controlled joints, \b position-controlled joints, and \b velocity-controlled joints, two types of interfaces have been created. The first is a saturation interface, used for joints that have normal limits but not soft limits. The second is an interface that implements soft limits, similar to the one used on the PR2. - - - Effort-controlled joints - - Saturation: \ref joint_limits_interface::EffortJointSaturationHandle "Handle", \ref joint_limits_interface::EffortJointSaturationInterface "Interface" - - Soft limits: \ref joint_limits_interface::EffortJointSoftLimitsHandle "Handle", \ref joint_limits_interface::EffortJointSoftLimitsInterface "Interface" - - Position-controlled joints - - Saturation: \ref joint_limits_interface::PositionJointSaturationHandle "Handle", \ref joint_limits_interface::PositionJointSaturationInterface "Interface" - - Soft limits: \ref joint_limits_interface::PositionJointSoftLimitsHandle "Handle", \ref joint_limits_interface::PositionJointSoftLimitsInterface "Interface" - - Velocity-controlled joints - - Saturation: \ref joint_limits_interface::VelocityJointSaturationHandle "Handle", \ref joint_limits_interface::VelocityJointSaturationInterface "interface" - - Soft limits: \ref joint_limits_interface::VelocityJointSoftLimitsHandle "Handle", \ref joint_limits_interface::VelocityJointSoftLimitsInterface "Interface" - -\section example Examples - -\subsection limits_representation_example Joint limits representation - -The first example shows the different ways of populating joint limits data structures. - -\code -#include - -#include -#include -#include - -int main(int argc, char** argv) -{ - // Init node handle and URDF model - ros::NodeHandle nh; - boost::shared_ptr urdf; - // ...initialize contents of urdf - - // Data structures - joint_limits_interface::JointLimits limits; - joint_limits_interface::SoftJointLimits soft_limits; - - // Manual value setting - limits.has_velocity_limits = true; - limits.max_velocity = 2.0; - - // Populate (soft) joint limits from URDF - // Limits specified in URDF overwrite existing values in 'limits' and 'soft_limits' - // Limits not specified in URDF preserve their existing values - urdf::JointConstSharedPtr urdf_joint = urdf->getJoint("foo_joint"); - const bool urdf_limits_ok = getJointLimits(urdf_joint, limits); - const bool urdf_soft_limits_ok = getSoftJointLimits(urdf_joint, soft_limits); - - // Populate (soft) joint limits from the ros parameter server - // Limits specified in the parameter server overwrite existing values in 'limits' and 'soft_limits' - // Limits not specified in the parameter server preserve their existing values - const bool rosparam_limits_ok = getJointLimits("foo_joint", nh, limits); -} -\endcode - -A joint limits specification in YAML format that can be loaded to the ROS parameter server can be found -\ref joint_limits_interface::getJointLimits(const std::string& joint_name, const ros::NodeHandle& nh, JointLimits& limits) "here". - -\subsection limits_interface_example Joint limits interface - -The second example integrates joint limits enforcing into an existing robot hardware implementation. - -\code -#include - -using namespace hardware_interface; -using joint_limits_interface::JointLimits; -using joint_limits_interface::SoftJointLimits; -using joint_limits_interface::PositionJointSoftLimitsHandle; -using joint_limits_interface::PositionJointSoftLimitsInterface; - -class MyRobot -{ -public: - MyRobot() {} - - bool init() - { - // Populate pos_cmd_interface_ with joint handles... - - // Get joint handle of interest - JointHandle joint_handle = pos_cmd_interface_.getHandle("foo_joint"); - - JointLimits limits; - SoftJointLimits soft_limits; - // Populate with any of the methods presented in the previous example... - - // Register handle in joint limits interface - PositionJointSoftLimitsHandle handle(joint_handle, // We read the state and read/write the command - limits, // Limits spec - soft_limits) // Soft limits spec - - jnt_limits_interface_.registerHandle(handle); - } - - void read(ros::Time time, ros::Duration period) - { - // Read actuator state from hardware... - - // Propagate current actuator state to joints... - } - - void write(ros::Time time, ros::Duration period) - { - // Enforce joint limits for all registered handles - // Note: one can also enforce limits on a per-handle basis: handle.enforce_limits(period) - jnt_limits_interface_.enforce_limits(period); - - // Propagate joint commands to actuators... - - // Send actuator command to hardware... - } - -private: - PositionJointInterface pos_cmd_interface_; - PositionJointSoftLimitsInterface jnt_limits_interface_; -}; -\endcode - -*/ diff --git a/joint_limits_interface/package.xml b/joint_limits_interface/package.xml deleted file mode 100644 index badbb51773..0000000000 --- a/joint_limits_interface/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - joint_limits_interface - 0.0.0 - Interface for enforcing joint limits. - - Bence Magyar - Denis Štogl - - Apache License 2.0 - - https://github.com/ros-controls/ros2_control/wiki - https://github.com/ros-controls/ros2_control/issues - https://github.com/ros-controls/ros2_control - - Adolfo Rodriguez Tsouroukdissian - - ament_cmake - - rclcpp - urdf - - hardware_interface - - hardware_interface - - ament_cmake_gtest - hardware_interface - launch - launch_ros - launch_testing - launch_testing_ament_cmake - - - ament_cmake - - diff --git a/joint_limits_interface/test/joint_limits_urdf_test.cpp b/joint_limits_interface/test/joint_limits_urdf_test.cpp deleted file mode 100644 index 55effc7117..0000000000 --- a/joint_limits_interface/test/joint_limits_urdf_test.cpp +++ /dev/null @@ -1,176 +0,0 @@ -// Copyright 2020 PAL Robotics S.L. -// -// 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 Adolfo Rodriguez Tsouroukdissian - -#include - -#include - -#include - -class JointLimitsUrdfTest : public ::testing::Test -{ -public: - JointLimitsUrdfTest() - { - urdf_limits.reset(new urdf::JointLimits); - urdf_limits->effort = 8.0; - urdf_limits->velocity = 2.0; - urdf_limits->lower = -1.0; - urdf_limits->upper = 1.0; - - urdf_safety.reset(new urdf::JointSafety); - urdf_safety->k_position = 20.0; - urdf_safety->k_velocity = 40.0; - urdf_safety->soft_lower_limit = -0.8; - urdf_safety->soft_upper_limit = 0.8; - - urdf_joint.reset(new urdf::Joint); - urdf_joint->limits = urdf_limits; - urdf_joint->safety = urdf_safety; - - urdf_joint->type = urdf::Joint::UNKNOWN; - } - -protected: - urdf::JointLimitsSharedPtr urdf_limits; - urdf::JointSafetySharedPtr urdf_safety; - urdf::JointSharedPtr urdf_joint; -}; - -TEST_F(JointLimitsUrdfTest, GetJointLimits) -{ - // Unset URDF joint - { - joint_limits_interface::JointLimits limits; - urdf::JointSharedPtr urdf_joint_bad; - EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); - } - - // Unset URDF limits - { - joint_limits_interface::JointLimits limits; - urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); - EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); - } - - // Valid URDF joint, CONTINUOUS type - { - urdf_joint->type = urdf::Joint::CONTINUOUS; - - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits(urdf_joint, limits)); - - // Position - EXPECT_FALSE(limits.has_position_limits); - EXPECT_TRUE(limits.angle_wraparound); - - // Velocity - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); - - // Acceleration - EXPECT_FALSE(limits.has_acceleration_limits); - - // Effort - EXPECT_TRUE(limits.has_effort_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); - } - - // Valid URDF joint, REVOLUTE type - { - urdf_joint->type = urdf::Joint::REVOLUTE; - - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits(urdf_joint, limits)); - - // Position - EXPECT_TRUE(limits.has_position_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); - EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); - EXPECT_FALSE(limits.angle_wraparound); - - // Velocity - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); - - // Acceleration - EXPECT_FALSE(limits.has_acceleration_limits); - - // Effort - EXPECT_TRUE(limits.has_effort_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); - } - - // Valid URDF joint, PRISMATIC type - { - urdf_joint->type = urdf::Joint::PRISMATIC; - - joint_limits_interface::JointLimits limits; - EXPECT_TRUE(getJointLimits(urdf_joint, limits)); - - // Position - EXPECT_TRUE(limits.has_position_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); - EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); - EXPECT_FALSE(limits.angle_wraparound); - - // Velocity - EXPECT_TRUE(limits.has_velocity_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); - - // Acceleration - EXPECT_FALSE(limits.has_acceleration_limits); - - // Effort - EXPECT_TRUE(limits.has_effort_limits); - EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); - } -} - -TEST_F(JointLimitsUrdfTest, GetSoftJointLimits) -{ - // Unset URDF joint - { - joint_limits_interface::SoftJointLimits soft_limits; - urdf::JointSharedPtr urdf_joint_bad; - EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); - } - - // Unset URDF limits - { - joint_limits_interface::SoftJointLimits soft_limits; - urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); - EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); - } - - // Valid URDF joint - { - joint_limits_interface::SoftJointLimits soft_limits; - EXPECT_TRUE(getSoftJointLimits(urdf_joint, soft_limits)); - - // Soft limits - EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_lower_limit, soft_limits.min_position); - EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_upper_limit, soft_limits.max_position); - EXPECT_DOUBLE_EQ(urdf_joint->safety->k_position, soft_limits.k_position); - EXPECT_DOUBLE_EQ(urdf_joint->safety->k_velocity, soft_limits.k_velocity); - } -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 2f21f0bab4..3a54f3249c 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.4.0 (2024-01-31) +------------------ + 4.3.0 (2024-01-20) ------------------ diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 9c33c40d08..9e7c446b66 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.3.0 + 4.4.0 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 3d29c094d1..9e2f966d0d 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.4.0 (2024-01-31) +------------------ + 4.3.0 (2024-01-20) ------------------ * [ResourceManager] adds test for uninitialized hardware (`#1243 `_) diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 78e93658ab..c403246015 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.3.0 + 4.4.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 7ddf9c472a..b659a5bf97 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.4.0 (2024-01-31) +------------------ + 4.3.0 (2024-01-20) ------------------ * [docs] Remove joint_state_controller (`#1263 `_) diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 3b1b82de70..33f265a607 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.3.0 + 4.4.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index b3c870e6e0..02c9f108d3 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.3.0", + version="4.4.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 2786604992..07724c98a3 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.4.0 (2024-01-31) +------------------ + 4.3.0 (2024-01-20) ------------------ * Fix rqt controller manager crash on ros2_control restart (`#1273 `_) diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 1e7c0683e7..c280751db7 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.3.0 + 4.4.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 05d95df62b..f93c231cd9 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="4.3.0", + version="4.4.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 312c187b3e..c63c29c3ab 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.4.0 (2024-01-31) +------------------ + 4.3.0 (2024-01-20) ------------------ * Improve transmission tests (`#1238 `_) diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 55ad994599..894bbd9b45 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.3.0 + 4.4.0 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl