diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml deleted file mode 100644 index b546520fbe..0000000000 --- a/.github/workflows/ci-coverage-build.yml +++ /dev/null @@ -1,53 +0,0 @@ -name: Coverage Build -on: - workflow_dispatch: - push: - branches: - - iron - pull_request: - branches: - - iron - -jobs: - coverage: - name: coverage build - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - env: - ROS_DISTRO: iron - steps: - - uses: ros-tooling/setup-ros@0.7.1 - with: - required-ros-distributions: ${{ env.ROS_DISTRO }} - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-ci@0.3.6 - with: - target-ros2-distro: ${{ env.ROS_DISTRO }} - import-token: ${{ secrets.GITHUB_TOKEN }} - # build all packages listed in the meta package - package-name: - controller_interface - controller_manager - hardware_interface - hardware_interface_testing - transmission_interface - - vcs-repo-file-url: | - https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/ros2_control-not-released.${{ env.ROS_DISTRO }}.repos?token=${{ secrets.GITHUB_TOKEN }} - colcon-defaults: | - { - "build": { - "mixin": ["coverage-gcc"] - } - } - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml - - uses: codecov/codecov-action@v4.0.1 - with: - file: ros_ws/lcov/total_coverage.info - flags: unittests - name: codecov-umbrella - - uses: actions/upload-artifact@v4.3.1 - with: - name: colcon-logs-ubuntu-22.04-coverage-iron - path: ros_ws/log diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml deleted file mode 100644 index 569bb95e24..0000000000 --- a/.github/workflows/ci-format.yml +++ /dev/null @@ -1,23 +0,0 @@ -# This is a format job. Pre-commit has a first-party GitHub action, so we use -# that: https://github.com/pre-commit/action - -name: Format - -on: - workflow_dispatch: - pull_request: - -jobs: - pre-commit: - name: Format - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5.0.0 - with: - python-version: '3.10' - - name: Install system hooks - run: sudo apt install -qq cppcheck - - uses: pre-commit/action@v3.0.1 - with: - extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml deleted file mode 100644 index d563d313db..0000000000 --- a/.github/workflows/ci-ros-lint.yml +++ /dev/null @@ -1,57 +0,0 @@ -name: ROS Lint -on: - pull_request: - -jobs: - ament_lint: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - matrix: - linter: [cppcheck, copyright, lint_cmake] - env: - AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true - steps: - - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.1 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: iron - linter: ${{ matrix.linter }} - package-name: - controller_interface - controller_manager - controller_manager_msgs - hardware_interface - hardware_interface_testing - ros2controlcli - ros2_control - ros2_control_test_assets - transmission_interface - - ament_lint_100: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-22.04 - strategy: - fail-fast: false - matrix: - linter: [cpplint] - steps: - - uses: actions/checkout@v4 - - uses: ros-tooling/setup-ros@0.7.1 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: iron - linter: cpplint - arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: - controller_interface - controller_manager - controller_manager_msgs - hardware_interface - hardware_interface_testing - ros2controlcli - ros2_control - ros2_control_test_assets - transmission_interface diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml new file mode 100644 index 0000000000..0910572227 --- /dev/null +++ b/.github/workflows/humble-coverage-build.yml @@ -0,0 +1,17 @@ +name: Coverage Build - Humble +on: + workflow_dispatch: + push: + branches: + - humble + pull_request: + branches: + - humble + +jobs: + coverage_humble: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: humble + os_name: ubuntu-22.04 diff --git a/.github/workflows/humble-pre-commit.yml b/.github/workflows/humble-pre-commit.yml new file mode 100644 index 0000000000..be8c84b05b --- /dev/null +++ b/.github/workflows/humble-pre-commit.yml @@ -0,0 +1,14 @@ +name: Pre-Commit - Humble + +on: + workflow_dispatch: + pull_request: + branches: + - humble + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: humble + os_name: ubuntu-22.04 diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml new file mode 100644 index 0000000000..d82c52bf51 --- /dev/null +++ b/.github/workflows/iron-coverage-build.yml @@ -0,0 +1,17 @@ +name: Coverage Build - Iron +on: + workflow_dispatch: + push: + branches: + - iron + pull_request: + branches: + - iron + +jobs: + coverage_iron: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: iron + os_name: ubuntu-22.04 diff --git a/.github/workflows/iron-pre-commit.yml b/.github/workflows/iron-pre-commit.yml new file mode 100644 index 0000000000..60ad26d073 --- /dev/null +++ b/.github/workflows/iron-pre-commit.yml @@ -0,0 +1,14 @@ +name: Pre-Commit - Iron + +on: + workflow_dispatch: + pull_request: + branches: + - iron + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: iron + os_name: ubuntu-22.04 diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml new file mode 100644 index 0000000000..4d4750c54c --- /dev/null +++ b/.github/workflows/rolling-coverage-build.yml @@ -0,0 +1,17 @@ +name: Coverage Build - Rolling +on: + workflow_dispatch: + push: + branches: + - master + pull_request: + branches: + - master + +jobs: + coverage_rolling: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-build-coverage.yml@master + secrets: inherit + with: + ros_distro: rolling + os_name: ubuntu-22.04 diff --git a/.github/workflows/rolling-pre-commit.yml b/.github/workflows/rolling-pre-commit.yml new file mode 100644 index 0000000000..9c87311bd7 --- /dev/null +++ b/.github/workflows/rolling-pre-commit.yml @@ -0,0 +1,14 @@ +name: Pre-Commit - Rolling + +on: + workflow_dispatch: + pull_request: + branches: + - master + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: rolling + os_name: ubuntu-22.04 diff --git a/.github/workflows/update-pre-commit.yml b/.github/workflows/update-pre-commit.yml new file mode 100644 index 0000000000..8b9545dff1 --- /dev/null +++ b/.github/workflows/update-pre-commit.yml @@ -0,0 +1,12 @@ +name: Auto Update pre-commit +# Update pre-commit config and create PR if changes are detected +# author: Christoph Fröhlich + +on: + workflow_dispatch: + schedule: + - cron: '0 0 * * 0' # Run every Sunday at midnight + +jobs: + auto_update_and_create_pr: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-update-pre-commit.yml@master diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 0ad4d0aa6c..6da427c6ee 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.4.0 + rev: v4.5.0 hooks: - id: check-added-large-files - id: check-ast @@ -36,7 +36,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.4.0 + rev: v3.15.1 hooks: - id: pyupgrade args: [--py36-plus] @@ -49,40 +49,38 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 23.3.0 + rev: 24.2.0 hooks: - id: black args: ["--line-length=99"] - repo: https://github.com/pycqa/flake8 - rev: 6.0.0 + rev: 7.0.0 hooks: - id: flake8 args: ["--extend-ignore=E501"] # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.6 + rev: v17.0.6 hooks: - id: clang-format + args: ['-fallback-style=none', '-i'] - repo: local hooks: - id: ament_cppcheck name: ament_cppcheck description: Static code analysis of C/C++ files. - stages: [commit] - entry: ament_cppcheck + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ - # Maybe use https://github.com/cpplint/cpplint instead - repo: local hooks: - id: ament_cpplint name: ament_cpplint description: Static code analysis of C/C++ files. - stages: [commit] entry: ament_cpplint language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -94,7 +92,6 @@ repos: - id: ament_lint_cmake name: ament_lint_cmake description: Check format of CMakeLists.txt files. - stages: [commit] entry: ament_lint_cmake language: system files: CMakeLists\.txt$ @@ -105,7 +102,6 @@ repos: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. - stages: [commit] entry: ament_copyright language: system @@ -128,8 +124,18 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.4 + rev: v2.2.6 hooks: - id: codespell - args: ['--write-changes'] - exclude: CHANGELOG\.rst|\.(svg|pyc)$ + args: ['--write-changes', '--uri-ignore-words-list=ist', '-L manuel'] + exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ + + - repo: https://github.com/python-jsonschema/check-jsonschema + rev: 0.28.0 + hooks: + - id: check-github-workflows + args: ["--verbose"] + - id: check-github-actions + args: ["--verbose"] + - id: check-dependabot + args: ["--verbose"] diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index d9cdc27041..df91cfbf20 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -54,7 +54,9 @@ As this project, by default, uses the default GitHub issue labels ## Licensing -Any contribution that you make to this repository will be under the Apache 2 License, as dictated by that [license]: +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): ~~~ 5. Submission of Contributions. Unless You explicitly state otherwise, @@ -69,4 +71,3 @@ Any contribution that you make to this repository will be under the Apache 2 Lic [issues]: https://github.com/ros-controls/ros2_control/issues [closed-issues]: https://github.com/ros-controls/ros2_control/issues?utf8=%E2%9C%93&q=is%3Aissue%20is%3Aclosed%20 [help-wanted]: https://github.com/ros-controls/ros2_control/issues?q=is%3Aopen+is%3Aissue+label%3A%22help+wanted%22 -[license]: http://www.apache.org/licenses/LICENSE-2.0.html diff --git a/controller_manager/doc/controller_chaining.rst b/controller_manager/doc/controller_chaining.rst index d79e730d3f..e9763630d0 100644 --- a/controller_manager/doc/controller_chaining.rst +++ b/controller_manager/doc/controller_chaining.rst @@ -76,7 +76,7 @@ One can also think of it as an actual chain, you can not add a chain link or bre Debugging outputs ---------------------------- -Flag ``unavailable`` on reference interface does not provide much information about anything at the moment. So don't get confused by it. The reason we have it are internal implementation reasons irelevant for the usage. +Flag ``unavailable`` on reference interface does not provide much information about anything at the moment. So don't get confused by it. The reason we have it are internal implementation reasons irrelevant for the usage. Closing remarks diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9340c61163..219f710064 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -341,7 +341,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & RCLCPP_INFO(get_logger(), "Received robot description from topic."); RCLCPP_DEBUG( get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); - // TODO(Manuel): errors should probably be caught since we don't want controller_manager node + // TODO(mamueluth): errors should probably be caught since we don't want controller_manager node // to die if a non valid urdf is passed. However, should maybe be fine tuned. try { diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index db5b98a5c1..2d10117fc9 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -88,7 +88,7 @@ class ControllerManagerFixture : public ::testing::Test } else { - // TODO(Manuel) : passing via topic not working in test setup, tested cm does + // TODO(mamueluth) : passing via topic not working in test setup, tested cm does // not receive msg. Have to check this... // this is just a workaround to skip passing diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 16ca710e9d..da8258c643 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -27,9 +27,7 @@ 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); @@ -38,9 +36,7 @@ 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/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index 82467514a3..aa4f540149 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -8,6 +8,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle + urdf ) find_package(ament_cmake REQUIRED) @@ -31,6 +32,9 @@ if(BUILD_TESTING) ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) target_link_libraries(joint_limits_rosparam_test joint_limits) + ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp) + target_link_libraries(joint_limits_urdf_test joint_limits) + add_launch_test(test/joint_limits_rosparam.launch.py) install( TARGETS joint_limits_rosparam_test diff --git a/joint_limits/include/joint_limits/joint_limits_urdf.hpp b/joint_limits/include/joint_limits/joint_limits_urdf.hpp new file mode 100644 index 0000000000..cdcbaf9c9d --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limits_urdf.hpp @@ -0,0 +1,85 @@ +// Copyright 2024 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ +#define JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ + +#include "joint_limits/joint_limits.hpp" +#include "urdf_model/joint.h" + +namespace joint_limits +{ + +/** + * \brief Populate a JointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will + * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged. + * \return True if \e urdf_joint has a valid limits specification, false otherwise. + */ +inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) +{ + if (!urdf_joint || !urdf_joint->limits) + { + return false; + } + + limits.has_position_limits = + urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC; + if (limits.has_position_limits) + { + limits.min_position = urdf_joint->limits->lower; + limits.max_position = urdf_joint->limits->upper; + } + + if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS) + { + limits.angle_wraparound = true; + } + + limits.has_velocity_limits = true; + limits.max_velocity = std::abs(urdf_joint->limits->velocity); + + limits.has_acceleration_limits = false; + + limits.has_effort_limits = true; + limits.max_effort = std::abs(urdf_joint->limits->effort); + + return true; +} + +/** + * \brief Populate a SoftJointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] soft_limits Where URDF soft joint limit data gets written into. + * \return True if \e urdf_joint has a valid soft limits specification, false otherwise. + */ +inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits) +{ + if (!urdf_joint || !urdf_joint->safety) + { + return false; + } + + soft_limits.min_position = urdf_joint->safety->soft_lower_limit; + soft_limits.max_position = urdf_joint->safety->soft_upper_limit; + soft_limits.k_position = urdf_joint->safety->k_position; + soft_limits.k_velocity = urdf_joint->safety->k_velocity; + + return true; +} +} // namespace joint_limits +#endif // JOINT_LIMITS__JOINT_LIMITS_URDF_HPP_ diff --git a/joint_limits/package.xml b/joint_limits/package.xml index ca90152c05..b96bde3575 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_lifecycle + urdf launch_testing_ament_cmake ament_cmake_gtest diff --git a/joint_limits/test/joint_limits_urdf_test.cpp b/joint_limits/test/joint_limits_urdf_test.cpp new file mode 100644 index 0000000000..562293d475 --- /dev/null +++ b/joint_limits/test/joint_limits_urdf_test.cpp @@ -0,0 +1,174 @@ +// Copyright 2024 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian +#include "joint_limits/joint_limits_urdf.hpp" +#include "gtest/gtest.h" + +using std::string; + +class JointLimitsUrdfTest : public ::testing::Test +{ +public: + JointLimitsUrdfTest() + { + urdf_limits.reset(new urdf::JointLimits); + urdf_limits->effort = 8.0; + urdf_limits->velocity = 2.0; + urdf_limits->lower = -1.0; + urdf_limits->upper = 1.0; + + urdf_safety.reset(new urdf::JointSafety); + urdf_safety->k_position = 20.0; + urdf_safety->k_velocity = 40.0; + urdf_safety->soft_lower_limit = -0.8; + urdf_safety->soft_upper_limit = 0.8; + + urdf_joint.reset(new urdf::Joint); + urdf_joint->limits = urdf_limits; + urdf_joint->safety = urdf_safety; + + urdf_joint->type = urdf::Joint::UNKNOWN; + } + +protected: + urdf::JointLimitsSharedPtr urdf_limits; + urdf::JointSafetySharedPtr urdf_safety; + urdf::JointSharedPtr urdf_joint; +}; + +TEST_F(JointLimitsUrdfTest, GetJointLimits) +{ + // Unset URDF joint + { + joint_limits::JointLimits limits; + urdf::JointSharedPtr urdf_joint_bad; + EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); + } + + // Unset URDF limits + { + joint_limits::JointLimits limits; + urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); + EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); + } + + // Valid URDF joint, CONTINUOUS type + { + urdf_joint->type = urdf::Joint::CONTINUOUS; + + joint_limits::JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } + + // Valid URDF joint, REVOLUTE type + { + urdf_joint->type = urdf::Joint::REVOLUTE; + + joint_limits::JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_TRUE(limits.has_position_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); + EXPECT_FALSE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } + + // Valid URDF joint, PRISMATIC type + { + urdf_joint->type = urdf::Joint::PRISMATIC; + + joint_limits::JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_TRUE(limits.has_position_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); + EXPECT_FALSE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } +} + +TEST_F(JointLimitsUrdfTest, GetSoftJointLimits) +{ + // Unset URDF joint + { + joint_limits::SoftJointLimits soft_limits; + urdf::JointSharedPtr urdf_joint_bad; + EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); + } + + // Unset URDF limits + { + joint_limits::SoftJointLimits soft_limits; + urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); + EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); + } + + // Valid URDF joint + { + joint_limits::SoftJointLimits soft_limits; + EXPECT_TRUE(getSoftJointLimits(urdf_joint, soft_limits)); + + // Soft limits + EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_lower_limit, soft_limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_upper_limit, soft_limits.max_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->k_position, soft_limits.k_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->k_velocity, soft_limits.k_velocity); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/joint_limits_interface/test/joint_limits_urdf_test.cpp b/joint_limits_interface/test/joint_limits_urdf_test.cpp index 55effc7117..24a943ebc3 100644 --- a/joint_limits_interface/test/joint_limits_urdf_test.cpp +++ b/joint_limits_interface/test/joint_limits_urdf_test.cpp @@ -15,11 +15,10 @@ /// \author Adolfo Rodriguez Tsouroukdissian #include +#include #include -#include - class JointLimitsUrdfTest : public ::testing::Test { public: diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index ca73e7afef..31ce814865 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -13,6 +13,7 @@ # limitations under the License. from controller_manager import list_controllers +from controller_manager.spawner import bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -22,7 +23,15 @@ def print_controller_state(c, args): - print(f"{c.name:20s}[{c.type:20s}] {c.state:10s}") + state_color = "" + if c.state == "active": + state_color = bcolors.OKGREEN + elif c.state == "inactive": + state_color = bcolors.OKCYAN + elif c.state == "unconfigured": + state_color = bcolors.WARNING + + print(f"{c.name:20s}[{c.type:20s}] {state_color}{c.state:10s}{bcolors.ENDC}") if args.claimed_interfaces or args.verbose: print("\tclaimed interfaces:") for claimed_interface in c.claimed_interfaces: diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index e9930bcb8a..a6dfb86a55 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -1,3 +1,17 @@ +# Copyright 2024 Apache License, Version 2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + from glob import glob from setuptools import setup