Merge branch 'master' into extend/spawner/parameter_file_parsing

ros-controls · Nov 18, 2024 · 6e131e9 · 6e131e9
2 parents c0762a4 + 23bd1c3
commit 6e131e9
Show file tree

Hide file tree

Showing 44 changed files with 569 additions and 139 deletions.
diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml
@@ -39,7 +39,7 @@ jobs:
       fail-fast: false
       matrix:
         ROS_DISTRO: [humble]
-        ROS_REPO: [main, testing]
+        ROS_REPO: [testing]
     with:
       ros_distro: ${{ matrix.ROS_DISTRO }}
       ros_repo: ${{ matrix.ROS_REPO }}

diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml
@@ -39,7 +39,7 @@ jobs:
       fail-fast: false
       matrix:
         ROS_DISTRO: [iron]
-        ROS_REPO: [main, testing]
+        ROS_REPO: [testing]
     with:
       ros_distro: ${{ matrix.ROS_DISTRO }}
       ros_repo: ${{ matrix.ROS_REPO }}

diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml
@@ -39,7 +39,7 @@ jobs:
       fail-fast: false
       matrix:
         ROS_DISTRO: [jazzy]
-        ROS_REPO: [main, testing]
+        ROS_REPO: [testing]
     with:
       ros_distro: ${{ matrix.ROS_DISTRO }}
       ros_repo: ${{ matrix.ROS_REPO }}

diff --git a/.github/workflows/rolling-compatibility-humble-binary-build.yml b/.github/workflows/rolling-compatibility-humble-binary-build.yml
@@ -0,0 +1,44 @@
+name: Check Rolling Compatibility on Humble
+# author: Christoph Froehlich <[email protected]>
+# description: 'Build & test the rolling version on Humble distro.'
+
+on:
+  workflow_dispatch:
+  pull_request:
+    branches:
+      - master
+    paths:
+      - '**.hpp'
+      - '**.h'
+      - '**.cpp'
+      - '**.py'
+      - '.github/workflows/rolling-compatibility-humble-binary-build.yml'
+      - '**/package.xml'
+      - '**/CMakeLists.txt'
+      - 'ros2_control.rolling.repos'
+  push:
+    branches:
+      - master
+    paths:
+      - '**.hpp'
+      - '**.h'
+      - '**.cpp'
+      - '**.py'
+      - '.github/workflows/rolling-compatibility-humble-binary-build.yml'
+      - '**/package.xml'
+      - '**/CMakeLists.txt'
+      - 'ros2_control.rolling.repos'
+
+jobs:
+  build-on-humble:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+    strategy:
+      fail-fast: false
+      matrix:
+        ROS_DISTRO: [humble]
+        ROS_REPO: [testing]
+    with:
+      ros_distro: ${{ matrix.ROS_DISTRO }}
+      ros_repo: ${{ matrix.ROS_REPO }}
+      upstream_workspace: ros2_control.rolling.repos
+      ref_for_scheduled_build: master
diff --git a/.github/workflows/rolling-compatibility-jazzy-binary-build.yml b/.github/workflows/rolling-compatibility-jazzy-binary-build.yml
@@ -0,0 +1,44 @@
+name: Check Rolling Compatibility on Jazzy
+# author: Christoph Froehlich <[email protected]>
+# description: 'Build & test the rolling version on Jazzy distro.'
+
+on:
+  workflow_dispatch:
+  pull_request:
+    branches:
+      - master
+    paths:
+      - '**.hpp'
+      - '**.h'
+      - '**.cpp'
+      - '**.py'
+      - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml'
+      - '**/package.xml'
+      - '**/CMakeLists.txt'
+      - 'ros2_control.rolling.repos'
+  push:
+    branches:
+      - master
+    paths:
+      - '**.hpp'
+      - '**.h'
+      - '**.cpp'
+      - '**.py'
+      - '.github/workflows/rolling-compatibility-jazzy-binary-build.yml'
+      - '**/package.xml'
+      - '**/CMakeLists.txt'
+      - 'ros2_control.rolling.repos'
+
+jobs:
+  build-on-jazzy:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-industrial-ci-with-cache.yml@master
+    strategy:
+      fail-fast: false
+      matrix:
+        ROS_DISTRO: [jazzy]
+        ROS_REPO: [testing]
+    with:
+      ros_distro: ${{ matrix.ROS_DISTRO }}
+      ros_repo: ${{ matrix.ROS_REPO }}
+      upstream_workspace: ros2_control.rolling.repos
+      ref_for_scheduled_build: master
diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml
@@ -39,7 +39,7 @@ jobs:
       fail-fast: false
       matrix:
         ROS_DISTRO: [rolling]
-        ROS_REPO: [main, testing]
+        ROS_REPO: [testing]
     with:
       ros_distro: ${{ matrix.ROS_DISTRO }}
       ros_repo: ${{ matrix.ROS_REPO }}

diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.20.0 (2024-11-08)
+-------------------
+* reset the async variables upon activation to work post exceptions (`#1860 <https://github.com/ros-controls/ros2_control/issues/1860>`_)
+* [CM] Fix controller missing update cycles in a real setup (`#1774 <https://github.com/ros-controls/ros2_control/issues/1774>`_)
+* Contributors: Sai Kishor Kothakota
+
 4.19.0 (2024-10-26)
 -------------------
 * [CM] Async Function Handler for Controllers (`#1489 <https://github.com/ros-controls/ros2_control/issues/1489>`_)

diff --git a/controller_interface/package.xml b/controller_interface/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_interface</name>
-  <version>4.19.0</version>
+  <version>4.20.0</version>
   <description>Description of controller_interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp
@@ -67,7 +67,15 @@ return_type ControllerInterfaceBase::init(
     });
 
   node_->register_on_activate(
-    std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1));
+    [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn
+    {
+      if (is_async() && async_handler_ && async_handler_->is_running())
+      {
+        // This is needed if it is disabled due to a thrown exception in the async callback thread
+        async_handler_->reset_variables();
+      }
+      return on_activate(previous_state);
+    });
 
   node_->register_on_deactivate(
     std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1));

diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst
@@ -2,6 +2,20 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.20.0 (2024-11-08)
+-------------------
+* change from thread_priority.hpp to realtime_helpers.hpp (`#1829 <https://github.com/ros-controls/ros2_control/issues/1829>`_)
+* Use Clock instead of Rate for backward compatibility of rolling (`#1864 <https://github.com/ros-controls/ros2_control/issues/1864>`_)
+* [ros2_control_node] Handle simulation environment clocks (`#1810 <https://github.com/ros-controls/ros2_control/issues/1810>`_)
+* [CM] Fix controller missing update cycles in a real setup (`#1774 <https://github.com/ros-controls/ros2_control/issues/1774>`_)
+* [ros2_control_node] Add option to set the CPU affinity  (`#1852 <https://github.com/ros-controls/ros2_control/issues/1852>`_)
+* [ros2_control_node] Add the realtime_tools lock_memory method to prevent page faults (`#1822 <https://github.com/ros-controls/ros2_control/issues/1822>`_)
+* Fix CMP0115 (`#1830 <https://github.com/ros-controls/ros2_control/issues/1830>`_)
+* fix: typo use thread_priority (`#1844 <https://github.com/ros-controls/ros2_control/issues/1844>`_)
+* Fix Hardware spawner and add tests for it (`#1759 <https://github.com/ros-controls/ros2_control/issues/1759>`_)
+* add thread_priority option to the ros2_control_node (`#1820 <https://github.com/ros-controls/ros2_control/issues/1820>`_)
+* Contributors: Baris Yazici, Christoph Fröhlich, Felix Exner (fexner), Sai Kishor Kothakota
+
 4.19.0 (2024-10-26)
 -------------------
 * Fix timeout value in std output (`#1807 <https://github.com/ros-controls/ros2_control/issues/1807>`_)

diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py
@@ -67,18 +67,15 @@ def handle_set_component_state_service_call(
     response = set_hardware_component_state(node, controller_manager_name, component, target_state)
     if response.ok and response.state == target_state:
         node.get_logger().info(
-            bcolors.OKGREEN
-            + f"{action} component '{component}'. Hardware now in state: {response.state}."
+            f"{bcolors.OKGREEN}{action} component '{component}'. Hardware now in state: {response.state}.{bcolors.ENDC}"
         )
     elif response.ok and not response.state == target_state:
         node.get_logger().warn(
-            bcolors.WARNING
-            + f"Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'."
+            f"{bcolors.WARNING}Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'.{bcolors.ENDC}"
         )
     else:
         node.get_logger().warn(
-            bcolors.WARNING
-            + f"Could not {action} component '{component}'. Service call failed. Wrong component name?"
+            f"{bcolors.WARNING}Could not {action} component '{component}'. Service call failed. Wrong component name?{bcolors.ENDC}"
         )
 
 
@@ -160,24 +157,22 @@ def main(args=None):
                 node, controller_manager_name, hardware_component, controller_manager_timeout
             ):
                 node.get_logger().warn(
-                    bcolors.WARNING
-                    + "Hardware Component is not loaded - state can not be changed."
-                    + bcolors.ENDC
+                    f"{bcolors.WARNING}Hardware Component is not loaded - state can not be changed.{bcolors.ENDC}"
                 )
             elif activate:
                 activate_component(node, controller_manager_name, hardware_component)
             elif configure:
                 configure_component(node, controller_manager_name, hardware_component)
             else:
                 node.get_logger().error(
-                    'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag'
+                    f'{bcolors.FAIL}You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag{bcolors.ENDC}'
                 )
                 parser.print_help()
                 return 0
     except KeyboardInterrupt:
         pass
     except ServiceNotFoundError as err:
