From 89292e3b29df240371fcf376f5b8d939388da87c Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Sun, 21 Jul 2024 21:12:10 +0200 Subject: [PATCH 01/42] Create debugging.rst (#1625) --------- Co-authored-by: Sai Kishor Kothakota Co-authored-by: Christoph Froehlich --- doc/debugging.rst | 80 +++++++++++++++++++++++++++++++++++++++++++++++ doc/index.rst | 9 ++++++ 2 files changed, 89 insertions(+) create mode 100644 doc/debugging.rst diff --git a/doc/debugging.rst b/doc/debugging.rst new file mode 100644 index 00000000000..5349c98a52b --- /dev/null +++ b/doc/debugging.rst @@ -0,0 +1,80 @@ +Debugging +^^^^^^^^^ + +All controllers and hardware components are plugins loaded into the ``controller_manager``. Therefore, the debugger must be attached to the ``controller_manager``. If multiple ``controller_manager`` instances are running on your robot or machine, you need to attach the debugger to the ``controller_manager`` associated with the hardware component or controller you want to debug. + +How-To +****************** + +* Install ``xterm``, ``gdb`` and ``gdbserver`` on your system + + .. code-block:: bash + + sudo apt install xterm gdb gdbserver + +* Make sure you run a "debug" or "release with debug information" build: + This is done by passing ``--cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo`` to ``colcon build``. + Remember that in release builds some breakpoints might not behave as you expect as the the corresponding line might have been optimized by the compiler. For such cases, a full Debug build (``--cmake-args -DCMAKE_BUILD_TYPE=Debug``) is recommended. + +* Adapt the launch file to run the controller manager with the debugger attached: + + * Version A: Run it directly with the gdb CLI: + + Add ``prefix=['xterm -e gdb -ex run --args']`` to the ``controller_manager`` node entry in your launch file. + Due to how ``ros2launch`` works we need to run the specific node in a separate terminal instance. + + * Version B: Run it with gdbserver: + + Add ``prefix=['gdbserver localhost:3000']`` to the ``controller_manager`` node entry in your launch file. + Afterwards, you can either attach a gdb CLI instance or any IDE of your choice to that ``gdbserver`` instance. + Ensure you start your debugger from a terminal where you have sourced your workspace to properly resolve all paths. + + Example launch file entry: + + .. code-block:: python + + # Obtain the controller config file for the ros2 control node + controller_config_file = get_package_file("", "config/controllers.yaml") + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[controller_config_file], + output="both", + emulate_tty=True, + remappings=[ + ("~/robot_description", "/robot_description") + ], + prefix=['xterm -e gdb -ex run --args'] # or prefix=['gdbserver localhost:3000'] + ) + + ld.add_action(controller_manager) + + +Additional notes +***************** + +* Debugging plugins + + You can only set breakpoints in plugins after the plugin has been loaded. In the ros2_control context this means after the controller / hardware component has been loaded: + +* Debug builds + + It's often practical to include debug information only for the specific package you want to debug. + ``colcon build --packages-select [package_name] --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo`` or ``colcon build --packages-select [package_name] --cmake-args -DCMAKE_BUILD_TYPE=Debug`` + +* Realtime + +.. warning:: + The ``update/on_activate/on_deactivate`` method of a controller and the ``read/write/on_activate/perform_command_mode_switch`` methods of a hardware component all run in the context of the realtime update loop. Setting breakpoints there can and will cause issues that might even break your hardware in the worst case. + +From experience, it might be better to use meaningful logs for the real-time context (with caution) or to add additional debug state interfaces (or publishers in the case of a controller). + +However, running the controller_manager and your plugin with gdb can still be very useful for debugging errors such as segfaults, as you can gather a full backtrace. + +References +*********** + +* `ROS 2 and GDB `_ +* `Using GDB to debug a plugin `_ +* `GDB CLI Tutorial `_ diff --git a/doc/index.rst b/doc/index.rst index b1d8e21c7d4..09a2ddf745e 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -28,3 +28,12 @@ Concepts Joint Kinematics <../hardware_interface/doc/joints_userdoc.rst> Hardware Components <../hardware_interface/doc/hardware_components_userdoc.rst> Mock Components <../hardware_interface/doc/mock_components_userdoc.rst> + +===================================== +Guidelines and Best Practices +===================================== + +.. toctree:: + :titlesonly: + + Debugging the Controller Manager and Plugins From 65cd1c558c033315744273c3a7c1dc6110da6f51 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 23 Jul 2024 17:22:09 +0100 Subject: [PATCH 02/42] Update changelogs --- controller_interface/CHANGELOG.rst | 6 ++++++ controller_manager/CHANGELOG.rst | 8 ++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 6 ++++++ hardware_interface_testing/CHANGELOG.rst | 6 ++++++ joint_limits/CHANGELOG.rst | 5 +++++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 5 +++++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 5 +++++ 11 files changed, 53 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index c4f9c56800c..cfab8cbd5da 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1627 `_) +* move critical variables to the private context (`#1623 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.13.0 (2024-07-08) ------------------- * [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 1ef5eef03b4..e0fd6683241 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1627 `_) +* Remove noqa (`#1626 `_) +* Fix controller starting with later load of robot description test (`#1624 `_) +* [CM] Remove support for the description parameter and use only `robot_description` topic (`#1358 `_) +* Contributors: Christoph Fröhlich, Dr. Denis, Henry Moore, Sai Kishor Kothakota + 4.13.0 (2024-07-08) ------------------- * Change the spamming checking interface ERROR to DEBUG (`#1605 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index f6d6fad69ed..9aa42461209 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-07-08) ------------------- * [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 76ddab43ccc..b6299dec205 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1627 `_) +* [ResourceManager] Make destructor virtual for use in derived classes (`#1607 `_) +* Contributors: Henry Moore, Sai Kishor Kothakota + 4.13.0 (2024-07-08) ------------------- * [ResourceManager] Propagate access to logger and clock interfaces to HardwareComponent (`#1585 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 520f28540d2..704ac5489c5 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1627 `_) +* Fix typos in test_resource_manager.cpp (`#1609 `_) +* Contributors: Henry Moore, Parker Drouillard + 4.13.0 (2024-07-08) ------------------- * [ResourceManager] Propagate access to logger and clock interfaces to HardwareComponent (`#1585 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 5fea915b6bb..5492d970c4b 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + 4.13.0 (2024-07-08) ------------------- * [JointLimits] Add Saturation Joint Limiter that uses clamping method (`#971 `_) diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 08617611888..de04e8b33a9 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-07-08) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 7dac8557943..fe978731db5 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + 4.13.0 (2024-07-08) ------------------- * [ControllerChaining] Export state interfaces from chainable controllers (`#1021 `_) diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 7ed24372b02..76b20b24b3c 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-07-08) ------------------- * Remove ament linters (`#1601 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 065861ca072..203d4169f7b 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.13.0 (2024-07-08) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 63bd6aeb891..e02f73ea8ce 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Unused header cleanup (`#1627 `_) +* Contributors: Henry Moore + 4.13.0 (2024-07-08) ------------------- From b3852dad595511250b5f96f6a3c4cc5eb91cf3b4 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 23 Jul 2024 17:22:31 +0100 Subject: [PATCH 03/42] 4.14.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index cfab8cbd5da..c8a69a2d507 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- * Unused header cleanup (`#1627 `_) * move critical variables to the private context (`#1623 `_) * Contributors: Henry Moore, Sai Kishor Kothakota diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 1ee83bd0190..30137a23e40 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.13.0 + 4.14.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index e0fd6683241..397ed8b5ca9 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- * Unused header cleanup (`#1627 `_) * Remove noqa (`#1626 `_) * Fix controller starting with later load of robot description test (`#1624 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index c8da38364a9..84a676a483b 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.13.0 + 4.14.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 9aa42461209..9e210bc4e78 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- 4.13.0 (2024-07-08) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index b0bd54a0a7d..143117f5128 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.13.0 + 4.14.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index b6299dec205..37f8b107959 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- * Unused header cleanup (`#1627 `_) * [ResourceManager] Make destructor virtual for use in derived classes (`#1607 `_) * Contributors: Henry Moore, Sai Kishor Kothakota diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index a3a1d6c77ad..c5f09e74bdb 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.13.0 + 4.14.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 704ac5489c5..b0a25e3835f 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- * Unused header cleanup (`#1627 `_) * Fix typos in test_resource_manager.cpp (`#1609 `_) * Contributors: Henry Moore, Parker Drouillard diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index f65de8598d0..7594d8e32ce 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.13.0 + 4.14.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 5492d970c4b..da9bd28fed3 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- * Unused header cleanup (`#1627 `_) * Contributors: Henry Moore diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 6e8f35255db..96095228383 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.13.0 + 4.14.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index de04e8b33a9..3c38360f508 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- 4.13.0 (2024-07-08) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index c7687e3cd6b..501c543ccbb 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.13.0 + 4.14.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 fe978731db5..a61a3273f73 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- * Unused header cleanup (`#1627 `_) * Contributors: Henry Moore diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 1912391b5a4..b6192e8c452 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.13.0 + 4.14.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 76b20b24b3c..b1d9dcd002a 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- 4.13.0 (2024-07-08) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index d1693cdf2a1..9adae5d2b54 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.13.0 + 4.14.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 5205c0f0241..0a19536fe57 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.13.0", + version="4.14.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 203d4169f7b..b5f99605750 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- 4.13.0 (2024-07-08) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index e4356b90fe9..e678b1f37a5 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.13.0 + 4.14.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 69d38c7273e..bbd9e75dcc4 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.13.0", + version="4.14.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index e02f73ea8ce..694f4c72cff 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.14.0 (2024-07-23) +------------------- * Unused header cleanup (`#1627 `_) * Contributors: Henry Moore diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 726ddc70cc4..5740c2c68f0 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.13.0 + 4.14.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 From 3bc4e456dd4846a277b8f898183d30d0c9f56680 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 23 Jul 2024 18:31:49 +0200 Subject: [PATCH 04/42] add missing rclcpp logging include for Humble compatibility build (#1635) --- hardware_interface/src/resource_manager.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 4c990526fc6..5b13e5d3318 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -35,6 +35,7 @@ #include "hardware_interface/system_interface.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "pluginlib/class_loader.hpp" +#include "rclcpp/logging.hpp" #include "rcutils/logging_macros.h" namespace hardware_interface From 3eae727915bf40938f64ab41ac6245a05f58f352 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 27 Jul 2024 12:31:57 +0200 Subject: [PATCH 05/42] [CM] Remove deprecated spawner args (#1639) --- .../controller_manager/launch_utils.py | 17 +++--------- .../controller_manager/spawner.py | 26 +++++-------------- .../test/test_spawner_unspawner.cpp | 16 ------------ doc/release_notes.rst | 1 + 4 files changed, 10 insertions(+), 50 deletions(-) diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 14d0ad68d14..3bbb050433a 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import warnings from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PythonExpression @@ -21,7 +20,7 @@ def generate_load_controller_launch_description( - controller_name, controller_type=None, controller_params_file=None, extra_spawner_args=[] + controller_name, controller_params_file=None, extra_spawner_args=[] ): """ Generate launch description for loading a controller using spawner. @@ -32,13 +31,12 @@ def generate_load_controller_launch_description( Examples -------- - # Assuming the controller type and controller parameters are known to the controller_manager + # Assuming the controller parameters are known to the controller_manager generate_load_controller_launch_description('joint_state_broadcaster') - # Passing controller type and controller parameter file to load + # Passing controller parameter file to load the controller (Controller type is retrieved from config file) generate_load_controller_launch_description( 'joint_state_broadcaster', - controller_type='joint_state_broadcaster/JointStateBroadcaster', controller_params_file=os.path.join(get_package_share_directory('my_pkg'), 'config', 'controller_params.yaml'), extra_spawner_args=[--load-only] @@ -62,15 +60,6 @@ def generate_load_controller_launch_description( LaunchConfiguration("controller_manager_name"), ] - if controller_type: - warnings.warn( - "The 'controller_type' argument is deprecated and will be removed in future releases." - " Declare the controller type parameter in the param file instead.", - DeprecationWarning, - stacklevel=2, - ) - spawner_arguments += ["--controller-type", controller_type] - if controller_params_file: spawner_arguments += ["--param-file", controller_params_file] diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index dbefd360b94..2778bddebd1 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -166,6 +166,7 @@ def main(args=None): "-p", "--param-file", help="Controller param file to be loaded into controller node before configure", + default=None, required=False, ) parser.add_argument( @@ -183,13 +184,6 @@ def main(args=None): action="store_true", required=False, ) - parser.add_argument( - "-t", - "--controller-type", - help="If not provided it should exist in the controller manager namespace (deprecated)", - default=None, - required=False, - ) parser.add_argument( "-u", "--unload-on-kill", @@ -227,14 +221,6 @@ def main(args=None): param_file = args.param_file controller_manager_timeout = args.controller_manager_timeout - if args.controller_type: - warnings.filterwarnings("always") - warnings.warn( - "The '--controller-type' argument is deprecated and will be removed in future releases." - " Declare the controller type parameter in the param file instead.", - DeprecationWarning, - ) - if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) @@ -258,7 +244,6 @@ def main(args=None): for controller_name in controller_names: fallback_controllers = args.fallback_controllers - controller_type = args.controller_type prefixed_controller_name = controller_name if controller_namespace: prefixed_controller_name = controller_namespace + "/" + controller_name @@ -270,10 +255,11 @@ def main(args=None): + bcolors.ENDC ) else: - if not controller_type and param_file: - controller_type = get_parameter_from_param_file( - controller_name, param_file, "type" - ) + controller_type = ( + None + if param_file is None + else get_parameter_from_param_file(controller_name, param_file, "type") + ) if controller_type: parameter = Parameter() parameter.name = prefixed_controller_name + ".type" diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 7680d8270cf..a7c24329598 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -244,22 +244,6 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) } } -TEST_F(TestLoadController, spawner_test_type_in_arg) -{ - // Provide controller type via -t argument - EXPECT_EQ( - call_spawner( - "ctrl_2 -c test_controller_manager -t " + - std::string(test_controller::TEST_CONTROLLER_CLASS_NAME)), - 0); - - ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); - auto ctrl_2 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); - ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); -} - TEST_F(TestLoadController, spawner_test_type_in_params_file) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + diff --git a/doc/release_notes.rst b/doc/release_notes.rst index a4a6f317341..e2b22bf7a62 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -69,6 +69,7 @@ controller_manager The parameters within the ``ros2_control`` tag are not supported any more. * The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). +* The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). hardware_interface ****************** From 872029cad3a363cf723429f85611df78c7c5af8b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 27 Jul 2024 12:32:46 +0200 Subject: [PATCH 06/42] Add a pytest launch file to test ros2_control_node (#1636) --- controller_manager/CMakeLists.txt | 5 + controller_manager/package.xml | 1 + .../test/test_ros2_control_node.yaml | 11 +++ .../test/test_ros2_control_node_launch.py | 95 +++++++++++++++++++ 4 files changed, 112 insertions(+) create mode 100644 controller_manager/test/test_ros2_control_node.yaml create mode 100644 controller_manager/test/test_ros2_control_node_launch.py diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 7c437d35e01..c4ef0e7df0c 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -209,6 +209,11 @@ if(BUILD_TESTING) ament_target_dependencies(test_hardware_management_srvs controller_manager_msgs ) + + find_package(ament_cmake_pytest REQUIRED) + install(FILES test/test_ros2_control_node.yaml + DESTINATION test) + ament_add_pytest_test(test_ros2_control_node test/test_ros2_control_node_launch.py) endif() install( diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 84a676a483b..f969b768349 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -30,6 +30,7 @@ std_msgs ament_cmake_gmock + ament_cmake_pytest hardware_interface_testing ros2_control_test_assets diff --git a/controller_manager/test/test_ros2_control_node.yaml b/controller_manager/test/test_ros2_control_node.yaml new file mode 100644 index 00000000000..ce0602d6b38 --- /dev/null +++ b/controller_manager/test/test_ros2_control_node.yaml @@ -0,0 +1,11 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + +ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint0"] + +joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster diff --git a/controller_manager/test/test_ros2_control_node_launch.py b/controller_manager/test/test_ros2_control_node_launch.py new file mode 100644 index 00000000000..c8c11368491 --- /dev/null +++ b/controller_manager/test/test_ros2_control_node_launch.py @@ -0,0 +1,95 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import pytest +import unittest +import time + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_testing.actions import ReadyToTest + +import launch_testing.markers +import rclpy +import launch_ros.actions +from rclpy.node import Node + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.launch_test +def generate_test_description(): + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("controller_manager"), + "test", + "test_ros2_control_node.yaml", + ] + ) + + control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + ) + + return LaunchDescription([control_node, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self): + start = time.time() + found = False + while time.time() - start < 2.0 and not found: + found = "controller_manager" in self.node.get_node_names() + time.sleep(0.1) + assert found, "controller_manager not found!" + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shutdown. +class TestDescriptionCraneShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) From 742a9fd9c38a916eb8c7629b25e4b4bdfe9fa084 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 29 Jul 2024 14:30:49 +0200 Subject: [PATCH 07/42] Fix rst markup (#1642) * Fix rqt_cm paragraph * Fix indent --- controller_manager/doc/userdoc.rst | 6 +++--- doc/debugging.rst | 7 ++++--- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 02e532866d4..676d877319c 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -190,14 +190,14 @@ A GUI tool to interact with the controller manager services to be able to switch .. image:: images/rqt_controller_manager.png -It can be launched independently using the following command or as rqt plugin. +It can be launched independently using the following command or as rqt plugin: .. code-block:: console ros2 run rqt_controller_manager rqt_controller_manager - * Double-click on a controller or hardware component to show the additional info. - * Right-click on a controller or hardware component to show a context menu with options for lifecycle management. +* Double-click on a controller or hardware component to show the additional info. +* Right-click on a controller or hardware component to show a context menu with options for lifecycle management. Using the Controller Manager in a Process ----------------------------------------- diff --git a/doc/debugging.rst b/doc/debugging.rst index 5349c98a52b..76f288e49d9 100644 --- a/doc/debugging.rst +++ b/doc/debugging.rst @@ -65,10 +65,11 @@ Additional notes * Realtime -.. warning:: - The ``update/on_activate/on_deactivate`` method of a controller and the ``read/write/on_activate/perform_command_mode_switch`` methods of a hardware component all run in the context of the realtime update loop. Setting breakpoints there can and will cause issues that might even break your hardware in the worst case. + .. warning:: -From experience, it might be better to use meaningful logs for the real-time context (with caution) or to add additional debug state interfaces (or publishers in the case of a controller). + The ``update/on_activate/on_deactivate`` method of a controller and the ``read/write/on_activate/perform_command_mode_switch`` methods of a hardware component all run in the context of the realtime update loop. Setting breakpoints there can and will cause issues that might even break your hardware in the worst case. + + From experience, it might be better to use meaningful logs for the real-time context (with caution) or to add additional debug state interfaces (or publishers in the case of a controller). However, running the controller_manager and your plugin with gdb can still be very useful for debugging errors such as segfaults, as you can gather a full backtrace. From df43a1cd52266b2f40d91dca572f9712c1fe1f88 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 29 Jul 2024 19:12:41 +0200 Subject: [PATCH 08/42] CM: Add missing includes (#1641) --- controller_manager/src/controller_manager.cpp | 1 + controller_manager/src/ros2_control_node.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 48dbbc720a6..87414ce4ec1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -15,6 +15,7 @@ #include "controller_manager/controller_manager.hpp" #include +#include #include #include #include diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 7addaf6cee9..44c9c37aedc 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -18,6 +18,7 @@ #include #include "controller_manager/controller_manager.hpp" +#include "rclcpp/executors.hpp" #include "realtime_tools/thread_priority.hpp" using namespace std::chrono_literals; From d3d1c95797cb8e7efcf365ac498a1ece78913b36 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Jul 2024 20:01:57 +0200 Subject: [PATCH 09/42] [RM] Add `get_hardware_info` method to the Hardware Components (#1643) --- doc/release_notes.rst | 4 +- .../hardware_interface/actuator_interface.hpp | 6 + .../hardware_interface/sensor_interface.hpp | 6 + .../hardware_interface/system_interface.hpp | 6 + .../src/mock_components/generic_system.cpp | 105 ++++++++++-------- .../test_force_torque_sensor.cpp | 4 +- .../test_imu_sensor.cpp | 4 +- .../test_single_joint_actuator.cpp | 10 +- .../test_system_with_command_modes.cpp | 35 +++--- .../test_two_joint_system.cpp | 14 ++- .../test/test_components/test_actuator.cpp | 26 +++-- .../test/test_components/test_sensor.cpp | 5 +- .../test/test_components/test_system.cpp | 27 ++--- 13 files changed, 146 insertions(+), 106 deletions(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index e2b22bf7a62..3c516e6320d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -21,8 +21,7 @@ For details see the controller_manager section. * Pass controller manager update rate on the init of the controller interface (`#1141 `_) * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) - - * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. +* All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. controller_manager ****************** @@ -100,6 +99,7 @@ hardware_interface * Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) * Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) +* Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) joint_limits ************ diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index b637068fe3b..1228d88bcd7 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -241,6 +241,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + /// Get the hardware info of the ActuatorInterface. + /** + * \return hardware info of the ActuatorInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 2cff8b2a3ae..6fde3f88919 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -180,6 +180,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + /// Get the hardware info of the SensorInterface. + /** + * \return hardware info of the SensorInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index f9173fb754e..06358f205e3 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -242,6 +242,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } + /// Get the hardware info of the SystemInterface. + /** + * \return hardware info of the SystemInterface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index abe36ba262e..8b46d21f4d4 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -59,8 +59,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i }; // check if to create mock command interface for sensor - auto it = info_.hardware_parameters.find("mock_sensor_commands"); - if (it != info_.hardware_parameters.end()) + auto it = get_hardware_info().hardware_parameters.find("mock_sensor_commands"); + if (it != get_hardware_info().hardware_parameters.end()) { use_mock_sensor_command_interfaces_ = hardware_interface::parse_bool(it->second); } @@ -70,8 +70,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // check if to create mock command interface for gpio - it = info_.hardware_parameters.find("mock_gpio_commands"); - if (it != info_.hardware_parameters.end()) + it = get_hardware_info().hardware_parameters.find("mock_gpio_commands"); + if (it != get_hardware_info().hardware_parameters.end()) { use_mock_gpio_command_interfaces_ = hardware_interface::parse_bool(it->second); } @@ -82,7 +82,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // check if there is parameter that disables commands // this way we simulate disconnected driver - it = info_.hardware_parameters.find("disable_commands"); + it = get_hardware_info().hardware_parameters.find("disable_commands"); if (it != info.hardware_parameters.end()) { command_propagation_disabled_ = hardware_interface::parse_bool(it->second); @@ -93,7 +93,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // check if there is parameter that enables dynamic calculation - it = info_.hardware_parameters.find("calculate_dynamics"); + it = get_hardware_info().hardware_parameters.find("calculate_dynamics"); if (it != info.hardware_parameters.end()) { calculate_dynamics_ = hardware_interface::parse_bool(it->second); @@ -107,12 +107,12 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; - it = info_.hardware_parameters.find("position_state_following_offset"); - if (it != info_.hardware_parameters.end()) + it = get_hardware_info().hardware_parameters.find("position_state_following_offset"); + if (it != get_hardware_info().hardware_parameters.end()) { position_state_following_offset_ = hardware_interface::stod(it->second); - it = info_.hardware_parameters.find("custom_interface_with_following_offset"); - if (it != info_.hardware_parameters.end()) + it = get_hardware_info().hardware_parameters.find("custom_interface_with_following_offset"); + if (it != get_hardware_info().hardware_parameters.end()) { custom_interface_with_following_offset_ = it->second; } @@ -121,9 +121,10 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i index_custom_interface_with_following_offset_ = std::numeric_limits::max(); // Initialize storage for standard interfaces - initialize_storage_vectors(joint_commands_, joint_states_, standard_interfaces_, info_.joints); + initialize_storage_vectors( + joint_commands_, joint_states_, standard_interfaces_, get_hardware_info().joints); // set all values without initial values to 0 - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { for (auto j = 0u; j < standard_interfaces_.size(); j++) { @@ -135,7 +136,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // search for non-standard joint interfaces - for (const auto & joint : info_.joints) + for (const auto & joint : get_hardware_info().joints) { // populate non-standard command interfaces to other_interfaces_ populate_non_standard_interfaces(joint.command_interfaces, other_interfaces_); @@ -145,7 +146,8 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } // Initialize storage for non-standard interfaces - initialize_storage_vectors(other_commands_, other_states_, other_interfaces_, info_.joints); + initialize_storage_vectors( + other_commands_, other_states_, other_interfaces_, get_hardware_info().joints); // when following offset is used on custom interface then find its index if (!custom_interface_with_following_offset_.empty()) @@ -170,7 +172,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } - for (const auto & sensor : info_.sensors) + for (const auto & sensor : get_hardware_info().sensors) { for (const auto & interface : sensor.state_interfaces) { @@ -183,10 +185,10 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i } } initialize_storage_vectors( - sensor_mock_commands_, sensor_states_, sensor_interfaces_, info_.sensors); + sensor_mock_commands_, sensor_states_, sensor_interfaces_, get_hardware_info().sensors); // search for gpio interfaces - for (const auto & gpio : info_.gpios) + for (const auto & gpio : get_hardware_info().gpios) { // populate non-standard command interfaces to gpio_interfaces_ populate_non_standard_interfaces(gpio.command_interfaces, gpio_interfaces_); @@ -198,12 +200,14 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i // Mock gpio command interfaces if (use_mock_gpio_command_interfaces_) { - initialize_storage_vectors(gpio_mock_commands_, gpio_states_, gpio_interfaces_, info_.gpios); + initialize_storage_vectors( + gpio_mock_commands_, gpio_states_, gpio_interfaces_, get_hardware_info().gpios); } // Real gpio command interfaces else { - initialize_storage_vectors(gpio_commands_, gpio_states_, gpio_interfaces_, info_.gpios); + initialize_storage_vectors( + gpio_commands_, gpio_states_, gpio_interfaces_, get_hardware_info().gpios); } return CallbackReturn::SUCCESS; @@ -214,9 +218,9 @@ std::vector GenericSystem::export_state_inte std::vector state_interfaces; // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { - const auto & joint = info_.joints[i]; + const auto & joint = get_hardware_info().joints[i]; for (const auto & interface : joint.state_interfaces) { // Add interface: if not in the standard list then use "other" interface list @@ -236,14 +240,15 @@ std::vector GenericSystem::export_state_inte // Sensor state interfaces if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) + get_hardware_info().sensors, sensor_interfaces_, sensor_states_, state_interfaces, true)) { throw std::runtime_error( "Interface is not found in the standard nor other list. This should never happen!"); }; // GPIO state interfaces - if (!populate_interfaces(info_.gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) + if (!populate_interfaces( + get_hardware_info().gpios, gpio_interfaces_, gpio_states_, state_interfaces, true)) { throw std::runtime_error("Interface is not found in the gpio list. This should never happen!"); } @@ -256,9 +261,9 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces - for (size_t i = 0; i < info_.joints.size(); ++i) + for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) { - const auto & joint = info_.joints[i]; + const auto & joint = get_hardware_info().joints[i]; for (const auto & interface : joint.command_interfaces) { // Add interface: if not in the standard list than use "other" interface list @@ -278,13 +283,14 @@ std::vector GenericSystem::export_command_ } } // Set position control mode per default - joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); + joint_control_mode_.resize(get_hardware_info().joints.size(), POSITION_INTERFACE_INDEX); // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) { if (!populate_interfaces( - info_.sensors, sensor_interfaces_, sensor_mock_commands_, command_interfaces, true)) + get_hardware_info().sensors, sensor_interfaces_, sensor_mock_commands_, + command_interfaces, true)) { throw std::runtime_error( "Interface is not found in the standard nor other list. This should never happen!"); @@ -295,7 +301,8 @@ std::vector GenericSystem::export_command_ if (use_mock_gpio_command_interfaces_) { if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, true)) + get_hardware_info().gpios, gpio_interfaces_, gpio_mock_commands_, command_interfaces, + true)) { throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!"); @@ -305,7 +312,7 @@ std::vector GenericSystem::export_command_ else { if (!populate_interfaces( - info_.gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) + get_hardware_info().gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false)) { throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!"); @@ -328,29 +335,30 @@ return_type GenericSystem::prepare_command_mode_switch( const size_t FOUND_ONCE_FLAG = 1000000; + const auto & info = get_hardware_info(); std::vector joint_found_in_x_requests_; - joint_found_in_x_requests_.resize(info_.joints.size(), 0); + joint_found_in_x_requests_.resize(info.joints.size(), 0); for (const auto & key : start_interfaces) { // check if interface is joint auto joint_it_found = std::find_if( - info_.joints.begin(), info_.joints.end(), + info.joints.begin(), info.joints.end(), [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); - if (joint_it_found != info_.joints.end()) + if (joint_it_found != info.joints.end()) { - const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + const size_t joint_index = std::distance(info.joints.begin(), joint_it_found); if (joint_found_in_x_requests_[joint_index] == 0) { joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { joint_found_in_x_requests_[joint_index] += 1; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { if (!calculate_dynamics_) { @@ -358,11 +366,11 @@ return_type GenericSystem::prepare_command_mode_switch( get_logger(), "Requested velocity mode for joint '%s' without dynamics calculation enabled - this " "might lead to wrong feedback and unexpected behavior.", - info_.joints[joint_index].name.c_str()); + info.joints[joint_index].name.c_str()); } joint_found_in_x_requests_[joint_index] += 1; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { if (!calculate_dynamics_) { @@ -370,7 +378,7 @@ return_type GenericSystem::prepare_command_mode_switch( get_logger(), "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " "this might lead to wrong feedback and unexpected behavior.", - info_.joints[joint_index].name.c_str()); + info.joints[joint_index].name.c_str()); } joint_found_in_x_requests_[joint_index] += 1; } @@ -382,14 +390,14 @@ return_type GenericSystem::prepare_command_mode_switch( } } - for (size_t i = 0; i < info_.joints.size(); ++i) + for (size_t i = 0; i < info.joints.size(); ++i) { // There has to always be at least one control mode from the above three set if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) { RCLCPP_ERROR( get_logger(), "Joint '%s' has to have '%s', '%s', or '%s' interface!", - info_.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, + info.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); ret_val = hardware_interface::return_type::ERROR; } @@ -401,7 +409,7 @@ return_type GenericSystem::prepare_command_mode_switch( get_logger(), "Got multiple (%zu) starting interfaces for joint '%s' - this is not " "supported!", - joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); + joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info.joints[i].name.c_str()); ret_val = hardware_interface::return_type::ERROR; } } @@ -421,23 +429,24 @@ return_type GenericSystem::perform_command_mode_switch( for (const auto & key : start_interfaces) { // check if interface is joint + const auto & info = get_hardware_info(); auto joint_it_found = std::find_if( - info_.joints.begin(), info_.joints.end(), + info.joints.begin(), info.joints.end(), [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); - if (joint_it_found != info_.joints.end()) + if (joint_it_found != info.joints.end()) { - const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); + const size_t joint_index = std::distance(info.joints.begin(), joint_it_found); - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) { joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) { joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) + if (key == info.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) { joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; } @@ -565,7 +574,7 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur return return_type::ERROR; } - for (const auto & mimic_joint : info_.mimic_joints) + for (const auto & mimic_joint : get_hardware_info().mimic_joints) { for (auto i = 0u; i < joint_states_.size(); ++i) { diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index ff57d6a63d4..acec0e59176 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -33,7 +33,7 @@ class TestForceTorqueSensor : public SensorInterface return CallbackReturn::ERROR; } - const auto & state_interfaces = info_.sensors[0].state_interfaces; + const auto & state_interfaces = get_hardware_info().sensors[0].state_interfaces; if (state_interfaces.size() != 6) { return CallbackReturn::ERROR; @@ -57,7 +57,7 @@ class TestForceTorqueSensor : public SensorInterface { std::vector state_interfaces; - const auto & sensor_name = info_.sensors[0].name; + const auto & sensor_name = get_hardware_info().sensors[0].name; state_interfaces.emplace_back( hardware_interface::StateInterface(sensor_name, "fx", &values_.fx)); state_interfaces.emplace_back( diff --git a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp index bb41f162288..0bd9e9121ce 100644 --- a/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_imu_sensor.cpp @@ -38,7 +38,7 @@ class TestIMUSensor : public SensorInterface return CallbackReturn::ERROR; } - const auto & state_interfaces = info_.sensors[0].state_interfaces; + const auto & state_interfaces = get_hardware_info().sensors[0].state_interfaces; if (state_interfaces.size() != 10) { return CallbackReturn::ERROR; @@ -65,7 +65,7 @@ class TestIMUSensor : public SensorInterface { std::vector state_interfaces; - const std::string & sensor_name = info_.sensors[0].name; + const std::string & sensor_name = get_hardware_info().sensors[0].name; state_interfaces.emplace_back( hardware_interface::StateInterface(sensor_name, "orientation.x", &orientation_.x)); state_interfaces.emplace_back( diff --git a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp index dacbf308fa5..eb61c3de5a5 100644 --- a/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp +++ b/hardware_interface/test/test_hardware_components/test_single_joint_actuator.cpp @@ -34,12 +34,12 @@ class TestSingleJointActuator : public ActuatorInterface } // can only control one joint - if (info_.joints.size() != 1) + if (get_hardware_info().joints.size() != 1) { return CallbackReturn::ERROR; } // can only control in position - const auto & command_interfaces = info_.joints[0].command_interfaces; + const auto & command_interfaces = get_hardware_info().joints[0].command_interfaces; if (command_interfaces.size() != 1) { return CallbackReturn::ERROR; @@ -49,7 +49,7 @@ class TestSingleJointActuator : public ActuatorInterface return CallbackReturn::ERROR; } // can only give feedback state for position and velocity - const auto & state_interfaces = info_.joints[0].state_interfaces; + const auto & state_interfaces = get_hardware_info().joints[0].state_interfaces; if (state_interfaces.size() < 1) { return CallbackReturn::ERROR; @@ -71,7 +71,7 @@ class TestSingleJointActuator : public ActuatorInterface { std::vector state_interfaces; - const auto & joint_name = info_.joints[0].name; + const auto & joint_name = get_hardware_info().joints[0].name; state_interfaces.emplace_back(hardware_interface::StateInterface( joint_name, hardware_interface::HW_IF_POSITION, &position_state_)); state_interfaces.emplace_back(hardware_interface::StateInterface( @@ -84,7 +84,7 @@ class TestSingleJointActuator : public ActuatorInterface { std::vector command_interfaces; - const auto & joint_name = info_.joints[0].name; + const auto & joint_name = get_hardware_info().joints[0].name; command_interfaces.emplace_back(hardware_interface::CommandInterface( joint_name, hardware_interface::HW_IF_POSITION, &position_command_)); diff --git a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp index aba2f86fe5d..a511344f520 100644 --- a/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp +++ b/hardware_interface/test/test_hardware_components/test_system_with_command_modes.cpp @@ -32,11 +32,11 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface } // Can only control two joints - if (info_.joints.size() != 2) + if (get_hardware_info().joints.size() != 2) { return CallbackReturn::ERROR; } - for (const hardware_interface::ComponentInfo & joint : info_.joints) + for (const hardware_interface::ComponentInfo & joint : get_hardware_info().joints) { // Can control in position or velocity const auto & command_interfaces = joint.command_interfaces; @@ -77,14 +77,17 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector export_state_interfaces() override { std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, + &velocity_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_ACCELERATION, + &acceleration_state_[i])); } return state_interfaces; @@ -93,12 +96,14 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface std::vector export_command_interfaces() override { std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_command_[i])); command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_VELOCITY, + &velocity_command_[i])); } return command_interfaces; @@ -127,20 +132,20 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface stop_modes_.clear(); for (const auto & key : start_interfaces) { - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) + if (key == get_hardware_info().joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { start_modes_.push_back(hardware_interface::HW_IF_POSITION); } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) + if (key == get_hardware_info().joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } } } // Example Criteria 1 - Starting: All interfaces must be given a new mode at the same time - if (start_modes_.size() != 0 && start_modes_.size() != info_.joints.size()) + if (start_modes_.size() != 0 && start_modes_.size() != get_hardware_info().joints.size()) { return hardware_interface::return_type::ERROR; } @@ -148,9 +153,9 @@ class TestSystemCommandModes : public hardware_interface::SystemInterface // Stopping interfaces for (const auto & key : stop_interfaces) { - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < get_hardware_info().joints.size(); i++) { - if (key.find(info_.joints[i].name) != std::string::npos) + if (key.find(get_hardware_info().joints[i].name) != std::string::npos) { stop_modes_.push_back(true); } diff --git a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp index 5942f14d424..7bc06a3e1e2 100644 --- a/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp +++ b/hardware_interface/test/test_hardware_components/test_two_joint_system.cpp @@ -35,11 +35,11 @@ class TestTwoJointSystem : public SystemInterface } // can only control two joint - if (info_.joints.size() != 2) + if (get_hardware_info().joints.size() != 2) { return CallbackReturn::ERROR; } - for (const auto & joint : info_.joints) + for (const auto & joint : get_hardware_info().joints) { // can only control in position const auto & command_interfaces = joint.command_interfaces; @@ -70,10 +70,11 @@ class TestTwoJointSystem : public SystemInterface std::vector export_state_interfaces() override { std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_state_[i])); } return state_interfaces; @@ -82,10 +83,11 @@ class TestTwoJointSystem : public SystemInterface std::vector export_command_interfaces() override { std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i])); + get_hardware_info().joints[i].name, hardware_interface::HW_IF_POSITION, + &position_command_[i])); } return command_interfaces; diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index b7149369ed8..86debc1f4d1 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -31,7 +31,7 @@ class TestActuator : public ActuatorInterface return CallbackReturn::ERROR; } - if (info_.joints[0].state_interfaces[1].name == "does_not_exist") + if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist") { return CallbackReturn::ERROR; } @@ -40,12 +40,12 @@ class TestActuator : public ActuatorInterface * a hardware can optional prove for incorrect info here. * * // can only control one joint - * if (info_.joints.size() != 1) {return CallbackReturn::ERROR;} + * if (get_hardware_info().joints.size() != 1) {return CallbackReturn::ERROR;} * // can only control in position - * if (info_.joints[0].command_interfaces.size() != 1) {return + * if (get_hardware_info().joints[0].command_interfaces.size() != 1) {return * CallbackReturn::ERROR;} * // can only give feedback state for position and velocity - * if (info_.joints[0].state_interfaces.size() != 2) {return + * if (get_hardware_info().joints[0].state_interfaces.size() != 2) {return * CallbackReturn::ERROR;} */ @@ -56,11 +56,13 @@ class TestActuator : public ActuatorInterface { std::vector state_interfaces; state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[0].name, &position_state_)); + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[0].name, + &position_state_)); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, info_.joints[0].state_interfaces[1].name, &velocity_state_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(info_.joints[0].name, "some_unlisted_interface", nullptr)); + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[1].name, + &velocity_state_)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_hardware_info().joints[0].name, "some_unlisted_interface", nullptr)); return state_interfaces; } @@ -69,12 +71,14 @@ class TestActuator : public ActuatorInterface { std::vector command_interfaces; command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[0].name, &velocity_command_)); + get_hardware_info().joints[0].name, get_hardware_info().joints[0].command_interfaces[0].name, + &velocity_command_)); - if (info_.joints[0].command_interfaces.size() > 1) + if (get_hardware_info().joints[0].command_interfaces.size() > 1) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, &max_velocity_command_)); + get_hardware_info().joints[0].name, + get_hardware_info().joints[0].command_interfaces[1].name, &max_velocity_command_)); } return command_interfaces; diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 7f5a476c14c..16692f55d94 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -29,7 +29,7 @@ class TestSensor : public SensorInterface return CallbackReturn::ERROR; } // can only give feedback state for velocity - if (info_.sensors[0].state_interfaces.size() == 2) + if (get_hardware_info().sensors[0].state_interfaces.size() == 2) { return CallbackReturn::ERROR; } @@ -40,7 +40,8 @@ class TestSensor : public SensorInterface { std::vector state_interfaces; state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.sensors[0].name, info_.sensors[0].state_interfaces[0].name, &velocity_state_)); + get_hardware_info().sensors[0].name, get_hardware_info().sensors[0].state_interfaces[0].name, + &velocity_state_)); return state_interfaces; } diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index 6724a1d0194..e30b74488e7 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -34,7 +34,7 @@ class TestSystem : public SystemInterface } // Simulating initialization error - if (info_.joints[0].state_interfaces[1].name == "does_not_exist") + if (get_hardware_info().joints[0].state_interfaces[1].name == "does_not_exist") { return CallbackReturn::ERROR; } @@ -44,22 +44,23 @@ class TestSystem : public SystemInterface std::vector export_state_interfaces() override { + const auto info = get_hardware_info(); std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < info.joints.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); + info.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); + info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); + info.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i])); } - if (info_.gpios.size() > 0) + if (info.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.gpios[0].name, info_.gpios[0].state_interfaces[0].name, &configuration_state_)); + info.gpios[0].name, info.gpios[0].state_interfaces[0].name, &configuration_state_)); } return state_interfaces; @@ -67,22 +68,22 @@ class TestSystem : public SystemInterface std::vector export_command_interfaces() override { + const auto info = get_hardware_info(); std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); ++i) + for (auto i = 0u; i < info.joints.size(); ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); + info.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i])); } // Add max_acceleration command interface command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, info_.joints[0].command_interfaces[1].name, - &max_acceleration_command_)); + info.joints[0].name, info.joints[0].command_interfaces[1].name, &max_acceleration_command_)); - if (info_.gpios.size() > 0) + if (info.gpios.size() > 0) { // Add configuration/max_tcp_jerk interface command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.gpios[0].name, info_.gpios[0].command_interfaces[0].name, &configuration_command_)); + info.gpios[0].name, info.gpios[0].command_interfaces[0].name, &configuration_command_)); } return command_interfaces; From 5cd1a43f8c95b5586ae4d5728e5052892ae1c80e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Jul 2024 20:02:19 +0200 Subject: [PATCH 10/42] Fix the namespaced controller_manager spawner + added tests (#1640) --- controller_manager/CMakeLists.txt | 1 + .../controller_manager/spawner.py | 83 ++++-- controller_manager/doc/userdoc.rst | 10 +- .../test/controller_manager_test_common.hpp | 5 +- .../test_controller_spawner_with_type.yaml | 14 + .../test/test_spawner_unspawner.cpp | 276 ++++++++++++++++++ doc/release_notes.rst | 1 + 7 files changed, 352 insertions(+), 38 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index c4ef0e7df0c..5dea15c0d1e 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -185,6 +185,7 @@ if(BUILD_TESTING) ament_add_gmock(test_spawner_unspawner test/test_spawner_unspawner.cpp + TIMEOUT 120 ) target_link_libraries(test_spawner_unspawner controller_manager diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 2778bddebd1..bc8d218ea8f 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -136,17 +136,20 @@ def is_controller_loaded(node, controller_manager, controller_name): return any(c.name == controller_name for c in controllers) -def get_parameter_from_param_file(controller_name, parameter_file, parameter_name): +def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): with open(parameter_file) as f: + namespaced_controller = ( + controller_name if namespace == "/" else f"{namespace}/{controller_name}" + ) parameters = yaml.safe_load(f) - if controller_name in parameters: - value = parameters[controller_name] + if namespaced_controller in parameters: + value = parameters[namespaced_controller] if not isinstance(value, dict) or "ros__parameters" not in value: raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {controller_name}" + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" ) - if parameter_name in parameters[controller_name]["ros__parameters"]: - return parameters[controller_name]["ros__parameters"][parameter_name] + if parameter_name in parameters[namespaced_controller]["ros__parameters"]: + return parameters[namespaced_controller]["ros__parameters"][parameter_name] else: return None @@ -170,7 +173,11 @@ def main(args=None): required=False, ) parser.add_argument( - "-n", "--namespace", help="Namespace for the controller", default="", required=False + "-n", + "--namespace", + help="DEPRECATED Namespace for the controller_manager and the controller(s)", + default=None, + required=False, ) parser.add_argument( "--load-only", @@ -217,7 +224,6 @@ def main(args=None): args = parser.parse_args(command_line_args) controller_names = args.controller_names controller_manager_name = args.controller_manager - controller_namespace = args.namespace param_file = args.param_file controller_manager_timeout = args.controller_manager_timeout @@ -226,9 +232,27 @@ def main(args=None): node = Node("spawner_" + controller_names[0]) + if node.get_namespace() != "/" and args.namespace: + raise RuntimeError( + f"Setting namespace through both '--namespace {args.namespace}' arg and the ROS 2 standard way " + f"'--ros-args -r __ns:={node.get_namespace()}' is not allowed!" + ) + + if args.namespace: + warnings.filterwarnings("always") + warnings.warn( + "The '--namespace' argument is deprecated and will be removed in future releases." + " Use the ROS 2 standard way of setting the node namespacing using --ros-args -r __ns:=", + DeprecationWarning, + ) + + spawner_namespace = args.namespace if args.namespace else node.get_namespace() + + if not spawner_namespace.startswith("/"): + spawner_namespace = f"/{spawner_namespace}" + if not controller_manager_name.startswith("/"): - spawner_namespace = node.get_namespace() - if spawner_namespace != "/": + if spawner_namespace and spawner_namespace != "/": controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" else: controller_manager_name = f"/{controller_manager_name}" @@ -244,11 +268,8 @@ def main(args=None): for controller_name in controller_names: fallback_controllers = args.fallback_controllers - prefixed_controller_name = controller_name - if controller_namespace: - prefixed_controller_name = controller_namespace + "/" + controller_name - if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): + if is_controller_loaded(node, controller_manager_name, controller_name): node.get_logger().warn( bcolors.WARNING + "Controller already loaded, skipping load_controller" @@ -258,11 +279,13 @@ def main(args=None): controller_type = ( None if param_file is None - else get_parameter_from_param_file(controller_name, param_file, "type") + else get_parameter_from_param_file( + controller_name, spawner_namespace, param_file, "type" + ) ) if controller_type: parameter = Parameter() - parameter.name = prefixed_controller_name + ".type" + parameter.name = controller_name + ".type" parameter.value = get_parameter_value(string_value=controller_type) response = call_set_parameters( @@ -277,7 +300,7 @@ def main(args=None): + controller_type + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -287,14 +310,14 @@ def main(args=None): + controller_type + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 if param_file: parameter = Parameter() - parameter.name = prefixed_controller_name + ".params_file" + parameter.name = controller_name + ".params_file" parameter.value = get_parameter_value(string_value=param_file) response = call_set_parameters( @@ -309,7 +332,7 @@ def main(args=None): + param_file + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -319,19 +342,19 @@ def main(args=None): + param_file + '" for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 if not fallback_controllers and param_file: fallback_controllers = get_parameter_from_param_file( - controller_name, param_file, "fallback_controllers" + controller_name, spawner_namespace, param_file, "fallback_controllers" ) if fallback_controllers: parameter = Parameter() - parameter.name = prefixed_controller_name + ".fallback_controllers" + parameter.name = controller_name + ".fallback_controllers" parameter.value = get_parameter_value(string_value=str(fallback_controllers)) response = call_set_parameters( @@ -346,7 +369,7 @@ def main(args=None): + ",".join(fallback_controllers) + '"] for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) else: @@ -356,7 +379,7 @@ def main(args=None): + ",".join(fallback_controllers) + '"] for ' + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 @@ -367,16 +390,12 @@ def main(args=None): bcolors.FAIL + "Failed loading controller " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) return 1 node.get_logger().info( - bcolors.OKBLUE - + "Loaded " - + bcolors.BOLD - + prefixed_controller_name - + bcolors.ENDC + bcolors.OKBLUE + "Loaded " + bcolors.BOLD + controller_name + bcolors.ENDC ) if not args.load_only: @@ -401,7 +420,7 @@ def main(args=None): bcolors.OKGREEN + "Configured and activated " + bcolors.BOLD - + prefixed_controller_name + + controller_name + bcolors.ENDC ) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 676d877319c..63382116659 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -149,7 +149,7 @@ There are two scripts to interact with controller manager from launch files: controller_name positional arguments: - controller_name Name of the controller + controller_names List of controllers options: -h, --help show this help message and exit @@ -158,14 +158,16 @@ There are two scripts to interact with controller manager from launch files: -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 + DEPRECATED Namespace for the controller_manager and the controller(s) --load-only Only load the controller and leave unconfigured. --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 --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT Time to wait for the controller manager + --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether + --fallback_controllers FALLBACK_CONTROLLERS [FALLBACK_CONTROLLERS ...] + Fallback controllers list are activated as a fallback strategy when the spawned controllers fail. When the argument is provided, it takes precedence over the fallback_controllers list in the + param file ``unspawner`` diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 72326e8c6c4..d5a60e6aac2 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -61,14 +61,15 @@ class ControllerManagerFixture : public ::testing::Test { public: explicit ControllerManagerFixture( - const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, + const std::string & cm_namespace = "") : robot_description_(robot_description) { executor_ = std::make_shared(); cm_ = std::make_shared( std::make_unique( rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), - executor_, TEST_CM_NAME); + executor_, TEST_CM_NAME, cm_namespace); // We want to be able to not pass robot description immediately if (!robot_description_.empty()) { diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 95a6efba309..892427bab76 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -11,3 +11,17 @@ chainable_ctrl_with_parameters_and_type: ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] + +/foo_namespace/ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] + +/foo_namespace/chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + +/foo_namespace/ctrl_with_parameters_and_no_type: + ros__parameters: + joint_names: ["joint2"] diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a7c24329598..58321d173dc 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -445,3 +445,279 @@ TEST_F( ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } + +class TestLoadControllerWithNamespacedCM +: public ControllerManagerFixture +{ +public: + TestLoadControllerWithNamespacedCM() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf, "foo_namespace") + { + } + + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(10), + [&]() + { + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + }); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + +protected: + rclcpp::TimerBase::SharedPtr update_timer_; + + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager"), 256) + << "Should fail without defining the namespace"; + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); + + auto validate_loaded_controllers = [&]() + { + auto loaded_controllers = cm_->get_loaded_controllers(); + for (size_t i = 0; i < loaded_controllers.size(); i++) + { + auto ctrl = loaded_controllers[i]; + const std::string controller_name = "ctrl_" + std::to_string(i + 1); + + RCLCPP_ERROR(rclcpp::get_logger("test_controller_manager"), "%s", controller_name.c_str()); + auto it = std::find_if( + loaded_controllers.begin(), loaded_controllers.end(), + [&](const auto & controller) { return controller.info.name == controller_name; }); + ASSERT_TRUE(it != loaded_controllers.end()); + ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + }; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + { + validate_loaded_controllers(); + } + + // Try to spawn again multiple but one of them is already active, it should fail because already + // active + EXPECT_NE( + call_spawner("ctrl_1 ctrl_3 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0) + << "Cannot configure from active"; + + 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 ctrl_3 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + validate_loaded_controllers(); + } + + stop_controllers = {"ctrl_1", "ctrl_2"}; + cm_->switch_controller( + start_controllers, stop_controllers, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + for (auto ctrl : stop_controllers) + { + cm_->unload_controller(ctrl); + cm_->load_controller(ctrl); + } + + // We should be able to reconfigure and activate am unconfigured loaded controller + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + validate_loaded_controllers(); + } + + // Unload and reload + EXPECT_EQ(call_unspawner("ctrl_1 ctrl_3 -c foo_namespace/test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been unloaded"; + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_3 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; + { + validate_loaded_controllers(); + } + + // Now unload everything and load them as a group in a single operation + EXPECT_EQ(call_unspawner("ctrl_1 ctrl_2 ctrl_3 -c /foo_namespace/test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul) << "Controller should have been unloaded"; + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 ctrl_3 -c test_controller_manager --activate-as-group --ros-args " + "-r __ns:=/foo_namespace"), + 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul) << "Controller should have been loaded"; + { + validate_loaded_controllers(); + } +} + +TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + test_file_path), + 256) + << "Should fail without the namespacing it"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + test_file_path + " --ros-args -r __ns:=/foo_namespace"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + + " --ros-args -r __ns:=/foo_namespace"), + 256) + << "Should fail as no type is defined!"; + // Will still be same as the current call will fail + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + 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_UNCONFIGURED); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F( + TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file_deprecated_namespace_arg) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager -p " + + test_file_path), + 256) + << "Should fail without the namespacing it"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path + " --ros-args -r __ns:=/random_namespace"), + 256) + << "Should fail when parsed namespace through both way with different namespaces"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path + " --ros-args -r __ns:=/foo_namespace"), + 256) + << "Should fail when parsed namespace through both ways even with same namespacing name"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 0) + << "Should work when parsed through the deprecated arg"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 256) + << "Should fail as no type is defined!"; + // Will still be same as the current call will fail + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + 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_UNCONFIGURED); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 3c516e6320d..caa28ca9d0e 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -69,6 +69,7 @@ controller_manager The parameters within the ``ros2_control`` tag are not supported any more. * The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). +* The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). hardware_interface ****************** From 42107676e96d6517c258cb9c06e0adcfbb23fc3b Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Thu, 1 Aug 2024 10:29:42 +0200 Subject: [PATCH 11/42] Bump version of pre-commit hooks (#1649) Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 29862f70e47..6fadbbdace2 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.16.0 + rev: v3.17.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.28.6 + rev: 0.29.1 hooks: - id: check-github-workflows args: ["--verbose"] From 774f0533102067539f4ccc66f9f1ca391a751cd7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 2 Aug 2024 11:05:01 +0200 Subject: [PATCH 12/42] Add missing include for executors (#1653) --- controller_manager/test/controller_manager_test_common.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index d5a60e6aac2..78cb9eaee94 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -29,6 +29,7 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" #include "std_msgs/msg/string.hpp" From 054a4edb822a7b54593f8b247306717151f4e134 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 5 Aug 2024 10:23:31 +0100 Subject: [PATCH 13/42] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 10 ++++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 6 ++++++ hardware_interface_testing/CHANGELOG.rst | 5 +++++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 11 files changed, 45 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index c8a69a2d507..2ba08903ad7 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 397ed8b5ca9..2310e8dfc0b 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Add missing include for executors (`#1653 `_) +* Fix the namespaced controller_manager spawner + added tests (`#1640 `_) +* CM: Add missing includes (`#1641 `_) +* Fix rst markup (`#1642 `_) +* Add a pytest launch file to test ros2_control_node (`#1636 `_) +* [CM] Remove deprecated spawner args (`#1639 `_) +* Contributors: Christoph Fröhlich, Sai Kishor Kothakota + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 9e210bc4e78..2fbdedeeaa1 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 37f8b107959..1d6c0f6b0fe 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) +* add missing rclcpp logging include for Humble compatibility build (`#1635 `_) +* Contributors: Sai Kishor Kothakota + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index b0a25e3835f..dfb9e4e03f3 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) +* Contributors: Sai Kishor Kothakota + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index da9bd28fed3..e217465332e 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 3c38360f508..a2968d85b98 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index a61a3273f73..e7bcde8086a 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index b1d9dcd002a..d840a558237 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index b5f99605750..0c3a905b707 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 694f4c72cff..5e8d45c60d2 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.14.0 (2024-07-23) ------------------- * Unused header cleanup (`#1627 `_) From 32e471a0f85dfbb120891171f8c35059f612f012 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 5 Aug 2024 10:23:57 +0100 Subject: [PATCH 14/42] 4.15.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 2ba08903ad7..7b2169ed00c 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 30137a23e40..a60e5e90b65 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.14.0 + 4.15.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 2310e8dfc0b..46d36633623 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- * Add missing include for executors (`#1653 `_) * Fix the namespaced controller_manager spawner + added tests (`#1640 `_) * CM: Add missing includes (`#1641 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index f969b768349..47b640c8927 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.14.0 + 4.15.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 2fbdedeeaa1..c8c11b580da 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 143117f5128..b6a57c26365 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.14.0 + 4.15.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 1d6c0f6b0fe..bb0b478e4b2 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- * [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) * add missing rclcpp logging include for Humble compatibility build (`#1635 `_) * Contributors: Sai Kishor Kothakota diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index c5f09e74bdb..77d084ae7a5 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.14.0 + 4.15.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index dfb9e4e03f3..3a60a2f5344 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- * [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) * Contributors: Sai Kishor Kothakota diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 7594d8e32ce..646923b4e66 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.14.0 + 4.15.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index e217465332e..8581d26f729 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 96095228383..1ff460cf141 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.14.0 + 4.15.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index a2968d85b98..5106f98c384 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 501c543ccbb..d5ef3ba862a 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.14.0 + 4.15.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 e7bcde8086a..f7e7d771b2e 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index b6192e8c452..7be61a2f912 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.14.0 + 4.15.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index d840a558237..265084e74e4 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 9adae5d2b54..f7ed71dae9c 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.14.0 + 4.15.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 0a19536fe57..d1c8e2d1508 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.14.0", + version="4.15.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 0c3a905b707..30a674acc50 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index e678b1f37a5..fc3de218975 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.14.0 + 4.15.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index bbd9e75dcc4..fad594c205a 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.14.0", + version="4.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 5e8d45c60d2..157a8ac09ab 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.15.0 (2024-08-05) +------------------- 4.14.0 (2024-07-23) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 5740c2c68f0..9ee959c5bcd 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.14.0 + 4.15.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 From a1ad5237c1ee0df7e82e9339b8fb566f265c19be Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 9 Aug 2024 14:03:12 +0200 Subject: [PATCH 15/42] refactor SwitchParams to fix the incosistencies in the spawner tests (#1638) --- .../controller_manager/controller_manager.hpp | 23 +++++++++--- controller_manager/src/controller_manager.cpp | 37 ++++++++++++++----- .../test/test_controller_manager_srvs.cpp | 31 +++++++++++++++- 3 files changed, 74 insertions(+), 17 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 2f326ff302f..810936554eb 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -583,14 +583,25 @@ class ControllerManager : public rclcpp::Node struct SwitchParams { - bool do_switch = {false}; - bool started = {false}; - rclcpp::Time init_time = {rclcpp::Time::max()}; + void reset() + { + do_switch = false; + started = false; + strictness = 0; + activate_asap = false; + } + + bool do_switch; + bool started; // Switch options - int strictness = {0}; - bool activate_asap = {false}; - rclcpp::Duration timeout = rclcpp::Duration{0, 0}; + int strictness; + bool activate_asap; + std::chrono::nanoseconds timeout; + + // conditional variable and mutex to wait for the switch to complete + std::condition_variable cv; + std::mutex mutex; }; SwitchParams switch_params_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 87414ce4ec1..25add2045ed 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -849,6 +849,7 @@ controller_interface::return_type ControllerManager::configure_controller( void ControllerManager::clear_requests() { + switch_params_.do_switch = false; deactivate_request_.clear(); activate_request_.clear(); // Set these interfaces as unavailable when clearing requests to avoid leaving them in available @@ -878,7 +879,8 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::ERROR; } - switch_params_ = SwitchParams(); + // reset the switch param internal variables + switch_params_.reset(); if (!deactivate_request_.empty() || !activate_request_.empty()) { @@ -1299,19 +1301,27 @@ controller_interface::return_type ControllerManager::switch_controller( // start the atomic controller switching switch_params_.strictness = strictness; switch_params_.activate_asap = activate_asap; - switch_params_.init_time = rclcpp::Clock().now(); - switch_params_.timeout = timeout; + if (timeout == rclcpp::Duration{0, 0}) + { + RCLCPP_INFO_ONCE(get_logger(), "Switch controller timeout is set to 0, using default 1s!"); + switch_params_.timeout = std::chrono::nanoseconds(1'000'000'000); + } + else + { + switch_params_.timeout = timeout.to_chrono(); + } switch_params_.do_switch = true; - // wait until switch is finished RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); - while (rclcpp::ok() && switch_params_.do_switch) + std::unique_lock switch_params_guard(switch_params_.mutex, std::defer_lock); + if (!switch_params_.cv.wait_for( + switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; })) { - if (!rclcpp::ok()) - { - return controller_interface::return_type::ERROR; - } - std::this_thread::sleep_for(std::chrono::microseconds(100)); + RCLCPP_ERROR( + get_logger(), "Switch controller timed out after %f seconds!", + static_cast(switch_params_.timeout.count()) / 1e9); + clear_requests(); + return controller_interface::return_type::ERROR; } // copy the controllers spec from the used to the unused list @@ -2147,6 +2157,12 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & void ControllerManager::manage_switch() { + std::unique_lock guard(switch_params_.mutex, std::try_to_lock); + if (!guard.owns_lock()) + { + RCLCPP_DEBUG(get_logger(), "Unable to lock switch mutex. Retrying in next cycle."); + return; + } // Ask hardware interfaces to change mode if (!resource_manager_->perform_command_mode_switch( activate_command_interface_request_, deactivate_command_interface_request_)) @@ -2175,6 +2191,7 @@ void ControllerManager::manage_switch() // All controllers switched --> switching done switch_params_.do_switch = false; + switch_params_.cv.notify_all(); } controller_interface::return_type ControllerManager::update( diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index af04cc54064..e53ad105784 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -168,9 +168,38 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv) "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + // Switch with a very low timeout 1 ns and it should fail as there is no enough time to switch + ASSERT_EQ( + controller_interface::return_type::ERROR, + cm_->switch_controller( + {}, {test_controller::TEST_CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 1))); + + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(1u, result->controller.size()); + ASSERT_EQ("active", result->controller[0].state); + ASSERT_THAT( + result->controller[0].claimed_interfaces, + UnorderedElementsAre( + "joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk", + "joint1/position", "joint1/max_velocity")); + ASSERT_THAT( + result->controller[0].required_command_interfaces, + UnorderedElementsAre( + "configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position", + "joint2/max_acceleration", "joint2/velocity", "joint3/velocity")); + ASSERT_THAT( + result->controller[0].required_state_interfaces, + UnorderedElementsAre( + "configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface", + "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", + "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + + // Try again with higher timeout cm_->switch_controller( {}, {test_controller::TEST_CONTROLLER_NAME}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(3, 0)); result = call_service_and_wait(*client, request, srv_executor); ASSERT_EQ(1u, result->controller.size()); From ace84165f3eda161db963415c71aa6d9da06ee34 Mon Sep 17 00:00:00 2001 From: sgmurray Date: Tue, 13 Aug 2024 06:18:18 -0700 Subject: [PATCH 16/42] Fix flaky transmission_interface tests by making them deterministic. (#1665) --- .../four_bar_linkage_transmission.hpp | 2 +- transmission_interface/test/random_generator_utils.hpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index a8757e9c020..107133128cb 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -289,7 +289,7 @@ inline void FourBarLinkageTransmission::actuator_to_joint() joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]); joint_eff[1].set_value( - jr[1] * (act_eff[1].get_value() * ar[1] - act_eff[0].get_value() * ar[0] * jr[0])); + jr[1] * (act_eff[1].get_value() * ar[1] - jr[0] * act_eff[0].get_value() * ar[0])); } } diff --git a/transmission_interface/test/random_generator_utils.hpp b/transmission_interface/test/random_generator_utils.hpp index fbacd1ba3b2..01e1245e48a 100644 --- a/transmission_interface/test/random_generator_utils.hpp +++ b/transmission_interface/test/random_generator_utils.hpp @@ -26,12 +26,14 @@ using std::vector; /// \brief Generator of pseudo-random double in the range [min_val, max_val]. // NOTE: Based on example code available at: // http://stackoverflow.com/questions/2860673/initializing-a-c-vector-to-random-values-fast +// Use a user specified seed instead of system time for deterministic tests struct RandomDoubleGenerator { public: - RandomDoubleGenerator(double min_val, double max_val) : min_val_(min_val), max_val_(max_val) + RandomDoubleGenerator(double min_val, double max_val, unsigned int seed = 1234) + : min_val_(min_val), max_val_(max_val) { - srand(static_cast(time(nullptr))); + srand(seed); } double operator()() { From 4919aba11469037cff455ce89367b0a633e12356 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 14 Aug 2024 12:59:28 +0200 Subject: [PATCH 17/42] [CM] Add more logging for easier debugging (#1645) --- controller_manager/src/controller_manager.cpp | 65 +++++++++++++++++-- 1 file changed, 58 insertions(+), 7 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 25add2045ed..541fc57d02a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -530,12 +530,16 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c get_logger(), "The 'type' param was not defined for '%s'.", controller_name.c_str()); return nullptr; } + RCLCPP_INFO( + get_logger(), "Loading controller : '%s' of type '%s'", controller_name.c_str(), + controller_type.c_str()); return load_controller(controller_name, controller_type); } controller_interface::return_type ControllerManager::unload_controller( const std::string & controller_name) { + RCLCPP_INFO(get_logger(), "Unloading controller: '%s'", controller_name.c_str()); std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); @@ -633,7 +637,7 @@ std::vector ControllerManager::get_loaded_controllers() const controller_interface::return_type ControllerManager::configure_controller( const std::string & controller_name) { - RCLCPP_INFO(get_logger(), "Configuring controller '%s'", controller_name.c_str()); + RCLCPP_INFO(get_logger(), "Configuring controller: '%s'", controller_name.c_str()); const auto & controllers = get_loaded_controllers(); @@ -919,16 +923,24 @@ controller_interface::return_type ControllerManager::switch_controller( strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; } - RCLCPP_DEBUG(get_logger(), "Activating controllers:"); + std::string activate_list, deactivate_list; + activate_list.reserve(500); + deactivate_list.reserve(500); for (const auto & controller : activate_controllers) { - RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str()); + activate_list.append(controller); + activate_list.append(" "); } - RCLCPP_DEBUG(get_logger(), "Deactivating controllers:"); for (const auto & controller : deactivate_controllers) { - RCLCPP_DEBUG(get_logger(), " - %s", controller.c_str()); + deactivate_list.append(controller); + deactivate_list.append(" "); } + RCLCPP_INFO_EXPRESSION( + get_logger(), !activate_list.empty(), "Activating controllers: [ %s]", activate_list.c_str()); + RCLCPP_INFO_EXPRESSION( + get_logger(), !deactivate_list.empty(), "Deactivating controllers: [ %s]", + deactivate_list.c_str()); const auto list_controllers = [this, strictness]( const std::vector & controller_list, @@ -2141,13 +2153,32 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & if (!ok) { std::vector stop_request = {}; + std::string failed_hardware_string; + failed_hardware_string.reserve(500); // Determine controllers to stop for (const auto & hardware_name : failed_hardware_names) { + failed_hardware_string.append(hardware_name); + failed_hardware_string.append(" "); auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); } - + std::string stop_request_string; + stop_request_string.reserve(500); + for (const auto & controller : stop_request) + { + stop_request_string.append(controller); + stop_request_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), + "Deactivating following hardware components as their read cycle resulted in an error: [ %s]", + failed_hardware_string.c_str()); + RCLCPP_ERROR_EXPRESSION( + get_logger(), !stop_request_string.empty(), + "Deactivating following controllers as their hardware components read cycle resulted in an " + "error: [ %s]", + stop_request_string.c_str()); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); deactivate_controllers(rt_controller_list, stop_request); @@ -2303,13 +2334,33 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration if (!ok) { std::vector stop_request = {}; + std::string failed_hardware_string; + failed_hardware_string.reserve(500); // Determine controllers to stop for (const auto & hardware_name : failed_hardware_names) { + failed_hardware_string.append(hardware_name); + failed_hardware_string.append(" "); auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); } - + std::string stop_request_string; + stop_request_string.reserve(500); + for (const auto & controller : stop_request) + { + stop_request_string.append(controller); + stop_request_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), + "Deactivating following hardware components as their write cycle resulted in an error: [ " + "%s]", + failed_hardware_string.c_str()); + RCLCPP_ERROR_EXPRESSION( + get_logger(), !stop_request_string.empty(), + "Deactivating following controllers as their hardware components write cycle resulted in an " + "error: [ %s]", + stop_request_string.c_str()); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); deactivate_controllers(rt_controller_list, stop_request); From 32155cc3f400107c3e84979c1ffeb963458c9c8c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 14 Aug 2024 13:09:41 +0200 Subject: [PATCH 18/42] [RM] Decouple read/write cycles of each component with mutex to not block other components (#1646) --- .../include/hardware_interface/actuator.hpp | 4 ++- .../hardware_interface/actuator_interface.hpp | 2 +- .../include/hardware_interface/sensor.hpp | 4 ++- .../hardware_interface/sensor_interface.hpp | 2 +- .../include/hardware_interface/system.hpp | 4 ++- .../hardware_interface/system_interface.hpp | 3 +- hardware_interface/src/actuator.cpp | 30 +++++++++++++++++++ hardware_interface/src/resource_manager.cpp | 2 -- hardware_interface/src/sensor.cpp | 22 ++++++++++++++ hardware_interface/src/system.cpp | 29 ++++++++++++++++++ 10 files changed, 94 insertions(+), 8 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index 3534d9f5875..bb91b62057f 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -42,7 +42,8 @@ class Actuator final HARDWARE_INTERFACE_PUBLIC explicit Actuator(std::unique_ptr impl); - Actuator(Actuator && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit Actuator(Actuator && other) noexcept; ~Actuator() = default; @@ -102,6 +103,7 @@ class Actuator final private: std::unique_ptr impl_; + mutable std::recursive_mutex actuators_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 1228d88bcd7..a74d2a49641 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -228,7 +228,6 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } -protected: /// Get the logger of the ActuatorInterface. /** * \return logger of the ActuatorInterface. @@ -247,6 +246,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ const HardwareInfo & get_hardware_info() const { return info_; } +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 4126910d447..98197bd0e97 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -42,7 +42,8 @@ class Sensor final HARDWARE_INTERFACE_PUBLIC explicit Sensor(std::unique_ptr impl); - Sensor(Sensor && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit Sensor(Sensor && other) noexcept; ~Sensor() = default; @@ -89,6 +90,7 @@ class Sensor final private: std::unique_ptr impl_; + mutable std::recursive_mutex sensors_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 6fde3f88919..0c7f43e7fc5 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -167,7 +167,6 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } -protected: /// Get the logger of the SensorInterface. /** * \return logger of the SensorInterface. @@ -186,6 +185,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ const HardwareInfo & get_hardware_info() const { return info_; } +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 9308872ef8b..1cc29a085da 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -42,7 +42,8 @@ class System final HARDWARE_INTERFACE_PUBLIC explicit System(std::unique_ptr impl); - System(System && other) = default; + HARDWARE_INTERFACE_PUBLIC + explicit System(System && other) noexcept; ~System() = default; @@ -102,6 +103,7 @@ class System final private: std::unique_ptr impl_; + mutable std::recursive_mutex system_mutex_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 06358f205e3..54f1af33a73 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -25,6 +25,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -229,7 +230,6 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } -protected: /// Get the logger of the SystemInterface. /** * \return logger of the SystemInterface. @@ -248,6 +248,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ const HardwareInfo & get_hardware_info() const { return info_; } +protected: HardwareInfo info_; rclcpp_lifecycle::State lifecycle_state_; diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 66419402f80..52cfc681a1c 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -35,10 +36,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Actuator::Actuator(std::unique_ptr impl) : impl_(std::move(impl)) {} +Actuator::Actuator(Actuator && other) noexcept +{ + std::lock_guard lock(other.actuators_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & Actuator::initialize( const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(actuator_info, logger, clock_interface)) @@ -60,6 +68,7 @@ const rclcpp_lifecycle::State & Actuator::initialize( const rclcpp_lifecycle::State & Actuator::configure() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -83,6 +92,7 @@ const rclcpp_lifecycle::State & Actuator::configure() const rclcpp_lifecycle::State & Actuator::cleanup() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) @@ -103,6 +113,7 @@ const rclcpp_lifecycle::State & Actuator::cleanup() const rclcpp_lifecycle::State & Actuator::shutdown() { + std::unique_lock lock(actuators_mutex_); if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -124,6 +135,7 @@ const rclcpp_lifecycle::State & Actuator::shutdown() const rclcpp_lifecycle::State & Actuator::activate() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -146,6 +158,7 @@ const rclcpp_lifecycle::State & Actuator::activate() const rclcpp_lifecycle::State & Actuator::deactivate() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -168,6 +181,7 @@ const rclcpp_lifecycle::State & Actuator::deactivate() const rclcpp_lifecycle::State & Actuator::error() { + std::unique_lock lock(actuators_mutex_); if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) @@ -221,6 +235,14 @@ const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(actuators_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping read() call for actuator '%s' since it is locked", + impl_->get_name().c_str()); + return return_type::OK; + } if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -243,6 +265,14 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(actuators_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping write() call for actuator '%s' since it is locked", + impl_->get_name().c_str()); + return return_type::OK; + } if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 5b13e5d3318..53fcad94d22 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1770,7 +1770,6 @@ void ResourceManager::shutdown_async_components() HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) { - std::lock_guard guard(resources_lock_); read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); @@ -1831,7 +1830,6 @@ HardwareReadWriteStatus ResourceManager::read( HardwareReadWriteStatus ResourceManager::write( const rclcpp::Time & time, const rclcpp::Duration & period) { - std::lock_guard guard(resources_lock_); read_write_status.ok = true; read_write_status.failed_hardware_names.clear(); diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 9b7f1f69d66..9f1d4b73e16 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -34,10 +35,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface Sensor::Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} +Sensor::Sensor(Sensor && other) noexcept +{ + std::lock_guard lock(other.sensors_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & Sensor::initialize( const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(sensor_info, logger, clock_interface)) @@ -59,6 +67,7 @@ const rclcpp_lifecycle::State & Sensor::initialize( const rclcpp_lifecycle::State & Sensor::configure() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -82,6 +91,7 @@ const rclcpp_lifecycle::State & Sensor::configure() const rclcpp_lifecycle::State & Sensor::cleanup() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) @@ -102,6 +112,7 @@ const rclcpp_lifecycle::State & Sensor::cleanup() const rclcpp_lifecycle::State & Sensor::shutdown() { + std::unique_lock lock(sensors_mutex_); if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -123,6 +134,7 @@ const rclcpp_lifecycle::State & Sensor::shutdown() const rclcpp_lifecycle::State & Sensor::activate() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -145,6 +157,7 @@ const rclcpp_lifecycle::State & Sensor::activate() const rclcpp_lifecycle::State & Sensor::deactivate() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -167,6 +180,7 @@ const rclcpp_lifecycle::State & Sensor::deactivate() const rclcpp_lifecycle::State & Sensor::error() { + std::unique_lock lock(sensors_mutex_); if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) @@ -199,6 +213,14 @@ const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_st return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(sensors_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping read() call for the sensor '%s' since it is locked", + impl_->get_name().c_str()); + return return_type::OK; + } if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index ba327d8ab24..82473563b67 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -34,10 +34,17 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface System::System(std::unique_ptr impl) : impl_(std::move(impl)) {} +System::System(System && other) noexcept +{ + std::lock_guard lock(other.system_mutex_); + impl_ = std::move(other.impl_); +} + const rclcpp_lifecycle::State & System::initialize( const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(system_info, logger, clock_interface)) @@ -59,6 +66,7 @@ const rclcpp_lifecycle::State & System::initialize( const rclcpp_lifecycle::State & System::configure() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { switch (impl_->on_configure(impl_->get_state())) @@ -82,6 +90,7 @@ const rclcpp_lifecycle::State & System::configure() const rclcpp_lifecycle::State & System::cleanup() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_cleanup(impl_->get_state())) @@ -102,6 +111,7 @@ const rclcpp_lifecycle::State & System::cleanup() const rclcpp_lifecycle::State & System::shutdown() { + std::unique_lock lock(system_mutex_); if ( impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -123,6 +133,7 @@ const rclcpp_lifecycle::State & System::shutdown() const rclcpp_lifecycle::State & System::activate() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { switch (impl_->on_activate(impl_->get_state())) @@ -145,6 +156,7 @@ const rclcpp_lifecycle::State & System::activate() const rclcpp_lifecycle::State & System::deactivate() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { switch (impl_->on_deactivate(impl_->get_state())) @@ -167,6 +179,7 @@ const rclcpp_lifecycle::State & System::deactivate() const rclcpp_lifecycle::State & System::error() { + std::unique_lock lock(system_mutex_); if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->on_error(impl_->get_state())) @@ -218,6 +231,14 @@ const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_st return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(system_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping read() call for system '%s' since it is locked", + impl_->get_name().c_str()); + return return_type::OK; + } if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -240,6 +261,14 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) { + std::unique_lock lock(system_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + RCLCPP_DEBUG( + impl_->get_logger(), "Skipping write() call for system '%s' since it is locked", + impl_->get_name().c_str()); + return return_type::OK; + } if ( impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) From 07fb4f3554645d76ba3187f88f4bc7e3ee977ab8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 14 Aug 2024 13:10:45 +0200 Subject: [PATCH 19/42] Fix spawner unload on kill test (#1675) --- controller_manager/test/test_spawner_unspawner.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 58321d173dc..83080ee0fa1 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -298,11 +298,11 @@ TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill // timeout command will kill it after the specified time with signal SIGINT + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); std::stringstream ss; ss << "timeout --signal=INT 5 " - << "ros2 run controller_manager spawner " - << "ctrl_3 -c test_controller_manager -t " - << std::string(test_controller::TEST_CONTROLLER_CLASS_NAME) << " --unload-on-kill"; + << "ros2 run controller_manager spawner ctrl_3 -c test_controller_manager" + << " --unload-on-kill"; EXPECT_NE(std::system(ss.str().c_str()), 0) << "timeout should have killed spawner and returned non 0 code"; From 3cf167d06c91a4da606e75895a37a3883dca8440 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 14 Aug 2024 14:51:31 +0200 Subject: [PATCH 20/42] [CI] Add coveragepy and remove ignore: test (#1668) --- codecov.yml | 2 -- controller_manager/package.xml | 1 + .../test/test_spawner_unspawner.cpp | 16 +++++++++++----- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/codecov.yml b/codecov.yml index 97106f32ffc..764afc34e66 100644 --- a/codecov.yml +++ b/codecov.yml @@ -14,8 +14,6 @@ fixes: comment: layout: "diff, flags, files" behavior: default -ignore: - - "**/test" flags: unittests: paths: diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 47b640c8927..9f9a9bf796d 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -31,6 +31,7 @@ ament_cmake_gmock ament_cmake_pytest + python3-coverage hardware_interface_testing ros2_control_test_assets diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 83080ee0fa1..fa27ee2c970 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -27,6 +27,7 @@ using ::testing::_; using ::testing::Return; +const char coveragepy_script[] = "python3 -m coverage run --append --branch"; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture @@ -68,14 +69,18 @@ class TestLoadController : public ControllerManagerFixtureset_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); std::stringstream ss; ss << "timeout --signal=INT 5 " - << "ros2 run controller_manager spawner ctrl_3 -c test_controller_manager" - << " --unload-on-kill"; + << std::string(coveragepy_script) + + " $(ros2 pkg prefix controller_manager)/lib/controller_manager/spawner " + << "ctrl_3 -c test_controller_manager --unload-on-kill"; EXPECT_NE(std::system(ss.str().c_str()), 0) << "timeout should have killed spawner and returned non 0 code"; From cce79ebb8c5b509b8bd97e1be26f0061a7227a70 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 15:15:11 +0200 Subject: [PATCH 21/42] Make list controller and list hardware components immediately visualize the state. (#1606) --- .../controller_manager/spawner.py | 2 +- .../ros2controlcli/verb/list_controllers.py | 14 +++++++++++--- .../verb/list_hardware_components.py | 17 ++++++++++++++--- 3 files changed, 26 insertions(+), 7 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index bc8d218ea8f..a8b5d25effd 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -47,7 +47,7 @@ class bcolors: - HEADER = "\033[95m" + MAGENTA = "\033[95m" OKBLUE = "\033[94m" OKCYAN = "\033[96m" OKGREEN = "\033[92m" diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 31ce8148652..a26a1168a11 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -22,7 +22,7 @@ from ros2controlcli.api import add_controller_mgr_parsers -def print_controller_state(c, args): +def print_controller_state(c, args, col_width_name, col_width_state, col_width_type): state_color = "" if c.state == "active": state_color = bcolors.OKGREEN @@ -31,7 +31,9 @@ def print_controller_state(c, args): elif c.state == "unconfigured": state_color = bcolors.WARNING - print(f"{c.name:20s}[{c.type:20s}] {state_color}{c.state:10s}{bcolors.ENDC}") + print( + f"{state_color}{c.name:<{col_width_name}}{bcolors.ENDC} {c.type:<{col_width_type}} {state_color}{c.state:<{col_width_state}}{bcolors.ENDC}" + ) if args.claimed_interfaces or args.verbose: print("\tclaimed interfaces:") for claimed_interface in c.claimed_interfaces: @@ -96,7 +98,13 @@ def add_arguments(self, parser, cli_name): def main(self, *, args): with NodeStrategy(args) as node: response = list_controllers(node, args.controller_manager) + + # Structure data as table for nicer output + col_width_name = max(len(ctrl.name) for ctrl in response.controller) + col_width_type = max(len(ctrl.type) for ctrl in response.controller) + col_width_state = max(len(ctrl.state) for ctrl in response.controller) + for c in response.controller: - print_controller_state(c, args) + print_controller_state(c, args, col_width_name, col_width_state, col_width_type) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index 8a5884f2cb1..e2b427b2338 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -15,6 +15,8 @@ from controller_manager import list_hardware_components from controller_manager.spawner import bcolors +from lifecycle_msgs.msg import State + from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy from ros2cli.verb import VerbExtension @@ -39,17 +41,26 @@ def main(self, *, args): hardware_components = list_hardware_components(node, args.controller_manager) for idx, component in enumerate(hardware_components.component): + # Set activity color for nicer visualization + activity_color = bcolors.FAIL + if component.state.id == State.PRIMARY_STATE_UNCONFIGURED: + activity_color = bcolors.WARNING + if component.state.id == State.PRIMARY_STATE_INACTIVE: + activity_color = bcolors.MAGENTA + if component.state.id == State.PRIMARY_STATE_ACTIVE: + activity_color = bcolors.OKGREEN + print( - f"Hardware Component {idx+1}\n\tname: {component.name}\n\ttype: {component.type}" + f"Hardware Component {idx+1}\n\tname: {activity_color}{component.name}{bcolors.ENDC}\n\ttype: {component.type}" ) if hasattr(component, "plugin_name"): - plugin_name = component.plugin_name + plugin_name = f"{component.plugin_name}" else: plugin_name = f"{bcolors.WARNING}plugin name missing!{bcolors.ENDC}" print( f"\tplugin name: {plugin_name}\n" - f"\tstate: id={component.state.id} label={component.state.label}\n" + f"\tstate: id={component.state.id} label={activity_color}{component.state.label}{bcolors.ENDC}\n" f"\tcommand interfaces" ) for cmd_interface in component.command_interfaces: From af4b48f75a0ff4c57f30df3477f9fa2786e07965 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 14 Aug 2024 16:24:23 +0100 Subject: [PATCH 22/42] Handle waiting in Spawner and align Hardware Spawner functionality (#1562) --- .../controller_manager_services.py | 37 +++++---- .../controller_manager/hardware_spawner.py | 77 ++++++------------- .../controller_manager/spawner.py | 76 ++++-------------- .../controller_manager/unspawner.py | 6 ++ controller_manager/doc/userdoc.rst | 20 +++++ .../test/test_spawner_unspawner.cpp | 22 +++--- 6 files changed, 98 insertions(+), 140 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 62b350f7d3e..be11a4b782a 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -28,15 +28,20 @@ import rclpy -def service_caller(node, service_name, service_type, request, service_timeout=10.0): +class ServiceNotFoundError(Exception): + pass + + +def service_caller(node, service_name, service_type, request, service_timeout=0.0): cli = node.create_client(service_type, service_name) - if not cli.service_is_ready(): - node.get_logger().debug( - 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}") + while not cli.service_is_ready(): + node.get_logger().info(f"waiting for service {service_name} to become available...") + if service_timeout: + if not cli.wait_for_service(service_timeout): + raise ServiceNotFoundError(f"Could not contact service {service_name}") + elif not cli.wait_for_service(10.0): + node.get_logger().warn(f"Could not contact service {service_name}") node.get_logger().debug(f"requester: making request: {request}\n") future = cli.call_async(request) @@ -47,7 +52,7 @@ def service_caller(node, service_name, service_type, request, service_timeout=10 raise RuntimeError(f"Exception while calling service: {future.exception()}") -def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = ConfigureController.Request() request.name = controller_name return service_caller( @@ -59,7 +64,7 @@ def configure_controller(node, controller_manager_name, controller_name, service ) -def list_controllers(node, controller_manager_name, service_timeout=10.0): +def list_controllers(node, controller_manager_name, service_timeout=0.0): request = ListControllers.Request() return service_caller( node, @@ -70,7 +75,7 @@ def list_controllers(node, controller_manager_name, service_timeout=10.0): ) -def list_controller_types(node, controller_manager_name, service_timeout=10.0): +def list_controller_types(node, controller_manager_name, service_timeout=0.0): request = ListControllerTypes.Request() return service_caller( node, @@ -81,7 +86,7 @@ def list_controller_types(node, controller_manager_name, service_timeout=10.0): ) -def list_hardware_components(node, controller_manager_name, service_timeout=10.0): +def list_hardware_components(node, controller_manager_name, service_timeout=0.0): request = ListHardwareComponents.Request() return service_caller( node, @@ -92,7 +97,7 @@ def list_hardware_components(node, controller_manager_name, service_timeout=10.0 ) -def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0): +def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0): request = ListHardwareInterfaces.Request() return service_caller( node, @@ -103,7 +108,7 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0 ) -def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = LoadController.Request() request.name = controller_name return service_caller( @@ -115,7 +120,7 @@ def load_controller(node, controller_manager_name, controller_name, service_time ) -def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0): +def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0): request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -128,7 +133,7 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi def set_hardware_component_state( - node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0 + node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 ): request = SetHardwareComponentState.Request() request.name = component_name @@ -165,7 +170,7 @@ def switch_controllers( ) -def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = UnloadController.Request() request.name = controller_name return service_caller( diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index c95fb6181eb..3e3a487c6ac 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -15,13 +15,15 @@ import argparse import sys -import time -from controller_manager import set_hardware_component_state +from controller_manager import ( + list_hardware_components, + set_hardware_component_state, +) +from controller_manager.controller_manager_services import ServiceNotFoundError from lifecycle_msgs.msg import State import rclpy -from rclpy.duration import Duration from rclpy.node import Node from rclpy.signals import SignalHandlerOptions @@ -43,17 +45,6 @@ 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 @@ -75,35 +66,11 @@ def has_service_names(node, node_name, node_namespace, service_names): 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 is_hardware_component_loaded( + node, controller_manager, hardware_component, service_timeout=0.0 +): + components = list_hardware_components(node, hardware_component, service_timeout).component + return any(c.name == hardware_component for c in components) def handle_set_component_state_service_call( @@ -167,10 +134,9 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=10, - type=int, + default=0, + type=float, ) - # add arguments which are mutually exclusive activate_or_confiigure_grp.add_argument( "--activate", @@ -202,13 +168,15 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: - if not wait_for_controller_manager( - node, controller_manager_name, controller_manager_timeout + if not is_hardware_component_loaded( + node, controller_manager_name, hardware_component, controller_manager_timeout ): - node.get_logger().error("Controller manager not available") - return 1 - - if activate: + node.get_logger().warn( + bcolors.WARNING + + "Hardware Component is not loaded - state can not be changed." + + bcolors.ENDC + ) + elif activate: activate_components(node, controller_manager_name, hardware_component) elif configure: configure_components(node, controller_manager_name, hardware_component) @@ -218,6 +186,11 @@ def main(args=None): ) parser.print_help() return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index a8b5d25effd..0e83eb9d073 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -28,10 +28,10 @@ switch_controllers, unload_controller, ) +from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy from rcl_interfaces.msg import Parameter -from rclpy.duration import Duration from rclpy.node import Node # @note: The versions conditioning is added here to support the source-compatibility with Humble @@ -62,17 +62,6 @@ 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 @@ -93,46 +82,8 @@ def has_service_names(node, node_name, node_namespace, service_names): 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}/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", - ) - - # 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 is_controller_loaded(node, controller_manager, controller_name): - controllers = list_controllers(node, controller_manager).controller +def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0): + controllers = list_controllers(node, controller_manager, service_timeout).controller return any(c.name == controller_name for c in controllers) @@ -201,8 +152,8 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=10, - type=int, + default=0, + type=float, ) parser.add_argument( "--activate-as-group", @@ -258,18 +209,12 @@ def main(args=None): 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( - bcolors.FAIL + "Controller manager not available" + bcolors.ENDC - ) - return 1 - for controller_name in controller_names: fallback_controllers = args.fallback_controllers - if is_controller_loaded(node, controller_manager_name, controller_name): + if is_controller_loaded( + node, controller_manager_name, controller_name, controller_manager_timeout + ): node.get_logger().warn( bcolors.WARNING + "Controller already loaded, skipping load_controller" @@ -471,6 +416,11 @@ def main(args=None): node.get_logger().info("Unloaded controller") return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 5acf05e65de..e42d85aee9f 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -19,6 +19,7 @@ import warnings from controller_manager import switch_controllers, unload_controller +from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy from rclpy.node import Node @@ -57,6 +58,11 @@ def main(args=None): node.get_logger().info("Unloaded controller") return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 63382116659..bc9f75384e9 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -136,6 +136,7 @@ There are two scripts to interact with controller manager from launch files: 1. ``spawner`` - loads, configures and start a controller on startup. 2. ``unspawner`` - stops and unloads a controller. + 3. ``hardware_spawner`` - activates and configures a hardware component. ``spawner`` @@ -186,6 +187,25 @@ 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 +``hardware_spawner`` +^^^^^^^^^^^^^^^^^^^^^^ + +.. code-block:: console + + $ ros2 run controller_manager hardware_spawner -h + usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name + + positional arguments: + hardware_component_name + The name of the hardware component which should be activated. + + options: + -h, --help show this help message and exit + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node + --activate Activates the given components. Note: Components are by default configured before activated. + --configure Configures the given components. + rqt_controller_manager ---------------------- A GUI tool to interact with the controller manager services to be able to switch the lifecycle states of the controllers as well as the hardware components. diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index fa27ee2c970..3edf7704367 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -88,9 +88,10 @@ TEST_F(TestLoadController, spawner_with_no_arguments_errors) EXPECT_NE(call_spawner(""), 0) << "Missing mandatory arguments"; } -TEST_F(TestLoadController, spawner_without_manager_errors) +TEST_F(TestLoadController, spawner_without_manager_errors_with_given_timeout) { - EXPECT_NE(call_spawner("ctrl_1"), 0) << "Wrong controller manager name"; + EXPECT_NE(call_spawner("ctrl_1 --controller-manager-timeout 1.0"), 0) + << "Wrong controller manager name"; } TEST_F(TestLoadController, spawner_without_type_parameter_or_arg_errors) @@ -283,7 +284,9 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path), + "ctrl_with_parameters_and_no_type -c test_controller_manager --controller-manager-timeout " + "1.0 -p " + + test_file_path), 256); // Will still be same as the current call will fail ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); @@ -422,7 +425,7 @@ TEST_F(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spaw cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 256) + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --controller-manager-timeout 1.0"), 256) << "could not spawn controller because not robot description and not services for controller " "manager are active"; } @@ -504,7 +507,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager"), 256) + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --controller-manager-timeout 1.0"), 256) << "Should fail without defining the namespace"; EXPECT_EQ( call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); @@ -603,7 +607,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager -p " + + "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; @@ -663,21 +667,21 @@ TEST_F( EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager -p " + + "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace -p " + + "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p " + test_file_path + " --ros-args -r __ns:=/random_namespace"), 256) << "Should fail when parsed namespace through both way with different namespaces"; EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace -p " + + "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p" + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail when parsed namespace through both ways even with same namespacing name"; From eb4316b1dc65d9a93a2463b4f2b22f3510f0ab1b Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Thu, 15 Aug 2024 14:34:02 +0200 Subject: [PATCH 23/42] Preparation of Handles for Variant Support (#1678) --- .../include/hardware_interface/handle.hpp | 80 ++++++++++--------- .../include/transmission_interface/handle.hpp | 8 +- 2 files changed, 45 insertions(+), 43 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index dc536e51be4..bfe0ff1eb89 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -16,41 +16,45 @@ #define HARDWARE_INTERFACE__HANDLE_HPP_ #include +#include #include "hardware_interface/macros.hpp" namespace hardware_interface { + +using HANDLE_DATATYPE = std::variant; + /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class Handle { public: - ReadOnlyHandle( + Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { } - explicit ReadOnlyHandle(const std::string & interface_name) + explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - explicit ReadOnlyHandle(const char * interface_name) + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + Handle(const Handle & other) = default; - ReadOnlyHandle(ReadOnlyHandle && other) = default; + Handle(Handle && other) = default; - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + Handle & operator=(const Handle & other) = default; - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + Handle & operator=(Handle && other) = default; - virtual ~ReadOnlyHandle() = default; + virtual ~Handle() = default; /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } @@ -70,60 +74,58 @@ class ReadOnlyHandle double get_value() const { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; + // END + } + + void set_value(double value) + { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) set value_ directly if old functionality is removed + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; + // END } protected: std::string prefix_name_; std::string interface_name_; + HANDLE_DATATYPE value_; + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; + // END }; -class ReadWriteHandle : public ReadOnlyHandle +class StateInterface : public Handle { public: - ReadWriteHandle( + explicit StateInterface( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) - { - } - - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} - - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} - - ReadWriteHandle(const ReadWriteHandle & other) = default; - - ReadWriteHandle(ReadWriteHandle && other) = default; - - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; - - ReadWriteHandle & operator=(ReadWriteHandle && other) = default; - - virtual ~ReadWriteHandle() = default; - - void set_value(double value) + : Handle(prefix_name, interface_name, value_ptr) { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; } -}; -class StateInterface : public ReadOnlyHandle -{ -public: StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using Handle::Handle; }; -class CommandInterface : public ReadWriteHandle +class CommandInterface : public Handle { public: + explicit CommandInterface( + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : Handle(prefix_name, interface_name, value_ptr) + { + } /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -134,7 +136,7 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + using Handle::Handle; }; } // namespace hardware_interface diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index 024a019bfda..4c401896481 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -20,17 +20,17 @@ namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::ReadWriteHandle +class ActuatorHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle +class JointHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; } // namespace transmission_interface From 80c264f024c06079707378e0bbaf0f361a1dff6c Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 16 Aug 2024 11:17:14 +0200 Subject: [PATCH 24/42] Robustify controller spawner and add integration test with many controllers (#1501) --------- Co-authored-by: Dr. Denis --- .../controller_manager_services.py | 54 ++++++++++++++++--- .../test/test_spawner_unspawner.cpp | 26 +++++++++ 2 files changed, 73 insertions(+), 7 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index be11a4b782a..3f5a820faef 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -32,7 +32,39 @@ class ServiceNotFoundError(Exception): pass -def service_caller(node, service_name, service_type, request, service_timeout=0.0): +def service_caller( + node, + service_name, + service_type, + request, + service_timeout=0.0, + call_timeout=10.0, + max_attempts=3, +): + """ + Abstraction of a service call. + + Has an optional timeout to find the service, receive the answer to a call + and a mechanism to retry a call of no response is received. + + @param node Node object to be associated with + @type rclpy.node.Node + @param service_name Service URL + @type str + @param request The request to be sent + @type service request type + @param service_timeout Timeout (in seconds) to wait until the service is available. 0 means + waiting forever, retrying every 10 seconds. + @type float + @param call_timeout Timeout (in seconds) for getting a response + @type float + @param max_attempts Number of attempts until a valid response is received. With some + middlewares it can happen, that the service response doesn't reach the client leaving it in + a waiting state forever. + @type int + @return The service response + + """ cli = node.create_client(service_type, service_name) while not cli.service_is_ready(): @@ -44,12 +76,20 @@ def service_caller(node, service_name, service_type, request, service_timeout=0. node.get_logger().warn(f"Could not contact service {service_name}") 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()}") + future = None + for attempt in range(max_attempts): + future = cli.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout) + if future.result() is None: + node.get_logger().warning( + f"Failed getting a result from calling {service_name} in " + f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)" + ) + else: + return future.result() + raise RuntimeError( + f"Could not successfully call service {service_name} after {max_attempts} attempts." + ) def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 3edf7704367..db3940dbccf 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -374,6 +374,32 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) } } +TEST_F(TestLoadController, spawner_with_many_controllers) +{ + std::stringstream ss; + const size_t num_controllers = 50; + const std::string controller_base_name = "ctrl_"; + for (size_t i = 0; i < num_controllers; i++) + { + const std::string controller_name = controller_base_name + std::to_string(static_cast(i)); + cm_->set_parameter( + rclcpp::Parameter(controller_name + ".type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + ss << controller_name << " "; + } + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner(ss.str() + " -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), num_controllers); + + for (size_t i = 0; i < num_controllers; i++) + { + auto ctrl = cm_->get_loaded_controllers()[i]; + ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } +} + class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture { From 0631e3edecadd8b4a50ad2a3dc1374030167f71a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 12:21:41 +0200 Subject: [PATCH 25/42] Refactor spawner to be able to reuse code for ros2controlcli (#1661) --- .../controller_manager/__init__.py | 8 + .../controller_manager_services.py | 117 +++++++++++++ .../controller_manager/hardware_spawner.py | 14 +- .../controller_manager/spawner.py | 165 +----------------- .../test/test_spawner_unspawner.cpp | 13 +- .../ros2controlcli/verb/list_controllers.py | 3 +- .../verb/list_hardware_components.py | 3 +- .../verb/list_hardware_interfaces.py | 3 +- 8 files changed, 141 insertions(+), 185 deletions(-) diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index f49bed4d343..4a8d7daee5a 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -23,6 +23,10 @@ set_hardware_component_state, switch_controllers, unload_controller, + get_parameter_from_param_file, + set_controller_parameters, + set_controller_parameters_from_param_file, + bcolors, ) __all__ = [ @@ -36,4 +40,8 @@ "set_hardware_component_state", "switch_controllers", "unload_controller", + "get_parameter_from_param_file", + "set_controller_parameters", + "set_controller_parameters_from_param_file", + "bcolors", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 3f5a820faef..7b0f726d687 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -26,6 +26,29 @@ ) import rclpy +import yaml +from rcl_interfaces.msg import Parameter + +# @note: The versions conditioning is added here to support the source-compatibility with Humble +# The `get_parameter_value` function is moved to `rclpy.parameter` module from `ros2param.api` module from version 3.6.0 +try: + from rclpy.parameter import get_parameter_value +except ImportError: + from ros2param.api import get_parameter_value +from ros2param.api import call_set_parameters + + +# from https://stackoverflow.com/a/287944 +class bcolors: + MAGENTA = "\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" class ServiceNotFoundError(Exception): @@ -220,3 +243,97 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti request, service_timeout, ) + + +def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): + with open(parameter_file) as f: + namespaced_controller = ( + controller_name if namespace == "/" else f"{namespace}/{controller_name}" + ) + parameters = yaml.safe_load(f) + if namespaced_controller in parameters: + value = parameters[namespaced_controller] + if not isinstance(value, dict) or "ros__parameters" not in value: + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" + ) + if parameter_name in parameters[namespaced_controller]["ros__parameters"]: + return parameters[namespaced_controller]["ros__parameters"][parameter_name] + else: + return None + else: + return None + + +def set_controller_parameters( + node, controller_manager_name, controller_name, parameter_name, parameter_value +): + parameter = Parameter() + parameter.name = controller_name + "." + parameter_name + parameter_string = str(parameter_value) + parameter.value = get_parameter_value(string_value=parameter_string) + + 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 + + 'Setting controller param "' + + parameter_name + + '" to "' + + parameter_string + + '" for ' + + bcolors.BOLD + + controller_name + + bcolors.ENDC + ) + else: + node.get_logger().fatal( + bcolors.FAIL + + 'Could not set controller param "' + + parameter_name + + '" to "' + + parameter_string + + '" for ' + + bcolors.BOLD + + controller_name + + bcolors.ENDC + ) + return False + return True + + +def set_controller_parameters_from_param_file( + node, controller_manager_name, controller_name, parameter_file, namespace=None +): + if parameter_file: + spawner_namespace = namespace if namespace else node.get_namespace() + set_controller_parameters( + node, controller_manager_name, controller_name, "param_file", parameter_file + ) + + controller_type = get_parameter_from_param_file( + controller_name, spawner_namespace, parameter_file, "type" + ) + if controller_type: + if not set_controller_parameters( + node, controller_manager_name, controller_name, "type", controller_type + ): + return False + + fallback_controllers = get_parameter_from_param_file( + controller_name, spawner_namespace, parameter_file, "fallback_controllers" + ) + if fallback_controllers: + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "fallback_controllers", + fallback_controllers, + ): + return False + return True diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 3e3a487c6ac..29c0b5e97cf 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -19,6 +19,7 @@ from controller_manager import ( list_hardware_components, set_hardware_component_state, + bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError @@ -28,19 +29,6 @@ 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) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 0e83eb9d073..bd277d5464a 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,7 +19,6 @@ import sys import time import warnings -import yaml from controller_manager import ( configure_controller, @@ -27,35 +26,14 @@ load_controller, switch_controllers, unload_controller, + set_controller_parameters_from_param_file, + bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy -from rcl_interfaces.msg import Parameter from rclpy.node import Node - -# @note: The versions conditioning is added here to support the source-compatibility with Humble -# The `get_parameter_value` function is moved to `rclpy.parameter` module from `ros2param.api` module from version 3.6.0 -try: - from rclpy.parameter import get_parameter_value -except ImportError: - from ros2param.api import get_parameter_value from rclpy.signals import SignalHandlerOptions -from ros2param.api import call_set_parameters - -# from https://stackoverflow.com/a/287944 - - -class bcolors: - MAGENTA = "\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): @@ -87,24 +65,6 @@ def is_controller_loaded(node, controller_manager, controller_name, service_time return any(c.name == controller_name for c in controllers) -def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): - with open(parameter_file) as f: - namespaced_controller = ( - controller_name if namespace == "/" else f"{namespace}/{controller_name}" - ) - parameters = yaml.safe_load(f) - if namespaced_controller in parameters: - value = parameters[namespaced_controller] - if not isinstance(value, dict) or "ros__parameters" not in value: - raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" - ) - if parameter_name in parameters[namespaced_controller]["ros__parameters"]: - return parameters[namespaced_controller]["ros__parameters"][parameter_name] - else: - return None - - def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() @@ -162,14 +122,6 @@ def main(args=None): action="store_true", required=False, ) - parser.add_argument( - "--fallback_controllers", - help="Fallback controllers list are activated as a fallback strategy when the" - " spawned controllers fail. When the argument is provided, it takes precedence over" - " the fallback_controllers list in the param file", - default=None, - nargs="+", - ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) @@ -210,7 +162,6 @@ def main(args=None): try: for controller_name in controller_names: - fallback_controllers = args.fallback_controllers if is_controller_loaded( node, controller_manager_name, controller_name, controller_manager_timeout @@ -221,112 +172,14 @@ def main(args=None): + bcolors.ENDC ) else: - controller_type = ( - None - if param_file is None - else get_parameter_from_param_file( - controller_name, spawner_namespace, param_file, "type" - ) - ) - if controller_type: - parameter = Parameter() - parameter.name = 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 - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller type to "' - + controller_type - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - return 1 - if param_file: - parameter = Parameter() - parameter.name = controller_name + ".params_file" - parameter.value = get_parameter_value(string_value=param_file) - - 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 params file to "' - + param_file - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller params file to "' - + param_file - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - return 1 - - if not fallback_controllers and param_file: - fallback_controllers = get_parameter_from_param_file( - controller_name, spawner_namespace, param_file, "fallback_controllers" - ) - - if fallback_controllers: - parameter = Parameter() - parameter.name = controller_name + ".fallback_controllers" - parameter.value = get_parameter_value(string_value=str(fallback_controllers)) - - 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 - + 'Setting fallback_controllers to ["' - + ",".join(fallback_controllers) - + '"] for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set fallback_controllers to ["' - + ",".join(fallback_controllers) - + '"] for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) + if not set_controller_parameters_from_param_file( + node, + controller_manager_name, + controller_name, + param_file, + spawner_namespace, + ): return 1 ret = load_controller(node, controller_manager_name, controller_name) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index db3940dbccf..852ea90bc79 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -329,20 +329,14 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ( - call_spawner( - "ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 " - "-p " + - test_file_path), - 0); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --load-only -p " + test_file_path), 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_THAT( - ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } @@ -355,8 +349,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) 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_THAT( - ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index a26a1168a11..3daf752b8ce 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_controllers -from controller_manager.spawner import bcolors +from controller_manager import list_controllers, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index e2b427b2338..e3e22699206 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -12,8 +12,7 @@ # 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 controller_manager import list_hardware_components, bcolors from lifecycle_msgs.msg import State diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index 7aa850f3bca..4510998ad9c 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_hardware_interfaces -from controller_manager.spawner import bcolors +from controller_manager import list_hardware_interfaces, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy From 079392b94867c3372c0050492327001057118090 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 12:23:33 +0200 Subject: [PATCH 26/42] Fix spawner tests timeout (#1692) --- controller_manager/test/test_spawner_unspawner.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 852ea90bc79..49326f9e796 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -255,6 +255,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; + ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file EXPECT_EQ( call_spawner( @@ -306,6 +307,7 @@ TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill // timeout command will kill it after the specified time with signal SIGINT + ControllerManagerRunner cm_runner(this); cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); std::stringstream ss; ss << "timeout --signal=INT 5 " @@ -622,6 +624,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; + ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file EXPECT_EQ( call_spawner( @@ -682,6 +685,7 @@ TEST_F( const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; + ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file EXPECT_EQ( call_spawner( From 4498d25ff7cfdc40780a34b36cf4d1fea40f7521 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 13:40:45 +0200 Subject: [PATCH 27/42] Avoid using the global arguments for internal controller nodes (#1694) --- .../controller_interface_base.hpp | 5 +++-- .../test/test_controller_interface.cpp | 13 +++++++++---- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 9eaee9f15b4..e881f08aae1 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -174,11 +174,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy { // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCLCPP_VERSION_MAJOR >= 21 - return rclcpp::NodeOptions().enable_logger_service(true); + return rclcpp::NodeOptions().enable_logger_service(true).use_global_arguments(false); #else return rclcpp::NodeOptions() .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true); + .automatically_declare_parameters_from_overrides(true) + .use_global_arguments(false); #endif } diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 03838b1a2e9..f5c0a6b3dec 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -16,6 +16,8 @@ #include #include +#include +#include #include "rclcpp/executor_options.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -57,13 +59,16 @@ TEST(TestableControllerInterface, init) TEST(TestableControllerInterface, setting_update_rate_in_configure) { // mocks the declaration of overrides parameters in a yaml file - char const * const argv[] = {"", "--ros-args", "-p", "update_rate:=2812"}; - int argc = arrlen(argv); - rclcpp::init(argc, argv); + rclcpp::init(0, nullptr); TestableControllerInterface controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); + auto node_options = controller.define_custom_node_options(); + std::vector node_options_arguments = node_options.arguments(); + node_options_arguments.push_back("--ros-args"); + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("update_rate:=2812"); + node_options = node_options.arguments(node_options_arguments); ASSERT_EQ( controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options), controller_interface::return_type::OK); From fb1d254cc73c4dd407d69d6caa813d290d8a2fa9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 21 Aug 2024 17:40:57 +0200 Subject: [PATCH 28/42] Fix params_file typo in spawner and update release notes for use_global_arguments (#1701) --- .../controller_interface/controller_interface_base.hpp | 5 ++--- .../controller_manager/controller_manager_services.py | 2 +- controller_manager/src/controller_manager.cpp | 1 + doc/migration.rst | 4 ++++ doc/release_notes.rst | 2 ++ 5 files changed, 10 insertions(+), 4 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index e881f08aae1..9eaee9f15b4 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -174,12 +174,11 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy { // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCLCPP_VERSION_MAJOR >= 21 - return rclcpp::NodeOptions().enable_logger_service(true).use_global_arguments(false); + return rclcpp::NodeOptions().enable_logger_service(true); #else return rclcpp::NodeOptions() .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true) - .use_global_arguments(false); + .automatically_declare_parameters_from_overrides(true); #endif } diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 7b0f726d687..a09be5d14ba 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -312,7 +312,7 @@ def set_controller_parameters_from_param_file( if parameter_file: spawner_namespace = namespace if namespace else node.get_namespace() set_controller_parameters( - node, controller_manager_name, controller_name, "param_file", parameter_file + node, controller_manager_name, controller_name, "params_file", parameter_file ) controller_type = get_parameter_from_param_file( diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 541fc57d02a..734596f8bdc 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2843,6 +2843,7 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( } controller_node_options = controller_node_options.arguments(node_options_arguments); + controller_node_options.use_global_arguments(false); return controller_node_options; } diff --git a/doc/migration.rst b/doc/migration.rst index 68359f072f0..f24f338e52b 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -2,6 +2,10 @@ Iron to Jazzy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +controller_interface +******************** +* The changes from `(PR #1694) `__ will affect how the controllers will be loading the parameters. Defining parameters in a single yaml file and loading it to the controller_manager node alone will no longer work. + In order to load the parameters to the controllers properly, it is needed to use ``--param-file`` option from the spawner. This is because the controllers will now set ``use_global_arguments`` from NodeOptions to false, to avoid getting influenced by global arguments. controller_manager ****************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index caa28ca9d0e..fa393d72ff4 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,6 +22,8 @@ For details see the controller_manager section. * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. +* The controllers will now set ``use_global_arguments`` from NodeOptions to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). + From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. controller_manager ****************** From 40ea19176d47c63e9e873b7744cfad8eef366b41 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 21 Aug 2024 17:41:57 +0200 Subject: [PATCH 29/42] Introduce Creation of Handles with InterfaceDescription (variant support) (#1679) --- .../hardware_interface/component_parser.hpp | 18 +++ .../include/hardware_interface/handle.hpp | 30 +++-- .../hardware_interface/hardware_info.hpp | 39 ++++++- hardware_interface/src/component_parser.cpp | 34 ++++++ .../test/test_component_parser.cpp | 110 ++++++++++++++++++ hardware_interface/test/test_handle.cpp | 31 +++++ 6 files changed, 252 insertions(+), 10 deletions(-) diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 38ca0cf89d7..2d0c067606c 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -32,5 +32,23 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's StateInterfaces for the component + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_state_interface_descriptions( + const std::vector & component_info); + +/** + * \param[in] component_info information about a component (gpio, joint, sensor) + * \return vector filled with information about hardware's CommandInterfaces for the component + * which are exported + */ +HARDWARE_INTERFACE_PUBLIC +std::vector parse_command_interface_descriptions( + const std::vector & component_info); + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index bfe0ff1eb89..6f919033de4 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -15,9 +15,11 @@ #ifndef HARDWARE_INTERFACE__HANDLE_HPP_ #define HARDWARE_INTERFACE__HANDLE_HPP_ +#include #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/macros.hpp" namespace hardware_interface @@ -29,6 +31,8 @@ using HANDLE_DATATYPE = std::variant; class Handle { public: + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) @@ -36,11 +40,25 @@ class Handle { } + explicit Handle(const InterfaceDescription & interface_description) + : prefix_name_(interface_description.prefix_name), + interface_name_(interface_description.interface_info.name) + { + // As soon as multiple datatypes are used in HANDLE_DATATYPE + // we need to initialize according the type passed in interface description + value_ = std::numeric_limits::quiet_NaN(); + value_ptr_ = std::get_if(&value_); + } + + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } + [[deprecated("Use InterfaceDescription for initializing the Interface")]] + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { @@ -103,10 +121,8 @@ class Handle class StateInterface : public Handle { public: - explicit StateInterface( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : Handle(prefix_name, interface_name, value_ptr) + explicit StateInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } @@ -120,10 +136,8 @@ class StateInterface : public Handle class CommandInterface : public Handle { public: - explicit CommandInterface( - const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : Handle(prefix_name, interface_name, value_ptr) + explicit CommandInterface(const InterfaceDescription & interface_description) + : Handle(interface_description) { } /// CommandInterface copy constructor is actively deleted. diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 2bd2099e695..b4e47c610e1 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -40,9 +40,9 @@ struct InterfaceInfo std::string max; /// (Optional) Initial value of the interface. std::string initial_value; - /// (Optional) The datatype of the interface, e.g. "bool", "int". Used by GPIOs. + /// (Optional) The datatype of the interface, e.g. "bool", "int". std::string data_type; - /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. + /// (Optional) If the handle is an array, the size of the array. int size; /// (Optional) enable or disable the limits for the command interfaces bool enable_limits; @@ -126,6 +126,41 @@ struct TransmissionInfo std::unordered_map parameters; }; +/** + * This structure stores information about an interface for a specific hardware which should be + * instantiated internally. + */ +struct InterfaceDescription +{ + InterfaceDescription(const std::string & prefix_name_in, const InterfaceInfo & interface_info_in) + : prefix_name(prefix_name_in), + interface_info(interface_info_in), + interface_name(prefix_name + "/" + interface_info.name) + { + } + + /** + * Name of the interface defined by the user. + */ + std::string prefix_name; + + /** + * Information about the Interface type (position, velocity,...) as well as limits and so on. + */ + InterfaceInfo interface_info; + + /** + * Name of the interface + */ + std::string interface_name; + + std::string get_prefix_name() const { return prefix_name; } + + std::string get_interface_name() const { return interface_info.name; } + + std::string get_name() const { return interface_name; } +}; + /// This structure stores information about hardware defined in a robot's URDF. struct HardwareInfo { diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 42432dda8de..0ef6c084f9d 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -905,4 +905,38 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } +std::vector parse_state_interface_descriptions( + const std::vector & component_info) +{ + std::vector component_state_interface_descriptions; + component_state_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & state_interface : component.state_interfaces) + { + component_state_interface_descriptions.emplace_back( + InterfaceDescription(component.name, state_interface)); + } + } + return component_state_interface_descriptions; +} + +std::vector parse_command_interface_descriptions( + const std::vector & component_info) +{ + std::vector component_command_interface_descriptions; + component_command_interface_descriptions.reserve(component_info.size()); + + for (const auto & component : component_info) + { + for (const auto & command_interface : component.command_interfaces) + { + component_command_interface_descriptions.emplace_back( + InterfaceDescription(component.name, command_interface)); + } + } + return component_command_interface_descriptions; +} + } // namespace hardware_interface diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index be891787f3e..6a78b0807dd 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1404,3 +1404,113 @@ TEST_F(TestComponentParser, urdf_incomplete_throws_error) std::string(ros2_control_test_assets::urdf_tail); ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } + +TEST_F(TestComponentParser, parse_joint_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].joints); + EXPECT_EQ(joint_state_descriptions[0].get_prefix_name(), "joint1"); + EXPECT_EQ(joint_state_descriptions[0].get_interface_name(), "position"); + EXPECT_EQ(joint_state_descriptions[0].get_name(), "joint1/position"); + + EXPECT_EQ(joint_state_descriptions[1].get_prefix_name(), "joint2"); + EXPECT_EQ(joint_state_descriptions[1].get_interface_name(), "position"); + EXPECT_EQ(joint_state_descriptions[1].get_name(), "joint2/position"); +} + +TEST_F(TestComponentParser, parse_joint_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto joint_command_descriptions = + parse_command_interface_descriptions(control_hardware[0].joints); + EXPECT_EQ(joint_command_descriptions[0].get_prefix_name(), "joint1"); + EXPECT_EQ(joint_command_descriptions[0].get_interface_name(), "position"); + EXPECT_EQ(joint_command_descriptions[0].get_name(), "joint1/position"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[0].interface_info.max, "1"); + + EXPECT_EQ(joint_command_descriptions[1].get_prefix_name(), "joint2"); + EXPECT_EQ(joint_command_descriptions[1].get_interface_name(), "position"); + EXPECT_EQ(joint_command_descriptions[1].get_name(), "joint2/position"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.min, "-1"); + EXPECT_EQ(joint_command_descriptions[1].interface_info.max, "1"); +} + +TEST_F(TestComponentParser, parse_sensor_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_sensor_only + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto sensor_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].sensors); + EXPECT_EQ(sensor_state_descriptions[0].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[0].get_interface_name(), "roll"); + EXPECT_EQ(sensor_state_descriptions[0].get_name(), "sensor1/roll"); + EXPECT_EQ(sensor_state_descriptions[1].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[1].get_interface_name(), "pitch"); + EXPECT_EQ(sensor_state_descriptions[1].get_name(), "sensor1/pitch"); + EXPECT_EQ(sensor_state_descriptions[2].get_prefix_name(), "sensor1"); + EXPECT_EQ(sensor_state_descriptions[2].get_interface_name(), "yaw"); + EXPECT_EQ(sensor_state_descriptions[2].get_name(), "sensor1/yaw"); + + EXPECT_EQ(sensor_state_descriptions[3].get_prefix_name(), "sensor2"); + EXPECT_EQ(sensor_state_descriptions[3].get_interface_name(), "image"); + EXPECT_EQ(sensor_state_descriptions[3].get_name(), "sensor2/image"); +} + +TEST_F(TestComponentParser, parse_gpio_state_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_state_interface_descriptions(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "analog_input1"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_analog_IOs/analog_input1"); + EXPECT_EQ(gpio_state_descriptions[2].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[2].get_interface_name(), "analog_input2"); + EXPECT_EQ(gpio_state_descriptions[2].get_name(), "flange_analog_IOs/analog_input2"); + + EXPECT_EQ(gpio_state_descriptions[3].get_prefix_name(), "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_interface_name(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[3].get_name(), "flange_vacuum/vacuum"); +} + +TEST_F(TestComponentParser, parse_gpio_command_interface_descriptions_from_hardware_info) +{ + const std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + + const auto gpio_state_descriptions = + parse_command_interface_descriptions(control_hardware[0].gpios); + EXPECT_EQ(gpio_state_descriptions[0].get_prefix_name(), "flange_analog_IOs"); + EXPECT_EQ(gpio_state_descriptions[0].get_interface_name(), "analog_output1"); + EXPECT_EQ(gpio_state_descriptions[0].get_name(), "flange_analog_IOs/analog_output1"); + + EXPECT_EQ(gpio_state_descriptions[1].get_prefix_name(), "flange_vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_interface_name(), "vacuum"); + EXPECT_EQ(gpio_state_descriptions[1].get_name(), "flange_vacuum/vacuum"); +} diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index da8258c6435..7d79c032f08 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -14,8 +14,11 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" using hardware_interface::CommandInterface; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using hardware_interface::StateInterface; namespace @@ -64,3 +67,31 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } + +TEST(TestHandle, interface_description_state_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + StateInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} + +TEST(TestHandle, interface_description_command_interface_name_getters_work) +{ + const std::string POSITION_INTERFACE = "position"; + const std::string JOINT_NAME_1 = "joint1"; + InterfaceInfo info; + info.name = POSITION_INTERFACE; + InterfaceDescription interface_descr(JOINT_NAME_1, info); + CommandInterface handle{interface_descr}; + + EXPECT_EQ(handle.get_name(), JOINT_NAME_1 + "/" + POSITION_INTERFACE); + EXPECT_EQ(handle.get_interface_name(), POSITION_INTERFACE); + EXPECT_EQ(handle.get_prefix_name(), JOINT_NAME_1); +} From ea215615c57afea054dc5b97e5287993f3dbaf6c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 22 Aug 2024 10:28:36 +0200 Subject: [PATCH 30/42] Add `use_global_arguments` NodeOptions documentation reference (#1707) --- doc/migration.rst | 2 +- doc/release_notes.rst | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/migration.rst b/doc/migration.rst index f24f338e52b..8e1ca096f1a 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -5,7 +5,7 @@ Iron to Jazzy controller_interface ******************** * The changes from `(PR #1694) `__ will affect how the controllers will be loading the parameters. Defining parameters in a single yaml file and loading it to the controller_manager node alone will no longer work. - In order to load the parameters to the controllers properly, it is needed to use ``--param-file`` option from the spawner. This is because the controllers will now set ``use_global_arguments`` from NodeOptions to false, to avoid getting influenced by global arguments. + In order to load the parameters to the controllers properly, it is needed to use ``--param-file`` option from the spawner. This is because the controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments. controller_manager ****************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index fa393d72ff4..4aaa828091f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,7 +22,7 @@ For details see the controller_manager section. * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. -* The controllers will now set ``use_global_arguments`` from NodeOptions to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). +* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. controller_manager From 0a607fe9a86d558f060a5ac1678878f90af78c16 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 22 Aug 2024 11:02:25 +0200 Subject: [PATCH 31/42] Use handle_name_ variable instead of allocating for every `get_name` call (#1706) --- .../include/hardware_interface/handle.hpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 6f919033de4..07ba1cd98ed 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -36,13 +36,17 @@ class Handle Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) - : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) + : prefix_name_(prefix_name), + interface_name_(interface_name), + handle_name_(prefix_name_ + "/" + interface_name_), + value_ptr_(value_ptr) { } explicit Handle(const InterfaceDescription & interface_description) : prefix_name_(interface_description.prefix_name), - interface_name_(interface_description.interface_info.name) + interface_name_(interface_description.interface_info.name), + handle_name_(prefix_name_ + "/" + interface_name_) { // As soon as multiple datatypes are used in HANDLE_DATATYPE // we need to initialize according the type passed in interface description @@ -53,14 +57,14 @@ class Handle [[deprecated("Use InterfaceDescription for initializing the Interface")]] explicit Handle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr) { } [[deprecated("Use InterfaceDescription for initializing the Interface")]] explicit Handle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + : interface_name_(interface_name), handle_name_("/" + interface_name_), value_ptr_(nullptr) { } @@ -77,12 +81,12 @@ class Handle /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } - const std::string get_name() const { return prefix_name_ + "/" + interface_name_; } + const std::string & get_name() const { return handle_name_; } const std::string & get_interface_name() const { return interface_name_; } [[deprecated( - "Replaced by get_name method, which is semantically more correct")]] const std::string + "Replaced by get_name method, which is semantically more correct")]] const std::string & get_full_name() const { return get_name(); @@ -111,6 +115,7 @@ class Handle protected: std::string prefix_name_; std::string interface_name_; + std::string handle_name_; HANDLE_DATATYPE value_; // BEGIN (Handle export change): for backward compatibility // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed From dd352eb45a9a983116f61dc1a5fc650003844cf3 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Thu, 22 Aug 2024 11:05:06 +0200 Subject: [PATCH 32/42] inform user what reason is for not setting rt policy, inform is policy (#1705) --- controller_manager/src/ros2_control_node.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 44c9c37aedc..8afc81877f4 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -51,10 +52,16 @@ int main(int argc, char ** argv) { RCLCPP_WARN( cm->get_logger(), - "Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT " - "scheduling. See " + "Could not enable FIFO RT scheduling policy: with error number <%i>(%s). See " "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] " - "for details."); + "for details on how to enable realtime scheduling.", + errno, strerror(errno)); + } + else + { + RCLCPP_INFO( + cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.", + kSchedPriority); } // for calculating sleep time From c619b8a1755514db8a1a2074d9ff7b06026c3648 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 22 Aug 2024 19:41:52 +0100 Subject: [PATCH 33/42] Update changelogs --- controller_interface/CHANGELOG.rst | 6 ++++++ controller_manager/CHANGELOG.rst | 15 +++++++++++++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 8 ++++++++ hardware_interface_testing/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 6 ++++++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 6 ++++++ 11 files changed, 59 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 7b2169ed00c..f7f5c5271b5 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) +* Avoid using the global arguments for internal controller nodes (`#1694 `_) +* Contributors: Sai Kishor Kothakota + 4.15.0 (2024-08-05) ------------------- diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 46d36633623..a8cccd39680 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* inform user what reason is for not setting rt policy, inform is policy (`#1705 `_) +* Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) +* Fix spawner tests timeout (`#1692 `_) +* Refactor spawner to be able to reuse code for ros2controlcli (`#1661 `_) +* Robustify controller spawner and add integration test with many controllers (`#1501 `_) +* Handle waiting in Spawner and align Hardware Spawner functionality (`#1562 `_) +* Make list controller and list hardware components immediately visualize the state. (`#1606 `_) +* [CI] Add coveragepy and remove ignore: test (`#1668 `_) +* Fix spawner unload on kill test (`#1675 `_) +* [CM] Add more logging for easier debugging (`#1645 `_) +* refactor SwitchParams to fix the incosistencies in the spawner tests (`#1638 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Dr. Denis, Felix Exner (fexner), Manuel Muth, Sai Kishor Kothakota + 4.15.0 (2024-08-05) ------------------- * Add missing include for executors (`#1653 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index c8c11b580da..129f12fdef7 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index bb0b478e4b2..63c5a8ccb27 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Use handle_name\_ variable instead of allocating for every `get_name` call (`#1706 `_) +* Introduce Creation of Handles with InterfaceDescription (variant support) (`#1679 `_) +* Preparation of Handles for Variant Support (`#1678 `_) +* [RM] Decouple read/write cycles of each component with mutex to not block other components (`#1646 `_) +* Contributors: Manuel Muth, Sai Kishor Kothakota + 4.15.0 (2024-08-05) ------------------- * [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 3a60a2f5344..9a8c46c82bc 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- * [RM] Add `get_hardware_info` method to the Hardware Components (`#1643 `_) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 8581d26f729..4a0c4340180 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 5106f98c384..2a67c358d76 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index f7e7d771b2e..b84fd787ee6 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 265084e74e4..39b8db6c85e 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Refactor spawner to be able to reuse code for ros2controlcli (`#1661 `_) +* Make list controller and list hardware components immediately visualize the state. (`#1606 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + 4.15.0 (2024-08-05) ------------------- diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 30a674acc50..94626a29180 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.15.0 (2024-08-05) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 157a8ac09ab..d6de7ed5211 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Preparation of Handles for Variant Support (`#1678 `_) +* Fix flaky transmission_interface tests by making them deterministic. (`#1665 `_) +* Contributors: Manuel Muth, sgmurray + 4.15.0 (2024-08-05) ------------------- From ecfec159921bc646cf6350eab4e8bdc7bb48b6ed Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 22 Aug 2024 19:42:18 +0100 Subject: [PATCH 34/42] 4.16.0 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index f7f5c5271b5..8a279ace350 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- * Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) * Avoid using the global arguments for internal controller nodes (`#1694 `_) * Contributors: Sai Kishor Kothakota diff --git a/controller_interface/package.xml b/controller_interface/package.xml index a60e5e90b65..951811ba962 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.15.0 + 4.16.0 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index a8cccd39680..138e7b5738a 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- * inform user what reason is for not setting rt policy, inform is policy (`#1705 `_) * Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) * Fix spawner tests timeout (`#1692 `_) diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 9f9a9bf796d..981fbd919c3 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.15.0 + 4.16.0 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 129f12fdef7..054243f0f80 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index b6a57c26365..5dc5d694376 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.15.0 + 4.16.0 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 63c5a8ccb27..71920c6f89b 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- * Use handle_name\_ variable instead of allocating for every `get_name` call (`#1706 `_) * Introduce Creation of Handles with InterfaceDescription (variant support) (`#1679 `_) * Preparation of Handles for Variant Support (`#1678 `_) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 77d084ae7a5..2d52a3327a1 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.15.0 + 4.16.0 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 9a8c46c82bc..9b1cb05d00e 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 646923b4e66..13afe926a5f 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.15.0 + 4.16.0 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 4a0c4340180..21f0d95a24f 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 1ff460cf141..d9e18baa1de 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.15.0 + 4.16.0 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 2a67c358d76..b7c06c1964f 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index d5ef3ba862a..d9ade902ca9 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.15.0 + 4.16.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 b84fd787ee6..918f5f35025 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 7be61a2f912..4bcf6cd1e4f 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.15.0 + 4.16.0 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 39b8db6c85e..81d2fe97b54 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- * Refactor spawner to be able to reuse code for ros2controlcli (`#1661 `_) * Make list controller and list hardware components immediately visualize the state. (`#1606 `_) * Contributors: Dr. Denis, Sai Kishor Kothakota diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index f7ed71dae9c..c11b4cf94fb 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.15.0 + 4.16.0 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index d1c8e2d1508..6b8a169fd64 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.15.0", + version="4.16.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 94626a29180..0593af27780 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- 4.15.0 (2024-08-05) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index fc3de218975..7b39a442c4d 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.15.0 + 4.16.0 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index fad594c205a..da798c0e6b9 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.15.0", + version="4.16.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index d6de7ed5211..46f665cdb5c 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.0 (2024-08-22) +------------------- * Preparation of Handles for Variant Support (`#1678 `_) * Fix flaky transmission_interface tests by making them deterministic. (`#1665 `_) * Contributors: Manuel Muth, sgmurray diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 9ee959c5bcd..44d479a62c5 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.15.0 + 4.16.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 From 08492a161b581b720a7b123a24ff74aef09dd370 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 23 Aug 2024 20:44:50 +0200 Subject: [PATCH 35/42] propage a portion of global args to the controller nodes (#1712) --- .../controller_manager/controller_manager.hpp | 1 + controller_manager/src/controller_manager.cpp | 51 +++++++++++++++---- controller_manager/src/ros2_control_node.cpp | 17 ++++++- 3 files changed, 58 insertions(+), 11 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 810936554eb..7c09377bb97 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -577,6 +577,7 @@ class ControllerManager : public rclcpp::Node std::map> controller_chained_reference_interfaces_cache_; std::map> controller_chained_state_interfaces_cache_; + rclcpp::NodeOptions cm_node_options_; std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 734596f8bdc..70b9c8d6281 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -24,6 +24,7 @@ #include "controller_manager_msgs/msg/hardware_component_state.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rcl/arguments.h" #include "rclcpp/version.h" #include "rclcpp_lifecycle/state.hpp" @@ -197,7 +198,8 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) { init_controller_manager(); } @@ -217,7 +219,8 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) { init_controller_manager(); } @@ -234,7 +237,8 @@ ControllerManager::ControllerManager( kControllerInterfaceNamespace, kControllerInterfaceClassName)), chainable_loader_( std::make_shared>( - kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) + kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)), + cm_node_options_(options) { init_controller_manager(); } @@ -2819,14 +2823,30 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( rclcpp::NodeOptions controller_node_options = controller.c->define_custom_node_options(); std::vector node_options_arguments = controller_node_options.arguments(); - const std::string ros_args_arg = "--ros-args"; + + for (const std::string & arg : cm_node_options_.arguments()) + { + if (arg.find("__ns") != std::string::npos || arg.find("__node") != std::string::npos) + { + if ( + node_options_arguments.back() == RCL_REMAP_FLAG || + node_options_arguments.back() == RCL_SHORT_REMAP_FLAG) + { + node_options_arguments.pop_back(); + } + continue; + } + + node_options_arguments.push_back(arg); + } + if (controller.info.parameters_file.has_value()) { - if (!check_for_element(node_options_arguments, ros_args_arg)) + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { - node_options_arguments.push_back(ros_args_arg); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } - node_options_arguments.push_back("--params-file"); + node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); node_options_arguments.push_back(controller.info.parameters_file.value()); } @@ -2834,14 +2854,25 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); if (use_sim_time.as_bool()) { - if (!check_for_element(node_options_arguments, ros_args_arg)) + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { - node_options_arguments.push_back(ros_args_arg); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } - node_options_arguments.push_back("-p"); + node_options_arguments.push_back(RCL_PARAM_FLAG); node_options_arguments.push_back("use_sim_time:=true"); } + std::string arguments; + arguments.reserve(1000); + for (const auto & arg : node_options_arguments) + { + arguments.append(arg); + arguments.append(" "); + } + RCLCPP_INFO( + get_logger(), "Controller '%s' node arguments: %s", controller.info.name.c_str(), + arguments.c_str()); + controller_node_options = controller_node_options.arguments(node_options_arguments); controller_node_options.use_global_arguments(false); return controller_node_options; diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 8afc81877f4..9c996d5aca4 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -41,7 +41,22 @@ int main(int argc, char ** argv) std::make_shared(); std::string manager_node_name = "controller_manager"; - auto cm = std::make_shared(executor, manager_node_name); + rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options(); + std::vector node_arguments = cm_node_options.arguments(); + for (int i = 1; i < argc; ++i) + { + if (node_arguments.empty() && std::string(argv[i]) != "--ros-args") + { + // A simple way to reject non ros args + continue; + } + RCLCPP_INFO(rclcpp::get_logger("CM args"), "Adding argument: %s", argv[i]); + node_arguments.push_back(argv[i]); + } + cm_node_options.arguments(node_arguments); + + auto cm = std::make_shared( + executor, manager_node_name, "", cm_node_options); RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); From 0aa7eef925476209bfeb69e84aa3355c007fcefc Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 24 Aug 2024 10:56:34 +0100 Subject: [PATCH 36/42] Update changelogs --- controller_interface/CHANGELOG.rst | 3 +++ controller_manager/CHANGELOG.rst | 5 +++++ controller_manager_msgs/CHANGELOG.rst | 3 +++ hardware_interface/CHANGELOG.rst | 3 +++ hardware_interface_testing/CHANGELOG.rst | 3 +++ joint_limits/CHANGELOG.rst | 3 +++ ros2_control/CHANGELOG.rst | 3 +++ ros2_control_test_assets/CHANGELOG.rst | 3 +++ ros2controlcli/CHANGELOG.rst | 3 +++ rqt_controller_manager/CHANGELOG.rst | 3 +++ transmission_interface/CHANGELOG.rst | 3 +++ 11 files changed, 35 insertions(+) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index 8a279ace350..de6c1e4566c 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- * Fix params_file typo in spawner and update release notes for use_global_arguments (`#1701 `_) diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 138e7b5738a..5157f6b73b5 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* propage a portion of global args to the controller nodes (`#1712 `_) +* Contributors: Sai Kishor Kothakota + 4.16.0 (2024-08-22) ------------------- * inform user what reason is for not setting rt policy, inform is policy (`#1705 `_) diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 054243f0f80..0c7c61ee3d3 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 71920c6f89b..fb38e49c867 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- * Use handle_name\_ variable instead of allocating for every `get_name` call (`#1706 `_) diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index 9b1cb05d00e..cf0769ece25 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 21f0d95a24f..3202626c661 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index b7c06c1964f..5ccc558858c 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 918f5f35025..869e1da56f4 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 81d2fe97b54..027e57aff62 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- * Refactor spawner to be able to reuse code for ros2controlcli (`#1661 `_) diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 0593af27780..27727d8b9f8 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 46f665cdb5c..83371dadd0c 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 4.16.0 (2024-08-22) ------------------- * Preparation of Handles for Variant Support (`#1678 `_) From 61648838293df9d3868f75f839908f6d224a2587 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 24 Aug 2024 10:57:09 +0100 Subject: [PATCH 37/42] 4.16.1 --- controller_interface/CHANGELOG.rst | 4 ++-- controller_interface/package.xml | 2 +- controller_manager/CHANGELOG.rst | 4 ++-- controller_manager/package.xml | 2 +- controller_manager_msgs/CHANGELOG.rst | 4 ++-- controller_manager_msgs/package.xml | 2 +- hardware_interface/CHANGELOG.rst | 4 ++-- hardware_interface/package.xml | 2 +- hardware_interface_testing/CHANGELOG.rst | 4 ++-- hardware_interface_testing/package.xml | 2 +- joint_limits/CHANGELOG.rst | 4 ++-- joint_limits/package.xml | 2 +- ros2_control/CHANGELOG.rst | 4 ++-- ros2_control/package.xml | 2 +- ros2_control_test_assets/CHANGELOG.rst | 4 ++-- ros2_control_test_assets/package.xml | 2 +- ros2controlcli/CHANGELOG.rst | 4 ++-- ros2controlcli/package.xml | 2 +- ros2controlcli/setup.py | 2 +- rqt_controller_manager/CHANGELOG.rst | 4 ++-- rqt_controller_manager/package.xml | 2 +- rqt_controller_manager/setup.py | 2 +- transmission_interface/CHANGELOG.rst | 4 ++-- transmission_interface/package.xml | 2 +- 24 files changed, 35 insertions(+), 35 deletions(-) diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index de6c1e4566c..5a0a5ff5732 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 951811ba962..39d15a6f200 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 4.16.0 + 4.16.1 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 5157f6b73b5..2729f2f7fe5 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- * propage a portion of global args to the controller nodes (`#1712 `_) * Contributors: Sai Kishor Kothakota diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 981fbd919c3..cb6b380092c 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 4.16.0 + 4.16.1 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 0c7c61ee3d3..56e80d736b9 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index 5dc5d694376..195fda32f14 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 4.16.0 + 4.16.1 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index fb38e49c867..405756a48c4 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 2d52a3327a1..8c19535675c 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 4.16.0 + 4.16.1 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst index cf0769ece25..f1cd9531d03 100644 --- a/hardware_interface_testing/CHANGELOG.rst +++ b/hardware_interface_testing/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package hardware_interface_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml index 13afe926a5f..a88ae6ba939 100644 --- a/hardware_interface_testing/package.xml +++ b/hardware_interface_testing/package.xml @@ -1,7 +1,7 @@ hardware_interface_testing - 4.16.0 + 4.16.1 ros2_control hardware interface testing Bence Magyar Denis Štogl diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 3202626c661..02f6673e883 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index d9e18baa1de..8305cff9e5c 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 4.16.0 + 4.16.1 Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 5ccc558858c..f522eda7728 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index d9ade902ca9..a8c423658d6 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 4.16.0 + 4.16.1 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index 869e1da56f4..d5e898924b3 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index 4bcf6cd1e4f..ea22ab6b11d 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 4.16.0 + 4.16.1 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index 027e57aff62..639a295a067 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index c11b4cf94fb..176334ad544 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 4.16.0 + 4.16.1 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index 6b8a169fd64..c0395ec63d2 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="4.16.0", + version="4.16.1", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 27727d8b9f8..abfab9c3c38 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 7b39a442c4d..90e9fd14620 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 4.16.0 + 4.16.1 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index da798c0e6b9..cd0b3141a7f 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.16.0", + version="4.16.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index 83371dadd0c..523f72c1ee5 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.16.1 (2024-08-24) +------------------- 4.16.0 (2024-08-22) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 44d479a62c5..6a982ddef97 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 4.16.0 + 4.16.1 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl From 7a5779c43d73b4be6ca5344d62a75b89b3ca76c7 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Mon, 26 Aug 2024 14:09:18 +0200 Subject: [PATCH 38/42] Rename `get_state` and `set_state` Functions to `get/set_lifecylce_state` (variant support) (#1683) --- .../controller_interface/async_controller.hpp | 3 +- .../controller_interface_base.hpp | 2 +- .../src/chainable_controller_interface.cpp | 5 +- .../src/controller_interface_base.cpp | 4 +- controller_manager/src/controller_manager.cpp | 13 +- .../test_chainable_controller.cpp | 8 +- .../test/test_controller/test_controller.cpp | 8 +- .../test/test_controller_manager.cpp | 47 ++- ...roller_manager_hardware_error_handling.cpp | 137 ++++--- .../test/test_controller_manager_srvs.cpp | 30 +- .../test_controller_manager_urdf_passing.cpp | 13 +- ...llers_chaining_with_controller_manager.cpp | 355 +++++++++++------- .../test/test_load_controller.cpp | 98 +++-- .../test/test_release_interfaces.cpp | 32 +- .../test/test_spawner_unspawner.cpp | 65 ++-- doc/release_notes.rst | 5 +- .../include/hardware_interface/actuator.hpp | 2 +- .../hardware_interface/actuator_interface.hpp | 7 +- .../hardware_interface/async_components.hpp | 4 +- .../include/hardware_interface/sensor.hpp | 2 +- .../hardware_interface/sensor_interface.hpp | 7 +- .../include/hardware_interface/system.hpp | 2 +- .../hardware_interface/system_interface.hpp | 7 +- hardware_interface/src/actuator.cpp | 97 ++--- hardware_interface/src/resource_manager.cpp | 21 +- hardware_interface/src/sensor.cpp | 89 ++--- hardware_interface/src/system.cpp | 97 ++--- .../test/test_component_interfaces.cpp | 20 +- 28 files changed, 700 insertions(+), 480 deletions(-) diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp index 5601ff4703b..357b3a2ce37 100644 --- a/controller_interface/include/controller_interface/async_controller.hpp +++ b/controller_interface/include/controller_interface/async_controller.hpp @@ -82,7 +82,8 @@ class AsyncControllerThread 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) + if ( + controller_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { auto const current_time = controller_->get_node()->now(); auto const measured_period = current_time - previous_time; diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 9eaee9f15b4..fbb2111cc9e 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -147,7 +147,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::shared_ptr get_node() const; CONTROLLER_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; CONTROLLER_INTERFACE_PUBLIC unsigned int get_update_rate() const; diff --git a/controller_interface/src/chainable_controller_interface.cpp b/controller_interface/src/chainable_controller_interface.cpp index 9f4a171ec37..ae5cd326b65 100644 --- a/controller_interface/src/chainable_controller_interface.cpp +++ b/controller_interface/src/chainable_controller_interface.cpp @@ -97,7 +97,7 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) { bool result = false; - if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = on_set_chained_mode(chained_mode); @@ -112,7 +112,8 @@ bool ChainableControllerInterface::set_chained_mode(bool chained_mode) get_node()->get_logger(), "Can not change controller's chained mode because it is no in '%s' state. " "Current state is '%s'.", - hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state().label().c_str()); + hardware_interface::lifecycle_state_names::UNCONFIGURED, + get_lifecycle_state().label().c_str()); } return result; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index db9fb99d81f..61d2c780b82 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -83,7 +83,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() // Then we don't need to do state-machine related checks. // // Other solution is to add check into the LifecycleNode if a transition is valid to trigger - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); is_async_ = get_node()->get_parameter("is_async").as_bool(); @@ -106,7 +106,7 @@ void ControllerInterfaceBase::release_interfaces() state_interfaces_.clear(); } -const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state() const +const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const { return node_->get_current_state(); } diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 70b9c8d6281..275f12f5427 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -58,7 +58,8 @@ static const rmw_qos_profile_t qos_services = { inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller) { - return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; + return controller.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; } inline bool is_controller_inactive( @@ -69,7 +70,7 @@ inline bool is_controller_inactive( inline bool is_controller_active(const controller_interface::ControllerInterfaceBase & controller) { - return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + return controller.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; } inline bool is_controller_active( @@ -659,7 +660,7 @@ controller_interface::return_type ControllerManager::configure_controller( } auto controller = found_it->c; - auto state = controller->get_state(); + auto state = controller->get_lifecycle_state(); if ( state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) @@ -670,7 +671,7 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } - auto new_state = controller->get_state(); + auto new_state = controller->get_lifecycle_state(); if (state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_DEBUG( @@ -1744,7 +1745,7 @@ void ControllerManager::list_controllers_srv_cb( controller_state.name = controllers[i].info.name; controller_state.type = controllers[i].info.type; controller_state.claimed_interfaces = controllers[i].info.claimed_interfaces; - controller_state.state = controllers[i].c->get_state().label(); + controller_state.state = controllers[i].c->get_lifecycle_state().label(); controller_state.is_chainable = controllers[i].c->is_chainable(); controller_state.is_chained = controllers[i].c->is_in_chained_mode(); @@ -2728,7 +2729,7 @@ void ControllerManager::controller_activity_diagnostic_callback( { all_active = false; } - stat.add(controllers[i].info.name, controllers[i].c->get_state().label()); + stat.add(controllers[i].info.name, controllers[i].c->get_lifecycle_state().label()); } if (all_active) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp index e43f2a13a1d..e68af6345f9 100644 --- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -33,8 +33,8 @@ controller_interface::InterfaceConfiguration TestChainableController::command_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return cmd_iface_cfg_; } @@ -49,8 +49,8 @@ controller_interface::InterfaceConfiguration TestChainableController::state_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { auto state_iface_cfg = state_iface_cfg_; if (imu_sensor_) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7e3419fbc90..7b9af6ecfc2 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -31,8 +31,8 @@ TestController::TestController() controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return cmd_iface_cfg_; } @@ -46,8 +46,8 @@ controller_interface::InterfaceConfiguration TestController::command_interface_c controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const { if ( - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return state_iface_cfg_; } diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 5093250dfe8..28e9f0477c7 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -127,7 +127,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) << "Update should not reach an unconfigured controller"; EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); // configure controller { @@ -141,7 +142,9 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; @@ -182,7 +185,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) ControllerManagerRunner cm_runner(this); EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); EXPECT_EQ( controller_interface::return_type::OK, @@ -210,7 +214,9 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); auto unload_future = std::async( std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, test_controller::TEST_CONTROLLER_NAME); @@ -221,7 +227,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(1, test_controller.use_count()); } @@ -242,7 +249,8 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) << "Update should not reach an unconfigured controller"; EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); test_controller->get_node()->set_parameter({"update_rate", 4}); // configure controller @@ -255,7 +263,9 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; @@ -276,7 +286,8 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); // As the controller frequency is 4Hz, it needs to pass 25 iterations for 1 update cycle for (size_t i = 0; i < 25; i++) @@ -323,7 +334,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd << "Update should not reach an unconfigured controller"; EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); rclcpp::Parameter update_rate_parameter("update_rate", static_cast(ctrl_update_rate)); test_controller->get_node()->set_parameter(update_rate_parameter); @@ -337,7 +349,9 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; @@ -360,7 +374,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); const auto pre_internal_counter = test_controller->internal_counter; rclcpp::Rate loop_rate(cm_->get_update_rate()); @@ -430,7 +445,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); test_controller->get_node()->set_parameter({"update_rate", static_cast(ctrl_update_rate)}); // configure controller @@ -443,7 +459,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function time_ = test_controller->get_node()->now(); // set to something nonzero @@ -467,7 +485,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); } - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); const auto cm_update_rate = cm_->get_update_rate(); diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 6e2fba23db8..936a2dd4c48 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -122,16 +122,16 @@ class TestControllerManagerWithTestableCM // check if all controllers are added correctly EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_controller_system->get_state().id()); + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_broadcaster_all->get_state().id()); + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - test_broadcaster_sensor->get_state().id()); + test_broadcaster_sensor->get_lifecycle_state().id()); // configure controllers cm_->configure_controller(TEST_CONTROLLER_ACTUATOR_NAME); @@ -147,14 +147,16 @@ class TestControllerManagerWithTestableCM EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_broadcaster_sensor->get_state().id()); + test_broadcaster_sensor->get_lifecycle_state().id()); // Start controller, will take effect at the end of the update function switch_test_controllers( @@ -180,13 +182,17 @@ TEST_P(TestControllerManagerWithTestableCM, check_cached_controllers_for_hardwar SetupAndConfigureControllers(strictness); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); { auto controllers = @@ -235,13 +241,17 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er } EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); // Execute first time without any errors { @@ -278,13 +288,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) @@ -350,13 +363,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) @@ -427,13 +443,17 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error } EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); // Execute first time without any errors { @@ -456,12 +476,13 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Executes the current cycle and returns ERROR"; EXPECT_EQ( - test_controller_actuator->get_state().id(), + test_controller_actuator->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Executes the current cycle and returns ERROR"; EXPECT_EQ( - test_controller_system->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + test_controller_system->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_broadcaster_sensor->internal_counter, new_counter) @@ -474,13 +495,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) @@ -495,13 +519,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_controller_error // The states shouldn't change as there are no more controller errors EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); } } @@ -527,13 +554,17 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e } EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_actuator->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); // Execute first time without any errors { @@ -570,13 +601,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) @@ -644,13 +678,16 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller_actuator->get_state().id()); + test_controller_actuator->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_system->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_system->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_broadcaster_all->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_broadcaster_all->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_broadcaster_sensor->get_lifecycle_state().id()); EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index e53ad105784..26a5299a078 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -365,7 +365,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) std::weak_ptr test_controller_weak(test_controller); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -388,7 +389,9 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) test_controller_weak = test_controller; cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should have have a copy of this shared ptr"; @@ -412,13 +415,15 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) cm_->switch_controller( {test_controller::TEST_CONTROLLER_NAME}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0)); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); // Failed reload due to active controller request->force_kill = false; result = call_service_and_wait(*client, request, srv_executor); ASSERT_FALSE(result->ok) << "Cannot reload if controllers are running"; - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); ASSERT_GT(test_controller.use_count(), 1) << "Controller manager should still have have a copy of " "this shared ptr, no unloading was performed"; @@ -460,7 +465,7 @@ TEST_F(TestControllerManagerSrvs, load_controller_srv) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - cm_->get_loaded_controllers()[0].c->get_state().id()); + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); } TEST_F(TestControllerManagerSrvs, unload_controller_srv) @@ -486,7 +491,8 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) result = call_service_and_wait(*client, request, srv_executor, true); ASSERT_TRUE(result->ok); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } @@ -520,7 +526,8 @@ TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controlle unload_request->name = test_controller::TEST_CONTROLLER_NAME; auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); // now load it and check if it got the new robot description @@ -561,15 +568,18 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - cm_->get_loaded_controllers()[0].c->get_state().id()); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); + cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); // now unload the controller and check the state auto unload_request = std::make_shared(); unload_request->name = test_controller::TEST_CONTROLLER_NAME; ASSERT_TRUE(call_service_and_wait(*unload_client, unload_request, srv_executor, true)->ok); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); } diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 1556211a7ff..e44db4838b2 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -68,7 +68,8 @@ class TestControllerManagerWithTestableCM cm_->add_controller(test_controller_, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); ASSERT_NE(test_controller_, nullptr); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller_->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_->get_lifecycle_state().id()); } void configure_and_try_switch_that_returns_error() @@ -76,7 +77,8 @@ class TestControllerManagerWithTestableCM // configure controller cm_->configure_controller(CONTROLLER_NAME); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_->get_lifecycle_state().id()); // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); @@ -86,7 +88,8 @@ class TestControllerManagerWithTestableCM controller_interface::return_type::ERROR); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_->get_lifecycle_state().id()); } void try_successful_switch() @@ -95,7 +98,9 @@ class TestControllerManagerWithTestableCM strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, controller_interface::return_type::OK); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_->get_lifecycle_state().id()); } std::shared_ptr test_controller_; diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 4a81f3bf123..c143ab4862a 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -220,31 +220,31 @@ class TestControllerChainingWithControllerManager EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - pid_left_wheel_controller->get_state().id()); + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - pid_right_wheel_controller->get_state().id()); + pid_right_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - diff_drive_controller->get_state().id()); + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - diff_drive_controller_two->get_state().id()); + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - sensor_fusion_controller->get_state().id()); + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); } // order or controller configuration is not important therefore we can reuse the same method @@ -257,7 +257,7 @@ class TestControllerChainingWithControllerManager // configure chainable controller and check exported interfaces cm_->configure_controller(PID_LEFT_WHEEL); EXPECT_EQ( - pid_left_wheel_controller->get_state().id(), + pid_left_wheel_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 1); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs); @@ -270,7 +270,7 @@ class TestControllerChainingWithControllerManager cm_->configure_controller(PID_RIGHT_WHEEL); EXPECT_EQ( - pid_right_wheel_controller->get_state().id(), + pid_right_wheel_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 2); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs); @@ -287,7 +287,8 @@ class TestControllerChainingWithControllerManager RCLCPP_ERROR_STREAM(cm_->get_logger(), x); } EXPECT_EQ( - diff_drive_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + diff_drive_controller->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 2); for (const auto & interface : @@ -306,7 +307,7 @@ class TestControllerChainingWithControllerManager cm_->configure_controller(DIFF_DRIVE_CONTROLLER_TWO); EXPECT_EQ( - diff_drive_controller_two->get_state().id(), + diff_drive_controller_two->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); @@ -326,35 +327,35 @@ class TestControllerChainingWithControllerManager cm_->configure_controller(POSITION_TRACKING_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 4); cm_->configure_controller(SENSOR_FUSION_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 7); cm_->configure_controller(ROBOT_LOCALIZATION_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); cm_->configure_controller(ODOM_PUBLISHER_CONTROLLER); EXPECT_EQ( - position_tracking_controller->get_state().id(), + position_tracking_controller->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); cm_->configure_controller(POSITION_TRACKING_CONTROLLER_TWO); EXPECT_EQ( - position_tracking_controller_two->get_state().id(), + position_tracking_controller_two->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 8); EXPECT_EQ(cm_->resource_manager_->state_interface_keys().size(), number_of_state_itfs + 8); @@ -963,17 +964,17 @@ TEST_P( EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the robot localization controller and see that the sensor fusion controller is still // active but not in the chained mode @@ -984,14 +985,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the odometry publisher controller DeactivateAndCheckController(odom_publisher_controller, ODOM_PUBLISHER_CONTROLLER, {}, 8u, true); @@ -1000,15 +1004,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the sensor_fusion controller and see that the diff_drive_controller is still active // but not in the chained mode @@ -1018,15 +1024,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the diff_drive_controller as all it's following controllers that uses it's // interfaces are deactivated @@ -1037,12 +1045,14 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); // all controllers are deactivated --> chained mode is not changed DeactivateAndCheckController( @@ -1054,15 +1064,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); } TEST_P( @@ -1131,18 +1143,22 @@ TEST_P( // Check if the controller activated (Should not be activated) ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); ActivateController( SENSOR_FUSION_CONTROLLER, expected.at(test_param.strictness).return_type, std::future_status::ready); // Check if the controller activated (Should not be activated) ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); // Test Case 2: Try to activate a preceding controller the same time when trying to // deactivate a following controller (using switch_controller function) @@ -1167,16 +1183,18 @@ TEST_P( // should be deactivated (if BEST_EFFORT) // If STRICT, preceding controller should stay deactivated and following controller // should stay activated - EXPECT_EQ(expected.at(test_param.strictness).state, pid_right_wheel_controller->get_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); } TEST_P( @@ -1243,20 +1261,22 @@ TEST_P( // depending controllers are active EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - diff_drive_controller_two->get_state().id()); + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Deactivate position_tracking_controller and activate position_tracking_controller_two switch_test_controllers( @@ -1264,10 +1284,10 @@ TEST_P( std::future_status::timeout, controller_interface::return_type::OK); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Now deactivate the position_tracking_controller_two and it should be in inactive state switch_test_controllers( @@ -1275,7 +1295,7 @@ TEST_P( controller_interface::return_type::OK); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Activate it again and deactivate it others to see if we can deactivate it in a group switch_test_controllers( @@ -1283,10 +1303,10 @@ TEST_P( controller_interface::return_type::OK); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Deactivate the first preceding controller (diff_drive_controller) and // activate the other preceding controller (diff_drive_controller_two) @@ -1298,29 +1318,34 @@ TEST_P( // Following controllers should stay active EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); // The original preceding controller (diff_drive_controller) should be inactive while // the other preceding controller should be active (diff_drive_controller_two) ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller_two->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); // Activate all the controllers again in group and deactivate the diff_drive_controller_two switch_test_controllers( @@ -1330,32 +1355,37 @@ TEST_P( controller_interface::return_type::OK); // Following controllers should stay active EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_TRUE(diff_drive_controller->is_in_chained_mode()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - diff_drive_controller_two->get_state().id()); + diff_drive_controller_two->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); // This is false, because it only uses the state interfaces and exposes state interfaces EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller_two->get_state().id()); + position_tracking_controller_two->get_lifecycle_state().id()); } TEST_P( @@ -1426,15 +1456,17 @@ TEST_P( // Verify preceding controller (diff_drive_controller) is inactive EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Attempt to deactivate inactive controller (diff_drive_controller) DeactivateController( @@ -1453,19 +1485,23 @@ TEST_P( // Check to see preceding controller (diff_drive_controller) is still inactive and // following controllers (pid_left_wheel_controller) (pid_left_wheel_controller) are still active EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Test Case 6: following controller is deactivated but preceding controller will be activated // --> return error; controllers stay in the same state @@ -1479,10 +1515,15 @@ TEST_P( // should be deactivated (if BEST_EFFORT) // If STRICT, preceding controller should stay deactivated and following controller // should stay activated - EXPECT_EQ(expected.at(test_param.strictness).state, pid_right_wheel_controller->get_state().id()); - EXPECT_EQ(expected.at(test_param.strictness).state, pid_left_wheel_controller->get_state().id()); + EXPECT_EQ( + expected.at(test_param.strictness).state, + pid_right_wheel_controller->get_lifecycle_state().id()); + EXPECT_EQ( + expected.at(test_param.strictness).state, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // Test Case 7: following controller deactivation but preceding controller is active // --> return error; controllers stay in the same state as they were @@ -1499,11 +1540,14 @@ TEST_P( // Expect all controllers to be active ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // Attempt to deactivate following controllers switch_test_controllers( @@ -1512,11 +1556,14 @@ TEST_P( // All controllers should still be active ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // Attempt to deactivate a following controller switch_test_controllers( @@ -1525,11 +1572,11 @@ TEST_P( // All controllers should still be active ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); } TEST_P( @@ -1602,21 +1649,26 @@ TEST_P( // Verify that initially all of them are in active state EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_left_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_left_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, pid_right_wheel_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + pid_right_wheel_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - position_tracking_controller->get_state().id()); + position_tracking_controller->get_lifecycle_state().id()); // There is different error and timeout behavior depending on strictness static std::unordered_map expected = { @@ -1643,16 +1695,19 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); // SensorFusionController continues to stay in the chained mode as it is still using the state // interfaces EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // DiffDrive (preceding) controller is activated --> PID controller in chained mod // Let's try to deactivate the diff_drive_control, it should fail as there are still other @@ -1668,14 +1723,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Trying to deactivate the sensor fusion controller, however, it won't be deactivated as the // robot localization controller is still active @@ -1688,14 +1746,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); EXPECT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the robot localization controller and see that the sensor fusion controller is still // active but not in the chained mode @@ -1706,14 +1767,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the sensor_fusion controller and this should be successful as there are no other // controllers using it's interfaces @@ -1723,14 +1787,17 @@ TEST_P( ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, odom_publisher_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the odometry publisher controller and now the diff_drive should continue active but // not in chained mode @@ -1740,15 +1807,17 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); // Deactivate the diff_drive_controller as all it's following controllers that uses it's // interfaces are deactivated @@ -1759,12 +1828,14 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); // all controllers are deactivated --> chained mode is not changed DeactivateAndCheckController( @@ -1776,15 +1847,17 @@ TEST_P( EXPECT_FALSE(diff_drive_controller->is_in_chained_mode()); ASSERT_FALSE(sensor_fusion_controller->is_in_chained_mode()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff_drive_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + diff_drive_controller->get_lifecycle_state().id()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, sensor_fusion_controller->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + sensor_fusion_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - odom_publisher_controller->get_state().id()); + odom_publisher_controller->get_lifecycle_state().id()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - robot_localization_controller->get_state().id()); + robot_localization_controller->get_lifecycle_state().id()); } TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_adding_in_random_order) diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 19d34c0c112..3645c06c242 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -63,12 +63,14 @@ TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) ASSERT_NE(controller_if, nullptr); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); controller_if->get_node()->set_parameter({"update_rate", 1337}); cm_->configure_controller("test_controller_01"); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); EXPECT_EQ(1337u, controller_if->get_update_rate()); } @@ -119,10 +121,12 @@ TEST_P(TestLoadedControllerParametrized, load_and_configure_one_known_controller EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); cm_->configure_controller(CONTROLLER_NAME_1); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) @@ -131,7 +135,8 @@ TEST_P(TestLoadedControllerParametrized, can_start_configured_controller) EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); start_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) @@ -144,7 +149,8 @@ TEST_P(TestLoadedControllerParametrized, can_stop_active_controller) // Stop controller stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) @@ -152,14 +158,16 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) const auto test_param = GetParam(); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); { // Test starting unconfigured controller, and starting configured afterwards start_test_controller( test_param.strictness, std::future_status::ready, test_param.expected_return); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); // Activate configured controller { @@ -167,12 +175,15 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller) cm_->configure_controller(CONTROLLER_NAME_1); } start_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); } { // Stop controller stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + controller_if->get_lifecycle_state().id()); } } @@ -185,7 +196,8 @@ TEST_P(TestLoadedControllerParametrized, can_not_configure_active_controller) // Can not configure active controller EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) @@ -193,7 +205,8 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) const auto test_param = GetParam(); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); // Shutdown controller on purpose for testing ASSERT_EQ( @@ -206,7 +219,8 @@ TEST_P(TestLoadedControllerParametrized, can_not_start_finalized_controller) // Can not configure finalize controller EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, controller_if->get_lifecycle_state().id()); } TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_up) @@ -219,7 +233,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); std::shared_ptr test_controller = std::dynamic_pointer_cast(controller_if); @@ -228,7 +243,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_cleaned_u // Configure from inactive state: controller can no be cleaned-up test_controller->simulate_cleanup_failure = true; EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); EXPECT_EQ(0u, cleanup_calls); } @@ -241,7 +257,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure start_test_controller(test_param.strictness); stop_test_controller(test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); std::shared_ptr test_controller = std::dynamic_pointer_cast(controller_if); @@ -253,7 +270,8 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure ControllerManagerRunner cm_runner(this); EXPECT_EQ(cm_->configure_controller(CONTROLLER_NAME_1), controller_interface::return_type::OK); } - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); EXPECT_EQ(1u, cleanup_calls); } @@ -278,7 +296,8 @@ TEST_P(SwitchTest, EmptyListOrNonExistentTest) EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if->get_lifecycle_state().id()); auto params = GetParam(); auto result = std::get<0>(params); @@ -382,19 +401,23 @@ class TestTwoLoadedControllers : public TestLoadController, ASSERT_NE(controller_if2, nullptr); EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if1->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if1->get_lifecycle_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); } }; TEST_F(TestTwoLoadedControllers, load_and_configure_two_known_controllers) { cm_->configure_controller(CONTROLLER_NAME_1); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); cm_->configure_controller(CONTROLLER_NAME_2); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); } TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) @@ -406,9 +429,11 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) // Start controller #1 RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); // Stop controller 1, start controller 2 // Both fail because controller 2 because it is not configured and STRICT is used @@ -423,9 +448,9 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) switch_test_controllers( strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, STRICT, std::future_status::ready, controller_interface::return_type::ERROR); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id()); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + controller_if2->get_lifecycle_state().id()); { ControllerManagerRunner cm_runner(this); @@ -435,26 +460,33 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers) // Stop controller 1 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1"); switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_1}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); // Start controller 1 again RCLCPP_INFO(cm_->get_logger(), "Starting stopped controller #1"); switch_test_controllers(strvec{CONTROLLER_NAME_1}, strvec{}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_state().id()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); // Stop controller 1, start controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1, starting controller #2"); switch_test_controllers( strvec{CONTROLLER_NAME_2}, strvec{CONTROLLER_NAME_1}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_state().id()); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if2->get_lifecycle_state().id()); // Stop controller 2 RCLCPP_INFO(cm_->get_logger(), "Stopping controller #2"); switch_test_controllers(strvec{}, strvec{CONTROLLER_NAME_2}, test_param.strictness); - ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if2->get_lifecycle_state().id()); } INSTANTIATE_TEST_SUITE_P( diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index c0305324b61..9caef761ab5 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -49,10 +49,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); { // Test starting the first controller RCLCPP_INFO(cm_->get_logger(), "Starting controller #1"); @@ -67,10 +67,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test starting the second controller when the first is running @@ -87,10 +87,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping controller #1 and starting controller #2 @@ -106,10 +106,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping controller #2 and starting controller #1 @@ -125,10 +125,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping both controllers when only controller #1 is running @@ -149,10 +149,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test stopping both controllers when only controller #1 is running @@ -170,10 +170,10 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test starting both controllers at the same time @@ -191,9 +191,9 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } } diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 49326f9e796..d073abaf95b 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -111,7 +111,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) 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); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } // Try to spawn again, it should fail because already active @@ -131,7 +132,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) 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); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } cm_->switch_controller( @@ -147,7 +149,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) 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); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } // Unload and reload @@ -159,7 +162,8 @@ TEST_F(TestLoadController, spawner_test_type_in_param) 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); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } @@ -186,7 +190,8 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) [&](const auto & controller) { return controller.info.name == controller_name; }); ASSERT_TRUE(it != loaded_controllers.end()); ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } }; @@ -270,7 +275,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - ctrl_with_parameters_and_type.c->get_state().id(), + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -280,7 +285,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - chain_ctrl_with_parameters_and_type.c->get_state().id(), + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( @@ -295,12 +300,14 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); 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_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } TEST_F(TestLoadController, unload_on_kill) @@ -339,7 +346,8 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } // Try to spawn now the controller with fallback controllers inside the yaml @@ -352,20 +360,23 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_THAT( ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_3 = cm_->get_loaded_controllers()[2]; ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); - ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_3.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } } @@ -391,7 +402,7 @@ TEST_F(TestLoadController, spawner_with_many_controllers) { auto ctrl = cm_->get_loaded_controllers()[i]; ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } @@ -472,7 +483,8 @@ TEST_F( 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); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } } @@ -548,7 +560,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) [&](const auto & controller) { return controller.info.name == controller_name; }); ASSERT_TRUE(it != loaded_controllers.end()); ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } }; @@ -646,7 +659,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - ctrl_with_parameters_and_type.c->get_state().id(), + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -656,7 +669,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - chain_ctrl_with_parameters_and_type.c->get_state().id(), + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( @@ -671,12 +684,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); 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_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } TEST_F( @@ -722,7 +737,7 @@ TEST_F( ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - ctrl_with_parameters_and_type.c->get_state().id(), + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -732,7 +747,7 @@ TEST_F( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - chain_ctrl_with_parameters_and_type.c->get_state().id(), + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); EXPECT_EQ( @@ -747,10 +762,12 @@ TEST_F( auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); 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_UNCONFIGURED); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 4aaa828091f..d0fe7b5962c 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,8 +22,8 @@ For details see the controller_manager section. * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. -* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). - From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. +* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_).From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. controller_manager ****************** @@ -103,6 +103,7 @@ hardware_interface * Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) * Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 `_) * Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 `_) +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. joint_limits ************ diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index bb91b62057f..4fcdd93307a 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -93,7 +93,7 @@ class Actuator final std::string get_group_name() const; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index a74d2a49641..d88fcd1f2de 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -220,13 +220,16 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod /** * \return state. */ - const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. /** * \return state. */ - void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } /// Get the logger of the ActuatorInterface. /** diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp index 0504419b38a..052c4ba9217 100644 --- a/hardware_interface/include/hardware_interface/async_components.hpp +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -84,7 +84,9 @@ class AsyncComponentThread TimePoint next_iteration_time = TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds())); - if (component->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if ( + component->get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { auto current_time = clock_interface_->get_clock()->now(); auto measured_period = current_time - previous_time; diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index 98197bd0e97..e1490c91fbc 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -80,7 +80,7 @@ class Sensor final std::string get_group_name() const; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 0c7f43e7fc5..4e9ac48d5dd 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -159,13 +159,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return state. */ - const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. /** * \return state. */ - void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } /// Get the logger of the SensorInterface. /** diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 1cc29a085da..34056c982e3 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -93,7 +93,7 @@ class System final std::string get_group_name() const; HARDWARE_INTERFACE_PUBLIC - const rclcpp_lifecycle::State & get_state() const; + const rclcpp_lifecycle::State & get_lifecycle_state() const; HARDWARE_INTERFACE_PUBLIC return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 54f1af33a73..37745b36fbb 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -222,13 +222,16 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI /** * \return state. */ - const rclcpp_lifecycle::State & get_state() const { return lifecycle_state_; } + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. /** * \return state. */ - void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; } + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } /// Get the logger of the SystemInterface. /** diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 52cfc681a1c..d586b463af9 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -47,158 +47,158 @@ const rclcpp_lifecycle::State & Actuator::initialize( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(actuator_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::configure() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - switch (impl_->on_configure(impl_->get_state())) + switch (impl_->on_configure(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::cleanup() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_cleanup(impl_->get_state())) + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::shutdown() { std::unique_lock lock(actuators_mutex_); if ( - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - switch (impl_->on_shutdown(impl_->get_state())) + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::activate() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_activate(impl_->get_state())) + switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::deactivate() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - switch (impl_->on_deactivate(impl_->get_state())) + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Actuator::error() { std::unique_lock lock(actuators_mutex_); - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_error(impl_->get_state())) + switch (impl_->on_error(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } std::vector Actuator::export_state_interfaces() @@ -231,7 +231,10 @@ std::string Actuator::get_name() const { return impl_->get_name(); } std::string Actuator::get_group_name() const { return impl_->get_group_name(); } -const rclcpp_lifecycle::State & Actuator::get_state() const { return impl_->get_state(); } +const rclcpp_lifecycle::State & Actuator::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -244,15 +247,15 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->read(time, period); if (result == return_type::ERROR) @@ -274,15 +277,15 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->write(time, period); if (result == return_type::ERROR) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 53fcad94d22..72e966fcab0 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -514,7 +514,7 @@ class ResourceStorage switch (target_state.id()) { case State::PRIMARY_STATE_UNCONFIGURED: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = true; @@ -538,7 +538,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_INACTIVE: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = configure_hardware(hardware); @@ -558,7 +558,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_ACTIVE: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = configure_hardware(hardware); @@ -582,7 +582,7 @@ class ResourceStorage } break; case State::PRIMARY_STATE_FINALIZED: - switch (hardware.get_state().id()) + switch (hardware.get_lifecycle_state().id()) { case State::PRIMARY_STATE_UNCONFIGURED: result = shutdown_hardware(hardware); @@ -1462,7 +1462,8 @@ std::unordered_map ResourceManager::get_comp { for (auto & component : container) { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); + resource_storage_->hardware_info_map_[component.get_name()].state = + component.get_lifecycle_state(); } }; @@ -1546,8 +1547,9 @@ bool ResourceManager::prepare_command_mode_switch( for (auto & component : components) { if ( - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + component.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { try { @@ -1611,8 +1613,9 @@ bool ResourceManager::perform_command_mode_switch( for (auto & component : components) { if ( - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - component.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + component.get_lifecycle_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { try { diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index 9f1d4b73e16..2cffa649fd8 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -46,158 +46,158 @@ const rclcpp_lifecycle::State & Sensor::initialize( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(sensor_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::configure() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - switch (impl_->on_configure(impl_->get_state())) + switch (impl_->on_configure(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::cleanup() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_cleanup(impl_->get_state())) + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::shutdown() { std::unique_lock lock(sensors_mutex_); if ( - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - switch (impl_->on_shutdown(impl_->get_state())) + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::activate() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_activate(impl_->get_state())) + switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::deactivate() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - switch (impl_->on_deactivate(impl_->get_state())) + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & Sensor::error() { std::unique_lock lock(sensors_mutex_); - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_error(impl_->get_state())) + switch (impl_->on_error(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } std::vector Sensor::export_state_interfaces() @@ -209,7 +209,10 @@ std::string Sensor::get_name() const { return impl_->get_name(); } std::string Sensor::get_group_name() const { return impl_->get_group_name(); } -const rclcpp_lifecycle::State & Sensor::get_state() const { return impl_->get_state(); } +const rclcpp_lifecycle::State & Sensor::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -222,15 +225,15 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->read(time, period); if (result == return_type::ERROR) diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 82473563b67..8f455a7bd59 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -45,158 +45,158 @@ const rclcpp_lifecycle::State & System::initialize( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { switch (impl_->init(system_info, logger, clock_interface)) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::configure() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - switch (impl_->on_configure(impl_->get_state())) + switch (impl_->on_configure(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::cleanup() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_cleanup(impl_->get_state())) + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::shutdown() { std::unique_lock lock(system_mutex_); if ( - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - switch (impl_->on_shutdown(impl_->get_state())) + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::activate() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - switch (impl_->on_activate(impl_->get_state())) + switch (impl_->on_activate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::deactivate() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - switch (impl_->on_deactivate(impl_->get_state())) + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); break; case CallbackReturn::FAILURE: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); break; case CallbackReturn::ERROR: - impl_->set_state(error()); + impl_->set_lifecycle_state(error()); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } const rclcpp_lifecycle::State & System::error() { std::unique_lock lock(system_mutex_); - if (impl_->get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + if (impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) { - switch (impl_->on_error(impl_->get_state())) + switch (impl_->on_error(impl_->get_lifecycle_state())) { case CallbackReturn::SUCCESS: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED)); break; case CallbackReturn::FAILURE: case CallbackReturn::ERROR: - impl_->set_state(rclcpp_lifecycle::State( + impl_->set_lifecycle_state(rclcpp_lifecycle::State( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); break; } } - return impl_->get_state(); + return impl_->get_lifecycle_state(); } std::vector System::export_state_interfaces() @@ -227,7 +227,10 @@ std::string System::get_name() const { return impl_->get_name(); } std::string System::get_group_name() const { return impl_->get_group_name(); } -const rclcpp_lifecycle::State & System::get_state() const { return impl_->get_state(); } +const rclcpp_lifecycle::State & System::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -240,15 +243,15 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->read(time, period); if (result == return_type::ERROR) @@ -270,15 +273,15 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe return return_type::OK; } if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { return return_type::OK; } return_type result = return_type::ERROR; if ( - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { result = impl_->write(time, period); if (result == return_type::ERROR) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 986c32d37ba..b6a8cc81c80 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -726,7 +726,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -749,7 +749,7 @@ TEST(TestComponentInterfaces, dummy_actuator_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.read(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -786,7 +786,7 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -809,7 +809,7 @@ TEST(TestComponentInterfaces, dummy_actuator_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, actuator_hw.write(TIME, PERIOD)); - state = actuator_hw.get_state(); + state = actuator_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -843,7 +843,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - state = sensor_hw.get_state(); + state = sensor_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -868,7 +868,7 @@ TEST(TestComponentInterfaces, dummy_sensor_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, sensor_hw.read(TIME, PERIOD)); - state = sensor_hw.get_state(); + state = sensor_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -911,7 +911,7 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -939,7 +939,7 @@ TEST(TestComponentInterfaces, dummy_system_read_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.read(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); @@ -976,7 +976,7 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label()); @@ -1004,7 +1004,7 @@ TEST(TestComponentInterfaces, dummy_system_write_error_behavior) } ASSERT_EQ(hardware_interface::return_type::ERROR, system_hw.write(TIME, PERIOD)); - state = system_hw.get_state(); + state = system_hw.get_lifecycle_state(); EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::FINALIZED, state.label()); From 9e57adfbc414f92492278fdba10e60716a5995b7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 26 Aug 2024 14:10:35 +0200 Subject: [PATCH 39/42] Fix unload of controllers when spawned with `--unload-on-kill` (#1717) --- .../controller_manager/spawner.py | 27 ++++++++++++------- .../test/test_spawner_unspawner.cpp | 19 +++++++++++++ 2 files changed, 37 insertions(+), 9 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index bd277d5464a..d6df3be01f3 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -234,7 +234,7 @@ def main(args=None): node.get_logger().info( bcolors.OKGREEN - + "Configured and activated all the parsed controllers list!" + + f"Configured and activated all the parsed controllers list : {controller_names}!" + bcolors.ENDC ) @@ -258,16 +258,25 @@ def main(args=None): ) return 1 - node.get_logger().info("Deactivated controller") - - ret = unload_controller(node, controller_manager_name, controller_name) - if not ret.ok: - node.get_logger().error( - bcolors.FAIL + "Failed to unload controller" + bcolors.ENDC + node.get_logger().info( + f"Successfully deactivated controllers : {controller_names}" ) - return 1 - node.get_logger().info("Unloaded controller") + unload_status = True + for controller_name in controller_names: + ret = unload_controller(node, controller_manager_name, controller_name) + if not ret.ok: + unload_status = False + node.get_logger().error( + bcolors.FAIL + + f"Failed to unload controller : {controller_name}" + + bcolors.ENDC + ) + + if unload_status: + node.get_logger().info(f"Successfully unloaded controllers : {controller_names}") + else: + return 1 return 0 except KeyboardInterrupt: pass diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index d073abaf95b..0812d7ffa41 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -328,6 +328,25 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +TEST_F(TestLoadController, unload_on_kill_activate_as_group) +{ + // Launch spawner with unload on kill + // timeout command will kill it after the specified time with signal SIGINT + ControllerManagerRunner cm_runner(this); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + std::stringstream ss; + ss << "timeout --signal=INT 5 " + << std::string(coveragepy_script) + + " $(ros2 pkg prefix controller_manager)/lib/controller_manager/spawner " + << "ctrl_3 ctrl_2 --activate-as-group -c test_controller_manager --unload-on-kill"; + + EXPECT_NE(std::system(ss.str().c_str()), 0) + << "timeout should have killed spawner and returned non 0 code"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); +} + TEST_F(TestLoadController, spawner_test_fallback_controllers) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + From 8f7374faf482e83c94c0a477bfbe8b2e672f241c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 26 Aug 2024 15:57:40 +0200 Subject: [PATCH 40/42] Add breaking changes to migration notes (#1718) --- doc/migration.rst | 2 ++ doc/release_notes.rst | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/doc/migration.rst b/doc/migration.rst index 8e1ca096f1a..255db9b8ae1 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -6,6 +6,7 @@ controller_interface ******************** * The changes from `(PR #1694) `__ will affect how the controllers will be loading the parameters. Defining parameters in a single yaml file and loading it to the controller_manager node alone will no longer work. In order to load the parameters to the controllers properly, it is needed to use ``--param-file`` option from the spawner. This is because the controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments. +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. controller_manager ****************** @@ -73,3 +74,4 @@ controller_manager hardware_interface ****************** * ``test_components`` was moved to its own package. Update the dependencies if you are using them. (`#1325 `_) +* With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. diff --git a/doc/release_notes.rst b/doc/release_notes.rst index d0fe7b5962c..50583b3bf4c 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,7 +22,7 @@ For details see the controller_manager section. * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) * All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. -* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_).From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. +* The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. controller_manager From 6890308f9cf5e29faf525dd80ef6e91d5110edcb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Aug 2024 14:41:36 +0200 Subject: [PATCH 41/42] remove unnecessary log of the CM args (#1720) --- controller_manager/src/ros2_control_node.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 9c996d5aca4..e0094b7e018 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -50,7 +50,6 @@ int main(int argc, char ** argv) // A simple way to reject non ros args continue; } - RCLCPP_INFO(rclcpp::get_logger("CM args"), "Adding argument: %s", argv[i]); node_arguments.push_back(argv[i]); } cm_node_options.arguments(node_arguments); From d2004083cc969ed14b0bde4011fa3172988c86b3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 27 Aug 2024 14:45:22 +0200 Subject: [PATCH 42/42] [ros2controlcli] fix list_controllers when no controllers are loaded (#1721) --- ros2controlcli/ros2controlcli/verb/list_controllers.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 3daf752b8ce..b4ebff94cd3 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -98,6 +98,10 @@ def main(self, *, args): with NodeStrategy(args) as node: response = list_controllers(node, args.controller_manager) + if not response.controller: + print("No controllers are currently loaded!") + return 0 + # Structure data as table for nicer output col_width_name = max(len(ctrl.name) for ctrl in response.controller) col_width_type = max(len(ctrl.type) for ctrl in response.controller)