Merge branch 'master' into fix/exclusive_hw_interface_switching

Author: saikishor

.pre-commit-config.yaml

@@ -63,7 +63,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v20.1.0
+    rev: v20.1.3
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -133,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.32.1
+    rev: 0.33.0
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/include/controller_interface/helpers.hpp

@@ -19,6 +19,9 @@
 #include <string>
 #include <vector>
 
+// Add hardware interface helpers here, so all inherited controllers can use them
+#include "hardware_interface/helpers.hpp"
+
 namespace controller_interface
 {
 /// Reorder interfaces with references according to joint names or full interface names.
@@ -79,13 +82,12 @@ inline bool interface_list_contains_interface_type(
 }
 
 template <typename T>
-void add_element_to_list(std::vector<T> & list, const T & element)
+[[deprecated(
+  "Use ros2_control::add_item method instead. This method will be removed by the ROS 2 Kilted "
+  "Kaiju release.")]] void
+add_element_to_list(std::vector<T> & list, const T & element)
 {
-  if (std::find(list.begin(), list.end(), element) == list.end())
-  {
-    // Only add to the list if it doesn't exist
-    list.push_back(element);
-  }
+  ros2_control::add_item(list, element);
 }
 
 }  // namespace controller_interface

controller_interface/src/chainable_controller_interface.cpp

@@ -90,8 +90,8 @@ ChainableControllerInterface::export_state_interfaces()
       state_interfaces_ptrs_vec.clear();
       throw std::runtime_error(error_msg);
     }
-    ordered_exported_state_interfaces_.push_back(state_interface);
-    add_element_to_list(exported_state_interface_names_, interface_name);
+    ros2_control::add_item(ordered_exported_state_interfaces_, state_interface);
+    ros2_control::add_item(exported_state_interface_names_, interface_name);
     state_interfaces_ptrs_vec.push_back(
       std::const_pointer_cast<const hardware_interface::StateInterface>(state_interface));
   }
@@ -176,8 +176,8 @@ ChainableControllerInterface::export_reference_interfaces()
       reference_interfaces_ptrs_vec.clear();
       throw std::runtime_error(error_msg);
     }
-    ordered_exported_reference_interfaces_.push_back(reference_interface);
-    add_element_to_list(exported_reference_interface_names_, interface_name);
+    ros2_control::add_item(ordered_exported_reference_interfaces_, reference_interface);
+    ros2_control::add_item(exported_reference_interface_names_, interface_name);
     reference_interfaces_ptrs_vec.push_back(reference_interface);
   }
 

controller_manager/controller_manager/spawner.py

@@ -32,6 +32,7 @@
 )
 from controller_manager.controller_manager_services import ServiceNotFoundError
 
+from filelock import Timeout, FileLock
 import rclpy
 from rclpy.node import Node
 from rclpy.signals import SignalHandlerOptions
@@ -169,34 +170,61 @@ def main(args=None):
             if not os.path.isfile(param_file):
                 raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
 
-    node = Node("spawner_" + controller_names[0])
+    try:
+        spawner_node_name = "spawner_" + controller_names[0]
+        lock = FileLock("/tmp/ros2-control-controller-spawner.lock")
+        max_retries = 5
+        retry_delay = 3  # seconds
+        for attempt in range(max_retries):
+            tmp_logger = rclpy.logging.get_logger(spawner_node_name)
+            try:
+                tmp_logger.debug(
+                    bcolors.OKGREEN + "Waiting for the spawner lock to be acquired!" + bcolors.ENDC
+                )
+                # timeout after 20 seconds and try again
+                lock.acquire(timeout=20)
+                tmp_logger.debug(bcolors.OKGREEN + "Spawner lock acquired!" + bcolors.ENDC)
+                break
+            except Timeout:
+                tmp_logger.warn(
+                    bcolors.WARNING
+                    + f"Attempt {attempt+1} failed. Retrying in {retry_delay} seconds..."
+                    + bcolors.ENDC
+                )
+                time.sleep(retry_delay)
+        else:
+            tmp_logger.error(
+                bcolors.ERROR + "Failed to acquire lock after multiple attempts." + bcolors.ENDC
+            )
+            return 1
 
-    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!"
-        )
+        node = Node(spawner_node_name)
 
-    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:=<namespace>",
-            DeprecationWarning,
-        )
+        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!"
+            )
 
-    spawner_namespace = args.namespace if args.namespace else node.get_namespace()
+        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:=<namespace>",
+                DeprecationWarning,
+            )
 