-        node.get_logger().fatal(str(err))
+        node.get_logger().fatal(f"{bcolors.FAIL}{str(err)}{bcolors.ENDC}")
         return 1
     finally:
         rclpy.shutdown()

diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py
@@ -115,7 +115,16 @@ def main(args=None):
         "--controller-manager-timeout",
         help="Time to wait for the controller manager",
         required=False,
-        default=0,
+        default=0.0,
+        type=float,
+    )
+    parser.add_argument(
+        "--switch-timeout",
+        help="Time to wait for a successful state switch of controllers."
+        " Useful when switching cannot be performed immediately, e.g.,"
+        " paused simulations at startup",
+        required=False,
+        default=5.0,
         type=float,
     )
     parser.add_argument(
@@ -132,6 +141,7 @@ def main(args=None):
     controller_manager_name = args.controller_manager
     param_files = args.param_file
     controller_manager_timeout = args.controller_manager_timeout
+    switch_timeout = args.switch_timeout
 
     if param_files:
         for param_file in param_files:
@@ -211,7 +221,13 @@ def main(args=None):
 
                 if not args.inactive and not args.activate_as_group:
                     ret = switch_controllers(
-                        node, controller_manager_name, [], [controller_name], True, True, 5.0
+                        node,
+                        controller_manager_name,
+                        [],
+                        [controller_name],
+                        True,
+                        True,
+                        switch_timeout,
                     )
                     if not ret.ok:
                         node.get_logger().error(
@@ -229,7 +245,13 @@ def main(args=None):
 
         if not args.inactive and args.activate_as_group:
             ret = switch_controllers(
-                node, controller_manager_name, [], controller_names, True, True, 5.0
+                node,
+                controller_manager_name,
+                [],
+                controller_names,
+                True,
+                True,
+                switch_timeout,
             )
             if not ret.ok:
                 node.get_logger().error(
@@ -255,7 +277,13 @@ def main(args=None):
                 node.get_logger().info("Interrupt captured, deactivating and unloading controller")
                 # TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode
                 ret = switch_controllers(
-                    node, controller_manager_name, controller_names, [], True, True, 5.0
+                    node,
+                    controller_manager_name,
+                    controller_names,
+                    [],
+                    True,
+                    True,
+                    switch_timeout,
                 )
                 if not ret.ok:
                     node.get_logger().error(

diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py
@@ -36,17 +36,33 @@ def main(args=None):
         default="/controller_manager",
         required=False,
     )
+    parser.add_argument(
+        "--switch-timeout",
+        help="Time to wait for a successful state switch of controllers."
+        " Useful when switching cannot be performed immediately, e.g.,"
+        " paused simulations at startup",
+        required=False,
+        default=5.0,
+        type=float,
+    )
 
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
     controller_names = args.controller_names
     controller_manager_name = args.controller_manager
+    switch_timeout = args.switch_timeout
 
     node = Node("unspawner_" + controller_names[0])
     try:
         # Ignore returncode, because message is already printed and we'll try to unload anyway
         ret = switch_controllers(
-            node, controller_manager_name, controller_names, [], True, True, 5.0
+            node,
+            controller_manager_name,
+            controller_names,
+            [],
+            True,
+            True,
+            switch_timeout,
         )
         node.get_logger().info("Deactivated controller")
 

diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst
@@ -157,7 +157,8 @@ There are two scripts to interact with controller manager from launch files:
 .. code-block:: console
 
     $ ros2 run controller_manager spawner -h
-    usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--activate-as-group]
+    usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
+                  [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group]
                   controller_names [controller_names ...]
 
     positional arguments:
@@ -176,6 +177,9 @@ There are two scripts to interact with controller manager from launch files:
       -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
+      --switch-timeout SWITCH_TIMEOUT
+                            Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused
+                            simulations at startup
       --activate-as-group   Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether
 
 
@@ -239,15 +243,18 @@ The parsed controller config file can follow the same conventions as the typical
 .. code-block:: console
 
     $ ros2 run controller_manager unspawner -h
-    usage: unspawner [-h] [-c CONTROLLER_MANAGER] controller_name
+    usage: unspawner [-h] [-c CONTROLLER_MANAGER] [--switch-timeout SWITCH_TIMEOUT] controller_names [controller_names ...]
 
     positional arguments:
-      controller_name       Name of the controller
+      controller_names      Name of the controller
 
-    optional arguments:
+    options:
       -h, --help            show this help message and exit
       -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
                             Name of the controller manager ROS node
+      --switch-timeout SWITCH_TIMEOUT
+                            Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused
+                            simulations at startup
 
 ``hardware_spawner``
 ^^^^^^^^^^^^^^^^^^^^^^

diff --git a/controller_manager/package.xml b/controller_manager/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_manager</name>
-  <version>4.19.0</version>
+  <version>4.20.0</version>
   <description>Description of controller_manager</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>