Merge branch 'master' into improve/memory_allocation/buffer_variables

.github/mergify.yml

@@ -32,14 +32,15 @@ pull_request_rules:
       - author=mergify[bot]
     actions:
         comment:
-          message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich?
+          message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor?
 
   - name: development targets master branch
     conditions:
       - base!=master
       - author!=bmagyar
       - author!=destogl
       - author!=christophfroehlich
+      - author!=saikishor
       - author!=mergify[bot]
       - author!=dependabot[bot]
     actions:

.pre-commit-config.yaml

@@ -16,7 +16,7 @@
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.6.0
+    rev: v5.0.0
     hooks:
       - id: check-added-large-files
       - id: check-ast
@@ -37,7 +37,7 @@ repos:
 
   # Python hooks
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.17.0
+    rev: v3.19.0
     hooks:
     -   id: pyupgrade
         args: [--py36-plus]
@@ -50,7 +50,7 @@ repos:
       args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
 
   - repo: https://github.com/psf/black
-    rev: 24.8.0
+    rev: 24.10.0
     hooks:
       - id: black
         args: ["--line-length=99"]
@@ -63,7 +63,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v19.1.0
+    rev: v19.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.29.3
+    rev: 0.29.4
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -160,7 +160,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    * **The method called in the (real-time) control loop.**
    *
    * \param[in] time The time at the start of this control loop iteration
-   * \param[in] period The measured time taken by the last control loop iteration
+   * \param[in] period The measured time since the last control loop iteration
    * \returns return_type::OK if update is successfully, otherwise return_type::ERROR.
    */
   CONTROLLER_INTERFACE_PUBLIC

controller_manager/CMakeLists.txt

@@ -87,6 +87,7 @@ if(BUILD_TESTING)
 
   ament_add_gmock(test_controller_manager
     test/test_controller_manager.cpp
+    TIMEOUT 180
   )
   target_link_libraries(test_controller_manager
     controller_manager
@@ -194,6 +195,15 @@ if(BUILD_TESTING)
     ros2_control_test_assets::ros2_control_test_assets
   )
 