-    if not spawner_namespace.startswith("/"):
-        spawner_namespace = f"/{spawner_namespace}"
+        spawner_namespace = args.namespace if args.namespace else node.get_namespace()
 
-    if not controller_manager_name.startswith("/"):
-        if spawner_namespace and spawner_namespace != "/":
-            controller_manager_name = f"{spawner_namespace}/{controller_manager_name}"
-        else:
-            controller_manager_name = f"/{controller_manager_name}"
+        if not spawner_namespace.startswith("/"):
+            spawner_namespace = f"/{spawner_namespace}"
+
+        if not controller_manager_name.startswith("/"):
+            if spawner_namespace and spawner_namespace != "/":
+                controller_manager_name = f"{spawner_namespace}/{controller_manager_name}"
+            else:
+                controller_manager_name = f"/{controller_manager_name}"
 
-    try:
         for controller_name in controller_names:
 
             if is_controller_loaded(
@@ -375,6 +403,7 @@ def main(args=None):
         return 1
     finally:
         node.destroy_node()
+        lock.release()
         rclpy.shutdown()
 
 

controller_manager/doc/parameters_context.yaml

@@ -15,7 +15,13 @@ hardware_components_initial_state: |
             - "base3"
 
 diagnostics.threshold.controllers.periodicity: |
-  The ``periodicity`` diagnostics will be published only for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity.
+  The ``periodicity`` diagnostics will be published for the asynchronous controllers, because any affect to the synchronous controllers will be reflected directly in the controller manager's periodicity. It is also published for the synchronous controllers that have a different update rate than the controller manager update rate.
 
-diagnostics.threshold.controllers.execution_time: |
-  The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period cycle to execute it's update cycle.
+diagnostics.threshold.controllers.execution_time.mean_error: |
+  The ``execution_time`` diagnostics will be published for all controllers. The ``mean_error`` for a synchronous controller will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous controller will be computed against the controller's desired update period, as the controller can take a maximum of the desired period to execute its update cycle.
+
+diagnostics.threshold.hardware_components.periodicity: |
+  The ``periodicity`` diagnostics will be published for the asynchronous hardware components, because any affect to the synchronous hardware components will be reflected directly in the controller manager's periodicity. It is also published for the synchronous hardware components that have a different read/write rate than the controller manager update rate.
+
+diagnostics.threshold.hardware_components.execution_time.mean_error: |
+  The ``execution_time`` diagnostics will be published for all hardware components. The ``mean_error`` for a synchronous hardware component will be computed against zero, as it should be as low as possible. However, the ``mean_error`` for an asynchronous hardware component will be computed against its desired read/write period, as the hardware component can take a maximum of the desired period to execute the read/write cycle.

controller_manager/include/controller_manager/controller_manager.hpp

@@ -375,6 +375,22 @@ class ControllerManager : public rclcpp::Node
    */
   void clear_requests();
 
+  /**
+   * Perform hardware command mode change for the given list of controllers to activate and
+   * deactivate.
+   * \param[in] rt_controller_list list of controllers in the real-time list.
+   * \param[in] activate_controllers_list list of controllers to activate.
+   * \param[in] deactivate_controllers_list list of controllers to deactivate.
+   * \param[in] rt_cycle_name name of the real-time cycle.
+   * \note This method is meant to be used only in the real-time control loops (`read`, `update` and
+   * `write`).
+   */
+  void perform_hardware_command_mode_change(
+    const std::vector<ControllerSpec> & rt_controller_list,
+    const std::vector<std::string> & activate_controllers_list,
+    const std::vector<std::string> & deactivate_controllers_list,
+    const std::string & rt_cycle_name);
+
   /**
    * If a controller is deactivated all following controllers (if any exist) should be switched
    * 'from' the chained mode.
@@ -433,7 +449,7 @@ class ControllerManager : public rclcpp::Node
    * \returns return_type::OK if all preceding controllers pass the checks, otherwise
    * return_type::ERROR.
    */
-  controller_interface::return_type check_preceeding_controllers_for_deactivate(
+  controller_interface::return_type check_preceding_controllers_for_deactivate(
     const std::vector<ControllerSpec> & controllers, int strictness,
     const ControllersListIterator controller_it, std::string & message);
 

controller_manager/package.xml

@@ -32,6 +32,7 @@
   <depend>std_msgs</depend>
   <depend>libstatistics_collector</depend>
   <depend>generate_parameter_library</depend>
+  <exec_depend>python3-filelock</exec_depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>