diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000000..05a48fc654 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,13 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" diff --git a/.github/mergify.yml b/.github/mergify.yml index 6bfd4470ac..e62f958961 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -1,4 +1,13 @@ pull_request_rules: + - name: Backport to humble at reviewers discretion + conditions: + - base=master + - "label=humble-galactic" + actions: + backport: + branches: + - humble + - name: Backport to galactic at reviewers discretion conditions: - base=master diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index a2d02ed8a0..31abce9855 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -16,11 +16,11 @@ jobs: env: ROS_DISTRO: rolling steps: - - uses: ros-tooling/setup-ros@0.3.4 + - uses: ros-tooling/setup-ros@0.6.1 with: required-ros-distributions: ${{ env.ROS_DISTRO }} - uses: actions/checkout@v3 - - uses: ros-tooling/action-ros-ci@0.2.6 + - uses: ros-tooling/action-ros-ci@0.3.0 with: target-ros2-distro: ${{ env.ROS_DISTRO }} import-token: ${{ secrets.GITHUB_TOKEN }} @@ -44,12 +44,12 @@ jobs: } } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v3.1.0 + - uses: codecov/codecov-action@v3.1.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests name: codecov-umbrella - - uses: actions/upload-artifact@v3.1.0 + - uses: actions/upload-artifact@v3.1.2 with: name: colcon-logs-ubuntu-22.04-coverage-rolling path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index acf5321db6..3e6def8be3 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,11 +13,11 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v2 + - uses: actions/setup-python@v4.5.0 with: - python-version: 3.9 + python-version: '3.10' - name: Install system hooks - run: sudo apt install -qq clang-format-12 cppcheck - - uses: pre-commit/action@v2.0.3 + run: sudo apt install -qq clang-format-14 cppcheck + - uses: pre-commit/action@v3.0.0 with: extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index 009c394bfe..67d1b2c5e9 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -12,7 +12,7 @@ jobs: linter: [cppcheck, copyright, lint_cmake] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/setup-ros@0.6.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling @@ -36,7 +36,7 @@ jobs: linter: [cpplint] steps: - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@v0.2 + - uses: ros-tooling/setup-ros@0.6.1 - uses: ros-tooling/action-ros-lint@v0.1 with: distribution: rolling diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml index 3ea9a63541..9f3858d59f 100644 --- a/.github/workflows/docker.yml +++ b/.github/workflows/docker.yml @@ -19,13 +19,13 @@ jobs: steps: - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v1 + uses: docker/setup-buildx-action@v2 - name: Checkout repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Log in to the Container registry - uses: docker/login-action@v1 + uses: docker/login-action@v2 with: registry: ${{ env.REGISTRY }} username: ${{ github.actor }} @@ -33,7 +33,7 @@ jobs: - name: Docker meta id: meta - uses: docker/metadata-action@v3 + uses: docker/metadata-action@v4 with: images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}_release tags: | @@ -42,7 +42,7 @@ jobs: type=semver,pattern={{major}}.{{minor}} - name: Build and push Docker image - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v4 with: context: . push: true @@ -59,13 +59,13 @@ jobs: steps: - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v1 + uses: docker/setup-buildx-action@v2 - name: Checkout repository - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Log in to the Container registry - uses: docker/login-action@v1 + uses: docker/login-action@v2 with: registry: ${{ env.REGISTRY }} username: ${{ github.actor }} @@ -73,7 +73,7 @@ jobs: - name: Docker meta id: meta - uses: docker/metadata-action@v3 + uses: docker/metadata-action@v4 with: images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}_source tags: | @@ -82,7 +82,7 @@ jobs: type=semver,pattern={{major}}.{{minor}} - name: Build and push Docker image - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v4 with: context: . push: true diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index 490b680e7c..bca08ce5d2 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -66,14 +66,14 @@ jobs: ref: ${{ inputs.ref_for_scheduled_build }} - name: cache target_ws if: ${{ ! matrix.env.CCOV }} - uses: pat-s/always-upload-cache@v2.1.5 + uses: pat-s/always-upload-cache@v3.0.11 with: path: ${{ env.BASEDIR }}/target_ws key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }} restore-keys: | target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }} - name: cache ccache - uses: pat-s/always-upload-cache@v2.1.5 + uses: pat-s/always-upload-cache@v3.0.11 with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} diff --git a/.github/workflows/reusable-ros-tooling-source-build.yml b/.github/workflows/reusable-ros-tooling-source-build.yml index 4e5174405d..5a715fd503 100644 --- a/.github/workflows/reusable-ros-tooling-source-build.yml +++ b/.github/workflows/reusable-ros-tooling-source-build.yml @@ -26,13 +26,13 @@ jobs: strategy: fail-fast: false steps: - - uses: ros-tooling/setup-ros@0.3.4 + - uses: ros-tooling/setup-ros@0.6.1 with: required-ros-distributions: ${{ inputs.ros_distro }} - uses: actions/checkout@v3 with: ref: ${{ inputs.ref }} - - uses: ros-tooling/action-ros-ci@0.2.6 + - uses: ros-tooling/action-ros-ci@0.3.0 with: target-ros2-distro: ${{ inputs.ros_distro }} # build all packages listed in the meta package @@ -49,7 +49,7 @@ jobs: https://raw.githubusercontent.com/ros2/ros2/${{ inputs.ros2_repo_branch }}/ros2.repos https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_control.${{ inputs.ros_distro }}.repos?token=${{ secrets.GITHUB_TOKEN }} colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: actions/upload-artifact@v1 + - uses: actions/upload-artifact@v3.1.2 with: name: colcon-logs-ubuntu-22.04 path: ros_ws/log diff --git a/.github/workflows/reviewer_lottery.yml b/.github/workflows/reviewer_lottery.yml index f7ae929e5b..94f3d9bde6 100644 --- a/.github/workflows/reviewer_lottery.yml +++ b/.github/workflows/reviewer_lottery.yml @@ -7,7 +7,7 @@ jobs: test: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 - uses: uesteibar/reviewer-lottery@v2 with: repo-token: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/rolling-abi-compatibility.yml b/.github/workflows/rolling-abi-compatibility.yml deleted file mode 100644 index 4589e92e3b..0000000000 --- a/.github/workflows/rolling-abi-compatibility.yml +++ /dev/null @@ -1,20 +0,0 @@ -name: Rolling - ABI Compatibility Check -on: - workflow_dispatch: - branches: - - master - pull_request: - branches: - - master - -jobs: - abi_check: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: ros-industrial/industrial_ci@master - env: - ROS_DISTRO: rolling - ROS_REPO: main - ABICHECK_URL: github:${{ github.repository }}#${{ github.base_ref }} - NOT_TEST_BUILD: true diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index cb127e8fcf..4af4875830 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.1.0 + rev: v4.4.0 hooks: - id: check-added-large-files - id: check-ast @@ -36,20 +36,26 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.31.1 + rev: v3.3.1 hooks: - id: pyupgrade args: [--py36-plus] - # PEP 257 - - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 - rev: v0.3.3 + # PyDocStyle + - repo: https://github.com/PyCQA/pydocstyle + rev: 6.3.0 hooks: - - id: pep257 + - id: pydocstyle args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + - repo: https://github.com/psf/black + rev: 23.1.0 + hooks: + - id: black + args: ["--line-length=99"] + - repo: https://github.com/pycqa/flake8 - rev: 4.0.1 + rev: 6.0.0 hooks: - id: flake8 args: ["--extend-ignore=E501"] @@ -60,7 +66,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-12 + entry: clang-format-14 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] @@ -110,14 +116,14 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: 0.10.1 + rev: v1.1.1 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks - rev: v1.9.0 + rev: v1.10.0 hooks: - id: rst-backticks exclude: CHANGELOG\.rst$ @@ -127,7 +133,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.1.0 + rev: v2.2.2 hooks: - id: codespell args: ['--write-changes'] diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index f40939fc3c..d4c1ef23c1 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,39 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Async params (`#927 `_) +* Contributors: Márk Szitanics, Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ +* Update imu_sensor.hpp (`#893 `_) + Covariances values should come from the IMU_Broadcaster, like the frame_id or the time +* Contributors: flochre + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ + +3.4.0 (2022-11-27) +------------------ + 3.3.0 (2022-11-15) ------------------ diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 4f28622bd6..034556d19f 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -1,104 +1,88 @@ -cmake_minimum_required(VERSION 3.5) -project(controller_interface) +cmake_minimum_required(VERSION 3.16) +project(controller_interface LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + rclcpp_lifecycle +) + find_package(ament_cmake REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -add_library( - ${PROJECT_NAME} - SHARED +add_library(controller_interface SHARED src/controller_interface_base.cpp src/controller_interface.cpp src/chainable_controller_interface.cpp ) -target_include_directories( - ${PROJECT_NAME} - PRIVATE - include -) -ament_target_dependencies( - ${PROJECT_NAME} - hardware_interface - rclcpp_lifecycle +target_compile_features(controller_interface PUBLIC cxx_std_17) +target_include_directories(controller_interface PUBLIC + $ + $ ) +ament_target_dependencies(controller_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") - -install(DIRECTORY include/ - DESTINATION include -) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) +target_compile_definitions(controller_interface PRIVATE "CONTROLLER_INTERFACE_BUILDING_DLL") if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - - find_package(hardware_interface REQUIRED) find_package(sensor_msgs REQUIRED) ament_add_gmock(test_controller_interface test/test_controller_interface.cpp) - target_link_libraries(test_controller_interface ${PROJECT_NAME}) - target_include_directories(test_controller_interface PRIVATE include) + target_link_libraries(test_controller_interface + controller_interface + ) ament_add_gmock(test_controller_with_options test/test_controller_with_options.cpp) - target_link_libraries(test_controller_with_options ${PROJECT_NAME}) - target_include_directories(test_controller_with_options PRIVATE include) + target_link_libraries(test_controller_with_options + controller_interface + ) ament_add_gmock(test_chainable_controller_interface test/test_chainable_controller_interface.cpp) - target_link_libraries(test_chainable_controller_interface ${PROJECT_NAME}) - target_include_directories(test_chainable_controller_interface PRIVATE include) - ament_target_dependencies(test_chainable_controller_interface hardware_interface) - - ament_add_gmock( - test_semantic_component_interface - test/test_semantic_component_interface.cpp - ) - target_include_directories(test_semantic_component_interface PRIVATE include) - ament_target_dependencies( - test_semantic_component_interface - hardware_interface + target_link_libraries(test_chainable_controller_interface + controller_interface + hardware_interface::hardware_interface ) - ament_add_gmock( - test_force_torque_sensor - test/test_force_torque_sensor.cpp + ament_add_gmock(test_semantic_component_interface test/test_semantic_component_interface.cpp) + target_link_libraries(test_semantic_component_interface + controller_interface + hardware_interface::hardware_interface ) - target_include_directories(test_force_torque_sensor PRIVATE include) - ament_target_dependencies( - test_force_torque_sensor - hardware_interface + + ament_add_gmock(test_force_torque_sensor test/test_force_torque_sensor.cpp) + target_link_libraries(test_force_torque_sensor + controller_interface + hardware_interface::hardware_interface ) - ament_add_gmock( - test_imu_sensor - test/test_imu_sensor.cpp + ament_add_gmock(test_imu_sensor test/test_imu_sensor.cpp) + target_link_libraries(test_imu_sensor + controller_interface + hardware_interface::hardware_interface ) - target_include_directories(test_imu_sensor PRIVATE include) - ament_target_dependencies( - test_imu_sensor - hardware_interface + ament_target_dependencies(test_imu_sensor sensor_msgs ) endif() -ament_export_dependencies( - hardware_interface - rclcpp_lifecycle - sensor_msgs -) -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/controller_interface ) -ament_export_libraries( - ${PROJECT_NAME} +install(TARGETS controller_interface + EXPORT export_controller_interface + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) + +ament_export_targets(export_controller_interface HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 71e4409d8b..fdaffac06f 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -153,6 +153,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC unsigned int get_update_rate() const; + CONTROLLER_INTERFACE_PUBLIC + bool is_async() const; + /// Declare and initialize a parameter with a type. /** * @@ -220,6 +223,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::vector command_interfaces_; std::vector state_interfaces_; unsigned int update_rate_ = 0; + bool is_async_ = false; private: std::shared_ptr node_; diff --git a/controller_interface/include/semantic_components/imu_sensor.hpp b/controller_interface/include/semantic_components/imu_sensor.hpp index be2f3e06d0..74c5524826 100644 --- a/controller_interface/include/semantic_components/imu_sensor.hpp +++ b/controller_interface/include/semantic_components/imu_sensor.hpp @@ -114,17 +114,14 @@ class IMUSensor : public SemanticComponentInterface message.orientation.y = orientation_[1]; message.orientation.z = orientation_[2]; message.orientation.w = orientation_[3]; - message.orientation_covariance.fill(.0); message.angular_velocity.x = angular_velocity_[0]; message.angular_velocity.y = angular_velocity_[1]; message.angular_velocity.z = angular_velocity_[2]; - message.angular_velocity_covariance.fill(.0); message.linear_acceleration.x = linear_acceleration_[0]; message.linear_acceleration.y = linear_acceleration_[1]; message.linear_acceleration.z = linear_acceleration_[2]; - message.linear_acceleration_covariance.fill(.0); return true; } diff --git a/controller_interface/package.xml b/controller_interface/package.xml index c449340f0a..9a1f2c771c 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.3.0 + 3.10.0 Description of controller_interface Bence Magyar Denis Štogl @@ -14,11 +14,8 @@ rclcpp_lifecycle sensor_msgs - hardware_interface - rclcpp_lifecycle - sensor_msgs - ament_cmake_gmock + sensor_msgs ament_cmake diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 035d2572e1..926449e201 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -34,6 +34,7 @@ return_type ControllerInterfaceBase::init( try { auto_declare("update_rate", 0); + auto_declare("is_async", false); } catch (const std::exception & e) { @@ -84,6 +85,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { update_rate_ = get_node()->get_parameter("update_rate").as_int(); + is_async_ = get_node()->get_parameter("is_async").as_bool(); } return get_node()->configure(); @@ -128,4 +130,6 @@ std::shared_ptr ControllerInterfaceBase::get_no unsigned int ControllerInterfaceBase::get_update_rate() const { return update_rate_; } +bool ControllerInterfaceBase::is_async() const { return is_async_; } + } // namespace controller_interface diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index bcf7a7b179..4b88e6df3d 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,60 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.10.0 (2023-03-16) +------------------- +* add spawner for hardware (`#941 `_) +* Contributors: Manuel Muth + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ +* fix AttributeError: Parameter object attribute name is read-only (`#957 `_) +* Remove deprecations from CLI and controller_manager (`#948 `_) +* Expose node options to controller manager (`#942 `_) +* Contributors: Christoph Fröhlich, Noel Jiménez García, methylDragon + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* 🖤 Add Black formatter for Python files. (`#936 `_) +* Add list_hardware_components CLI `_ - Adds list_hardware_components to CLI (`#891 `_) +* Contributors: Andy McEvoy, Dr. Denis, Tyler Weaver + +3.7.0 (2023-01-24) +------------------ +* Do not use CLI calls but direct API for setting parameters. (`#910 `_) +* Optimize output of controller spawner (`#909 `_) +* ControllerManager: catch exception by reference (`#906 `_) +* Test fix: don't keep reference to the controller in the test when it should be destroyed in the controller manager (`#883 `_) +* Merge branch 'fix-update-rate' into humble (`#874 `_) +* Contributors: Christopher Wecht, Dr. Denis, Tony Najjar, sgmurray + +3.6.0 (2023-01-12) +------------------ +* Fix QoS deprecation warnings (`#879 `_) +* Add backward_ros to controller_manager (`#886 `_) +* Contributors: Andy McEvoy, Bence Magyar + +3.5.1 (2023-01-06) +------------------ +* Prevent controller manager from crashing when controller's plugin has error during loading. (`#881 `_) +* Contributors: Denis Štogl + +3.5.0 (2022-12-06) +------------------ +* Rename class type to plugin name #api-breaking #abi-breaking (`#780 `_) +* Namespace Loaded Controllers (`#852 `_) +* Contributors: Bence Magyar, sp-sophia-labs + +3.4.0 (2022-11-27) +------------------ +* Use a thread priority library from realtime_tools (`#794 `_) +* [Doc] Correct type of update_rate parameter (`#858 `_) +* Contributors: Andy Zelenak, Denis Štogl, Bence Magyar + 3.3.0 (2022-11-15) ------------------ * Adding activation/deactivation tests for chain controllers (`#809 `_) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 4dd1c32ae7..1d9b062efa 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -1,197 +1,214 @@ -cmake_minimum_required(VERSION 3.5) -project(controller_manager) +cmake_minimum_required(VERSION 3.16) +project(controller_manager LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - ament_index_cpp - controller_interface - controller_manager_msgs - diagnostic_updater - hardware_interface - pluginlib - rclcpp - realtime_tools + ament_index_cpp + controller_interface + controller_manager_msgs + diagnostic_updater + hardware_interface + pluginlib + rclcpp + realtime_tools ) find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(ament_cmake_core REQUIRED) +find_package(backward_ros REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library(${PROJECT_NAME} SHARED +add_library(controller_manager SHARED src/controller_manager.cpp ) -target_include_directories(${PROJECT_NAME} PRIVATE include) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_features(controller_manager PUBLIC cxx_std_17) +target_include_directories(controller_manager PUBLIC + $ + $ +) +ament_target_dependencies(controller_manager PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") +target_compile_definitions(controller_manager PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") add_executable(ros2_control_node src/ros2_control_node.cpp) -target_include_directories(ros2_control_node PRIVATE include) -target_link_libraries(ros2_control_node - ${PROJECT_NAME} -) -ament_target_dependencies(ros2_control_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -install(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib -) - -install(TARGETS ros2_control_node - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include/ - DESTINATION include +target_link_libraries(ros2_control_node PRIVATE + controller_manager ) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) + # Plugin Libraries that are built and installed for use in testing add_library(test_controller SHARED test/test_controller/test_controller.cpp) - target_include_directories(test_controller PRIVATE include) - target_link_libraries(test_controller ${PROJECT_NAME}) + target_link_libraries(test_controller PUBLIC controller_manager) target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") - pluginlib_export_plugin_description_file( - controller_interface test/test_controller/test_controller.xml) - install(TARGETS test_controller + pluginlib_export_plugin_description_file(controller_interface test/test_controller/test_controller.xml) + install( + TARGETS test_controller DESTINATION lib ) - add_library(test_controller_failed_init SHARED test/test_controller_failed_init/test_controller_failed_init.cpp) - target_include_directories(test_controller_failed_init PRIVATE include) - target_link_libraries(test_controller_failed_init ${PROJECT_NAME}) + add_library(test_controller_failed_init SHARED + test/test_controller_failed_init/test_controller_failed_init.cpp + ) + target_link_libraries(test_controller_failed_init PUBLIC controller_manager) target_compile_definitions(test_controller_failed_init PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface test/test_controller_failed_init/test_controller_failed_init.xml) - install(TARGETS test_controller_failed_init + install( + TARGETS test_controller_failed_init DESTINATION lib ) - add_library(test_chainable_controller SHARED test/test_chainable_controller/test_chainable_controller.cpp) - ament_target_dependencies(test_chainable_controller realtime_tools) - target_include_directories(test_chainable_controller PRIVATE include) - target_link_libraries(test_chainable_controller controller_manager) + add_library(test_chainable_controller SHARED + test/test_chainable_controller/test_chainable_controller.cpp + ) + ament_target_dependencies(test_chainable_controller PUBLIC realtime_tools) + target_link_libraries(test_chainable_controller PUBLIC controller_manager) target_compile_definitions(test_chainable_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface test/test_chainable_controller/test_chainable_controller.xml) - install(TARGETS test_chainable_controller + install( + TARGETS test_chainable_controller DESTINATION lib ) - ament_add_gmock( - test_controller_manager + ament_add_gmock(test_controller_manager test/test_controller_manager.cpp ) - target_include_directories(test_controller_manager PRIVATE include) - target_link_libraries(test_controller_manager ${PROJECT_NAME} test_controller) - ament_target_dependencies(test_controller_manager ros2_control_test_assets) + target_link_libraries(test_controller_manager + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) - ament_add_gmock( - test_controller_manager_with_namespace.cpp + ament_add_gmock(test_controller_manager_with_namespace test/test_controller_manager_with_namespace.cpp ) - target_include_directories(test_controller_manager_with_namespace.cpp PRIVATE include) - target_link_libraries(test_controller_manager_with_namespace.cpp ${PROJECT_NAME} test_controller) - ament_target_dependencies(test_controller_manager_with_namespace.cpp ros2_control_test_assets) + target_link_libraries(test_controller_manager_with_namespace + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) - ament_add_gmock( - test_controller_manager_hardware_error_handling + ament_add_gmock(test_controller_manager_hardware_error_handling test/test_controller_manager_hardware_error_handling.cpp ) - target_include_directories(test_controller_manager_hardware_error_handling PRIVATE include) - target_link_libraries(test_controller_manager_hardware_error_handling ${PROJECT_NAME} test_controller) - ament_target_dependencies(test_controller_manager_hardware_error_handling ros2_control_test_assets) + target_link_libraries(test_controller_manager_hardware_error_handling + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) - ament_add_gmock( - test_load_controller + ament_add_gmock(test_load_controller test/test_load_controller.cpp APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ ) - target_include_directories(test_load_controller PRIVATE include) - target_link_libraries(test_load_controller ${PROJECT_NAME} test_controller test_controller_failed_init) - ament_target_dependencies(test_load_controller ros2_control_test_assets) + target_link_libraries(test_load_controller + controller_manager + test_controller + test_controller_failed_init + ros2_control_test_assets::ros2_control_test_assets + ) - ament_add_gmock( - test_controllers_chaining_with_controller_manager + ament_add_gmock(test_controllers_chaining_with_controller_manager test/test_controllers_chaining_with_controller_manager.cpp ) - target_include_directories(test_controllers_chaining_with_controller_manager PRIVATE include) - target_link_libraries(test_controllers_chaining_with_controller_manager controller_manager test_chainable_controller test_controller) - ament_target_dependencies(test_controllers_chaining_with_controller_manager ros2_control_test_assets) + target_link_libraries(test_controllers_chaining_with_controller_manager + controller_manager + test_chainable_controller + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) - ament_add_gmock( - test_controller_manager_srvs + ament_add_gmock(test_controller_manager_srvs test/test_controller_manager_srvs.cpp APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ ) - target_include_directories(test_controller_manager_srvs PRIVATE include) - target_link_libraries(test_controller_manager_srvs ${PROJECT_NAME} test_controller test_chainable_controller) - ament_target_dependencies( - test_controller_manager_srvs + target_link_libraries(test_controller_manager_srvs + controller_manager + test_controller + test_chainable_controller + ros2_control_test_assets::ros2_control_test_assets + ) + ament_target_dependencies(test_controller_manager_srvs controller_manager_msgs - ros2_control_test_assets ) - add_library(test_controller_with_interfaces SHARED test/test_controller_with_interfaces/test_controller_with_interfaces.cpp) - target_include_directories(test_controller_with_interfaces PRIVATE include) - target_link_libraries(test_controller_with_interfaces ${PROJECT_NAME}) + add_library(test_controller_with_interfaces SHARED + test/test_controller_with_interfaces/test_controller_with_interfaces.cpp + ) + target_link_libraries(test_controller_with_interfaces PUBLIC + controller_manager + ) target_compile_definitions(test_controller_with_interfaces PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") pluginlib_export_plugin_description_file( controller_interface test/test_controller_with_interfaces/test_controller_with_interfaces.xml) - install(TARGETS test_controller_with_interfaces + install( + TARGETS test_controller_with_interfaces DESTINATION lib ) - ament_add_gmock( - test_release_interfaces + ament_add_gmock(test_release_interfaces test/test_release_interfaces.cpp APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ ) - target_include_directories(test_release_interfaces PRIVATE include) - target_link_libraries(test_release_interfaces ${PROJECT_NAME} test_controller_with_interfaces) - ament_target_dependencies(test_release_interfaces ros2_control_test_assets) + target_link_libraries(test_release_interfaces + controller_manager + test_controller_with_interfaces + ros2_control_test_assets::ros2_control_test_assets + ) - ament_add_gmock( - test_spawner_unspawner + ament_add_gmock(test_spawner_unspawner test/test_spawner_unspawner.cpp ) - target_include_directories(test_spawner_unspawner PRIVATE include) - target_link_libraries(test_spawner_unspawner ${PROJECT_NAME} test_controller) - ament_target_dependencies(test_spawner_unspawner ros2_control_test_assets) + target_link_libraries(test_spawner_unspawner + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) - ament_add_gmock( - test_hardware_management_srvs + ament_add_gmock(test_hardware_management_srvs test/test_hardware_management_srvs.cpp ) - target_include_directories(test_hardware_management_srvs PRIVATE include) - target_link_libraries(test_hardware_management_srvs ${PROJECT_NAME} test_controller) - ament_target_dependencies( - test_hardware_management_srvs + target_link_libraries(test_hardware_management_srvs + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) + ament_target_dependencies(test_hardware_management_srvs controller_manager_msgs - ros2_control_test_assets ) endif() -# Install Python modules -ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) - -ament_export_libraries( - ${PROJECT_NAME} +install( + DIRECTORY include/ + DESTINATION include/controller_manager +) +install( + TARGETS controller_manager + EXPORT export_controller_manager + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib ) -ament_export_include_directories( - include +install( + TARGETS ros2_control_node + RUNTIME DESTINATION lib/controller_manager ) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} + +ament_python_install_package(controller_manager + SCRIPTS_DESTINATION lib/controller_manager ) +ament_export_targets(export_controller_manager HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index f1cd926f85..f49bed4d34 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -16,20 +16,24 @@ configure_controller, list_controller_types, list_controllers, + list_hardware_components, list_hardware_interfaces, load_controller, reload_controller_libraries, + set_hardware_component_state, switch_controllers, - unload_controller + unload_controller, ) __all__ = [ - 'configure_controller', - 'list_controller_types', - 'list_controllers', - 'list_hardware_interfaces', - 'load_controller', - 'reload_controller_libraries', - 'switch_controllers', - 'unload_controller', + "configure_controller", + "list_controller_types", + "list_controllers", + "list_hardware_components", + "list_hardware_interfaces", + "load_controller", + "reload_controller_libraries", + "set_hardware_component_state", + "switch_controllers", + "unload_controller", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 08f68fa663..5fe78c69b8 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -12,9 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager_msgs.srv import ConfigureController, \ - ListControllers, ListControllerTypes, ListHardwareInterfaces, \ - LoadController, ReloadControllerLibraries, SwitchController, UnloadController +from controller_manager_msgs.srv import ( + ConfigureController, + ListControllers, + ListControllerTypes, + ListHardwareComponents, + ListHardwareInterfaces, + LoadController, + ReloadControllerLibraries, + SetHardwareComponentState, + SwitchController, + UnloadController, +) import rclpy @@ -24,62 +33,102 @@ def service_caller(node, service_name, service_type, request, service_timeout=10 if not cli.service_is_ready(): node.get_logger().debug( - f'waiting {service_timeout} seconds for service {service_name} to become available...') + f"waiting {service_timeout} seconds for service {service_name} to become available..." + ) if not cli.wait_for_service(service_timeout): - raise RuntimeError(f'Could not contact service {service_name}') + raise RuntimeError(f"Could not contact service {service_name}") - node.get_logger().debug(f'requester: making request: {request}\n') + node.get_logger().debug(f"requester: making request: {request}\n") future = cli.call_async(request) rclpy.spin_until_future_complete(node, future) if future.result() is not None: return future.result() else: - raise RuntimeError(f'Exception while calling service: {future.exception()}') + raise RuntimeError(f"Exception while calling service: {future.exception()}") def configure_controller(node, controller_manager_name, controller_name): request = ConfigureController.Request() request.name = controller_name - return service_caller(node, f'{controller_manager_name}/configure_controller', - ConfigureController, request) + return service_caller( + node, f"{controller_manager_name}/configure_controller", ConfigureController, request + ) def list_controllers(node, controller_manager_name): request = ListControllers.Request() - return service_caller(node, f'{controller_manager_name}/list_controllers', - ListControllers, request) + return service_caller( + node, f"{controller_manager_name}/list_controllers", ListControllers, request + ) def list_controller_types(node, controller_manager_name): request = ListControllerTypes.Request() - return service_caller(node, - f'{controller_manager_name}/list_controller_types', - ListControllerTypes, request) + return service_caller( + node, f"{controller_manager_name}/list_controller_types", ListControllerTypes, request + ) + + +def list_hardware_components(node, controller_manager_name): + request = ListHardwareComponents.Request() + return service_caller( + node, + f"{controller_manager_name}/list_hardware_components", + ListHardwareComponents, + request, + ) def list_hardware_interfaces(node, controller_manager_name): request = ListHardwareInterfaces.Request() - return service_caller(node, f'{controller_manager_name}/list_hardware_interfaces', - ListHardwareInterfaces, request) + return service_caller( + node, + f"{controller_manager_name}/list_hardware_interfaces", + ListHardwareInterfaces, + request, + ) def load_controller(node, controller_manager_name, controller_name): request = LoadController.Request() request.name = controller_name - return service_caller(node, f'{controller_manager_name}/load_controller', - LoadController, request) + return service_caller( + node, f"{controller_manager_name}/load_controller", LoadController, request + ) def reload_controller_libraries(node, controller_manager_name, force_kill): request = ReloadControllerLibraries.Request() request.force_kill = force_kill - return service_caller(node, - f'{controller_manager_name}/reload_controller_libraries', - ReloadControllerLibraries, request) - - -def switch_controllers(node, controller_manager_name, deactivate_controllers, - activate_controllers, strict, activate_asap, timeout): + return service_caller( + node, + f"{controller_manager_name}/reload_controller_libraries", + ReloadControllerLibraries, + request, + ) + + +def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state): + request = SetHardwareComponentState.Request() + request.name = component_name + request.target_state = lifecyle_state + return service_caller( + node, + f"{controller_manager_name}/set_hardware_component_state", + SetHardwareComponentState, + request, + ) + + +def switch_controllers( + node, + controller_manager_name, + deactivate_controllers, + activate_controllers, + strict, + activate_asap, + timeout, +): request = SwitchController.Request() request.activate_controllers = activate_controllers request.deactivate_controllers = deactivate_controllers @@ -89,12 +138,14 @@ def switch_controllers(node, controller_manager_name, deactivate_controllers, request.strictness = SwitchController.Request.BEST_EFFORT request.activate_asap = activate_asap request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg() - return service_caller(node, f'{controller_manager_name}/switch_controller', - SwitchController, request) + return service_caller( + node, f"{controller_manager_name}/switch_controller", SwitchController, request + ) def unload_controller(node, controller_manager_name, controller_name): request = UnloadController.Request() request.name = controller_name - return service_caller(node, f'{controller_manager_name}/unload_controller', - UnloadController, request) + return service_caller( + node, f"{controller_manager_name}/unload_controller", UnloadController, request + ) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py new file mode 100644 index 0000000000..c95fb6181e --- /dev/null +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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. + +import argparse +import sys +import time + +from controller_manager import set_hardware_component_state + +from lifecycle_msgs.msg import State +import rclpy +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.signals import SignalHandlerOptions + + +# from https://stackoverflow.com/a/287944 +class bcolors: + HEADER = "\033[95m" + OKBLUE = "\033[94m" + OKCYAN = "\033[96m" + OKGREEN = "\033[92m" + WARNING = "\033[93m" + FAIL = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + UNDERLINE = "\033[4m" + + +def first_match(iterable, predicate): + return next((n for n in iterable if predicate(n)), None) + + +def wait_for_value_or(function, node, timeout, default, description): + while node.get_clock().now() < timeout: + if result := function(): + return result + node.get_logger().info( + f"Waiting for {description}", throttle_duration_sec=2, skip_first=True + ) + time.sleep(0.2) + return default + + +def combine_name_and_namespace(name_and_namespace): + node_name, namespace = name_and_namespace + return namespace + ("" if namespace.endswith("/") else "/") + node_name + + +def find_node_and_namespace(node, full_node_name): + node_names_and_namespaces = node.get_node_names_and_namespaces() + return first_match( + node_names_and_namespaces, + lambda n: combine_name_and_namespace(n) == full_node_name, + ) + + +def has_service_names(node, node_name, node_namespace, service_names): + client_names_and_types = node.get_service_names_and_types_by_node(node_name, node_namespace) + if not client_names_and_types: + return False + client_names, _ = zip(*client_names_and_types) + return all(service in client_names for service in service_names) + + +def wait_for_controller_manager(node, controller_manager, timeout_duration): + # List of service names from controller_manager we wait for + service_names = ( + f"{controller_manager}/list_hardware_components", + f"{controller_manager}/set_hardware_component_state", + ) + + # Wait for controller_manager + timeout = node.get_clock().now() + Duration(seconds=timeout_duration) + node_and_namespace = wait_for_value_or( + lambda: find_node_and_namespace(node, controller_manager), + node, + timeout, + None, + f"'{controller_manager}' node to exist", + ) + + # Wait for the services if the node was found + if node_and_namespace: + node_name, namespace = node_and_namespace + return wait_for_value_or( + lambda: has_service_names(node, node_name, namespace, service_names), + node, + timeout, + False, + f"'{controller_manager}' services to be available", + ) + + return False + + +def handle_set_component_state_service_call( + node, controller_manager_name, component, target_state, action +): + response = set_hardware_component_state(node, controller_manager_name, component, target_state) + if response.ok and response.state == target_state: + node.get_logger().info( + bcolors.OKGREEN + + f"{action} component '{component}'. Hardware now in state: {response.state}." + ) + elif response.ok and not response.state == target_state: + node.get_logger().warn( + bcolors.WARNING + + f"Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'." + ) + else: + node.get_logger().warn( + bcolors.WARNING + + f"Could not {action} component '{component}'. Service call failed. Wrong component name?" + ) + + +def activate_components(node, controller_manager_name, components_to_activate): + active_state = State() + active_state.id = State.PRIMARY_STATE_ACTIVE + active_state.label = "active" + for component in components_to_activate: + handle_set_component_state_service_call( + node, controller_manager_name, component, active_state, "activated" + ) + + +def configure_components(node, controller_manager_name, components_to_configure): + inactive_state = State() + inactive_state.id = State.PRIMARY_STATE_INACTIVE + inactive_state.label = "inactive" + for component in components_to_configure: + handle_set_component_state_service_call( + node, controller_manager_name, component, inactive_state, "configured" + ) + + +def main(args=None): + rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) + parser = argparse.ArgumentParser() + activate_or_confiigure_grp = parser.add_mutually_exclusive_group(required=True) + + parser.add_argument( + "hardware_component_name", + help="The name of the hardware component which should be activated.", + ) + parser.add_argument( + "-c", + "--controller-manager", + help="Name of the controller manager ROS node", + default="controller_manager", + required=False, + ) + parser.add_argument( + "--controller-manager-timeout", + help="Time to wait for the controller manager", + required=False, + default=10, + type=int, + ) + + # add arguments which are mutually exclusive + activate_or_confiigure_grp.add_argument( + "--activate", + help="Activates the given components. Note: Components are by default configured before activated. ", + action="store_true", + required=False, + ) + activate_or_confiigure_grp.add_argument( + "--configure", + help="Configures the given components.", + action="store_true", + required=False, + ) + + command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] + args = parser.parse_args(command_line_args) + controller_manager_name = args.controller_manager + controller_manager_timeout = args.controller_manager_timeout + hardware_component = [args.hardware_component_name] + activate = args.activate + configure = args.configure + + node = Node("hardware_spawner") + if not controller_manager_name.startswith("/"): + spawner_namespace = node.get_namespace() + if spawner_namespace != "/": + controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" + else: + controller_manager_name = f"/{controller_manager_name}" + + try: + if not wait_for_controller_manager( + node, controller_manager_name, controller_manager_timeout + ): + node.get_logger().error("Controller manager not available") + return 1 + + if activate: + activate_components(node, controller_manager_name, hardware_component) + elif configure: + configure_components(node, controller_manager_name, hardware_component) + else: + node.get_logger().error( + 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' + ) + parser.print_help() + return 0 + finally: + rclpy.shutdown() + + +if __name__ == "__main__": + ret = main() + sys.exit(ret) diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index d3f9356eae..93941cc11f 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -19,9 +19,9 @@ from launch_ros.actions import Node -def generate_load_controller_launch_description(controller_name, - controller_type=None, - controller_params_file=None): +def generate_load_controller_launch_description( + controller_name, controller_type=None, controller_params_file=None +): """ Generate launch description for loading a controller using spawner. @@ -44,38 +44,53 @@ def generate_load_controller_launch_description(controller_name, """ declare_controller_mgr_name = DeclareLaunchArgument( - 'controller_manager_name', default_value='controller_manager', - description='Controller manager node name' + "controller_manager_name", + default_value="controller_manager", + description="Controller manager node name", ) declare_unload_on_kill = DeclareLaunchArgument( - 'unload_on_kill', default_value='false', - description='Wait until the node is interrupted and then unload controller' + "unload_on_kill", + default_value="false", + description="Wait until the node is interrupted and then unload controller", ) spawner_arguments = [ controller_name, - '--controller-manager', - LaunchConfiguration('controller_manager_name') + "--controller-manager", + LaunchConfiguration("controller_manager_name"), ] if controller_type: - spawner_arguments += ['--controller-type', controller_type] + spawner_arguments += ["--controller-type", controller_type] if controller_params_file: - spawner_arguments += ['--param-file', controller_params_file] + spawner_arguments += ["--param-file", controller_params_file] # Setting --unload-on-kill if launch arg unload_on_kill is "true" # See https://github.com/ros2/launch/issues/290 - spawner_arguments += [PythonExpression( - ['"--unload-on-kill"', ' if "true" == "', - LaunchConfiguration('unload_on_kill'), '" else ""'] - )] + spawner_arguments += [ + PythonExpression( + [ + '"--unload-on-kill"', + ' if "true" == "', + LaunchConfiguration("unload_on_kill"), + '" else ""', + ] + ) + ] - spawner = Node(package='controller_manager', executable='spawner', - arguments=spawner_arguments, shell=True, output='screen') + spawner = Node( + package="controller_manager", + executable="spawner", + arguments=spawner_arguments, + shell=True, + output="screen", + ) - return LaunchDescription([ - declare_controller_mgr_name, - declare_unload_on_kill, - spawner, - ]) + return LaunchDescription( + [ + declare_controller_mgr_name, + declare_unload_on_kill, + spawner, + ] + ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index be55f69223..24aad0ae78 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -16,32 +16,42 @@ import argparse import errno import os -import subprocess import sys import time import warnings +import io +from contextlib import redirect_stdout, redirect_stderr -from controller_manager import configure_controller, list_controllers, \ - load_controller, switch_controllers, unload_controller +from controller_manager import ( + configure_controller, + list_controllers, + load_controller, + switch_controllers, + unload_controller, +) import rclpy +from rcl_interfaces.msg import Parameter from rclpy.duration import Duration from rclpy.node import Node +from rclpy.parameter import get_parameter_value from rclpy.signals import SignalHandlerOptions +from ros2param.api import call_set_parameters +from ros2param.api import load_parameter_file # from https://stackoverflow.com/a/287944 class bcolors: - HEADER = '\033[95m' - OKBLUE = '\033[94m' - OKCYAN = '\033[96m' - OKGREEN = '\033[92m' - WARNING = '\033[93m' - FAIL = '\033[91m' - ENDC = '\033[0m' - BOLD = '\033[1m' - UNDERLINE = '\033[4m' + HEADER = "\033[95m" + OKBLUE = "\033[94m" + OKCYAN = "\033[96m" + OKGREEN = "\033[92m" + WARNING = "\033[93m" + FAIL = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + UNDERLINE = "\033[4m" def first_match(iterable, predicate): @@ -53,21 +63,22 @@ def wait_for_value_or(function, node, timeout, default, description): if result := function(): return result node.get_logger().info( - f'Waiting for {description}', - throttle_duration_sec=2) + f"Waiting for {description}", throttle_duration_sec=2, skip_first=True + ) time.sleep(0.2) return default def combine_name_and_namespace(name_and_namespace): node_name, namespace = name_and_namespace - return namespace + ('' if namespace.endswith('/') else '/') + node_name + return namespace + ("" if namespace.endswith("/") else "/") + node_name def find_node_and_namespace(node, full_node_name): node_names_and_namespaces = node.get_node_names_and_namespaces() - return first_match(node_names_and_namespaces, - lambda n: combine_name_and_namespace(n) == full_node_name) + return first_match( + node_names_and_namespaces, lambda n: combine_name_and_namespace(n) == full_node_name + ) def has_service_names(node, node_name, node_namespace, service_names): @@ -81,28 +92,37 @@ def has_service_names(node, node_name, node_namespace, service_names): def wait_for_controller_manager(node, controller_manager, timeout_duration): # List of service names from controller_manager we wait for service_names = ( - f'{controller_manager}/configure_controller', - f'{controller_manager}/list_controllers', - f'{controller_manager}/list_controller_types', - f'{controller_manager}/list_hardware_interfaces', - f'{controller_manager}/load_controller', - f'{controller_manager}/reload_controller_libraries', - f'{controller_manager}/switch_controller', - f'{controller_manager}/unload_controller' + f"{controller_manager}/configure_controller", + f"{controller_manager}/list_controllers", + f"{controller_manager}/list_controller_types", + f"{controller_manager}/list_hardware_components", + f"{controller_manager}/list_hardware_interfaces", + f"{controller_manager}/load_controller", + f"{controller_manager}/reload_controller_libraries", + f"{controller_manager}/switch_controller", + f"{controller_manager}/unload_controller", ) # Wait for controller_manager timeout = node.get_clock().now() + Duration(seconds=timeout_duration) node_and_namespace = wait_for_value_or( lambda: find_node_and_namespace(node, controller_manager), - node, timeout, None, f'\'{controller_manager}\' node to exist') + node, + timeout, + None, + f"'{controller_manager}' node to exist", + ) # Wait for the services if the node was found if node_and_namespace: node_name, namespace = node_and_namespace return wait_for_value_or( lambda: has_service_names(node, node_name, namespace, service_names), - node, timeout, False, f"'{controller_manager}' services to be available") + node, + timeout, + False, + f"'{controller_manager}' services to be available", + ) return False @@ -113,152 +133,257 @@ def is_controller_loaded(node, controller_manager, controller_name): def main(args=None): - rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() + parser.add_argument("controller_name", help="Name of the controller") parser.add_argument( - 'controller_name', help='Name of the controller') + "-c", + "--controller-manager", + help="Name of the controller manager ROS node", + default="controller_manager", + required=False, + ) parser.add_argument( - '-c', '--controller-manager', help='Name of the controller manager ROS node', - default='controller_manager', required=False) + "-p", + "--param-file", + help="Controller param file to be loaded into controller node before configure", + required=False, + ) parser.add_argument( - '-p', '--param-file', - help='Controller param file to be loaded into controller node before configure', - required=False) + "-n", "--namespace", help="Namespace for the controller", default="", required=False + ) parser.add_argument( - '--load-only', help='Only load the controller and leave unconfigured.', - action='store_true', required=False) + "--load-only", + help="Only load the controller and leave unconfigured.", + action="store_true", + required=False, + ) parser.add_argument( - '--stopped', help='Load and configure the controller, however do not activate them', - action='store_true', required=False) + "--inactive", + help="Load and configure the controller, however do not activate them", + action="store_true", + required=False, + ) parser.add_argument( - '--inactive', help='Load and configure the controller, however do not activate them', - action='store_true', required=False) + "-t", + "--controller-type", + help="If not provided it should exist in the controller manager namespace", + default=None, + required=False, + ) parser.add_argument( - '-t', '--controller-type', - help='If not provided it should exist in the controller manager namespace', - default=None, required=False) + "-u", + "--unload-on-kill", + help="Wait until this application is interrupted and unload controller", + action="store_true", + ) parser.add_argument( - '-u', '--unload-on-kill', - help='Wait until this application is interrupted and unload controller', - action='store_true') + "--controller-manager-timeout", + help="Time to wait for the controller manager", + required=False, + default=10, + type=int, + ) parser.add_argument( - '--controller-manager-timeout', - help='Time to wait for the controller manager', required=False, default=10, type=int) + "--log-level", + help="Log level for spawner node", + required=False, + choices=["debug", "info", "warn", "error", "fatal"], + default="info", + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) controller_name = args.controller_name controller_manager_name = args.controller_manager + controller_namespace = args.namespace param_file = args.param_file controller_type = args.controller_type controller_manager_timeout = args.controller_manager_timeout + log_level = args.log_level + + loglevel_to_severity = { + "debug": rclpy.logging.LoggingSeverity.DEBUG, + "info": rclpy.logging.LoggingSeverity.INFO, + "warn": rclpy.logging.LoggingSeverity.WARN, + "error": rclpy.logging.LoggingSeverity.ERROR, + "fatal": rclpy.logging.LoggingSeverity.FATAL, + } if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) - node = Node('spawner_' + controller_name) - if not controller_manager_name.startswith('/'): + prefixed_controller_name = controller_name + if controller_namespace: + prefixed_controller_name = controller_namespace + "/" + controller_name + + node = Node("spawner_" + controller_name) + rclpy.logging.set_logger_level("spawner_" + controller_name, loglevel_to_severity[log_level]) + + if not controller_manager_name.startswith("/"): spawner_namespace = node.get_namespace() - if spawner_namespace != '/': + if spawner_namespace != "/": controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" else: controller_manager_name = f"/{controller_manager_name}" try: - if not wait_for_controller_manager(node, controller_manager_name, - controller_manager_timeout): - node.get_logger().error('Controller manager not available') + if not wait_for_controller_manager( + node, controller_manager_name, controller_manager_timeout + ): + node.get_logger().error( + bcolors.FAIL + "Controller manager not available" + bcolors.ENDC + ) return 1 - if is_controller_loaded(node, controller_manager_name, controller_name): - node.get_logger().info('Controller already loaded, skipping load_controller') + if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): + node.get_logger().warn( + bcolors.WARNING + + "Controller already loaded, skipping load_controller" + + bcolors.ENDC + ) else: if controller_type: - ret = subprocess.run(['ros2', 'param', 'set', controller_manager_name, - controller_name + '.type', controller_type]) - ret = load_controller( - node, controller_manager_name, controller_name) + parameter = Parameter() + parameter.name = prefixed_controller_name + ".type" + parameter.value = get_parameter_value(string_value=controller_type) + + response = call_set_parameters( + node=node, node_name=controller_manager_name, parameters=[parameter] + ) + assert len(response.results) == 1 + result = response.results[0] + if result.successful: + node.get_logger().info( + bcolors.OKCYAN + + 'Set controller type to "' + + controller_type + + '" for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + else: + node.get_logger().fatal( + bcolors.FAIL + + 'Could not set controller type to "' + + controller_type + + '" for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + return 1 + + ret = load_controller(node, controller_manager_name, controller_name) if not ret.ok: - # Error message printed by ros2 control + node.get_logger().fatal( + bcolors.FAIL + + "Failed loading controller " + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) return 1 - node.get_logger().info(bcolors.OKBLUE + 'Loaded ' + controller_name + bcolors.ENDC) + node.get_logger().info( + bcolors.OKBLUE + "Loaded " + bcolors.BOLD + prefixed_controller_name + bcolors.ENDC + ) if param_file: - ret = subprocess.run(['ros2', 'param', 'load', controller_name, - param_file]) - if ret.returncode != 0: - # Error message printed by ros2 param - return ret.returncode - node.get_logger().info('Loaded ' + param_file + ' into ' + controller_name) + # load_parameter_file writes to stdout/stderr. Here we capture that and use node logging instead + with redirect_stdout(io.StringIO()) as f_stdout, redirect_stderr( + io.StringIO() + ) as f_stderr: + load_parameter_file( + node=node, + node_name=prefixed_controller_name, + parameter_file=param_file, + use_wildcard=True, + ) + if f_stdout.getvalue(): + node.get_logger().info(bcolors.OKCYAN + f_stdout.getvalue() + bcolors.ENDC) + if f_stderr.getvalue(): + node.get_logger().error(bcolors.FAIL + f_stderr.getvalue() + bcolors.ENDC) + node.get_logger().info( + bcolors.OKCYAN + + 'Loaded parameters file "' + + param_file + + '" for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + # TODO(destogl): use return value when upstream return value is merged + # ret = + # if ret.returncode != 0: + # Error message printed by ros2 param + # return ret.returncode + node.get_logger().info("Loaded " + param_file + " into " + prefixed_controller_name) if not args.load_only: - ret = configure_controller( - node, controller_manager_name, controller_name) + ret = configure_controller(node, controller_manager_name, controller_name) if not ret.ok: - node.get_logger().info('Failed to configure controller') + node.get_logger().error( + bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC + ) return 1 - if not args.stopped and not args.inactive: + if not args.inactive: ret = switch_controllers( - node, - controller_manager_name, - [], - [controller_name], - True, - True, - 5.0) + node, controller_manager_name, [], [controller_name], True, True, 5.0 + ) if not ret.ok: - node.get_logger().info('Failed to activate controller') + node.get_logger().error( + bcolors.FAIL + "Failed to activate controller" + bcolors.ENDC + ) return 1 - node.get_logger().info(bcolors.OKGREEN + 'Configured and activated ' + - bcolors.OKCYAN + controller_name + bcolors.ENDC) - elif args.stopped: - node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') + node.get_logger().info( + bcolors.OKGREEN + + "Configured and activated " + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) if not args.unload_on_kill: return 0 try: - node.get_logger().info('Waiting until interrupt to unload controllers') + node.get_logger().info("Waiting until interrupt to unload controllers") while True: time.sleep(1) except KeyboardInterrupt: - if not args.stopped and not args.inactive: - node.get_logger().info('Interrupt captured, deactivating and unloading controller') + if not args.inactive: + node.get_logger().info("Interrupt captured, deactivating and unloading controller") ret = switch_controllers( - node, - controller_manager_name, - [controller_name], - [], - True, - True, - 5.0) + node, controller_manager_name, [controller_name], [], True, True, 5.0 + ) if not ret.ok: - node.get_logger().info('Failed to deactivate controller') + node.get_logger().error( + bcolors.FAIL + "Failed to deactivate controller" + bcolors.ENDC + ) return 1 - node.get_logger().info('Deactivated controller') + node.get_logger().info("Deactivated controller") - elif args.stopped: - node.get_logger().warn('"--stopped" flag is deprecated use "--inactive" instead') - - ret = unload_controller( - node, controller_manager_name, controller_name) + ret = unload_controller(node, controller_manager_name, controller_name) if not ret.ok: - node.get_logger().info('Failed to unload controller') + node.get_logger().error( + bcolors.FAIL + "Failed to unload controller" + bcolors.ENDC + ) return 1 - node.get_logger().info('Unloaded controller') + node.get_logger().info("Unloaded controller") return 0 finally: rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": warnings.warn( "'spawner.py' is deprecated, please use 'spawner' (without .py extension)", - DeprecationWarning) + DeprecationWarning, + ) ret = main() sys.exit(ret) diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 4020e2ad06..766dc5d4f8 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -25,48 +25,45 @@ def main(args=None): - rclpy.init(args=args) parser = argparse.ArgumentParser() + parser.add_argument("controller_name", help="Name of the controller") parser.add_argument( - 'controller_name', help='Name of the controller') - parser.add_argument( - '-c', '--controller-manager', help='Name of the controller manager ROS node', - default='/controller_manager', required=False) + "-c", + "--controller-manager", + help="Name of the controller manager ROS node", + default="/controller_manager", + required=False, + ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) controller_name = args.controller_name controller_manager_name = args.controller_manager - node = Node('unspawner_' + controller_name) + node = Node("unspawner_" + controller_name) try: # Ignore returncode, because message is already printed and we'll try to unload anyway ret = switch_controllers( - node, - controller_manager_name, - [controller_name], - [], - True, - True, - 5.0) - node.get_logger().info('Deactivated controller') + node, controller_manager_name, [controller_name], [], True, True, 5.0 + ) + node.get_logger().info("Deactivated controller") ret = unload_controller(node, controller_manager_name, controller_name) if not ret.ok: - node.get_logger().info('Failed to unload controller') + node.get_logger().info("Failed to unload controller") return 1 - node.get_logger().info('Unloaded controller') + node.get_logger().info("Unloaded controller") return 0 finally: rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": warnings.warn( "'unspawner.py' is deprecated, please use 'unspawner' (without .py extension)", - DeprecationWarning + DeprecationWarning, ) ret = main() sys.exit(ret) diff --git a/controller_manager/doc/images/global_general_remap.png b/controller_manager/doc/images/global_general_remap.png new file mode 100644 index 0000000000..2c35c1f2ad Binary files /dev/null and b/controller_manager/doc/images/global_general_remap.png differ diff --git a/controller_manager/doc/images/global_specific_remap.png b/controller_manager/doc/images/global_specific_remap.png new file mode 100644 index 0000000000..174563c1c2 Binary files /dev/null and b/controller_manager/doc/images/global_specific_remap.png differ diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 7c5bcc1aa2..b23ed876b4 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -81,21 +81,23 @@ There are two scripts to interact with controller manager from launch files: .. code-block:: console $ ros2 run controller_manager spawner -h - usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [--load-only] [--stopped] [-t CONTROLLER_TYPE] [-u] + usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-t CONTROLLER_TYPE] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] controller_name positional arguments: controller_name Name of the controller - optional arguments: + options: -h, --help show this help message and exit -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node -p PARAM_FILE, --param-file PARAM_FILE Controller param file to be loaded into controller node before configure + -n NAMESPACE, --namespace NAMESPACE + Namespace for the controller --load-only Only load the controller and leave unconfigured. - --stopped Load and configure the controller, however do not start them + --inactive Load and configure the controller, however do not activate them -t CONTROLLER_TYPE, --controller-type CONTROLLER_TYPE If not provided it should exist in the controller manager namespace -u, --unload-on-kill Wait until this application is interrupted and unload controller @@ -119,6 +121,35 @@ There are two scripts to interact with controller manager from launch files: -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node +Using the Controller Manager in a Process +----------------------------------------- + +The ``ControllerManager`` may also be instantiated in a process as a class, but proper care must be taken when doing so. +The reason for this is because the ``ControllerManager`` class inherits from ``rclcpp::Node``. + +If there is more than one Node in the process, global node name remap rules can forcibly change the ``ControllerManager's`` node name as well, leading to duplicate node names. +This occurs whether the Nodes are siblings or exist in a hierarchy. + +.. image:: images/global_general_remap.png + +The workaround for this is to specify another node name remap rule in the ``NodeOptions`` passed to the ``ControllerManager`` node (causing it to ignore the global rule), or ensure that any remap rules are targeted to specific nodes. + +.. image:: images/global_specific_remap.png + +.. + TODO: (methylDragon) Update the proposed solution when https://github.com/ros2/ros2/issues/1377 is resolved + +.. code-block:: cpp + + auto options = controller_manager::get_cm_node_options(); + options.arguments({ + "--ros-args", + "--remap", "_target_node_name:__node:=dst_node_name", + "--log-level", "info"}); + + auto cm = std::make_shared( + executor, "_target_node_name", "some_optional_namespace", options); + Concepts ----------- diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 79694b96b4..abe952abcf 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -56,6 +56,8 @@ namespace controller_manager { using ControllersListIterator = std::vector::const_iterator; +rclcpp::NodeOptions get_cm_node_options(); + class ControllerManager : public rclcpp::Node { public: @@ -67,13 +69,15 @@ class ControllerManager : public rclcpp::Node std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = ""); + const std::string & namespace_ = "", + const rclcpp::NodeOptions & options = get_cm_node_options()); CONTROLLER_MANAGER_PUBLIC ControllerManager( std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = ""); + const std::string & namespace_ = "", + const rclcpp::NodeOptions & options = get_cm_node_options()); CONTROLLER_MANAGER_PUBLIC virtual ~ControllerManager() = default; @@ -509,6 +513,67 @@ class ControllerManager : public rclcpp::Node }; SwitchParams switch_params_; + + class ControllerThreadWrapper + { + public: + ControllerThreadWrapper( + std::shared_ptr & controller, + int cm_update_rate) + : terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate) + { + } + + ControllerThreadWrapper(const ControllerThreadWrapper & t) = delete; + ControllerThreadWrapper(ControllerThreadWrapper && t) = default; + ~ControllerThreadWrapper() + { + terminated_.store(true, std::memory_order_seq_cst); + if (thread_.joinable()) + { + thread_.join(); + } + } + + void activate() + { + thread_ = std::thread(&ControllerThreadWrapper::call_controller_update, this); + } + + void call_controller_update() + { + using TimePoint = std::chrono::system_clock::time_point; + unsigned int used_update_rate = + controller_->get_update_rate() == 0 + ? cm_update_rate_ + : controller_ + ->get_update_rate(); // determines if the controller's or CM's update rate is used + + while (!terminated_.load(std::memory_order_relaxed)) + { + auto const period = std::chrono::nanoseconds(1'000'000'000 / used_update_rate); + TimePoint next_iteration_time = + TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds())); + + if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + // critical section, not implemented yet + } + + next_iteration_time += period; + std::this_thread::sleep_until(next_iteration_time); + } + } + + private: + std::atomic terminated_; + std::shared_ptr controller_; + std::thread thread_; + unsigned int cm_update_rate_; + }; + + std::unordered_map> + async_controller_threads_; }; } // namespace controller_manager diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 826eeed6ad..85124abf0f 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.3.0 + 3.10.0 Description of controller_manager Bence Magyar Denis Štogl @@ -12,6 +12,7 @@ ament_cmake_python ament_index_cpp + backward_ros controller_interface controller_manager_msgs diagnostic_updater @@ -27,6 +28,7 @@ ros2run ament_cmake_gmock + ros2_control_test_assets ament_cmake diff --git a/controller_manager/setup.cfg b/controller_manager/setup.cfg index ef02ee65bd..92c5581e92 100644 --- a/controller_manager/setup.cfg +++ b/controller_manager/setup.cfg @@ -2,3 +2,4 @@ console_scripts = spawner = controller_manager.spawner:main unspawner = controller_manager.unspawner:main + hardware_spawner = controller_manager.hardware_spawner:main diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1fb3e6cf74..1fda25ba26 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -36,16 +36,10 @@ static constexpr const char * kChainableControllerInterfaceClassName = "controller_interface::ChainableControllerInterface"; // Changed services history QoS to keep all so we don't lose any client service calls -static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1, // message queue depth - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { @@ -139,8 +133,8 @@ rclcpp::NodeOptions get_cm_node_options() ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, - const std::string & namespace_) -: rclcpp::Node(manager_node_name, namespace_, get_cm_node_options()), + const std::string & namespace_, const rclcpp::NodeOptions & options) +: rclcpp::Node(manager_node_name, namespace_, options), resource_manager_(std::make_unique()), diagnostics_updater_(this), executor_(executor), @@ -170,14 +164,18 @@ ControllerManager::ControllerManager( ControllerManager::ControllerManager( std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name, - const std::string & namespace_) -: rclcpp::Node(manager_node_name, namespace_, get_cm_node_options()), + const std::string & namespace_, const rclcpp::NodeOptions & options) +: rclcpp::Node(manager_node_name, namespace_, options), resource_manager_(std::move(resource_manager)), diagnostics_updater_(this), executor_(executor), loader_(kControllerInterfaceNamespace, kControllerInterfaceClassName), chainable_loader_(kControllerInterfaceNamespace, kChainableControllerInterfaceClassName) { + if (!get_parameter("update_rate", update_rate_)) + { + RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); + } diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); @@ -192,27 +190,42 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript using lifecycle_msgs::msg::State; std::vector configure_components_on_start = std::vector({}); - get_parameter("configure_components_on_start", configure_components_on_start); - rclcpp_lifecycle::State inactive_state( - State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); - for (const auto & component : configure_components_on_start) + if (get_parameter("configure_components_on_start", configure_components_on_start)) { - resource_manager_->set_component_state(component, inactive_state); + RCLCPP_WARN_STREAM( + get_logger(), + "[Deprecated]: Usage of parameter \"configure_components_on_start\" is deprecated. Use " + "hardware_spawner instead."); + rclcpp_lifecycle::State inactive_state( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); + for (const auto & component : configure_components_on_start) + { + resource_manager_->set_component_state(component, inactive_state); + } } std::vector activate_components_on_start = std::vector({}); - get_parameter("activate_components_on_start", activate_components_on_start); - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - for (const auto & component : activate_components_on_start) + if (get_parameter("activate_components_on_start", activate_components_on_start)) { - resource_manager_->set_component_state(component, active_state); + RCLCPP_WARN_STREAM( + get_logger(), + "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " + "hardware_spawner instead."); + rclcpp_lifecycle::State active_state( + State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); + for (const auto & component : activate_components_on_start) + { + resource_manager_->set_component_state(component, active_state); + } } - // if both parameter are empty or non-existing preserve behavior where all components are // activated per default if (configure_components_on_start.empty() && activate_components_on_start.empty()) { + RCLCPP_WARN_STREAM( + get_logger(), + "[Deprecated]: Automatic activation of all hardware components will not be supported in the " + "future anymore. Use hardware_spawner instead."); resource_manager_->activate_all_components(); } } @@ -228,47 +241,47 @@ void ControllerManager::init_services() using namespace std::placeholders; list_controllers_service_ = create_service( "~/list_controllers", std::bind(&ControllerManager::list_controllers_srv_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + qos_services, best_effort_callback_group_); list_controller_types_service_ = create_service( "~/list_controller_types", - std::bind(&ControllerManager::list_controller_types_srv_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + std::bind(&ControllerManager::list_controller_types_srv_cb, this, _1, _2), qos_services, + best_effort_callback_group_); load_controller_service_ = create_service( "~/load_controller", std::bind(&ControllerManager::load_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + qos_services, best_effort_callback_group_); configure_controller_service_ = create_service( "~/configure_controller", - std::bind(&ControllerManager::configure_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + std::bind(&ControllerManager::configure_controller_service_cb, this, _1, _2), qos_services, + best_effort_callback_group_); reload_controller_libraries_service_ = create_service( "~/reload_controller_libraries", std::bind(&ControllerManager::reload_controller_libraries_service_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + qos_services, best_effort_callback_group_); switch_controller_service_ = create_service( "~/switch_controller", - std::bind(&ControllerManager::switch_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + std::bind(&ControllerManager::switch_controller_service_cb, this, _1, _2), qos_services, + best_effort_callback_group_); unload_controller_service_ = create_service( "~/unload_controller", - std::bind(&ControllerManager::unload_controller_service_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + std::bind(&ControllerManager::unload_controller_service_cb, this, _1, _2), qos_services, + best_effort_callback_group_); list_hardware_components_service_ = create_service( "~/list_hardware_components", - std::bind(&ControllerManager::list_hardware_components_srv_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + std::bind(&ControllerManager::list_hardware_components_srv_cb, this, _1, _2), qos_services, + best_effort_callback_group_); list_hardware_interfaces_service_ = create_service( "~/list_hardware_interfaces", - std::bind(&ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + std::bind(&ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2), qos_services, + best_effort_callback_group_); set_hardware_component_state_service_ = create_service( "~/set_hardware_component_state", std::bind(&ControllerManager::set_hardware_component_state_srv_cb, this, _1, _2), - rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_); + qos_services, best_effort_callback_group_); } controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller( @@ -298,13 +311,23 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_interface::ControllerInterfaceBaseSharedPtr controller; - if (loader_.isClassAvailable(controller_type)) + try { - controller = loader_.createSharedInstance(controller_type); + if (loader_.isClassAvailable(controller_type)) + { + controller = loader_.createSharedInstance(controller_type); + } + if (chainable_loader_.isClassAvailable(controller_type)) + { + controller = chainable_loader_.createSharedInstance(controller_type); + } } - if (chainable_loader_.isClassAvailable(controller_type)) + catch (const pluginlib::CreateClassException & e) { - controller = chainable_loader_.createSharedInstance(controller_type); + RCLCPP_ERROR( + get_logger(), "Error happened during creation of controller '%s' with type '%s':\n%s", + controller_name.c_str(), controller_type.c_str(), e.what()); + return nullptr; } ControllerSpec controller_spec; @@ -374,6 +397,13 @@ controller_interface::return_type ControllerManager::unload_controller( controller_name.c_str()); return controller_interface::return_type::ERROR; } + if (controller.c->is_async()) + { + RCLCPP_DEBUG( + get_logger(), "Removing controller '%s' from the list of async controllers", + controller_name.c_str()); + async_controller_threads_.erase(controller_name); + } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for @@ -391,6 +421,7 @@ controller_interface::return_type ControllerManager::unload_controller( RCLCPP_DEBUG(get_logger(), "Destruct controller finished"); RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str()); + return controller_interface::return_type::OK; } @@ -458,6 +489,13 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } + // ASYNCHRONOUS CONTROLLERS: Start background thread for update + if (controller->is_async()) + { + async_controller_threads_.emplace( + controller_name, std::make_unique(controller, update_rate_)); + } + // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers if (controller->is_chainable()) { @@ -1149,6 +1187,7 @@ void ControllerManager::activate_controllers( continue; } auto controller = found_it->c; + auto controller_name = found_it->info.name; bool assignment_successful = true; // assign command interfaces to the controller @@ -1237,6 +1276,11 @@ void ControllerManager::activate_controllers( get_logger(), "After activating, controller '%s' is in state '%s', expected Active", controller->get_node()->get_name(), new_state.label().c_str()); } + + if (controller->is_async()) + { + async_controller_threads_.at(controller_name)->activate(); + } } // All controllers activated, switching done switch_params_.do_switch = false; @@ -1506,46 +1550,10 @@ void ControllerManager::switch_controller_service_cb( std::lock_guard guard(services_lock_); RCLCPP_DEBUG(get_logger(), "switching service locked"); - // response->ok = switch_controller( - // request->activate_controllers, request->deactivate_controllers, request->strictness, - // request->activate_asap, request->timeout) == controller_interface::return_type::OK; - // TODO(destogl): remove this after deprecated fields are removed from service and use the - // commented three lines above - // BEGIN: remove when deprecated removed - auto activate_controllers = request->activate_controllers; - auto deactivate_controllers = request->deactivate_controllers; - - if (!request->start_controllers.empty()) - { - RCLCPP_WARN( - get_logger(), - "'start_controllers' field is deprecated, use 'activate_controllers' field instead!"); - activate_controllers.insert( - activate_controllers.end(), request->start_controllers.begin(), - request->start_controllers.end()); - } - if (!request->stop_controllers.empty()) - { - RCLCPP_WARN( - get_logger(), - "'stop_controllers' field is deprecated, use 'deactivate_controllers' field instead!"); - deactivate_controllers.insert( - deactivate_controllers.end(), request->stop_controllers.begin(), - request->stop_controllers.end()); - } - - auto activate_asap = request->activate_asap; - if (request->start_asap) - { - RCLCPP_WARN( - get_logger(), "'start_asap' field is deprecated, use 'activate_asap' field instead!"); - activate_asap = request->start_asap; - } - - response->ok = switch_controller( - activate_controllers, deactivate_controllers, request->strictness, activate_asap, - request->timeout) == controller_interface::return_type::OK; - // END: remove when deprecated removed + response->ok = + switch_controller( + request->activate_controllers, request->deactivate_controllers, request->strictness, + request->activate_asap, request->timeout) == controller_interface::return_type::OK; RCLCPP_DEBUG(get_logger(), "switching service finished"); } @@ -1583,7 +1591,14 @@ void ControllerManager::list_hardware_components_srv_cb( auto component = controller_manager_msgs::msg::HardwareComponentState(); component.name = component_name; component.type = component_info.type; - component.class_type = component_info.class_type; + component.class_type = + component_info.plugin_name; // TODO(bence): deprecated currently. Remove soon + RCLCPP_WARN( + get_logger(), + "The 'class_type' field in 'controller_manager_msgs/msg/HardwareComponentState.msg' is " + "deprecated and will be removed soon. Please switch over client code to use 'plugin_name' " + "instead."); + component.plugin_name = component_info.plugin_name; component.state.id = component_info.state.id(); component.state.label = component_info.state.label(); @@ -1739,7 +1754,7 @@ controller_interface::return_type ControllerManager::update( { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 - if (is_controller_active(*loaded_controller.c)) + if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c)) { const auto controller_update_rate = loaded_controller.c->get_update_rate(); diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index d3a92bfce4..9df237a9af 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -36,19 +36,19 @@ using hardware_interface::lifecycle_state_names::FINALIZED; using hardware_interface::lifecycle_state_names::INACTIVE; using hardware_interface::lifecycle_state_names::UNCONFIGURED; -using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_PLUGIN_NAME; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_TYPE; -using ros2_control_test_assets::TEST_SENSOR_HARDWARE_CLASS_TYPE; using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_PLUGIN_NAME; using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SENSOR_HARDWARE_TYPE; -using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_CLASS_TYPE; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_PLUGIN_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; @@ -88,12 +88,12 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs void check_component_fileds( const controller_manager_msgs::msg::HardwareComponentState & component, - const std::string & name, const std::string & type, const std::string & class_type, + const std::string & name, const std::string & type, const std::string & plugin_name, const uint8_t state_id, const std::string & state_label) { EXPECT_EQ(component.name, name); EXPECT_EQ(component.type, type); - EXPECT_EQ(component.class_type, class_type); + EXPECT_EQ(component.plugin_name, plugin_name); EXPECT_EQ(component.state.id, state_id); EXPECT_EQ(component.state.label, state_label); } @@ -135,7 +135,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs { check_component_fileds( component, TEST_ACTUATOR_HARDWARE_NAME, TEST_ACTUATOR_HARDWARE_TYPE, - TEST_ACTUATOR_HARDWARE_CLASS_TYPE, hw_state_ids[0], hw_state_labels[0]); + TEST_ACTUATOR_HARDWARE_PLUGIN_NAME, hw_state_ids[0], hw_state_labels[0]); check_interfaces( component.command_interfaces, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, hw_itfs_available_status[0][0], hw_itfs_claimed_status[0][0]); @@ -147,7 +147,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs { check_component_fileds( component, TEST_SENSOR_HARDWARE_NAME, TEST_SENSOR_HARDWARE_TYPE, - TEST_SENSOR_HARDWARE_CLASS_TYPE, hw_state_ids[1], hw_state_labels[1]); + TEST_SENSOR_HARDWARE_PLUGIN_NAME, hw_state_ids[1], hw_state_labels[1]); check_interfaces( component.command_interfaces, TEST_SENSOR_HARDWARE_COMMAND_INTERFACES, hw_itfs_available_status[1][0], hw_itfs_claimed_status[1][0]); @@ -159,7 +159,7 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs { check_component_fileds( component, TEST_SYSTEM_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_TYPE, - TEST_SYSTEM_HARDWARE_CLASS_TYPE, hw_state_ids[2], hw_state_labels[2]); + TEST_SYSTEM_HARDWARE_PLUGIN_NAME, hw_state_ids[2], hw_state_labels[2]); check_interfaces( component.command_interfaces, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, hw_itfs_available_status[2][0], hw_itfs_claimed_status[2][0]); diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 8baffc54d8..c9ba0e6355 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -99,29 +99,60 @@ TEST_F(TestLoadController, spawner_test_type_in_param) EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); - auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); - ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } // Try to spawn again, it should fail because already active EXPECT_NE(call_spawner("ctrl_1 -c test_controller_manager"), 0) << "Cannot configure from active"; - ctrl_1.c->get_node()->deactivate(); + + std::vector start_controllers = {}; + std::vector stop_controllers = {"ctrl_1"}; + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + // We should be able to reconfigure and activate a configured controller EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); - ctrl_1.c->get_node()->deactivate(); - ctrl_1.c->get_node()->cleanup(); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + cm_->unload_controller("ctrl_1"); + cm_->load_controller("ctrl_1"); + // We should be able to reconfigure and activate am unconfigured loaded controller EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } // Unload and reload EXPECT_EQ(call_unspawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul) << "Controller should have been unloaded"; EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been loaded"; - ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } } TEST_F(TestLoadController, spawner_test_type_in_arg) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 8850425bd5..0136701e7f 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,40 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ +* Remove deprecations from CLI and controller_manager (`#948 `_) + Co-authored-by: Bence Magyar +* Contributors: Christoph Fröhlich + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Contributors: Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ +* Rename class type to plugin name #api-breaking #abi-breaking (`#780 `_) +* Contributors: Bence Magyar + +3.4.0 (2022-11-27) +------------------ + 3.3.0 (2022-11-15) ------------------ diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index 2ba8f23194..2a863c29dd 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -1,10 +1,6 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.16) project(controller_manager_msgs) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(lifecycle_msgs REQUIRED) diff --git a/controller_manager_msgs/msg/HardwareComponentState.msg b/controller_manager_msgs/msg/HardwareComponentState.msg index 51fd170701..9720b53161 100644 --- a/controller_manager_msgs/msg/HardwareComponentState.msg +++ b/controller_manager_msgs/msg/HardwareComponentState.msg @@ -1,6 +1,7 @@ string name string type -string class_type +string class_type # DEPRECATED +string plugin_name lifecycle_msgs/State state HardwareInterface[] command_interfaces HardwareInterface[] state_interfaces diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 62b4799ee8..a61ca0516d 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.3.0 + 3.10.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/srv/SwitchController.srv b/controller_manager_msgs/srv/SwitchController.srv index 29cbb7c1bd..840433d9a8 100644 --- a/controller_manager_msgs/srv/SwitchController.srv +++ b/controller_manager_msgs/srv/SwitchController.srv @@ -21,12 +21,9 @@ string[] activate_controllers string[] deactivate_controllers -string[] start_controllers # DEPRECATED: Use activate_controllers filed instead -string[] stop_controllers # DEPRECATED: Use deactivate_controllers filed instead int32 strictness int32 BEST_EFFORT=1 int32 STRICT=2 -bool start_asap # DEPRECATED: Use activate_asap filed instead bool activate_asap builtin_interfaces/Duration timeout --- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 130456efd9..5070815a59 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,45 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.10.0 (2023-03-16) +------------------- +* Split transmission interfaces (`#938 `_) +* Contributors: Noel Jiménez García + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Async params (`#927 `_) +* Contributors: Márk Szitanics, Tyler Weaver + +3.7.0 (2023-01-24) +------------------ +* Make double parsing locale independent (`#921 `_) +* Contributors: Henning Kayser + +3.6.0 (2023-01-12) +------------------ +* 🔧 Fixes and updated on pre-commit hooks and their action (`#890 `_) +* Contributors: Denis Štogl + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ +* ResourceManager doesn't always log an error on shutdown anymore (`#867 `_) +* Rename class type to plugin name #api-breaking #abi-breaking (`#780 `_) +* Contributors: Bence Magyar, Christopher Wecht + +3.4.0 (2022-11-27) +------------------ + 3.3.0 (2022-11-15) ------------------ * [MockHardware] Enalbe initialization non-joint components(`#822 `_) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 9af1a359cd..77eec3ec86 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -1,19 +1,19 @@ -cmake_minimum_required(VERSION 3.5) -project(hardware_interface) +cmake_minimum_required(VERSION 3.16) +project(hardware_interface LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs - lifecycle_msgs - pluginlib - rclcpp_lifecycle - rcpputils - rcutils - tinyxml2_vendor - TinyXML2 + control_msgs + lifecycle_msgs + pluginlib + rclcpp_lifecycle + rcpputils + rcutils + TinyXML2 + tinyxml2_vendor ) find_package(ament_cmake REQUIRED) @@ -21,97 +21,55 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -add_library( - ${PROJECT_NAME} - SHARED +add_library(hardware_interface SHARED src/actuator.cpp src/component_parser.cpp src/resource_manager.cpp src/sensor.cpp src/system.cpp ) -target_include_directories( - ${PROJECT_NAME} - PUBLIC - include +target_compile_features(hardware_interface PUBLIC cxx_std_17) +target_include_directories(hardware_interface PUBLIC + $ + $ ) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(hardware_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") +target_compile_definitions(hardware_interface PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") -# Mock components -add_library( - mock_components - SHARED +add_library(mock_components SHARED src/mock_components/generic_system.cpp ) -target_include_directories( - mock_components - PUBLIC - include +target_compile_features(mock_components PUBLIC cxx_std_17) +target_include_directories(mock_components PUBLIC + $ + $ ) -target_link_libraries( - mock_components - ${PROJECT_NAME} -) -ament_target_dependencies( - mock_components - pluginlib - rcpputils -) - +ament_target_dependencies(mock_components PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(mock_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") pluginlib_export_plugin_description_file( - ${PROJECT_NAME} mock_components_plugin_description.xml) + hardware_interface mock_components_plugin_description.xml) -# Fake components -add_library( - fake_components - SHARED +add_library(fake_components SHARED src/mock_components/generic_system.cpp src/mock_components/fake_generic_system.cpp ) -target_include_directories( - fake_components - PUBLIC - include -) -target_link_libraries( - fake_components - ${PROJECT_NAME} +target_compile_features(fake_components PUBLIC cxx_std_17) +target_include_directories(fake_components PUBLIC + $ + $ ) -ament_target_dependencies( - fake_components - pluginlib - rcpputils -) - +ament_target_dependencies(fake_components PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(fake_components PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") pluginlib_export_plugin_description_file( - ${PROJECT_NAME} fake_components_plugin_description.xml) - - -install( - DIRECTORY include/ - DESTINATION include -) - -install( - TARGETS - fake_components - mock_components - ${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) + hardware_interface fake_components_plugin_description.xml) if(BUILD_TESTING) @@ -123,28 +81,28 @@ if(BUILD_TESTING) ament_target_dependencies(test_macros rcpputils) ament_add_gmock(test_joint_handle test/test_handle.cpp) - target_link_libraries(test_joint_handle ${PROJECT_NAME}) + target_link_libraries(test_joint_handle hardware_interface) ament_target_dependencies(test_joint_handle rcpputils) ament_add_gmock(test_component_interfaces test/test_component_interfaces.cpp) - target_link_libraries(test_component_interfaces ${PROJECT_NAME}) + target_link_libraries(test_component_interfaces hardware_interface) ament_add_gmock(test_component_parser test/test_component_parser.cpp) - target_link_libraries(test_component_parser ${PROJECT_NAME}) + 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 ${PROJECT_NAME}) + target_link_libraries(test_components hardware_interface) ament_target_dependencies(test_components pluginlib) install(TARGETS test_components DESTINATION lib ) pluginlib_export_plugin_description_file( - ${PROJECT_NAME} test/test_components/test_components.xml) + hardware_interface test/test_components/test_components.xml) add_library(test_hardware_components SHARED test/test_hardware_components/test_single_joint_actuator.cpp @@ -152,36 +110,44 @@ if(BUILD_TESTING) test/test_hardware_components/test_two_joint_system.cpp test/test_hardware_components/test_system_with_command_modes.cpp ) - target_link_libraries(test_hardware_components ${PROJECT_NAME}) + target_link_libraries(test_hardware_components hardware_interface) ament_target_dependencies(test_hardware_components pluginlib) install(TARGETS test_hardware_components DESTINATION lib ) pluginlib_export_plugin_description_file( - ${PROJECT_NAME} test/test_hardware_components/test_hardware_components.xml + 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 ${PROJECT_NAME}) + target_link_libraries(test_resource_manager hardware_interface) ament_target_dependencies(test_resource_manager 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 ${PROJECT_NAME}) + target_link_libraries(test_generic_system hardware_interface) ament_target_dependencies(test_generic_system pluginlib ros2_control_test_assets ) endif() -ament_export_include_directories( - include +install( + DIRECTORY include/ + DESTINATION include/hardware_interface ) -ament_export_libraries( - fake_components - mock_components - ${PROJECT_NAME} +install( + TARGETS + fake_components + mock_components + hardware_interface + EXPORT export_hardware_interface + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) + +ament_export_targets(export_hardware_interface HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index 71dbfebade..45afebdb34 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -39,8 +39,11 @@ struct HardwareComponentInfo /// Component "classification": "actuator", "sensor" or "system" std::string type; - /// Component class type. - std::string class_type; + /// Component pluginlib plugin name. + std::string plugin_name; + + /// Component is async + bool is_async; /// Component current state. rclcpp_lifecycle::State state; diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 0a5cb1cc18..eb6b63cfc3 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -54,6 +54,7 @@ struct ComponentInfo std::string name; /// Type of the component: sensor, joint, or GPIO. std::string type; + /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. @@ -72,7 +73,8 @@ struct ComponentInfo struct JointInfo { std::string name; - std::vector interfaces; + std::vector state_interfaces; + std::vector command_interfaces; std::string role; double mechanical_reduction = 1.0; double offset = 0.0; @@ -82,7 +84,8 @@ struct JointInfo struct ActuatorInfo { std::string name; - std::vector interfaces; + std::vector state_interfaces; + std::vector command_interfaces; std::string role; double mechanical_reduction = 1.0; double offset = 0.0; @@ -106,8 +109,10 @@ struct HardwareInfo std::string name; /// Type of the hardware: actuator, sensor or system. std::string type; - /// Class of the hardware that will be dynamically loaded. - std::string hardware_class_type; + /// Component is async + bool is_async; + /// Name of the pluginlib plugin of the hardware that will be loaded. + std::string hardware_plugin_name; /// (Optional) Key-value pairs for hardware parameters. std::unordered_map hardware_parameters; /** diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 9b28eb5a0c..c67eae2f81 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.3.0 + 3.10.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 2a223b0f63..14f2dee21e 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -27,7 +28,7 @@ namespace constexpr const auto kRobotTag = "robot"; constexpr const auto kROS2ControlTag = "ros2_control"; constexpr const auto kHardwareTag = "hardware"; -constexpr const auto kClassTypeTag = "plugin"; +constexpr const auto kPluginNameTag = "plugin"; constexpr const auto kParamTag = "param"; constexpr const auto kActuatorTag = "actuator"; constexpr const auto kJointTag = "joint"; @@ -46,6 +47,8 @@ constexpr const auto kTypeAttribute = "type"; constexpr const auto kRoleAttribute = "role"; constexpr const auto kReductionAttribute = "mechanical_reduction"; constexpr const auto kOffsetAttribute = "offset"; +constexpr const auto kIsAsyncAttribute = "is_async"; + } // namespace namespace hardware_interface @@ -123,26 +126,30 @@ double get_parameter_value_or( { while (params_it) { - try + // Fill the map with parameters + const auto tag_name = params_it->Name(); + if (strcmp(tag_name, parameter_name) == 0) { - // Fill the map with parameters - const auto tag_name = params_it->Name(); - if (strcmp(tag_name, parameter_name) == 0) + const auto tag_text = params_it->GetText(); + if (tag_text) { - const auto tag_text = params_it->GetText(); - if (tag_text) + // Parse and return double value if there is no parsing error + double result_value; + const auto parse_result = + std::from_chars(tag_text, tag_text + std::strlen(tag_text), result_value); + if (parse_result.ec == std::errc()) { - return std::stod(tag_text); + return result_value; } + + // Parsing failed - exit loop and return default value + break; } } - catch (const std::exception & e) - { - return default_value; - } params_it = params_it->NextSiblingElement(); } + return default_value; } @@ -206,6 +213,20 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem) return data_type; } +/// Parse is_async attribute +/** + * Parses an XMLElement and returns the value of the is_async attribute. + * Defaults to "false" if not specified. + * + * \param[in] elem XMLElement that has the data_type attribute. + * \return boolean specifying the if the value read was true or false. + */ +bool parse_is_async_attribute(const tinyxml2::XMLElement * elem) +{ + const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kIsAsyncAttribute); + return attr ? strcasecmp(attr->Value(), "true") == 0 : false; +} + /// Search XML snippet from URDF for parameters. /** * \param[in] params_it pointer to the iterator where parameters info should be found @@ -404,8 +425,8 @@ TransmissionInfo parse_transmission_from_xml(const tinyxml2::XMLElement * transm // Find name, type and class of a transmission transmission.name = get_attribute_value(transmission_it, kNameAttribute, transmission_it->Name()); - const auto * type_it = transmission_it->FirstChildElement(kClassTypeTag); - transmission.type = get_text_for_element(type_it, kClassTypeTag); + const auto * type_it = transmission_it->FirstChildElement(kPluginNameTag); + transmission.type = get_text_for_element(type_it, kPluginNameTag); // Parse joints const auto * joint_it = transmission_it->FirstChildElement(kJointTag); @@ -460,7 +481,12 @@ void auto_fill_transmission_interfaces(HardwareInfo & hardware) // copy interface names from their definitions in the component std::transform( found_it->command_interfaces.cbegin(), found_it->command_interfaces.cend(), - std::back_inserter(joint.interfaces), + std::back_inserter(joint.command_interfaces), + [](const auto & interface) { return interface.name; }); + + std::transform( + found_it->state_interfaces.cbegin(), found_it->state_interfaces.cend(), + std::back_inserter(joint.state_interfaces), [](const auto & interface) { return interface.name; }); } @@ -475,8 +501,9 @@ void auto_fill_transmission_interfaces(HardwareInfo & hardware) std::to_string(transmission.joints.size())); } - transmission.actuators.push_back( - ActuatorInfo{"actuator1", transmission.joints[0].interfaces, "actuator1", 1.0, 0.0}); + transmission.actuators.push_back(ActuatorInfo{ + "actuator1", transmission.joints[0].state_interfaces, + transmission.joints[0].command_interfaces, "actuator1", 1.0, 0.0}); } } } @@ -494,17 +521,18 @@ HardwareInfo parse_resource_from_xml( HardwareInfo hardware; hardware.name = get_attribute_value(ros2_control_it, kNameAttribute, kROS2ControlTag); hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag); + hardware.is_async = parse_is_async_attribute(ros2_control_it); // Parse everything under ros2_control tag - hardware.hardware_class_type = ""; + hardware.hardware_plugin_name = ""; const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(); while (ros2_control_child_it) { if (!std::string(kHardwareTag).compare(ros2_control_child_it->Name())) { - const auto * type_it = ros2_control_child_it->FirstChildElement(kClassTypeTag); - hardware.hardware_class_type = - get_text_for_element(type_it, std::string("hardware ") + kClassTypeTag); + const auto * type_it = ros2_control_child_it->FirstChildElement(kPluginNameTag); + hardware.hardware_plugin_name = + get_text_for_element(type_it, std::string("hardware ") + kPluginNameTag); const auto * params_it = ros2_control_child_it->FirstChildElement(kParamTag); if (params_it) { diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 244fdcd58d..d1405dac8c 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -17,6 +17,7 @@ #include "mock_components/generic_system.hpp" #include +#include #include #include #include @@ -29,6 +30,18 @@ namespace mock_components { +double parse_double(const std::string & text) +{ + double result_value; + const auto parse_result = std::from_chars(text.data(), text.data() + text.size(), result_value); + if (parse_result.ec == std::errc()) + { + return result_value; + } + + return 0.0; +} + CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) @@ -101,7 +114,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i it = info_.hardware_parameters.find("position_state_following_offset"); if (it != info_.hardware_parameters.end()) { - position_state_following_offset_ = std::stod(it->second); + position_state_following_offset_ = parse_double(it->second); it = info_.hardware_parameters.find("custom_interface_with_following_offset"); if (it != info_.hardware_parameters.end()) { @@ -147,7 +160,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i auto param_it = joint.parameters.find("multiplier"); if (param_it != joint.parameters.end()) { - mimic_joint.multiplier = std::stod(joint.parameters.at("multiplier")); + mimic_joint.multiplier = parse_double(joint.parameters.at("multiplier")); } mimic_joints_.push_back(mimic_joint); } @@ -455,7 +468,7 @@ void GenericSystem::initialize_storage_vectors( // Check the initial_value param is used if (!interface.initial_value.empty()) { - states[index][i] = std::stod(interface.initial_value); + states[index][i] = parse_double(interface.initial_value); } else { @@ -463,7 +476,7 @@ void GenericSystem::initialize_storage_vectors( auto it2 = component.parameters.find("initial_" + interface.name); if (it2 != component.parameters.end()) { - states[index][i] = std::stod(it2->second); + states[index][i] = parse_double(it2->second); print_hint = true; } else diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 4b80bc09dd..33236afd1c 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -87,18 +87,17 @@ class ResourceStorage { RCUTILS_LOG_INFO_NAMED( "resource_manager", "Loading hardware '%s' ", hardware_info.name.c_str()); - // hardware_class_type has to match class name in plugin xml description - // TODO(karsten1987) extract package from hardware_class_type - // e.g.: / + // hardware_plugin_name has to match class name in plugin xml description auto interface = std::unique_ptr( - loader.createUnmanagedInstance(hardware_info.hardware_class_type)); + loader.createUnmanagedInstance(hardware_info.hardware_plugin_name)); HardwareT hardware(std::move(interface)); container.emplace_back(std::move(hardware)); // initialize static data about hardware component to reduce later calls HardwareComponentInfo component_info; component_info.name = hardware_info.name; component_info.type = hardware_info.type; - component_info.class_type = hardware_info.hardware_class_type; + component_info.plugin_name = hardware_info.hardware_plugin_name; + component_info.is_async = hardware_info.is_async; hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); hardware_used_by_controllers_.insert( @@ -268,7 +267,7 @@ class ResourceStorage { bool result = trigger_and_print_hardware_state_transition( std::bind(&HardwareT::shutdown, &hardware), "shutdown", hardware.get_name(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); if (result) { @@ -1184,6 +1183,23 @@ void ResourceManager::validate_storage( } } } + for (const auto & gpio : hardware.gpios) + { + for (const auto & state_interface : gpio.state_interfaces) + { + if (!state_interface_exists(gpio.name + "/" + state_interface.name)) + { + missing_state_keys.emplace_back(gpio.name + "/" + state_interface.name); + } + } + for (const auto & command_interface : gpio.command_interfaces) + { + if (!command_interface_exists(gpio.name + "/" + command_interface.name)) + { + missing_command_keys.emplace_back(gpio.name + "/" + command_interface.name); + } + } + } } if (!missing_state_keys.empty() || !missing_command_keys.empty()) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index a2881ff200..97d4a77d21 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -123,7 +123,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnly"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( - hardware_info.hardware_class_type, + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -162,7 +162,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( - hardware_info.hardware_class_type, + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -199,7 +199,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens EXPECT_EQ(hardware_info.name, "RRBotSystemWithSensor"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( - hardware_info.hardware_class_type, "ros2_control_demo_hardware/RRBotSystemWithSensorHardware"); + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithSensorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -243,7 +243,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte EXPECT_EQ(hardware_info.name, "RRBotSystemPositionOnlyWithExternalSensor"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( - hardware_info.hardware_class_type, + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2"); @@ -284,7 +284,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.name, "RRBotModularJoint1"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( - hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionActuatorHardware"); + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionActuatorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); @@ -297,7 +297,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.name, "RRBotModularJoint2"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( - hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionActuatorHardware"); + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionActuatorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "3"); @@ -319,7 +319,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.name, "RRBotModularJoint1"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( - hardware_info.hardware_class_type, "ros2_control_demo_hardware/VelocityActuatorHardware"); + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/VelocityActuatorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); @@ -344,7 +344,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.name, "RRBotModularJoint2"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( - hardware_info.hardware_class_type, "ros2_control_demo_hardware/VelocityActuatorHardware"); + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/VelocityActuatorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "3"); @@ -360,7 +360,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint1"); EXPECT_EQ(hardware_info.type, "sensor"); - EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionSensorHardware"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionSensorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); @@ -376,7 +377,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.name, "RRBotModularPositionSensorJoint2"); EXPECT_EQ(hardware_info.type, "sensor"); - EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionSensorHardware"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/PositionSensorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); @@ -402,7 +404,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_tr EXPECT_EQ(hardware_info.name, "RRBotModularWrist"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( - hardware_info.hardware_class_type, "ros2_control_demo_hardware/ActuatorHardwareMultiDOF"); + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/ActuatorHardwareMultiDOF"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.23"); @@ -443,7 +445,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) EXPECT_EQ(hardware_info.name, "CameraWithIMU"); EXPECT_EQ(hardware_info.type, "sensor"); - EXPECT_EQ(hardware_info.hardware_class_type, "ros2_control_demo_hardware/CameraWithIMUSensor"); + EXPECT_EQ(hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/CameraWithIMUSensor"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(1)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); @@ -473,7 +475,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) EXPECT_EQ(hardware_info.name, "ActuatorModularJoint1"); EXPECT_EQ(hardware_info.type, "actuator"); EXPECT_EQ( - hardware_info.hardware_class_type, "ros2_control_demo_hardware/VelocityActuatorHardware"); + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/VelocityActuatorHardware"); ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2)); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.13"); @@ -495,19 +497,45 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) const auto joint = transmission.joints[0]; EXPECT_EQ(joint.name, "joint1"); EXPECT_EQ(joint.role, "joint1"); - EXPECT_THAT(joint.interfaces, ElementsAre("velocity")); + EXPECT_THAT(joint.state_interfaces, ElementsAre("velocity")); + EXPECT_THAT(joint.command_interfaces, ElementsAre("velocity")); EXPECT_THAT(joint.mechanical_reduction, DoubleEq(325.949)); EXPECT_THAT(joint.offset, DoubleEq(0.0)); EXPECT_THAT(transmission.actuators, SizeIs(1)); const auto actuator = transmission.actuators[0]; EXPECT_EQ(actuator.name, "actuator1"); EXPECT_EQ(actuator.role, "actuator1"); - EXPECT_THAT(actuator.interfaces, ContainerEq(joint.interfaces)); + EXPECT_THAT(actuator.state_interfaces, ContainerEq(joint.state_interfaces)); + EXPECT_THAT(actuator.command_interfaces, ContainerEq(joint.command_interfaces)); EXPECT_THAT(actuator.offset, DoubleEq(0.0)); ASSERT_THAT(transmission.parameters, SizeIs(1)); EXPECT_EQ(transmission.parameters.at("additional_special_parameter"), "1337"); } +TEST_F(TestComponentParser, successfully_parse_locale_independent_double) +{ + // Set to locale with comma-separated decimals + std::setlocale(LC_NUMERIC, "de_DE.UTF-8"); + + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_actuator_only + + ros2_control_test_assets::urdf_tail; + + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + const auto hardware_info = control_hardware.at(0); + + EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "1.13"); + + ASSERT_THAT(hardware_info.transmissions, SizeIs(1)); + const auto transmission = hardware_info.transmissions[0]; + EXPECT_THAT(transmission.joints, SizeIs(1)); + const auto joint = transmission.joints[0]; + + // Test that we still parse doubles using dot notation + EXPECT_THAT(joint.mechanical_reduction, DoubleEq(325.949)); +} + TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio) { std::string urdf_to_test = @@ -521,7 +549,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( - hardware_info.hardware_class_type, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); ASSERT_THAT(hardware_info.joints, SizeIs(2)); @@ -564,7 +592,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d EXPECT_EQ(hardware_info.name, "RRBotSystemWithSizeAndDataType"); EXPECT_EQ(hardware_info.type, "system"); EXPECT_EQ( - hardware_info.hardware_class_type, "ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType"); + hardware_info.hardware_plugin_name, + "ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType"); ASSERT_THAT(hardware_info.joints, SizeIs(1)); diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index da8258c643..16ca710e9d 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -27,7 +27,9 @@ constexpr auto FOO_INTERFACE = "FooInterface"; TEST(TestHandle, command_interface) { double value = 1.337; - CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; + CommandInterface interface { + JOINT_NAME, FOO_INTERFACE, &value + }; EXPECT_DOUBLE_EQ(interface.get_value(), value); EXPECT_NO_THROW(interface.set_value(0.0)); EXPECT_DOUBLE_EQ(interface.get_value(), 0.0); @@ -36,7 +38,9 @@ TEST(TestHandle, command_interface) TEST(TestHandle, state_interface) { double value = 1.337; - StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value}; + StateInterface interface { + JOINT_NAME, FOO_INTERFACE, &value + }; EXPECT_DOUBLE_EQ(interface.get_value(), value); // interface.set_value(5); compiler error, no set_value function } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 7674213903..59257f4edc 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -29,19 +29,19 @@ #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" -using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_PLUGIN_NAME; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_TYPE; -using ros2_control_test_assets::TEST_SENSOR_HARDWARE_CLASS_TYPE; using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SENSOR_HARDWARE_PLUGIN_NAME; using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SENSOR_HARDWARE_TYPE; -using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_CLASS_TYPE; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; +using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_PLUGIN_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; @@ -78,7 +78,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager } }; -void set_components_state( +std::vector set_components_state( TestableResourceManager & rm, const std::vector & components, const uint8_t state_id, const std::string & state_name) { @@ -87,17 +87,20 @@ void set_components_state( { int_components = {"TestActuatorHardware", "TestSensorHardware", "TestSystemHardware"}; } + std::vector results; for (const auto & component : int_components) { rclcpp_lifecycle::State state(state_id, state_name); - rm.set_component_state(component, state); + const auto result = rm.set_component_state(component, state); + results.push_back(result); } + return results; } auto configure_components = [](TestableResourceManager & rm, const std::vector & components = {}) { - set_components_state( + return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); }; @@ -105,7 +108,7 @@ auto configure_components = auto activate_components = [](TestableResourceManager & rm, const std::vector & components = {}) { - set_components_state( + return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); }; @@ -113,7 +116,7 @@ auto activate_components = auto deactivate_components = [](TestableResourceManager & rm, const std::vector & components = {}) { - set_components_state( + return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); }; @@ -121,7 +124,7 @@ auto deactivate_components = auto cleanup_components = [](TestableResourceManager & rm, const std::vector & components = {}) { - set_components_state( + return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED); }; @@ -129,7 +132,7 @@ auto cleanup_components = auto shutdown_components = [](TestableResourceManager & rm, const std::vector & components = {}) { - set_components_state( + return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, hardware_interface::lifecycle_state_names::FINALIZED); }; @@ -448,10 +451,11 @@ TEST_F(ResourceManagerTest, resource_status) EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].type, TEST_ACTUATOR_HARDWARE_TYPE); EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].type, TEST_SENSOR_HARDWARE_TYPE); EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].type, TEST_SYSTEM_HARDWARE_TYPE); - // class_type - EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].class_type, TEST_ACTUATOR_HARDWARE_CLASS_TYPE); - EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].class_type, TEST_SENSOR_HARDWARE_CLASS_TYPE); - EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].class_type, TEST_SYSTEM_HARDWARE_CLASS_TYPE); + // plugin_name + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].plugin_name, TEST_ACTUATOR_HARDWARE_PLUGIN_NAME); + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].plugin_name, TEST_SENSOR_HARDWARE_PLUGIN_NAME); + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].plugin_name, TEST_SYSTEM_HARDWARE_PLUGIN_NAME); // state EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), @@ -533,7 +537,7 @@ TEST_F(ResourceManagerTest, lifecycle_all_resources) hardware_interface::lifecycle_state_names::UNCONFIGURED); } - configure_components(rm); + ASSERT_THAT(configure_components(rm), ::testing::Each(hardware_interface::return_type::OK)); { auto status_map = rm.get_components_status(); EXPECT_EQ( @@ -556,7 +560,7 @@ TEST_F(ResourceManagerTest, lifecycle_all_resources) hardware_interface::lifecycle_state_names::INACTIVE); } - activate_components(rm); + ASSERT_THAT(activate_components(rm), ::testing::Each(hardware_interface::return_type::OK)); { auto status_map = rm.get_components_status(); EXPECT_EQ( @@ -579,7 +583,7 @@ TEST_F(ResourceManagerTest, lifecycle_all_resources) hardware_interface::lifecycle_state_names::ACTIVE); } - deactivate_components(rm); + ASSERT_THAT(deactivate_components(rm), ::testing::Each(hardware_interface::return_type::OK)); { auto status_map = rm.get_components_status(); EXPECT_EQ( @@ -602,7 +606,7 @@ TEST_F(ResourceManagerTest, lifecycle_all_resources) hardware_interface::lifecycle_state_names::INACTIVE); } - cleanup_components(rm); + ASSERT_THAT(cleanup_components(rm), ::testing::Each(hardware_interface::return_type::OK)); { auto status_map = rm.get_components_status(); EXPECT_EQ( @@ -625,7 +629,7 @@ TEST_F(ResourceManagerTest, lifecycle_all_resources) hardware_interface::lifecycle_state_names::UNCONFIGURED); } - shutdown_components(rm); + ASSERT_THAT(shutdown_components(rm), ::testing::Each(hardware_interface::return_type::OK)); { auto status_map = rm.get_components_status(); EXPECT_EQ( @@ -837,7 +841,6 @@ TEST_F(ResourceManagerTest, lifecycle_individual_resources) hardware_interface::lifecycle_state_names::FINALIZED); } - // TODO(destogl): Watch-out this fails in output, why is this not caught?!!! shutdown_components(rm, {TEST_SENSOR_HARDWARE_NAME}); { auto status_map = rm.get_components_status(); diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index c40b850d5a..5143c47c24 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* 🖤 Add Black formatter for Python files. (`#936 `_) +* Contributors: Dr. Denis, Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ + +3.4.0 (2022-11-27) +------------------ + 3.3.0 (2022-11-15) ------------------ diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 314ec67272..9ec9221220 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -1,53 +1,58 @@ -cmake_minimum_required(VERSION 3.5) -project(joint_limits) +cmake_minimum_required(VERSION 3.16) +project(joint_limits LANGUAGES CXX) -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra) endif() -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + rclcpp_lifecycle +) -install(DIRECTORY include/ - DESTINATION include +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(joint_limits INTERFACE) +target_compile_features(joint_limits INTERFACE cxx_std_17) +target_include_directories(joint_limits INTERFACE + $ + $ ) +ament_target_dependencies(joint_limits INTERFACE ${THIS_PACKAGE_INCLUDE_DEPENDS}) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - find_package(launch_testing_ament_cmake) - find_package(rclcpp) - find_package(rclcpp_lifecycle) + find_package(launch_testing_ament_cmake REQUIRED) ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) - target_include_directories(joint_limits_rosparam_test PRIVATE include) - ament_target_dependencies(joint_limits_rosparam_test rclcpp rclcpp_lifecycle) + target_link_libraries(joint_limits_rosparam_test joint_limits) add_launch_test(test/joint_limits_rosparam.launch.py) install( - TARGETS - joint_limits_rosparam_test - DESTINATION lib/${PROJECT_NAME} + TARGETS joint_limits_rosparam_test + DESTINATION lib/joint_limits ) install( - FILES - test/joint_limits_rosparam.yaml - DESTINATION share/${PROJECT_NAME}/test + FILES test/joint_limits_rosparam.yaml + DESTINATION share/joint_limits/test ) - endif() -ament_export_dependencies( - rclcpp +install( + DIRECTORY include/ + DESTINATION include/joint_limits ) - -ament_export_include_directories( - include +install(TARGETS joint_limits + EXPORT export_joint_limits + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) +ament_export_targets(export_joint_limits HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/joint_limits/package.xml b/joint_limits/package.xml index af8f48b50e..eae47914ab 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.3.0 + 3.10.0 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/joint_limits/test/joint_limits_rosparam.launch.py b/joint_limits/test/joint_limits_rosparam.launch.py index c808954207..c92f51f863 100644 --- a/joint_limits/test/joint_limits_rosparam.launch.py +++ b/joint_limits/test/joint_limits_rosparam.launch.py @@ -33,9 +33,7 @@ def generate_test_description(): package="joint_limits", executable="joint_limits_rosparam_test", output="screen", - parameters=[ - os.path.join(joint_limits_path, "test", "joint_limits_rosparam.yaml") - ], + parameters=[os.path.join(joint_limits_path, "test", "joint_limits_rosparam.yaml")], ) return ( LaunchDescription( 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 index bd883ee31c..3f17ceea78 100644 --- a/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp +++ b/joint_limits_interface/include/joint_limits_interface/joint_limits_interface.hpp @@ -17,14 +17,6 @@ #ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ #define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_ -#include -#include - -#include -#include - -#include - #include #include #include @@ -35,6 +27,12 @@ #include "joint_limits_interface/joint_limits.hpp" #include "joint_limits_interface/joint_limits_interface_exception.hpp" +#include +#include + +#include +#include + namespace joint_limits_interface { /** @@ -199,7 +197,7 @@ class PositionJointSaturationHandle : public JointLimitHandle } // clamp command position to our computed min/max position - const double cmd = rcppmath::clamp(jcmdh_.get_value(), min_pos, max_pos); + const double cmd = std::clamp(jcmdh_.get_value(), min_pos, max_pos); jcmdh_.set_value(cmd); prev_pos_ = cmd; @@ -283,11 +281,11 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle if (limits_.has_position_limits) { // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = rcppmath::clamp( + soft_min_vel = std::clamp( -soft_limits_.k_position * (pos - soft_limits_.min_position), -limits_.max_velocity, limits_.max_velocity); - soft_max_vel = rcppmath::clamp( + soft_max_vel = std::clamp( -soft_limits_.k_position * (pos - soft_limits_.max_position), -limits_.max_velocity, limits_.max_velocity); } @@ -312,7 +310,7 @@ class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle } // Saturate position command according to bounds - const double pos_cmd = rcppmath::clamp(jcmdh_.get_value(), pos_low, pos_high); + const double pos_cmd = std::clamp(jcmdh_.get_value(), pos_low, pos_high); jcmdh_.set_value(pos_cmd); // Cache variables @@ -393,7 +391,7 @@ class EffortJointSaturationHandle : public JointLimitHandle max_eff = 0.0; } - double clamped = rcppmath::clamp(jcmdh_.get_value(), min_eff, max_eff); + double clamped = std::clamp(jcmdh_.get_value(), min_eff, max_eff); jcmdh_.set_value(clamped); } }; @@ -456,11 +454,11 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle if (limits_.has_position_limits) { // Velocity bounds depend on the velocity limit and the proximity to the position limit - soft_min_vel = rcppmath::clamp( + soft_min_vel = std::clamp( -soft_limits_.k_position * (pos - soft_limits_.min_position), -limits_.max_velocity, limits_.max_velocity); - soft_max_vel = rcppmath::clamp( + soft_max_vel = std::clamp( -soft_limits_.k_position * (pos - soft_limits_.max_position), -limits_.max_velocity, limits_.max_velocity); } @@ -472,14 +470,14 @@ class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle } // Effort bounds depend on the velocity and effort bounds - const double soft_min_eff = rcppmath::clamp( + 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 = rcppmath::clamp( + 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 = rcppmath::clamp(jcmdh_.get_value(), soft_min_eff, soft_max_eff); + const double eff_cmd = std::clamp(jcmdh_.get_value(), soft_min_eff, soft_max_eff); jcmdh_.set_value(eff_cmd); } }; @@ -545,7 +543,7 @@ class VelocityJointSaturationHandle : public JointLimitHandle } // Saturate velocity command according to limits - const double vel_cmd = rcppmath::clamp(jcmdh_.get_value(), vel_low, vel_high); + const double vel_cmd = std::clamp(jcmdh_.get_value(), vel_low, vel_high); jcmdh_.set_value(vel_cmd); // Cache variables @@ -592,10 +590,10 @@ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle { // Velocity bounds depend on the velocity limit and the proximity to the position limit. const double pos = jposh_.get_value(); - min_vel = rcppmath::clamp( + min_vel = std::clamp( -soft_limits_.k_position * (pos - soft_limits_.min_position), -max_vel_limit_, max_vel_limit_); - max_vel = rcppmath::clamp( + max_vel = std::clamp( -soft_limits_.k_position * (pos - soft_limits_.max_position), -max_vel_limit_, max_vel_limit_); } @@ -613,7 +611,7 @@ class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle max_vel = std::min(vel + limits_.max_acceleration * delta_t, max_vel); } - jcmdh_.set_value(rcppmath::clamp(jcmdh_.get_value(), min_vel, max_vel)); + jcmdh_.set_value(std::clamp(jcmdh_.get_value(), min_vel, max_vel)); } private: diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 659f52ad8c..5866622b1f 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Contributors: Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ + +3.4.0 (2022-11-27) +------------------ + 3.3.0 (2022-11-15) ------------------ diff --git a/ros2_control/CMakeLists.txt b/ros2_control/CMakeLists.txt index 28f50184e2..a8ba99730e 100644 --- a/ros2_control/CMakeLists.txt +++ b/ros2_control/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.16) project(ros2_control) find_package(ament_cmake REQUIRED) diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 92cac2fdeb..fc99a5a222 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.3.0 + 3.10.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 c53d294da2..713bdb72f6 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,37 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Contributors: Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ +* Rename class type to plugin name #api-breaking #abi-breaking (`#780 `_) +* Contributors: Bence Magyar + +3.4.0 (2022-11-27) +------------------ + 3.3.0 (2022-11-15) ------------------ diff --git a/ros2_control_test_assets/CMakeLists.txt b/ros2_control_test_assets/CMakeLists.txt index 47fe15a7f7..d1bb895eed 100644 --- a/ros2_control_test_assets/CMakeLists.txt +++ b/ros2_control_test_assets/CMakeLists.txt @@ -1,14 +1,25 @@ -cmake_minimum_required(VERSION 3.5) -project(ros2_control_test_assets) +cmake_minimum_required(VERSION 3.16) +project(ros2_control_test_assets LANGUAGES CXX) find_package(ament_cmake REQUIRED) +add_library(ros2_control_test_assets INTERFACE) +target_compile_features(ros2_control_test_assets INTERFACE cxx_std_17) +target_include_directories(ros2_control_test_assets INTERFACE + $ + $ +) + install( DIRECTORY include/ - DESTINATION include + DESTINATION include/ros2_control_test_assets ) - -ament_export_include_directories( - include +install(TARGETS ros2_control_test_assets + EXPORT export_ros2_control_test_assets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) + +ament_export_targets(export_ros2_control_test_assets HAS_LIBRARY_TARGET) ament_package() diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 73dc32dd2f..f3e2bda2c0 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -421,7 +421,7 @@ const auto invalid_urdf_ros2_control_missing_attribute = )"; -const auto invalid_urdf_ros2_control_component_missing_class_type = +const auto invalid_urdf_ros2_control_component_missing_plugin_name = R"( @@ -452,7 +452,7 @@ const auto invalid_urdf_ros2_control_parameter_missing_name = )"; -const auto invalid_urdf_ros2_control_component_class_type_empty = +const auto invalid_urdf_ros2_control_component_plugin_name_empty = R"( diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 83281bd3d7..05ffad00d6 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -417,7 +417,7 @@ const auto minimal_robot_missing_command_keys_urdf = [[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_NAME = "TestActuatorHardware"; [[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_TYPE = "actuator"; -[[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_CLASS_TYPE = "test_actuator"; +[[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_PLUGIN_NAME = "test_actuator"; [[maybe_unused]] const std::vector TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES = { "joint1/position", "joint1/max_velocity"}; [[maybe_unused]] const std::vector TEST_ACTUATOR_HARDWARE_STATE_INTERFACES = { @@ -425,14 +425,14 @@ const auto minimal_robot_missing_command_keys_urdf = [[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_NAME = "TestSensorHardware"; [[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_TYPE = "sensor"; -[[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_CLASS_TYPE = "test_sensor"; +[[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_PLUGIN_NAME = "test_sensor"; [[maybe_unused]] const std::vector TEST_SENSOR_HARDWARE_COMMAND_INTERFACES = {""}; [[maybe_unused]] const std::vector TEST_SENSOR_HARDWARE_STATE_INTERFACES = { "sensor1/velocity"}; [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_NAME = "TestSystemHardware"; [[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_TYPE = "system"; -[[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_CLASS_TYPE = "test_system"; +[[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_PLUGIN_NAME = "test_system"; [[maybe_unused]] const std::vector TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES = { "joint2/velocity", "joint2/max_acceleration", "joint3/velocity", "configuration/max_tcp_jerk"}; [[maybe_unused]] const std::vector TEST_SYSTEM_HARDWARE_STATE_INTERFACES = { diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 96f1354d45..650ef82ac8 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.3.0 + 3.10.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 3c6ff78645..329a814dba 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,43 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ +* Remove deprecations from CLI and controller_manager (`#948 `_) +* [CLI] Fix wrong output of controller states for `load_controller` command (`#947 `_) +* Contributors: Christoph Fröhlich + +3.8.0 (2023-02-10) +------------------ +* 🖤 Add Black formatter for Python files. (`#936 `_) +* Add list_hardware_components CLI `_ - Adds list_hardware_components to CLI (`#891 `_) +* Contributors: Andy McEvoy, Dr. Denis + +3.7.0 (2023-01-24) +------------------ +* Do not use CLI calls but direct API for setting parameters. (`#910 `_) +* Contributors: Dr. Denis + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ +* Fix hardware interface CLI description (`#864 `_) +* Contributors: Christoph Fröhlich + +3.4.0 (2022-11-27) +------------------ + 3.3.0 (2022-11-15) ------------------ diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 97d042b087..6ae26df325 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -9,6 +9,7 @@ Currently supported commands are - ros2 control list_controllers - ros2 control list_controller_types + - ros2 control list_hardware_components - ros2 control list_hardware_interfaces - ros2 control load_controller - ros2 control reload_controller_libraries @@ -74,6 +75,40 @@ Example output: joint_trajectory_controller/JointTrajectoryController controller_interface::ControllerInterface +list_hardware_components +------------------------ + +.. code-block:: console + + $ ros2 control list_hardware_components -h + usage: ros2 control list_hardware_components [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + + Output the list of available hardware components + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + --verbose, -v List hardware components with command and state interfaces + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node + --include-hidden-nodes + Consider hidden nodes as well + + +Example output: + +.. code-block:: console + + $ ros2 control list_hardware_components + Hardware Component 0 + name: RRBot + type: system + plugin name: ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + state: id=3 label=active + + list_hardware_interfaces ------------------------ @@ -82,7 +117,7 @@ list_hardware_interfaces $ ros2 control list_hardware_interfaces -h usage: ros2 control list_hardware_interfaces [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] - Output the list of loaded controllers, their type and status + Output the list of available command and state interfaces optional arguments: -h, --help show this help message and exit @@ -122,7 +157,7 @@ load_controller -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) - --set_state {configured,active} + --set_state {inactive,active} Set the state of the loaded controller -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index fc4136f3e1..e2b2a7b07b 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.3.0 + 3.10.0 The ROS 2 command line tools for ROS2 Control. @@ -11,6 +11,7 @@ Apache License 2.0 Victor Lopez + rcl_interfaces rclpy ros2cli ros2node diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py index 9729b90c7b..df522670ec 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -33,19 +33,19 @@ def service_caller(service_name, service_type, request): cli = node.create_client(service_type, service_name) if not cli.service_is_ready(): - node.get_logger().debug(f'waiting for service {service_name} to become available...') + node.get_logger().debug(f"waiting for service {service_name} to become available...") if not cli.wait_for_service(2.0): - raise RuntimeError(f'Could not contact service {service_name}') + raise RuntimeError(f"Could not contact service {service_name}") - node.get_logger().debug(f'requester: making request: { repr(request) }\n') + node.get_logger().debug(f"requester: making request: { repr(request) }\n") future = cli.call_async(request) rclpy.spin_until_future_complete(node, future) if future.result() is not None: return future.result() else: future_exception = future.exception() - raise RuntimeError(f'Exception while calling service: { repr(future_exception) }') + raise RuntimeError(f"Exception while calling service: { repr(future_exception) }") finally: node.destroy_node() rclpy.shutdown() @@ -59,14 +59,14 @@ def __call__(self, prefix, parsed_args, **kwargs): parameter_names = call_list_parameters( node=node, node_name=parsed_args.controller_manager ) - suffix = '.type' + suffix = ".type" return [n[: -len(suffix)] for n in parameter_names if n.endswith(suffix)] class LoadedControllerNameCompleter: """Callable returning a list of loaded controllers.""" - def __init__(self, valid_states=['active', 'inactive', 'configured', 'unconfigured']): + def __init__(self, valid_states=["active", "inactive", "configured", "unconfigured"]): self.valid_states = valid_states def __call__(self, prefix, parsed_args, **kwargs): @@ -78,13 +78,13 @@ def __call__(self, prefix, parsed_args, **kwargs): def add_controller_mgr_parsers(parser): """Parser arguments to get controller manager node name, defaults to /controller_manager.""" arg = parser.add_argument( - '-c', - '--controller-manager', - help='Name of the controller manager ROS node', - default='/controller_manager', + "-c", + "--controller-manager", + help="Name of the controller manager ROS node", + default="/controller_manager", required=False, ) - arg.completer = NodeNameCompleter(include_hidden_nodes_key='include_hidden_nodes') + arg.completer = NodeNameCompleter(include_hidden_nodes_key="include_hidden_nodes") parser.add_argument( - '--include-hidden-nodes', action='store_true', help='Consider hidden nodes as well' + "--include-hidden-nodes", action="store_true", help="Consider hidden nodes as well" ) diff --git a/ros2controlcli/ros2controlcli/command/control.py b/ros2controlcli/ros2controlcli/command/control.py index 6ca4386709..dad12f2065 100644 --- a/ros2controlcli/ros2controlcli/command/control.py +++ b/ros2controlcli/ros2controlcli/command/control.py @@ -23,15 +23,15 @@ class ControlCommand(CommandExtension): def add_arguments(self, parser, cli_name): self._subparser = parser # get verb extensions and let them add their arguments - add_subparsers_on_demand(parser, cli_name, '_verb', 'ros2controlcli.verb', required=False) + add_subparsers_on_demand(parser, cli_name, "_verb", "ros2controlcli.verb", required=False) def main(self, *, parser, args): - if not hasattr(args, '_verb'): + if not hasattr(args, "_verb"): # in case no verb was passed self._subparser.print_help() return 0 - extension = getattr(args, '_verb') + extension = getattr(args, "_verb") # call the verb's main method return extension.main(args=args) diff --git a/ros2controlcli/ros2controlcli/verb/list_controller_types.py b/ros2controlcli/ros2controlcli/verb/list_controller_types.py index 7b4e778f8f..086b820124 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controller_types.py +++ b/ros2controlcli/ros2controlcli/verb/list_controller_types.py @@ -33,6 +33,6 @@ def main(self, *, args): response = list_controller_types(node, args.controller_manager) types_and_classes = zip(response.types, response.base_classes) for c in types_and_classes: - print(f'{c[0]:70s} {c[1]}') + print(f"{c[0]:70s} {c[1]}") return 0 diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index d21ec7d2e1..ca73e7afef 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -22,28 +22,28 @@ def print_controller_state(c, args): - print(f'{c.name:20s}[{c.type:20s}] {c.state:10s}') + print(f"{c.name:20s}[{c.type:20s}] {c.state:10s}") if args.claimed_interfaces or args.verbose: - print('\tclaimed interfaces:') + print("\tclaimed interfaces:") for claimed_interface in c.claimed_interfaces: - print(f'\t\t{claimed_interface}') + print(f"\t\t{claimed_interface}") if args.required_command_interfaces or args.verbose: - print('\trequired command interfaces:') + print("\trequired command interfaces:") for required_command_interface in c.required_command_interfaces: - print(f'\t\t{required_command_interface}') + print(f"\t\t{required_command_interface}") if args.required_state_interfaces or args.verbose: - print('\trequired state interfaces:') + print("\trequired state interfaces:") for required_state_interface in c.required_state_interfaces: - print(f'\t\t{required_state_interface}') + print(f"\t\t{required_state_interface}") if args.chained_interfaces or args.verbose: - print('\tchained to interfaces:') + print("\tchained to interfaces:") for connection in c.chain_connections: for reference in connection.reference_interfaces: - print(f'\t\t{reference:20s}') + print(f"\t\t{reference:20s}") if args.reference_interfaces or args.verbose: - print('\texported reference interfaces:') + print("\texported reference interfaces:") for reference_interfaces in c.reference_interfaces: - print(f'\t\t{reference_interfaces}') + print(f"\t\t{reference_interfaces}") class ListControllersVerb(VerbExtension): @@ -52,34 +52,35 @@ class ListControllersVerb(VerbExtension): def add_arguments(self, parser, cli_name): add_arguments(parser) parser.add_argument( - '--claimed-interfaces', - action='store_true', - help='List controller\'s claimed interfaces', + "--claimed-interfaces", + action="store_true", + help="List controller's claimed interfaces", ) parser.add_argument( - '--required-state-interfaces', - action='store_true', - help='List controller\'s required state interfaces', + "--required-state-interfaces", + action="store_true", + help="List controller's required state interfaces", ) parser.add_argument( - '--required-command-interfaces', - action='store_true', - help='List controller\'s required command interfaces', + "--required-command-interfaces", + action="store_true", + help="List controller's required command interfaces", ) parser.add_argument( - '--chained-interfaces', - action='store_true', - help='List interfaces that the controllers are chained to', + "--chained-interfaces", + action="store_true", + help="List interfaces that the controllers are chained to", ) parser.add_argument( - '--reference-interfaces', - action='store_true', - help='List controller\'s exported references', + "--reference-interfaces", + action="store_true", + help="List controller's exported references", ) parser.add_argument( - '--verbose', '-v', - action='store_true', - help='List controller\'s claimed interfaces, required state interfaces and required command interfaces', + "--verbose", + "-v", + action="store_true", + help="List controller's claimed interfaces, required state interfaces and required command interfaces", ) add_controller_mgr_parsers(parser) diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py new file mode 100644 index 0000000000..2f94d70969 --- /dev/null +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -0,0 +1,81 @@ +# Copyright 2023 ROS2-Control Development Team +# +# 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 controller_manager import list_hardware_components +from controller_manager.spawner import bcolors + +from ros2cli.node.direct import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2cli.verb import VerbExtension +from ros2controlcli.api import add_controller_mgr_parsers + + +class ListHardwareComponentsVerb(VerbExtension): + """Output the list of available hardware components.""" + + def add_arguments(self, parser, cli_name): + add_arguments(parser) + parser.add_argument( + "--verbose", + "-v", + action="store_true", + help="List hardware components with command and state interfaces", + ) + add_controller_mgr_parsers(parser) + + def main(self, *, args): + with NodeStrategy(args) as node: + hardware_components = list_hardware_components(node, args.controller_manager) + + for idx, component in enumerate(hardware_components.component): + print( + f"Hardware Component {idx}\n\tname: {component.name}\n\ttype: {component.type}" + ) + if hasattr(component, "plugin_name"): + plugin_name = component.plugin_name + else: + plugin_name = f"{bcolors.WARNING}plugin name missing!{bcolors.ENDC}" + + print( + f"\tplugin name: {plugin_name}\n\tstate: id={component.state.id} label={component.state.label}\n\tcommand interfaces" + ) + for cmd_interface in component.command_interfaces: + if cmd_interface.is_available: + available_str = f"{bcolors.OKBLUE}[available]{bcolors.ENDC}" + else: + available_str = f"{bcolors.WARNING}[unavailable]{bcolors.ENDC}" + + if cmd_interface.is_claimed: + claimed_str = f"{bcolors.OKBLUE}[claimed]{bcolors.ENDC}" + else: + claimed_str = "[unclaimed]" + + print(f"\t\t{cmd_interface.name} {available_str} {claimed_str}") + + if args.verbose: + print("\tstate interfaces") + for state_interface in component.command_interfaces: + if state_interface.is_available: + available_str = f"{bcolors.OKBLUE}[available]{bcolors.ENDC}" + else: + available_str = f"{bcolors.WARNING}[unavailable]{bcolors.ENDC}" + + if state_interface.is_claimed: + claimed_str = f"{bcolors.OKBLUE}[claimed]{bcolors.ENDC}" + else: + claimed_str = "[unclaimed]" + + print(f"\t\t{state_interface.name} {available_str} {claimed_str}") + + return 0 diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index 6065e080b1..7aa850f3bc 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -22,7 +22,7 @@ class ListHardwareInterfacesVerb(VerbExtension): - """Output the list of loaded controllers, their type and status.""" + """Output the list of available command and state interfaces.""" def add_arguments(self, parser, cli_name): add_arguments(parser) @@ -37,21 +37,27 @@ def main(self, *, args): state_interfaces = sorted( hardware_interfaces.state_interfaces, key=lambda hwi: hwi.name ) - print('command interfaces') + print("command interfaces") for command_interface in command_interfaces: if command_interface.is_available: if command_interface.is_claimed: - print(f'\t{bcolors.OKBLUE}{command_interface.name} ' - f'[available] [claimed]{bcolors.ENDC}') + print( + f"\t{bcolors.OKBLUE}{command_interface.name} " + f"[available] [claimed]{bcolors.ENDC}" + ) else: - print(f'\t{bcolors.OKCYAN}{command_interface.name} ' - f'[available] [unclaimed]{bcolors.ENDC}') + print( + f"\t{bcolors.OKCYAN}{command_interface.name} " + f"[available] [unclaimed]{bcolors.ENDC}" + ) else: - print(f'\t{bcolors.WARNING}{command_interface.name} ' - f'[unavailable] [unclaimed]{bcolors.ENDC}') + print( + f"\t{bcolors.WARNING}{command_interface.name} " + f"[unavailable] [unclaimed]{bcolors.ENDC}" + ) - print('state interfaces') + print("state interfaces") for state_interface in state_interfaces: - print(f'\t{state_interface.name}') + print(f"\t{state_interface.name}") return 0 diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index e8fc47b903..babd6d2307 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -26,12 +26,12 @@ class LoadControllerVerb(VerbExtension): def add_arguments(self, parser, cli_name): add_arguments(parser) - arg = parser.add_argument('controller_name', help='Name of the controller') + arg = parser.add_argument("controller_name", help="Name of the controller") arg.completer = ControllerNameCompleter() arg = parser.add_argument( - '--set-state', - choices=['configured', 'active'], - help='Set the state of the loaded controller', + "--set-state", + choices=["inactive", "active"], + help="Set the state of the loaded controller", ) add_controller_mgr_parsers(parser) @@ -39,29 +39,26 @@ def main(self, *, args): with NodeStrategy(args) as node: response = load_controller(node, args.controller_manager, args.controller_name) if not response.ok: - return 'Error loading controller, check controller_manager logs' + return "Error loading controller, check controller_manager logs" if not args.set_state: - print(f'Successfully loaded controller {args.controller_name}') + print(f"Successfully loaded controller {args.controller_name}") return 0 # we in any case configure the controller response = configure_controller(node, args.controller_manager, args.controller_name) if not response.ok: - return 'Error configuring controller' + return "Error configuring controller" - # TODO(destogl): remove in humble+ - if args.set_state == 'start': - print('Setting state "start" is deprecated "activate" instead!') - args.set_state == 'activate' - - if args.set_state == 'active': + if args.set_state == "active": response = switch_controllers( node, args.controller_manager, [], [args.controller_name], True, True, 5.0 ) if not response.ok: - return 'Error activating controller, check controller_manager logs' + return "Error activating controller, check controller_manager logs" - print(f'Sucessfully loaded controller {args.controller_name} into ' - f'state { "inactive" if args.set_state == "configure" else "active" }') + print( + f"Successfully loaded controller {args.controller_name} into " + f'state { "inactive" if args.set_state == "inactive" else "active" }' + ) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py b/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py index 9d56c6d17e..82bc2e480f 100644 --- a/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py +++ b/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py @@ -27,7 +27,7 @@ class ReloadControllerLibrariesVerb(VerbExtension): def add_arguments(self, parser, cli_name): add_arguments(parser) parser.add_argument( - '--force-kill', action='store_true', help='Force stop of loaded controllers' + "--force-kill", action="store_true", help="Force stop of loaded controllers" ) add_controller_mgr_parsers(parser) @@ -37,7 +37,7 @@ def main(self, *, args): node, args.controller_manager, force_kill=args.force_kill ) if not response.ok: - return 'Error reloading libraries, check controller_manager logs' + return "Error reloading libraries, check controller_manager logs" - print('Reload successful') + print("Reload successful") return 0 diff --git a/ros2controlcli/ros2controlcli/verb/set_controller_state.py b/ros2controlcli/ros2controlcli/verb/set_controller_state.py index 4ce6cdf7c7..d584abe987 100644 --- a/ros2controlcli/ros2controlcli/verb/set_controller_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_controller_state.py @@ -26,13 +26,13 @@ class SetControllerStateVerb(VerbExtension): def add_arguments(self, parser, cli_name): add_arguments(parser) - arg = parser.add_argument('controller_name', help='Name of the controller to be changed') + arg = parser.add_argument("controller_name", help="Name of the controller to be changed") arg.completer = LoadedControllerNameCompleter() arg = parser.add_argument( - 'state', + "state", # choices=['unconfigured', 'inactive', 'active'], TODO(destogl): when cleanup is impl - choices=['inactive', 'active'], - help='State in which the controller should be changed to', + choices=["inactive", "active"], + help="State in which the controller should be changed to", ) add_controller_mgr_parsers(parser) @@ -43,7 +43,7 @@ def main(self, *, args): try: matched_controller = [c for c in controllers if c.name == args.controller_name][0] except IndexError: - return f'controller {args.controller_name} does not seem to be loaded' + return f"controller {args.controller_name} does not seem to be loaded" # TODO(destogl): This has to be implemented in CLI and controller manager # if args.state == 'unconfigured': @@ -58,52 +58,44 @@ def main(self, *, args): # # print(f'successfully cleaned up {args.controller_name}') # return 0 - if args.state == 'configure': - args.state = 'inactive' - print('Setting state "configure" is deprecated, use "inactive" instead!') - - if args.state == 'stop': - args.state = 'inactive' - print('Setting state "stop" is deprecated, use "inactive" instead!') - - if args.state == 'inactive': - if matched_controller.state == 'unconfigured': + if args.state == "inactive": + if matched_controller.state == "unconfigured": response = configure_controller( node, args.controller_manager, args.controller_name ) if not response.ok: - return 'Error configuring controller, check controller_manager logs' + return "Error configuring controller, check controller_manager logs" - print(f'successfully configured {args.controller_name}') + print(f"Successfully configured {args.controller_name}") return 0 - elif matched_controller.state == 'active': + elif matched_controller.state == "active": response = switch_controllers( node, args.controller_manager, [args.controller_name], [], True, True, 5.0 ) if not response.ok: - return 'Error stopping controller, check controller_manager logs' + return "Error stopping controller, check controller_manager logs" - print(f'successfully deactivated {args.controller_name}') + print(f"Successfully deactivated {args.controller_name}") return 0 else: - return f'cannot put {matched_controller.name} in "inactive" state' \ - f'from its current state {matched_controller.state}' - - if args.state == 'start': - args.state = 'active' - print('Setting state "start" is deprecated, use "active" instead!') + return ( + f'cannot put {matched_controller.name} in "inactive" state' + f"from its current state {matched_controller.state}" + ) - if args.state == 'active': - if matched_controller.state != 'inactive': - return f'cannot activate {matched_controller.name} ' \ - f'from its current state {matched_controller.state}' + if args.state == "active": + if matched_controller.state != "inactive": + return ( + f"cannot activate {matched_controller.name} " + f"from its current state {matched_controller.state}" + ) response = switch_controllers( node, args.controller_manager, [], [args.controller_name], True, True, 5.0 ) if not response.ok: - return 'Error activating controller, check controller_manager logs' + return "Error activating controller, check controller_manager logs" - print(f'successfully activated {args.controller_name}') + print(f"Successfully activated {args.controller_name}") return 0 diff --git a/ros2controlcli/ros2controlcli/verb/switch_controllers.py b/ros2controlcli/ros2controlcli/verb/switch_controllers.py index c6436635bc..1c798c29b8 100644 --- a/ros2controlcli/ros2controlcli/verb/switch_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/switch_controllers.py @@ -27,56 +27,31 @@ class SwitchControllersVerb(VerbExtension): def add_arguments(self, parser, cli_name): add_arguments(parser) arg = parser.add_argument( - '--stop', - nargs='*', + "--deactivate", + nargs="*", default=[], - help='Name of the controllers to be deactivated', + help="Name of the controllers to be deactivated", ) - arg.completer = LoadedControllerNameCompleter(['active']) + arg.completer = LoadedControllerNameCompleter(["active"]) arg = parser.add_argument( - '--deactivate', - nargs='*', + "--activate", + nargs="*", default=[], - help='Name of the controllers to be deactivated', + help="Name of the controllers to be activated", ) - arg.completer = LoadedControllerNameCompleter(['active']) - arg = parser.add_argument( - '--start', - nargs='*', - default=[], - help='Name of the controllers to be activated', - ) - arg.completer = LoadedControllerNameCompleter(['inactive']) - arg = parser.add_argument( - '--activate', - nargs='*', - default=[], - help='Name of the controllers to be activated', - ) - arg.completer = LoadedControllerNameCompleter(['inactive']) - parser.add_argument('--strict', action='store_true', help='Strict switch') - parser.add_argument('--start-asap', action='store_true', help='Start asap controllers') - parser.add_argument('--activate-asap', action='store_true', help='Start asap controllers') + arg.completer = LoadedControllerNameCompleter(["inactive"]) + parser.add_argument("--strict", action="store_true", help="Strict switch") + parser.add_argument("--activate-asap", action="store_true", help="Start asap controllers") parser.add_argument( - '--switch-timeout', + "--switch-timeout", default=5.0, required=False, - help='Timeout for switching controllers', + help="Timeout for switching controllers", ) - arg.completer = LoadedControllerNameCompleter(['inactive']) + arg.completer = LoadedControllerNameCompleter(["inactive"]) add_controller_mgr_parsers(parser) def main(self, *, args): - if (args.stop): - print('"--stop" flag is deprecated, use "--deactivate" instead!') - args.deactivate = args.stop - if (args.start): - print('"--start" flag is deprecated, use "--activate" instead!') - args.activate = args.start - if (args.start_asap): - print('"--start-asap" flag is deprecated, use "--activate-asap" instead!') - args.activate_asap = args.start_asap - with NodeStrategy(args) as node: response = switch_controllers( node, @@ -84,11 +59,11 @@ def main(self, *, args): args.deactivate, args.activate, args.strict, - args.start_asap, + args.activate_asap, args.switch_timeout, ) if not response.ok: - return 'Error switching controllers, check controller_manager logs' + return "Error switching controllers, check controller_manager logs" - print('Successfully switched controllers') + print("Successfully switched controllers") return 0 diff --git a/ros2controlcli/ros2controlcli/verb/unload_controller.py b/ros2controlcli/ros2controlcli/verb/unload_controller.py index cf24d4847b..81612eb3ad 100644 --- a/ros2controlcli/ros2controlcli/verb/unload_controller.py +++ b/ros2controlcli/ros2controlcli/verb/unload_controller.py @@ -26,7 +26,7 @@ class UnloadControllerVerb(VerbExtension): def add_arguments(self, parser, cli_name): add_arguments(parser) - arg = parser.add_argument('controller_name', help='Name of the controller') + arg = parser.add_argument("controller_name", help="Name of the controller") arg.completer = LoadedControllerNameCompleter() add_controller_mgr_parsers(parser) @@ -34,7 +34,7 @@ def main(self, *, args): with NodeStrategy(args) as node: response = unload_controller(node, args.controller_manager, args.controller_name) if not response.ok: - return 'Error unloading controllers, check controller_manager logs' + return "Error unloading controllers, check controller_manager logs" - print(f'Successfully unloaded controller {args.controller_name}') + print(f"Successfully unloaded controller {args.controller_name}") return 0 diff --git a/ros2controlcli/ros2controlcli/verb/view_controller_chains.py b/ros2controlcli/ros2controlcli/verb/view_controller_chains.py index 891ee7c088..fbd999192e 100644 --- a/ros2controlcli/ros2controlcli/verb/view_controller_chains.py +++ b/ros2controlcli/ros2controlcli/verb/view_controller_chains.py @@ -24,70 +24,98 @@ import pygraphviz as pgz -def make_controller_node(s, controller_name, state_interfaces, command_interfaces, input_controllers, - output_controllers, port_map): +def make_controller_node( + s, + controller_name, + state_interfaces, + command_interfaces, + input_controllers, + output_controllers, + port_map, +): state_interfaces = sorted(list(state_interfaces)) command_interfaces = sorted(list(command_interfaces)) input_controllers = sorted(list(input_controllers)) output_controllers = sorted(list(output_controllers)) - inputs_str = '' + inputs_str = "" for ind, state_interface in enumerate(state_interfaces): - deliminator = '|' + deliminator = "|" if ind == len(state_interface) - 1: - deliminator = '' - inputs_str += '<{}> {} {} '.format("state_end_" + state_interface, state_interface, deliminator) + deliminator = "" + inputs_str += "<{}> {} {} ".format( + "state_end_" + state_interface, state_interface, deliminator + ) for ind, input_controller in enumerate(input_controllers): - deliminator = '|' + deliminator = "|" if ind == len(input_controller) - 1: - deliminator = '' - inputs_str += '<{}> {} {} '.format("controller_end_" + input_controller, input_controller, deliminator) + deliminator = "" + inputs_str += "<{}> {} {} ".format( + "controller_end_" + input_controller, input_controller, deliminator + ) port_map["controller_end_" + input_controller] = controller_name - outputs_str = '' + outputs_str = "" for ind, command_interface in enumerate(command_interfaces): - deliminator = '|' + deliminator = "|" if ind == len(command_interface) - 1: - deliminator = '' - outputs_str += '<{}> {} {} '.format("command_start_" + command_interface, command_interface, deliminator) + deliminator = "" + outputs_str += "<{}> {} {} ".format( + "command_start_" + command_interface, command_interface, deliminator + ) for ind, output_controller in enumerate(output_controllers): - deliminator = '|' + deliminator = "|" if ind == len(output_controller) - 1: - deliminator = '' - outputs_str += '<{}> {} {} '.format("controller_start_" + output_controller, output_controller, deliminator) + deliminator = "" + outputs_str += "<{}> {} {} ".format( + "controller_start_" + output_controller, output_controller, deliminator + ) - s.add_node(controller_name, label=f'{controller_name}|{{{{{inputs_str}}}|{{{outputs_str}}}}}') + s.add_node(controller_name, label=f"{controller_name}|{{{{{inputs_str}}}|{{{outputs_str}}}}}") def make_command_node(s, command_interfaces): command_interfaces = sorted(list(command_interfaces)) - outputs_str = '' + outputs_str = "" for ind, command_interface in enumerate(command_interfaces): - deliminator = '|' + deliminator = "|" if ind == len(command_interfaces) - 1: - deliminator = '' - outputs_str += '<{}> {} {} '.format("command_end_" + command_interface, command_interface, deliminator) + deliminator = "" + outputs_str += "<{}> {} {} ".format( + "command_end_" + command_interface, command_interface, deliminator + ) - s.add_node("command_interfaces", label='{}|{{{{{}}}}}'.format("command_interfaces", outputs_str)) + s.add_node( + "command_interfaces", label="{}|{{{{{}}}}}".format("command_interfaces", outputs_str) + ) def make_state_node(s, state_interfaces): state_interfaces = sorted(list(state_interfaces)) - inputs_str = '' + inputs_str = "" for ind, state_interface in enumerate(state_interfaces): - deliminator = '|' + deliminator = "|" if ind == len(state_interfaces) - 1: - deliminator = '' - inputs_str += '<{}> {} {} '.format("state_start_" + state_interface, state_interface, deliminator) - - s.add_node("state_interfaces", label='{}|{{{{{}}}}}'.format("state_interfaces", inputs_str)) - - -def show_graph(input_chain_connections, output_chain_connections, command_connections, state_connections, - command_interfaces, state_interfaces, visualize): - s = pgz.AGraph(name='g', strict=False, directed=True, rankdir='LR') + deliminator = "" + inputs_str += "<{}> {} {} ".format( + "state_start_" + state_interface, state_interface, deliminator + ) + + s.add_node("state_interfaces", label="{}|{{{{{}}}}}".format("state_interfaces", inputs_str)) + + +def show_graph( + input_chain_connections, + output_chain_connections, + command_connections, + state_connections, + command_interfaces, + state_interfaces, + visualize, +): + s = pgz.AGraph(name="g", strict=False, directed=True, rankdir="LR") s.node_attr["shape"] = "record" s.node_attr["style"] = "rounded" port_map = dict() @@ -99,29 +127,42 @@ def show_graph(input_chain_connections, output_chain_connections, command_connec controller_names = controller_names.union({name for name in state_connections}) # create node for each controller for controller_name in controller_names: - make_controller_node(s, controller_name, state_connections[controller_name], - command_connections[controller_name], - input_chain_connections[controller_name], - output_chain_connections[controller_name], port_map) + make_controller_node( + s, + controller_name, + state_connections[controller_name], + command_connections[controller_name], + input_chain_connections[controller_name], + output_chain_connections[controller_name], + port_map, + ) make_state_node(s, state_interfaces) make_command_node(s, command_interfaces) for controller_name in controller_names: for connection in output_chain_connections[controller_name]: - s.add_edge('{}:{}'.format(controller_name, "controller_start_" + connection), - '{}:{}'.format(port_map['controller_end_' + connection], 'controller_end_' + connection)) + s.add_edge( + "{}:{}".format(controller_name, "controller_start_" + connection), + "{}:{}".format( + port_map["controller_end_" + connection], "controller_end_" + connection + ), + ) for state_connection in state_connections[controller_name]: - s.add_edge('{}:{}'.format("state_interfaces", "state_start_" + state_connection), - '{}:{}'.format(controller_name, 'state_end_' + state_connection)) + s.add_edge( + "{}:{}".format("state_interfaces", "state_start_" + state_connection), + "{}:{}".format(controller_name, "state_end_" + state_connection), + ) for command_connection in command_connections[controller_name]: - s.add_edge('{}:{}'.format(controller_name, "command_start_" + command_connection), - '{}:{}'.format("command_interfaces", 'command_end_' + command_connection)) + s.add_edge( + "{}:{}".format(controller_name, "command_start_" + command_connection), + "{}:{}".format("command_interfaces", "command_end_" + command_connection), + ) - s.graph_attr.update(ranksep='2') - s.layout(prog='dot') + s.graph_attr.update(ranksep="2") + s.layout(prog="dot") if visualize: - s.draw('/tmp/controller_diagram.gv.pdf', format='pdf') + s.draw("/tmp/controller_diagram.gv.pdf", format="pdf") def parse_response(list_controllers_response, list_hardware_response, visualize=True): @@ -142,8 +183,15 @@ def parse_response(list_controllers_response, list_hardware_response, visualize= command_connections[controller.name] = set(controller.required_command_interfaces) state_connections[controller.name] = set(controller.required_state_interfaces) - show_graph(input_chain_connections, output_chain_connections, command_connections, state_connections, - command_interfaces, state_interfaces, visualize) + show_graph( + input_chain_connections, + output_chain_connections, + command_connections, + state_connections, + command_interfaces, + state_interfaces, + visualize, + ) class ViewControllerChainsVerb(VerbExtension): diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 8bba93df18..add129be20 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -1,53 +1,69 @@ +# Copyright 2023 ros2_control Development Team +# +# 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 find_packages from setuptools import setup -package_name = 'ros2controlcli' +package_name = "ros2controlcli" setup( name=package_name, - version='3.3.0', - packages=find_packages(exclude=['test']), + version="3.10.0", + packages=find_packages(exclude=["test"]), data_files=[ - ('share/' + package_name, ['package.xml']), - ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ], - install_requires=['ros2cli'], + install_requires=["ros2cli"], zip_safe=True, - author='Victor Lopez', - author_email='victor.lopez@pal-robotics.com', - maintainer='Victor Lopez', - maintainer_email='victor.lopez@pal-robotics.com', - url='https://github.com/ros-controls/ros2_control', + author="Victor Lopez", + author_email="victor.lopez@pal-robotics.com", + maintainer="Victor Lopez", + maintainer_email="victor.lopez@pal-robotics.com", + url="https://github.com/ros-controls/ros2_control", keywords=[], classifiers=[ - 'Environment :: Console', - 'Intended Audience :: Developers', - 'License :: OSI Approved :: Apache Software License', - 'Programming Language :: Python', + "Environment :: Console", + "Intended Audience :: Developers", + "License :: OSI Approved :: Apache Software License", + "Programming Language :: Python", ], - description='ROS2 Control command interface.', + description="ROS2 Control command interface.", long_description="""\ ROS2 Control command interface.""", - license='Apache License, Version 2.0', - tests_require=['pytest'], + license="Apache License, Version 2.0", + tests_require=["pytest"], entry_points={ - 'ros2cli.command': [ - 'control = ros2controlcli.command.control:ControlCommand', + "ros2cli.command": [ + "control = ros2controlcli.command.control:ControlCommand", ], - 'ros2controlcli.verb': [ - 'list_controllers = ros2controlcli.verb.list_controllers:ListControllersVerb', - 'view_controller_chains = ros2controlcli.verb.view_controller_chains:ViewControllerChainsVerb', - 'list_hardware_interfaces = \ - ros2controlcli.verb.list_hardware_interfaces:ListHardwareInterfacesVerb', - 'list_controller_types = \ - ros2controlcli.verb.list_controller_types:ListControllerTypesVerb', - 'load_controller = ros2controlcli.verb.load_controller:LoadControllerVerb', - 'reload_controller_libraries = \ - ros2controlcli.verb.reload_controller_libraries:ReloadControllerLibrariesVerb', - 'set_controller_state = \ - ros2controlcli.verb.set_controller_state:SetControllerStateVerb', - 'switch_controllers = ros2controlcli.verb.switch_controllers:SwitchControllersVerb', - 'unload_controller = ros2controlcli.verb.unload_controller:UnloadControllerVerb', + "ros2controlcli.verb": [ + "list_controllers = ros2controlcli.verb.list_controllers:ListControllersVerb", + "view_controller_chains = ros2controlcli.verb.view_controller_chains:ViewControllerChainsVerb", + "list_hardware_components = \ + ros2controlcli.verb.list_hardware_components:ListHardwareComponentsVerb", + "list_hardware_interfaces = \ + ros2controlcli.verb.list_hardware_interfaces:ListHardwareInterfacesVerb", + "list_controller_types = \ + ros2controlcli.verb.list_controller_types:ListControllerTypesVerb", + "load_controller = ros2controlcli.verb.load_controller:LoadControllerVerb", + "reload_controller_libraries = \ + ros2controlcli.verb.reload_controller_libraries:ReloadControllerLibrariesVerb", + "set_controller_state = \ + ros2controlcli.verb.set_controller_state:SetControllerStateVerb", + "switch_controllers = ros2controlcli.verb.switch_controllers:SwitchControllersVerb", + "unload_controller = ros2controlcli.verb.unload_controller:UnloadControllerVerb", ], }, ) diff --git a/ros2controlcli/test/test_view_controller_chains.py b/ros2controlcli/test/test_view_controller_chains.py index 3ac37198a5..517c451faf 100644 --- a/ros2controlcli/test/test_view_controller_chains.py +++ b/ros2controlcli/test/test_view_controller_chains.py @@ -23,9 +23,7 @@ class TestViewControllerChains(unittest.TestCase): - def test_expected(self): - list_controllers_response = ListControllers.Response() list_hardware_response = ListHardwareInterfaces.Response() @@ -62,9 +60,13 @@ def test_expected(self): chain_connection.reference_interfaces.append(f"joint{i}/position") chain_connection.reference_interfaces.append(f"joint{i}/velocity") - chained_to_controller.required_command_interfaces = chained_to_controller.claimed_interfaces + chained_to_controller.required_command_interfaces = ( + chained_to_controller.claimed_interfaces + ) - chained_from_controller.required_command_interfaces = chained_from_controller.claimed_interfaces + chained_from_controller.required_command_interfaces = ( + chained_from_controller.claimed_interfaces + ) chained_from_controller.chain_connections.append(chain_connection) controller_list = [chained_from_controller, chained_to_controller] diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst new file mode 100644 index 0000000000..4311cfa8c7 --- /dev/null +++ b/rqt_controller_manager/CHANGELOG.rst @@ -0,0 +1,156 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rqt_controller_manager +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.10.0 (2023-03-16) +------------------- + +3.9.1 (2023-03-09) +------------------ + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* 🖤 Add Black formatter for Python files. (`#936 `_) +* Contributors: Dr. Denis + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ +* 🔧 Fixes and updated on pre-commit hooks and their action (`#890 `_) +* Contributors: Denis Štogl + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ + +3.4.0 (2022-11-27) +------------------ +* rqt controller manager ros 2 port (`#813 `_) +* Contributors: Kenji Brameld + +3.3.0 (2022-11-15) +------------------ + +3.2.0 (2022-10-15) +------------------ + +3.1.0 (2022-10-05) +------------------ + +3.0.0 (2022-09-19) +------------------ + +2.14.0 (2022-09-04) +------------------- + +2.13.0 (2022-08-03) +------------------- + +2.12.1 (2022-07-14) +------------------- + +2.12.0 (2022-07-09) +------------------- + +2.11.0 (2022-07-03) +------------------- + +2.10.0 (2022-06-18) +------------------- + +2.9.0 (2022-05-19) +------------------ + +2.8.0 (2022-05-13) +------------------ + +2.7.0 (2022-04-29) +------------------ + +2.6.0 (2022-04-20) +------------------ + +2.5.0 (2022-03-25) +------------------ + +2.4.0 (2022-02-23) +------------------ + +2.3.0 (2022-02-18) +------------------ + +2.2.0 (2022-01-24) +------------------ + +2.1.0 (2022-01-11) +------------------ + +2.0.0 (2021-12-29) +------------------ + +1.2.0 (2021-11-05) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.8.0 (2021-08-28) +------------------ + +0.7.1 (2021-06-15) +------------------ + +0.7.0 (2021-06-06) +------------------ + +0.6.1 (2021-05-31) +------------------ + +0.6.0 (2021-05-23) +------------------ + +0.5.0 (2021-05-03) +------------------ + +0.4.0 (2021-04-07) +------------------ + +0.3.0 (2021-03-21) +------------------ + +0.2.1 (2021-03-02) +------------------ + +0.2.0 (2021-02-26) +------------------ + +0.1.6 (2021-02-05) +------------------ + +0.1.5 (2021-02-04) +------------------ + +0.1.4 (2021-02-03) +------------------ + +0.1.3 (2021-01-21) +------------------ + +0.1.2 (2021-01-06) +------------------ + +0.1.1 (2020-12-23) +------------------ + +0.1.0 (2020-12-22) +------------------ diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 094ac1745b..1d93f82d53 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 0.0.0 + 3.10.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/resource/controller_manager.ui b/rqt_controller_manager/resource/controller_manager.ui index d6394c1b0d..2ede975248 100644 --- a/rqt_controller_manager/resource/controller_manager.ui +++ b/rqt_controller_manager/resource/controller_manager.ui @@ -23,7 +23,7 @@ - <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namesapce.</p></body></html> + <html><head/><body><p><span style=" font-weight:600;">controller manager</span> namespace. It is assumed that the <span style=" font-weight:600;">robot_description</span> parameter also lives in the same namespace.</p></body></html> namespace diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 18d1ec0925..4d3c247e5b 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -17,8 +17,13 @@ import os from ament_index_python.packages import get_package_share_directory -from controller_manager.controller_manager_services import configure_controller, \ - list_controllers, load_controller, switch_controllers, unload_controller +from controller_manager.controller_manager_services import ( + configure_controller, + list_controllers, + load_controller, + switch_controllers, + unload_controller, +) from controller_manager_msgs.msg import ControllerState from controller_manager_msgs.srv import SwitchController from python_qt_binding import loadUi @@ -39,7 +44,7 @@ class ControllerManager(Plugin): def __init__(self, context): super().__init__(context) - self.setObjectName('ControllerManager') + self.setObjectName("ControllerManager") # Create QWidget and extend it with all the attributes and children # from the UI file @@ -47,18 +52,18 @@ def __init__(self, context): ui_file = os.path.join( get_package_share_directory("rqt_controller_manager"), "resource", - "controller_manager.ui") + "controller_manager.ui", + ) loadUi(ui_file, self._widget) - self._widget.setObjectName('ControllerManagerUi') + self._widget.setObjectName("ControllerManagerUi") # Pop-up that displays controller information self._popup_widget = QWidget() ui_file = os.path.join( - get_package_share_directory("rqt_controller_manager"), - 'resource', - 'controller_info.ui') + get_package_share_directory("rqt_controller_manager"), "resource", "controller_info.ui" + ) loadUi(ui_file, self._popup_widget) - self._popup_widget.setObjectName('ControllerInfoUi') + self._popup_widget.setObjectName("ControllerInfoUi") # Show _widget.windowTitle on left-top of each plugin (when # it's set in _widget). This is useful when you open multiple @@ -66,12 +71,12 @@ def __init__(self, context): # plugin at once, these lines add number to make it easy to # tell from pane to pane. if context.serial_number() > 1: - self._widget.setWindowTitle(f'{self._widget.windowTitle()} {context.serial_number()}') + self._widget.setWindowTitle(f"{self._widget.windowTitle()} {context.serial_number()}") # Add widget to the user interface context.add_widget(self._widget) # Initialize members - self._cm_name = '' # Name of the selected controller manager's node + self._cm_name = "" # Name of the selected controller manager's node self._controllers = [] # State of each controller self._table_model = None @@ -81,10 +86,10 @@ def __init__(self, context): # Controller state icons path = get_package_share_directory("rqt_controller_manager") self._icons = { - 'active': QIcon(f'{path}/resource/led_green.png'), - 'finalized': QIcon(f'{path}/resource/led_off.png'), - 'inactive': QIcon(f'{path}/resource/led_red.png'), - 'unconfigured': QIcon(f'{path}/resource/led_off.png'), + "active": QIcon(f"{path}/resource/led_green.png"), + "finalized": QIcon(f"{path}/resource/led_off.png"), + "inactive": QIcon(f"{path}/resource/led_red.png"), + "unconfigured": QIcon(f"{path}/resource/led_off.png"), } # Controllers display @@ -121,18 +126,18 @@ def shutdown_plugin(self): self._popup_widget.hide() def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('cm_name', self._cm_name) + instance_settings.set_value("cm_name", self._cm_name) def restore_settings(self, plugin_settings, instance_settings): # Restore last session's controller_manager, if present self._update_cm_list() - cm_name = instance_settings.value('cm_name') + cm_name = instance_settings.value("cm_name") cm_combo = self._widget.cm_combo cm_list = [cm_combo.itemText(i) for i in range(cm_combo.count())] try: idx = cm_list.index(cm_name) cm_combo.setCurrentIndex(idx) - except (ValueError): + except ValueError: pass def _update_cm_list(self): @@ -145,7 +150,6 @@ def _on_cm_change(self, cm_name): self._update_controllers() def _update_controllers(self): - if not self._cm_name: return @@ -175,8 +179,7 @@ def _list_controllers(self): add_ctrl = all(name != ctrl.name for ctrl in controllers) if add_ctrl: type_str = _get_controller_type(self._node, self._cm_name, name) - uninit_ctrl = ControllerState(name=name, - type=type_str) + uninit_ctrl = ControllerState(name=name, type=type_str) controllers.append(uninit_ctrl) return controllers @@ -195,39 +198,36 @@ def _on_ctrl_menu(self, pos): # Show context menu menu = QMenu(self._widget.table_view) - if ctrl.state == 'active': - action_deactivate = menu.addAction(self._icons['inactive'], 'Deactivate') - action_kill = menu.addAction(self._icons['finalized'], - 'Deactivate and Unload') - elif ctrl.state == 'inactive': - action_activate = menu.addAction(self._icons['active'], 'Activate') - action_unload = menu.addAction(self._icons['unconfigured'], - 'Unload') - elif ctrl.state == 'unconfigured': - action_configure = menu.addAction(self._icons['inactive'], 'Configure') - action_spawn = menu.addAction(self._icons['active'], - 'Configure and Activate') + if ctrl.state == "active": + action_deactivate = menu.addAction(self._icons["inactive"], "Deactivate") + action_kill = menu.addAction(self._icons["finalized"], "Deactivate and Unload") + elif ctrl.state == "inactive": + action_activate = menu.addAction(self._icons["active"], "Activate") + action_unload = menu.addAction(self._icons["unconfigured"], "Unload") + elif ctrl.state == "unconfigured": + action_configure = menu.addAction(self._icons["inactive"], "Configure") + action_spawn = menu.addAction(self._icons["active"], "Configure and Activate") else: # Controller isn't loaded - action_load = menu.addAction(self._icons['unconfigured'], 'Load') - action_configure = menu.addAction(self._icons['inactive'], 'Load and Configure') - action_activate = menu.addAction(self._icons['active'], 'Load, Configure and Activate') + action_load = menu.addAction(self._icons["unconfigured"], "Load") + action_configure = menu.addAction(self._icons["inactive"], "Load and Configure") + action_activate = menu.addAction(self._icons["active"], "Load, Configure and Activate") action = menu.exec_(self._widget.table_view.mapToGlobal(pos)) # Evaluate user action - if ctrl.state == 'active': + if ctrl.state == "active": if action is action_deactivate: self._deactivate_controller(ctrl.name) elif action is action_kill: self._deactivate_controller(ctrl.name) unload_controller(self._node, self._cm_name, ctrl.name) - elif ctrl.state in ('finalized', 'inactive'): + elif ctrl.state in ("finalized", "inactive"): if action is action_activate: self._activate_controller(ctrl.name) elif action is action_unload: unload_controller(self._node, self._cm_name, ctrl.name) - elif ctrl.state == 'unconfigured': + elif ctrl.state == "unconfigured": if action is action_configure: configure_controller(self._node, self._cm_name, ctrl.name) elif action is action_spawn: @@ -253,7 +253,7 @@ def _on_ctrl_info(self, index): popup.ctrl_type.setText(ctrl.type) res_model = QStandardItemModel() - model_root = QStandardItem('Claimed Interfaces') + model_root = QStandardItem("Claimed Interfaces") res_model.appendRow(model_root) for claimed_interface in ctrl.claimed_interfaces: hw_iface_item = QStandardItem(claimed_interface) @@ -270,7 +270,7 @@ def _on_header_menu(self, pos): # Show context menu menu = QMenu(self._widget.table_view) - action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') + action_toggle_auto_resize = menu.addAction("Toggle Auto-Resize") action = menu.exec_(header.mapToGlobal(pos)) # Evaluate user action @@ -288,7 +288,7 @@ def _activate_controller(self, name): activate_controllers=[name], strict=SwitchController.Request.STRICT, activate_asap=False, - timeout=0.3 + timeout=0.3, ) def _deactivate_controller(self, name): @@ -299,7 +299,7 @@ def _deactivate_controller(self, name): activate_controllers=[], strict=SwitchController.Request.STRICT, activate_asap=False, - timeout=0.3 + timeout=0.3, ) @@ -311,7 +311,7 @@ class ControllerTable(QAbstractTableModel): name and state. """ - def __init__(self, controller_info, icons, parent=None): + def __init__(self, controller_info, icons, parent=None): QAbstractTableModel.__init__(self, parent) self._data = controller_info self._icons = icons @@ -326,9 +326,9 @@ def headerData(self, col, orientation, role): if orientation != Qt.Horizontal or role != Qt.DisplayRole: return None if col == 0: - return 'controller' + return "controller" elif col == 1: - return 'state' + return "state" def data(self, index, role): if not index.isValid(): @@ -340,7 +340,7 @@ def data(self, index, role): if index.column() == 0: return ctrl.name elif index.column() == 1: - return ctrl.state or 'not loaded' + return ctrl.state or "not loaded" if role == Qt.DecorationRole and index.column() == 0: return self._icons.get(ctrl.state) @@ -385,7 +385,7 @@ def _get_controller_type(node, node_name, ctrl_name): @rtype str """ response = call_get_parameters(node=node, node_name=node_name, parameter_names=[ctrl_name]) - return response.values[0].string_value if response.values else '' + return response.values[0].string_value if response.values else "" def _list_controller_managers(node): @@ -398,14 +398,14 @@ def _list_controller_managers(node): @rtype list of str """ return [ - name.rstrip('list_controllers').rstrip('/') + name.rstrip("list_controllers").rstrip("/") for name, _ in get_service_names_and_types(node=node) - if name.endswith('list_controllers') + if name.endswith("list_controllers") ] def _get_parameter_controller_names(node, node_name): """Get list of ROS parameter names that potentially represent a controller configuration.""" parameter_names = call_list_parameters(node=node, node_name=node_name) - suffix = '.type' + suffix = ".type" return [n[: -len(suffix)] for n in parameter_names.result().result.names if n.endswith(suffix)] diff --git a/rqt_controller_manager/rqt_controller_manager/main.py b/rqt_controller_manager/rqt_controller_manager/main.py index 4dd77d350b..14de0aea54 100644 --- a/rqt_controller_manager/rqt_controller_manager/main.py +++ b/rqt_controller_manager/rqt_controller_manager/main.py @@ -20,8 +20,8 @@ def main(): main = Main() - sys.exit(main.main(sys.argv, standalone='rqt_controller_manager')) + sys.exit(main.main(sys.argv, standalone="rqt_controller_manager")) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/rqt_controller_manager/rqt_controller_manager/update_combo.py b/rqt_controller_manager/rqt_controller_manager/update_combo.py index a838f28112..4a61736aa3 100644 --- a/rqt_controller_manager/rqt_controller_manager/update_combo.py +++ b/rqt_controller_manager/rqt_controller_manager/update_combo.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. + def update_combo(combo, new_vals): """ Update the contents of a combo box with a set of new values. @@ -33,7 +34,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_controller_manager/setup.py b/rqt_controller_manager/setup.py index 52ef39f0d0..beb73b5b8f 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -2,27 +2,27 @@ from setuptools import setup -package_name = 'rqt_controller_manager' +package_name = "rqt_controller_manager" setup( name=package_name, - version='0.0.0', + version="3.10.0", packages=[package_name], data_files=[ - ('share/ament_index/resource_index/packages', ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), ("share/" + package_name + "/resource", glob("resource/*.*")), ("share/" + package_name, ["plugin.xml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='Bence Magyar', - maintainer_email='bence.magyar.robotics@gmail.com', - description='Graphical frontend for interacting with the controller manager.', - license='Apache License, Version 2.0', - tests_require=['pytest'], + maintainer="Bence Magyar", + maintainer_email="bence.magyar.robotics@gmail.com", + description="Graphical frontend for interacting with the controller manager.", + license="Apache License, Version 2.0", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ + "console_scripts": [ "rqt_controller_manager = \ rqt_controller_manager.main:main", ], diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index b7a0b18533..201d3267f0 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,39 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.10.0 (2023-03-16) +------------------- +* Split transmission interfaces (`#938 `_) +* Contributors: Noel Jiménez García + +3.9.1 (2023-03-09) +------------------ +* Fix missing include (`#963 `_) +* Contributors: Bence Magyar + +3.9.0 (2023-02-28) +------------------ + +3.8.0 (2023-02-10) +------------------ +* Fix CMake install so overriding works (`#926 `_) +* Contributors: Tyler Weaver + +3.7.0 (2023-01-24) +------------------ + +3.6.0 (2023-01-12) +------------------ + +3.5.1 (2023-01-06) +------------------ + +3.5.0 (2022-12-06) +------------------ + +3.4.0 (2022-11-27) +------------------ + 3.3.0 (2022-11-15) ------------------ diff --git a/transmission_interface/CMakeLists.txt b/transmission_interface/CMakeLists.txt index bc5b8253de..8ae2cc224c 100644 --- a/transmission_interface/CMakeLists.txt +++ b/transmission_interface/CMakeLists.txt @@ -1,14 +1,14 @@ -cmake_minimum_required(VERSION 3.5) -project(transmission_interface) +cmake_minimum_required(VERSION 3.16) +project(transmission_interface LANGUAGES CXX) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Wpedantic) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS - hardware_interface - pluginlib - rclcpp + hardware_interface + pluginlib + rclcpp ) find_package(ament_cmake REQUIRED) @@ -16,83 +16,70 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -install( - DIRECTORY include/ - DESTINATION include -) - -add_library(${PROJECT_NAME} SHARED +add_library(transmission_interface SHARED src/simple_transmission_loader.cpp src/four_bar_linkage_transmission_loader.cpp src/differential_transmission_loader.cpp ) -target_include_directories(${PROJECT_NAME} PUBLIC include) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -pluginlib_export_plugin_description_file(${PROJECT_NAME} ros2_control_plugins.xml) - -install(TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib +target_include_directories(transmission_interface PUBLIC + $ + $ ) +ament_target_dependencies(transmission_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +pluginlib_export_plugin_description_file(transmission_interface ros2_control_plugins.xml) if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - ament_add_gmock( - test_simple_transmission + ament_add_gmock(test_simple_transmission test/simple_transmission_test.cpp ) - target_include_directories(test_simple_transmission PUBLIC include hardware_interface) - ament_target_dependencies(test_simple_transmission hardware_interface) + target_link_libraries(test_simple_transmission transmission_interface) - ament_add_gmock( - test_differential_transmission + ament_add_gmock(test_differential_transmission test/differential_transmission_test.cpp ) - target_include_directories(test_differential_transmission PUBLIC include hardware_interface) - ament_target_dependencies(test_differential_transmission hardware_interface) + target_link_libraries(test_differential_transmission transmission_interface) - ament_add_gmock( - test_four_bar_linkage_transmission + ament_add_gmock(test_four_bar_linkage_transmission test/four_bar_linkage_transmission_test.cpp ) - target_include_directories(test_four_bar_linkage_transmission PUBLIC include hardware_interface) - ament_target_dependencies(test_four_bar_linkage_transmission hardware_interface) + target_link_libraries(test_four_bar_linkage_transmission transmission_interface) - ament_add_gmock( - test_simple_transmission_loader + ament_add_gmock(test_simple_transmission_loader test/simple_transmission_loader_test.cpp ) - target_include_directories(test_simple_transmission_loader PUBLIC include hardware_interface) - ament_target_dependencies(test_simple_transmission_loader hardware_interface) + target_link_libraries(test_simple_transmission_loader transmission_interface) - ament_add_gmock( - test_four_bar_linkage_transmission_loader + ament_add_gmock(test_four_bar_linkage_transmission_loader test/four_bar_linkage_transmission_loader_test.cpp ) - target_include_directories(test_four_bar_linkage_transmission_loader PUBLIC include hardware_interface) - ament_target_dependencies(test_four_bar_linkage_transmission_loader hardware_interface) + target_link_libraries(test_four_bar_linkage_transmission_loader transmission_interface) - ament_add_gmock( - test_differential_transmission_loader + ament_add_gmock(test_differential_transmission_loader test/differential_transmission_loader_test.cpp ) - target_include_directories(test_differential_transmission_loader PUBLIC include hardware_interface) - ament_target_dependencies(test_differential_transmission_loader hardware_interface) + target_link_libraries(test_differential_transmission_loader transmission_interface) + + ament_add_gmock( + test_utils + test/utils_test.cpp + ) + target_include_directories(test_utils PUBLIC include hardware_interface) + ament_target_dependencies(test_utils hardware_interface) endif() -ament_export_include_directories( - include -) -ament_export_dependencies( - hardware_interface +install( + DIRECTORY include/ + DESTINATION include/transmission_interface ) - -ament_export_libraries( - ${PROJECT_NAME} +install(TARGETS transmission_interface + EXPORT export_transmission_interface + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) +ament_export_targets(export_transmission_interface HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() diff --git a/transmission_interface/include/transmission_interface/accessor.hpp b/transmission_interface/include/transmission_interface/accessor.hpp index f8d64671a0..dd58e55744 100644 --- a/transmission_interface/include/transmission_interface/accessor.hpp +++ b/transmission_interface/include/transmission_interface/accessor.hpp @@ -46,7 +46,7 @@ std::vector get_names(const std::vector & handles) std::set names; std::transform( handles.cbegin(), handles.cend(), std::inserter(names, names.end()), - [](const auto & handle) { return handle.get_name(); }); + [](const auto & handle) { return handle.get_prefix_name(); }); return std::vector(names.begin(), names.end()); } @@ -62,8 +62,8 @@ std::vector get_ordered_handles( unordered_handles.cbegin(), unordered_handles.cend(), std::back_inserter(result), [&](const auto & handle) { - return (handle.get_name() == name) && (handle.get_interface_name() == interface_type) && - handle; + return (handle.get_prefix_name() == name) && + (handle.get_interface_name() == interface_type) && handle; }); } return result; diff --git a/transmission_interface/include/transmission_interface/transmission_loader.hpp b/transmission_interface/include/transmission_interface/transmission_loader.hpp index c563a5fae6..962ba90d16 100644 --- a/transmission_interface/include/transmission_interface/transmission_loader.hpp +++ b/transmission_interface/include/transmission_interface/transmission_loader.hpp @@ -19,6 +19,8 @@ #include "hardware_interface/hardware_info.hpp" +#include "transmission_interface/transmission.hpp" + namespace transmission_interface { /** diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 8d7ac65cbb..869eff00f7 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.3.0 + 3.10.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 diff --git a/transmission_interface/test/utils_test.cpp b/transmission_interface/test/utils_test.cpp new file mode 100644 index 0000000000..9afaccaa47 --- /dev/null +++ b/transmission_interface/test/utils_test.cpp @@ -0,0 +1,36 @@ +// Copyright (c) 2023 PAL Robotics S.L. All rights reserved. +// +// 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. + +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +#include "transmission_interface/accessor.hpp" +#include "transmission_interface/handle.hpp" + +using hardware_interface::HW_IF_POSITION; +using transmission_interface::JointHandle; + +TEST(UtilsTest, AccessorTest) +{ + const std::string NAME = "joint"; + double joint_value = 0.0; + const JointHandle joint_handle(NAME, HW_IF_POSITION, &joint_value); + const std::vector joint_handles = {joint_handle}; + + ASSERT_EQ(transmission_interface::get_names(joint_handles), std::vector{NAME}); + ASSERT_EQ( + transmission_interface::get_ordered_handles(joint_handles, {NAME}, HW_IF_POSITION), + joint_handles); +}