diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml deleted file mode 100644 index c6580eacd4..0000000000 --- a/.github/reviewer-lottery.yml +++ /dev/null @@ -1,33 +0,0 @@ -groups: - # Default reviewers for all pull requests. - # Usually, at least on of the maintainers should approve PR before merging. - # The best is if two maintainers do that. - - name: maintainers # name of the group - reviewers: 2 # how many reviewers do you want to assign? - internal_reviewers: 1 # how many reviewers do you want to assign when the PR author belongs to this group? - usernames: # github usernames of the reviewers - - bmagyar - - destogl - - # Reviewers group to get broader feedback. - - name: reviewers - reviewers: 5 - usernames: - - aprotyas - - arne48 - - bijoua29 - - christophfroehlich - - DasRoteSkelett - - duringhof - - erickisos - - fmauch - - jaron-l - - livanov93 - - mcbed - - moriarty - - olivier-stasse - - peterdavidfagan - - progtologist - - saikishor - - VanshGehlot - - VX792 diff --git a/.github/workflows/ci-coverage-build-humble.yml b/.github/workflows/ci-coverage-build-humble.yml deleted file mode 100644 index 0065223501..0000000000 --- a/.github/workflows/ci-coverage-build-humble.yml +++ /dev/null @@ -1,65 +0,0 @@ -name: Coverage Build - Humble -on: - workflow_dispatch: - push: - branches: - - humble - pull_request: - branches: - - humble - -jobs: - coverage: - name: coverage build - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - env: - ROS_DISTRO: humble - steps: - - uses: ros-tooling/setup-ros@0.7.1 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.6 - with: - target-ros2-distro: ${{ env.ROS_DISTRO }} - import-token: ${{ secrets.GITHUB_TOKEN }} - # build all packages listed here - package-name: - ackermann_steering_controller - admittance_controller - bicycle_steering_controller - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - gripper_controllers - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - position_controllers - range_sensor_broadcaster - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers - - vcs-repo-file-url: | - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} - colcon-defaults: | - { - "build": { - "mixin": ["coverage-gcc"] - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - 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.3.1 - with: - name: colcon-logs-coverage-humble - path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build-iron.yml b/.github/workflows/ci-coverage-build-iron.yml deleted file mode 100644 index 201a631923..0000000000 --- a/.github/workflows/ci-coverage-build-iron.yml +++ /dev/null @@ -1,65 +0,0 @@ -name: Coverage Build - Iron -on: - workflow_dispatch: - push: - branches: - - iron - pull_request: - branches: - - iron - -jobs: - coverage: - name: coverage build - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - env: - ROS_DISTRO: iron - steps: - - uses: ros-tooling/setup-ros@0.7.1 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.6 - with: - target-ros2-distro: ${{ env.ROS_DISTRO }} - import-token: ${{ secrets.GITHUB_TOKEN }} - # build all packages listed here - package-name: - ackermann_steering_controller - admittance_controller - bicycle_steering_controller - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - gripper_controllers - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - position_controllers - range_sensor_broadcaster - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers - - vcs-repo-file-url: | - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} - colcon-defaults: | - { - "build": { - "mixin": ["coverage-gcc"] - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - 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.3.1 - with: - name: colcon-logs-coverage-iron - path: ros_ws/log diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml deleted file mode 100644 index ffda7a2066..0000000000 --- a/.github/workflows/ci-coverage-build.yml +++ /dev/null @@ -1,65 +0,0 @@ -name: Coverage Build -on: - workflow_dispatch: - push: - branches: - - master - pull_request: - branches: - - master - -jobs: - coverage: - name: coverage build - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - env: - ROS_DISTRO: rolling - steps: - - uses: ros-tooling/setup-ros@0.7.1 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.6 - with: - target-ros2-distro: ${{ env.ROS_DISTRO }} - import-token: ${{ secrets.GITHUB_TOKEN }} - # build all packages listed here - package-name: - ackermann_steering_controller - admittance_controller - bicycle_steering_controller - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - gripper_controllers - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - position_controllers - range_sensor_broadcaster - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers - - vcs-repo-file-url: | - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_controllers.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} - colcon-defaults: | - { - "build": { - "mixin": ["coverage-gcc"] - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - 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.3.1 - with: - name: colcon-logs-coverage-rolling - path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml deleted file mode 100644 index 824baf1b77..0000000000 --- a/.github/workflows/ci-format.yml +++ /dev/null @@ -1,22 +0,0 @@ -# This is a format job. Pre-commit has a first-party GitHub action, so we use -# that: https://github.com/pre-commit/action - -name: Format - -on: - pull_request: - -jobs: - pre-commit: - name: Format - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5.0.0 - with: - python-version: '3.10' - - name: Install system hooks - run: sudo apt install -qq clang-format-14 cppcheck - - uses: pre-commit/action@v3.0.1 - with: - extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml deleted file mode 100644 index b95e3c36a2..0000000000 --- a/.github/workflows/ci-ros-lint.yml +++ /dev/null @@ -1,80 +0,0 @@ -name: ROS Lint -on: - pull_request: - -jobs: - ament_lint: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - matrix: - linter: [cppcheck, copyright, lint_cmake] - env: - AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true - steps: - - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.1 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: iron - linter: ${{ matrix.linter }} - package-name: - ackermann_steering_controller - admittance_controller - bicycle_steering_controller - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - gripper_controllers - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - position_controllers - range_sensor_broadcaster - ros2_controllers - ros2_controllers_test_nodes - rqt_joint_trajectory_controller - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers - - - ament_lint_100: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - matrix: - linter: [cpplint] - steps: - - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@master - - uses: ros-tooling/action-ros-lint@master - with: - distribution: iron - linter: cpplint - arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: - ackermann_steering_controller - admittance_controller - bicycle_steering_controller - diff_drive_controller - effort_controllers - force_torque_sensor_broadcaster - forward_command_controller - gripper_controllers - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - position_controllers - range_sensor_broadcaster - ros2_controllers - ros2_controllers_test_nodes - rqt_joint_trajectory_controller - steering_controllers_library - tricycle_controller - tricycle_steering_controller - velocity_controllers diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index 989805733f..df449caecb 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -18,6 +18,7 @@ jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml new file mode 100644 index 0000000000..0910572227 --- /dev/null +++ b/.github/workflows/humble-coverage-build.yml @@ -0,0 +1,17 @@ +name: Coverage Build - Humble +on: + workflow_dispatch: + push: + branches: + - humble + pull_request: + branches: + - humble + +jobs: + coverage_humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: humble + os_name: ubuntu-22.04 diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml new file mode 100644 index 0000000000..be8c84b05b --- /dev/null +++ b/.github/workflows/humble-pre-commit.yml @@ -0,0 +1,14 @@ +name: Pre-Commit - Humble + +on: + workflow_dispatch: + pull_request: + branches: + - humble + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: humble + os_name: ubuntu-22.04 diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 1d9d3bd1fb..aaed9c8ca2 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -17,6 +17,7 @@ jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml index 079620771c..ec1e93c805 100644 --- a/.github/workflows/iron-binary-build.yml +++ b/.github/workflows/iron-binary-build.yml @@ -24,6 +24,7 @@ jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml new file mode 100644 index 0000000000..d82c52bf51 --- /dev/null +++ b/.github/workflows/iron-coverage-build.yml @@ -0,0 +1,17 @@ +name: Coverage Build - Iron +on: + workflow_dispatch: + push: + branches: + - iron + pull_request: + branches: + - iron + +jobs: + coverage_iron: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: iron + os_name: ubuntu-22.04 diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml new file mode 100644 index 0000000000..60ad26d073 --- /dev/null +++ b/.github/workflows/iron-pre-commit.yml @@ -0,0 +1,14 @@ +name: Pre-Commit - Iron + +on: + workflow_dispatch: + pull_request: + branches: + - iron + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: iron + os_name: ubuntu-22.04 diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index 064be40555..a704875a18 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -23,6 +23,7 @@ jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index ed28964e01..0584f4a7f3 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -1,14 +1,10 @@ name: Reviewer lottery +# pull_request_target takes the same events as pull_request, +# but it runs on the base branch instead of the head branch. on: pull_request_target: types: [opened, ready_for_review, reopened] jobs: - test: - runs-on: ubuntu-latest - if: github.actor != 'dependabot[bot]' && github.actor != 'mergify[bot]' - steps: - - uses: actions/checkout@v4 - - uses: uesteibar/reviewer-lottery@v3 - with: - repo-token: ${{ secrets.GITHUB_TOKEN }} + assign_reviewers: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-reviewer-lottery.yml@master diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index 7b5a3bda65..acd19e25aa 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -24,6 +24,7 @@ jobs: binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml new file mode 100644 index 0000000000..4d4750c54c --- /dev/null +++ b/.github/workflows/rolling-coverage-build.yml @@ -0,0 +1,17 @@ +name: Coverage Build - Rolling +on: + workflow_dispatch: + push: + branches: + - master + pull_request: + branches: + - master + +jobs: + coverage_rolling: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: rolling + os_name: ubuntu-22.04 diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml new file mode 100644 index 0000000000..9c87311bd7 --- /dev/null +++ b/.github/workflows/rolling-pre-commit.yml @@ -0,0 +1,14 @@ +name: Pre-Commit - Rolling + +on: + workflow_dispatch: + pull_request: + branches: + - master + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: rolling + os_name: ubuntu-22.04 diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index a20c4d4fd3..432e739b74 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -23,6 +23,7 @@ jobs: semi_binary: uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master strategy: + fail-fast: false matrix: ROS_REPO: [main, testing] with: diff --git a/.github/workflows/update-pre-commit.yml b/.github/workflows/update-pre-commit.yml new file mode 100644 index 0000000000..8b9545dff1 --- /dev/null +++ b/.github/workflows/update-pre-commit.yml @@ -0,0 +1,12 @@ +name: Auto Update pre-commit +# Update pre-commit config and create PR if changes are detected +# author: Christoph Fröhlich + +on: + workflow_dispatch: + schedule: + - cron: '0 0 * * 0' # Run every Sunday at midnight + +jobs: + auto_update_and_create_pr: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-update-pre-commit.yml@master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 32d194d73d..6da427c6ee 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,3 +1,4 @@ + # To use: # # pre-commit run -a @@ -15,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v4.5.0 hooks: - id: check-added-large-files - id: check-ast @@ -29,57 +30,57 @@ repos: - id: end-of-file-fixer - id: mixed-line-ending - id: trailing-whitespace + exclude_types: [rst] - id: fix-byte-order-marker + # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.3.1 + rev: v3.15.1 hooks: - id: pyupgrade args: [--py36-plus] - - repo: https://github.com/psf/black - rev: 22.12.0 - hooks: - - id: black - args: ["--line-length=99"] - # PyDocStyle - repo: https://github.com/PyCQA/pydocstyle - rev: 6.2.2 + rev: 6.3.0 hooks: - id: pydocstyle args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + - repo: https://github.com/psf/black + rev: 24.2.0 + hooks: + - id: black + args: ["--line-length=99"] + - repo: https://github.com/pycqa/flake8 - rev: 6.0.0 + rev: 7.0.0 hooks: - id: flake8 - args: ["--ignore=E501"] + args: ["--extend-ignore=E501"] # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.6 + rev: v17.0.6 hooks: - id: clang-format + args: ['-fallback-style=none', '-i'] - repo: local hooks: - id: ament_cppcheck name: ament_cppcheck description: Static code analysis of C/C++ files. - stages: [commit] - entry: ament_cppcheck + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - # Maybe use https://github.com/cpplint/cpplint instead - repo: local hooks: - id: ament_cpplint name: ament_cpplint description: Static code analysis of C/C++ files. - stages: [commit] entry: ament_cpplint language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -91,7 +92,6 @@ repos: - id: ament_lint_cmake name: ament_lint_cmake description: Check format of CMakeLists.txt files. - stages: [commit] entry: ament_lint_cmake language: system files: CMakeLists\.txt$ @@ -102,7 +102,6 @@ repos: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. - stages: [commit] entry: ament_copyright language: system @@ -125,8 +124,18 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.2 + rev: v2.2.6 hooks: - id: codespell - args: ['--write-changes', '--uri-ignore-words-list=ist'] + args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel'] exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ + + - repo: https://github.com/python-jsonschema/check-jsonschema + rev: 0.28.0 + hooks: + - id: check-github-workflows + args: ["--verbose"] + - id: check-github-actions + args: ["--verbose"] + - id: check-dependabot + args: ["--verbose"] diff --git a/README.md b/README.md index 7c51b3d1ab..f63dc8dfdb 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ros2_controllers [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) -[![codecov](https://codecov.io/gh/ros-controls/ros2_controllers/branch/iron/graph/badge.svg?token=KSdY0tsHm6)](https://codecov.io/gh/ros-controls/ros2_controllers) +[![codecov](https://codecov.io/gh/ros-controls/ros2_controllers/branch/iron/graph/badge.svg?token=KSdY0tsHm6)](https://codecov.io/gh/ros-controls/ros2_controllers/tree/iron) Commonly used and generalized controllers for ros2-control framework that are ready to use with many robots, MoveIt2 and Nav2. diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index a8a8832fce..97a1d9b813 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -10,7 +10,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs control_toolbox controller_interface - Eigen3 generate_parameter_library geometry_msgs hardware_interface @@ -33,6 +32,7 @@ find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(Eigen3 REQUIRED NO_MODULE) generate_parameter_library(admittance_controller_parameters src/admittance_controller_parameters.yaml @@ -48,6 +48,7 @@ target_include_directories(admittance_controller PUBLIC ) target_link_libraries(admittance_controller PUBLIC admittance_controller_parameters + Eigen3::Eigen ) ament_target_dependencies(admittance_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index b944ff1e88..aa71c70745 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -53,8 +53,7 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) ament_add_gmock(test_diff_drive_controller - test/test_diff_drive_controller.cpp - ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml) + test/test_diff_drive_controller.cpp) target_link_libraries(test_diff_drive_controller diff_drive_controller ) @@ -69,8 +68,9 @@ if(BUILD_TESTING) tf2_msgs ) - ament_add_gmock(test_load_diff_drive_controller + add_rostest_with_parameters_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml ) ament_target_dependencies(test_load_diff_drive_controller controller_manager diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 5923a27d4d..56d4970945 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -110,6 +110,12 @@ class DiffDriveController : public controller_interface::ControllerInterface // Parameters from ROS for diff_drive_controller std::shared_ptr param_listener_; Params params_; + /* Number of wheels on each side of the robot. This is important to take the wheels slip into + * account when multiple wheels on each side are present. If there are more wheels then control + * signals for each side, you should enter number for control signals. For example, Husky has two + * wheels on each side, but they use one control signal, in this case '1' is the correct value of + * the parameter. */ + int wheels_per_side_; Odometry odometry_; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 23097c0df2..b1c74625cb 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -150,7 +150,7 @@ controller_interface::return_type DiffDriveController::update( { double left_feedback_mean = 0.0; double right_feedback_mean = 0.0; - for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = @@ -167,8 +167,8 @@ controller_interface::return_type DiffDriveController::update( left_feedback_mean += left_feedback; right_feedback_mean += right_feedback; } - left_feedback_mean /= static_cast(params_.wheels_per_side); - right_feedback_mean /= static_cast(params_.wheels_per_side); + left_feedback_mean /= static_cast(wheels_per_side_); + right_feedback_mean /= static_cast(wheels_per_side_); if (params_.position_feedback) { @@ -258,7 +258,7 @@ controller_interface::return_type DiffDriveController::update( (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius; // Set wheels velocities: - for (size_t index = 0; index < static_cast(params_.wheels_per_side); ++index) + for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left); registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right); @@ -287,12 +287,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - if (params_.left_wheel_names.empty()) - { - RCLCPP_ERROR(logger, "Wheel names parameters are empty!"); - return controller_interface::CallbackReturn::ERROR; - } - const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; @@ -322,7 +316,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( } // left and right sides are both equal at this point - params_.wheels_per_side = params_.left_wheel_names.size(); + wheels_per_side_ = static_cast(params_.left_wheel_names.size()); if (publish_limited_velocity_) { diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index e878ad5481..509a7ab1e1 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -2,23 +2,24 @@ diff_drive_controller: left_wheel_names: { type: string_array, default_value: [], - description: "Link names of the left side wheels", + description: "Names of the left side wheels' joints", + validation: { + not_empty<>: [] + } } right_wheel_names: { type: string_array, default_value: [], - description: "Link names of the right side wheels", + description: "Names of the right side wheels' joints", + validation: { + not_empty<>: [] + } } wheel_separation: { type: double, default_value: 0.0, description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.", } - wheels_per_side: { - type: int, - default_value: 0, - description: "Number of wheels on each side of the robot. This is important to take the wheels slip into account when multiple wheels on each side are present. If there are more wheels then control signals for each side, you should enter number for control signals. For example, Husky has two wheels on each side, but they use one control signal, in this case '1' is the correct value of the parameter.", - } wheel_radius: { type: double, default_value: 0.0, diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index a2149eb6bc..bfbf8f2d19 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/diff_drive_controller/test/config/test_diff_drive_controller.yaml @@ -2,7 +2,6 @@ test_diff_drive_controller: ros__parameters: left_wheel_names: ["left_wheels"] right_wheel_names: ["right_wheels"] - write_op_modes: ["motor_controller"] wheel_separation: 0.40 wheels_per_side: 1 # actually 2, but both are controlled by 1 signal @@ -21,7 +20,7 @@ test_diff_drive_controller: open_loop: true enable_odom_tf: true - cmd_vel_timeout: 500 # milliseconds + cmd_vel_timeout: 0.5 # seconds publish_limited_velocity: true velocity_rolling_window_size: 10 diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index db89d4f873..61ab57c0a4 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -36,6 +36,12 @@ using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; using testing::SizeIs; +namespace +{ +const std::vector left_wheel_names = {"left_wheel_joint"}; +const std::vector right_wheel_names = {"right_wheel_joint"}; +} // namespace + class TestableDiffDriveController : public diff_drive_controller::DiffDriveController { public: @@ -166,11 +172,28 @@ class TestDiffDriveController : public ::testing::Test controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } + controller_interface::return_type InitController( + const std::vector left_wheel_joints_init = left_wheel_names, + const std::vector right_wheel_joints_init = right_wheel_names, + const std::vector & parameters = {}, const std::string ns = "") + { + auto node_options = rclcpp::NodeOptions(); + std::vector parameter_overrides; + + parameter_overrides.push_back( + rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_joints_init))); + parameter_overrides.push_back( + rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_joints_init))); + + parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end()); + node_options.parameter_overrides(parameter_overrides); + + return controller_->init(controller_name, ns, node_options); + } + const std::string controller_name = "test_diff_drive_controller"; std::unique_ptr controller_; - const std::vector left_wheel_names = {"left_wheel_joint"}; - const std::vector right_wheel_names = {"right_wheel_joint"}; std::vector position_values_ = {0.1, 0.2}; std::vector velocity_values_ = {0.01, 0.02}; @@ -191,59 +214,31 @@ class TestDiffDriveController : public ::testing::Test rclcpp::Publisher::SharedPtr velocity_publisher; }; -TEST_F(TestDiffDriveController, configure_fails_without_parameters) +TEST_F(TestDiffDriveController, init_fails_without_parameters) { const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(ret, controller_interface::return_type::ERROR); } -TEST_F(TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined) +TEST_F(TestDiffDriveController, init_fails_with_only_left_or_only_right_side_defined) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(std::vector()))); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(std::vector()))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(left_wheel_names, {}), controller_interface::return_type::ERROR); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + ASSERT_EQ(InitController({}, right_wheel_names), controller_interface::return_type::ERROR); } TEST_F(TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - - auto extended_right_wheel_names = right_wheel_names; - extended_right_wheel_names.push_back("extra_wheel"); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(extended_right_wheel_names))); + ASSERT_EQ( + InitController(left_wheel_names, {right_wheel_names[0], "extra_wheel"}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); } TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -257,26 +252,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -290,26 +277,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_names TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -325,26 +304,18 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_no_namesp TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -361,26 +332,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -396,26 +360,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_true_set_names { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = "test_prefix"; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -433,26 +390,19 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name { std::string test_namespace = "/test_namespace"; - const auto ret = controller_->init(controller_name, test_namespace); - ASSERT_EQ(ret, controller_interface::return_type::OK); - std::string odom_id = "odom"; std::string base_link_id = "base_link"; std::string frame_prefix = ""; - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)), + rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)), + rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)), + rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}, + test_namespace), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -467,13 +417,7 @@ TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_name TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); @@ -481,15 +425,9 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(InitController(), controller_interface::return_type::OK); // We implicitly test that by default position feedback is required - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); @@ -497,15 +435,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned) TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -514,15 +448,11 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesPosFeedback(); @@ -531,15 +461,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}), + controller_interface::return_type::OK); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); assignResourcesVelFeedback(); @@ -548,15 +474,11 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2) TEST_F(TestDiffDriveController, cleanup) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.1)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 0.1)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); @@ -597,15 +519,11 @@ TEST_F(TestDiffDriveController, cleanup) TEST_F(TestDiffDriveController, correct_initialization_using_parameters) { - const auto ret = controller_->init(controller_name); - ASSERT_EQ(ret, controller_interface::return_type::OK); - - controller_->get_node()->set_parameter( - rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names))); - controller_->get_node()->set_parameter( - rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names))); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_separation", 0.4)); - controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + ASSERT_EQ( + InitController( + left_wheel_names, right_wheel_names, + {rclcpp::Parameter("wheel_separation", 0.4), rclcpp::Parameter("wheel_radius", 1.0)}), + controller_interface::return_type::OK); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(controller_->get_node()->get_node_base_interface()); diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index 660208c560..4c9d2f984f 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -24,8 +24,6 @@ TEST(TestLoadDiffDriveController, load_controller) { - rclcpp::init(0, nullptr); - std::shared_ptr executor = std::make_shared(); @@ -36,6 +34,14 @@ TEST(TestLoadDiffDriveController, load_controller) ASSERT_NE( cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), nullptr); +} +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); rclcpp::shutdown(); + return result; } diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 162977cdfe..72c11625e4 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -214,9 +214,9 @@ def restore_settings(self, plugin_settings, instance_settings): try: idx = jtc_list.index(jtc_name) jtc_combo.setCurrentIndex(idx) - except (ValueError): + except ValueError: pass - except (ValueError): + except ValueError: pass # def trigger_configuration(self): @@ -427,7 +427,7 @@ def _update_cmd_cb(self): cmd = pos try: cmd = self._joint_pos[name]["command"] - except (KeyError): + except KeyError: pass max_vel = self._robot_joint_limits[name]["max_velocity"] dur.append(max(abs(cmd - pos) / max_vel, self._min_traj_dur)) @@ -445,7 +445,7 @@ def _update_joint_widgets(self): try: joint_pos = self._joint_pos[joint_name]["position"] joint_widgets[id].setValue(joint_pos) - except (KeyError): + except KeyError: pass # Can happen when first connected to controller def _joint_widgets(self): # TODO: Cache instead of compute every time? diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py index f0b632322f..6f736910e2 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/update_combo.py @@ -36,7 +36,7 @@ def update_combo(combo, new_vals): selected_id = -1 try: selected_id = new_vals.index(selected_val) - except (ValueError): + except ValueError: combo.setCurrentIndex(-1) # Re-populate items diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index d72a13ad45..d4248d5f7c 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -1,5 +1,19 @@ #!/usr/bin/env python +# Copyright 2024 Apache License, Version 2.0 +# +# 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. + from setuptools import setup from glob import glob