+  ament_add_gmock(test_hardware_spawner
+    test/test_hardware_spawner.cpp
+    TIMEOUT 120
+  )
+  target_link_libraries(test_hardware_spawner
+    controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
   install(FILES test/test_controller_spawner_with_fallback_controllers.yaml
     DESTINATION test)
 

controller_manager/controller_manager/hardware_spawner.py

@@ -57,7 +57,7 @@ def has_service_names(node, node_name, node_namespace, service_names):
 def is_hardware_component_loaded(
     node, controller_manager, hardware_component, service_timeout=0.0
 ):
-    components = list_hardware_components(node, hardware_component, service_timeout).component
+    components = list_hardware_components(node, controller_manager, service_timeout).component
     return any(c.name == hardware_component for c in components)
 
 
@@ -82,34 +82,33 @@ def handle_set_component_state_service_call(
         )
 
 
-def activate_components(node, controller_manager_name, components_to_activate):
+def activate_component(node, controller_manager_name, component_to_activate):
     active_state = State()
     active_state.id = State.PRIMARY_STATE_ACTIVE
     active_state.label = "active"
-    for component in components_to_activate:
-        handle_set_component_state_service_call(
-            node, controller_manager_name, component, active_state, "activated"
-        )
+    handle_set_component_state_service_call(
+        node, controller_manager_name, component_to_activate, active_state, "activated"
+    )
 
 
-def configure_components(node, controller_manager_name, components_to_configure):
+def configure_component(node, controller_manager_name, component_to_configure):
     inactive_state = State()
     inactive_state.id = State.PRIMARY_STATE_INACTIVE
     inactive_state.label = "inactive"
-    for component in components_to_configure:
-        handle_set_component_state_service_call(
-            node, controller_manager_name, component, inactive_state, "configured"
-        )
+    handle_set_component_state_service_call(
+        node, controller_manager_name, component_to_configure, inactive_state, "configured"
+    )
 
 
 def main(args=None):
     rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
     parser = argparse.ArgumentParser()
-    activate_or_confiigure_grp = parser.add_mutually_exclusive_group(required=True)
+    activate_or_configure_grp = parser.add_mutually_exclusive_group(required=True)
 
     parser.add_argument(
-        "hardware_component_name",
-        help="The name of the hardware component which should be activated.",
+        "hardware_component_names",
+        help="The name of the hardware components which should be activated.",
+        nargs="+",
     )
     parser.add_argument(
         "-c",
@@ -126,13 +125,13 @@ def main(args=None):
         type=float,
     )
     # add arguments which are mutually exclusive
-    activate_or_confiigure_grp.add_argument(
+    activate_or_configure_grp.add_argument(
         "--activate",
         help="Activates the given components. Note: Components are by default configured before activated. ",
         action="store_true",
         required=False,
     )
-    activate_or_confiigure_grp.add_argument(
+    activate_or_configure_grp.add_argument(
         "--configure",
         help="Configures the given components.",
         action="store_true",
@@ -141,9 +140,9 @@ def main(args=None):
 
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
+    hardware_components = args.hardware_component_names
     controller_manager_name = args.controller_manager
     controller_manager_timeout = args.controller_manager_timeout
-    hardware_component = [args.hardware_component_name]
     activate = args.activate
     configure = args.configure
 
@@ -156,24 +155,25 @@ def main(args=None):
             controller_manager_name = f"/{controller_manager_name}"
 
     try:
-        if not is_hardware_component_loaded(
-            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
-            )
-        elif activate:
-            activate_components(node, controller_manager_name, hardware_component)
-        elif configure:
-            configure_components(node, controller_manager_name, hardware_component)
-        else:
-            node.get_logger().error(
-                'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag'
-            )
-            parser.print_help()
-            return 0
+        for hardware_component in hardware_components:
+            if not is_hardware_component_loaded(
+                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
+                )
+            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'
+                )
+                parser.print_help()
+                return 0
     except KeyboardInterrupt:
         pass
     except ServiceNotFoundError as err:

controller_manager/doc/userdoc.rst

@@ -259,16 +259,20 @@ The parsed controller config file can follow the same conventions as the typical
 .. code-block:: console
 
     $ ros2 run controller_manager hardware_spawner -h
-    usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name
+    usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
+                            (--activate | --configure)
+                            hardware_component_names [hardware_component_names ...]
 
     positional arguments:
-      hardware_component_name
-                            The name of the hardware component which should be activated.
+      hardware_component_names
+                            The name of the hardware components 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
+      --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT
+                            Time to wait for the controller manager
       --activate            Activates the given components. Note: Components are by default configured before activated.
       --configure           Configures the given components.
 

controller_manager/include/controller_manager/controller_manager.hpp

@@ -123,7 +123,7 @@ class ControllerManager : public rclcpp::Node
     controller_spec.c = controller;
     controller_spec.info.name = controller_name;
     controller_spec.info.type = controller_type;
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     return add_controller_impl(controller_spec);
   }
 

controller_manager/include/controller_manager/controller_spec.hpp

@@ -37,7 +37,7 @@ struct ControllerSpec
 {
   hardware_interface::ControllerInfo info;
   controller_interface::ControllerInterfaceBaseSharedPtr c;
-  std::shared_ptr<rclcpp::Time> next_update_cycle_time;
+  std::shared_ptr<rclcpp::Time> last_update_cycle_time;
 };
 
 struct ControllerChainSpec

controller_manager/src/controller_manager.cpp

@@ -543,7 +543,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
   controller_spec.c = controller;
   controller_spec.info.name = controller_name;
   controller_spec.info.type = controller_type;
-  controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(
+  controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(
     0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
 
   // We have to fetch the parameters_file at the time of loading the controller, because this way we
@@ -1668,8 +1668,8 @@ void ControllerManager::activate_controllers(
       continue;
     }
     auto controller = found_it->c;
-    // reset the next update cycle time for newly activated controllers
-    *found_it->next_update_cycle_time =
+    // reset the last update cycle time for newly activated controllers
+    *found_it->last_update_cycle_time =
       rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
 
     bool assignment_successful = true;
@@ -2344,21 +2344,31 @@ controller_interface::return_type ControllerManager::update(
         run_controller_at_cm_rate ? period
                                   : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
 
+      const rclcpp::Time current_time = get_clock()->now();
       if (
-        *loaded_controller.next_update_cycle_time ==
+        *loaded_controller.last_update_cycle_time ==
         rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
       {
         // it is zero after activation
+        *loaded_controller.last_update_cycle_time = current_time - controller_period;
         RCLCPP_DEBUG(
-          get_logger(), "Setting next_update_cycle_time to %fs for the controller : %s",
-          time.seconds(), loaded_controller.info.name.c_str());
-        *loaded_controller.next_update_cycle_time = time;
+          get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s",
+          loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str());
       }
-
-      bool controller_go =
+      const auto controller_actual_period =
+        (current_time - *loaded_controller.last_update_cycle_time);
+
+      /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the
+      /// jitter in the system sleep cycles.
+      // For instance, A controller running at 50 Hz and the CM running at 100Hz, then when we have
+      // an update cycle at 0.019s (ideally, the controller should only trigger >= 0.02s), if we
+      // wait for next cycle, then trigger will happen at ~0.029 sec and this is creating an issue
+      // to keep up with the controller update rate (see issue #1769).
+      const bool controller_go =
+        run_controller_at_cm_rate ||
         (time ==
          rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
-        (time.seconds() >= loaded_controller.next_update_cycle_time->seconds());
+        (controller_actual_period.seconds() * controller_update_rate >= 0.99);
 
       RCLCPP_DEBUG(
         get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
@@ -2367,8 +2377,6 @@ controller_interface::return_type ControllerManager::update(
 
       if (controller_go)
       {
-        const auto controller_actual_period =
-          (time - *loaded_controller.next_update_cycle_time) + controller_period;
         auto controller_ret = controller_interface::return_type::OK;
         bool trigger_status = true;
         // Catch exceptions thrown by the controller update function
@@ -2392,7 +2400,7 @@ controller_interface::return_type ControllerManager::update(
           controller_ret = controller_interface::return_type::ERROR;
         }
 
-        *loaded_controller.next_update_cycle_time += controller_period;
+        *loaded_controller.last_update_cycle_time = current_time;
 
         if (controller_ret != controller_interface::return_type::OK)
         {

controller_manager/src/ros2_control_node.cpp

@@ -57,12 +57,34 @@ int main(int argc, char ** argv)
   auto cm = std::make_shared<controller_manager::ControllerManager>(
     executor, manager_node_name, "", cm_node_options);
 
+  const bool lock_memory = cm->get_parameter_or<bool>("lock_memory", true);
+  std::string message;
+  if (lock_memory && !realtime_tools::lock_memory(message))
+  {
+    RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str());
+  }
+
+  const int cpu_affinity = cm->get_parameter_or<int>("cpu_affinity", -1);
+  if (cpu_affinity >= 0)
+  {
+    const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity);
+    if (!affinity_result.first)
+    {
+      RCLCPP_WARN(
+        cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str());
+    }
+  }
+
   RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
+  const int thread_priority = cm->get_parameter_or<int>("thread_priority", kSchedPriority);
+  RCLCPP_INFO(
+    cm->get_logger(), "Spawning %s RT thread with scheduler priority: %d", cm->get_name(),
+    thread_priority);
 
   std::thread cm_thread(
-    [cm]()
+    [cm, thread_priority]()
     {
-      if (!realtime_tools::configure_sched_fifo(kSchedPriority))
+      if (!realtime_tools::configure_sched_fifo(thread_priority))
       {
         RCLCPP_WARN(
           cm->get_logger(),
@@ -75,7 +97,7 @@ int main(int argc, char ** argv)
       {
         RCLCPP_INFO(
           cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.",
-          kSchedPriority);
+          thread_priority);
       }
 
       // for calculating sleep time