Merge branch 'master' into optimize-cm-code

.github/workflows/reusable-ros-tooling-source-build.yml

@@ -36,6 +36,7 @@ jobs:
         with:
           target-ros2-distro: ${{ inputs.ros_distro }}
           # build all packages listed in the meta package
+          ref: ${{ inputs.ref }} # otherwise the default branch is used for scheduled workflows
           package-name:
             controller_interface
             controller_manager

.github/workflows/reviewer_lottery.yml

@@ -6,6 +6,7 @@ on:
 jobs:
   test:
     runs-on: ubuntu-latest
+    if: github.actor != 'dependabot[bot]' && github.actor != 'mergify[bot]'
     steps:
     - uses: actions/checkout@v4
     - uses: uesteibar/reviewer-lottery@v3

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -115,7 +115,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual return_type init(
     const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-    const std::string & namespace_ = "",
+    const std::string & node_namespace = "",
     const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true));
 
   /// Custom configure method to read additional parameters for controller-nodes

controller_interface/src/controller_interface_base.cpp

@@ -26,10 +26,11 @@ namespace controller_interface
 {
 return_type ControllerInterfaceBase::init(
   const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-  const std::string & namespace_, const rclcpp::NodeOptions & node_options)
+  const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
 {
   node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
-    controller_name, namespace_, node_options, false);  // disable LifecycleNode service interfaces
+    controller_name, node_namespace, node_options,
+    false);  // disable LifecycleNode service interfaces
   urdf_ = urdf;
 
   try

controller_interface/test/test_controller_with_options.hpp

@@ -41,13 +41,13 @@ class ControllerWithOptions : public controller_interface::ControllerInterface
 
   controller_interface::return_type init(
     const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-    const std::string & namespace_ = "",
+    const std::string & node_namespace = "",
     const rclcpp::NodeOptions & node_options =
       rclcpp::NodeOptions()
         .allow_undeclared_parameters(true)
         .automatically_declare_parameters_from_overrides(true)) override
   {
-    ControllerInterface::init(controller_name, urdf, cm_update_rate, namespace_, node_options);
+    ControllerInterface::init(controller_name, urdf, cm_update_rate, node_namespace, node_options);
 
     switch (on_init())
     {

controller_manager/controller_manager/controller_manager_services.py

@@ -47,68 +47,89 @@ 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):
+def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
     request = ConfigureController.Request()
     request.name = controller_name
     return service_caller(
-        node, f"{controller_manager_name}/configure_controller", ConfigureController, request
+        node,
+        f"{controller_manager_name}/configure_controller",
+        ConfigureController,
+        request,
+        service_timeout,
     )
 
 
-def list_controllers(node, controller_manager_name):
+def list_controllers(node, controller_manager_name, service_timeout=10.0):
     request = ListControllers.Request()
     return service_caller(
-        node, f"{controller_manager_name}/list_controllers", ListControllers, request
+        node,
+        f"{controller_manager_name}/list_controllers",
+        ListControllers,
+        request,
+        service_timeout,
     )
 
 
-def list_controller_types(node, controller_manager_name):
+def list_controller_types(node, controller_manager_name, service_timeout=10.0):
     request = ListControllerTypes.Request()
     return service_caller(
-        node, f"{controller_manager_name}/list_controller_types", ListControllerTypes, request
+        node,
+        f"{controller_manager_name}/list_controller_types",
+        ListControllerTypes,
+        request,
+        service_timeout,
     )
 
 
-def list_hardware_components(node, controller_manager_name):
+def list_hardware_components(node, controller_manager_name, service_timeout=10.0):
     request = ListHardwareComponents.Request()
     return service_caller(
         node,
         f"{controller_manager_name}/list_hardware_components",
         ListHardwareComponents,
         request,
+        service_timeout,
     )
 
 
-def list_hardware_interfaces(node, controller_manager_name):
+def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0):
     request = ListHardwareInterfaces.Request()
     return service_caller(
         node,
         f"{controller_manager_name}/list_hardware_interfaces",
         ListHardwareInterfaces,
         request,
+        service_timeout,
     )
 
 
-def load_controller(node, controller_manager_name, controller_name):
+def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
     request = LoadController.Request()
     request.name = controller_name
     return service_caller(
-        node, f"{controller_manager_name}/load_controller", LoadController, request
+        node,
+        f"{controller_manager_name}/load_controller",
+        LoadController,
+        request,
+        service_timeout,
     )
 
 
-def reload_controller_libraries(node, controller_manager_name, force_kill):
+def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0):
     request = ReloadControllerLibraries.Request()
     request.force_kill = force_kill
     return service_caller(
         node,
         f"{controller_manager_name}/reload_controller_libraries",
         ReloadControllerLibraries,
         request,
+        service_timeout,
     )
 
 
-def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state):
+def set_hardware_component_state(
+    node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0
+):
     request = SetHardwareComponentState.Request()
     request.name = component_name
     request.target_state = lifecyle_state
@@ -117,6 +138,7 @@ def set_hardware_component_state(node, controller_manager_name, component_name,
         f"{controller_manager_name}/set_hardware_component_state",
         SetHardwareComponentState,
         request,
+        service_timeout,
     )
 
 
@@ -143,9 +165,13 @@ def switch_controllers(
     )
 
 
-def unload_controller(node, controller_manager_name, controller_name):
+def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
     request = UnloadController.Request()
     request.name = controller_name
     return service_caller(
-        node, f"{controller_manager_name}/unload_controller", UnloadController, request
+        node,
+        f"{controller_manager_name}/unload_controller",
+        UnloadController,
+        request,
+        service_timeout,
     )

controller_manager/controller_manager/launch_utils.py

@@ -32,12 +32,12 @@ def generate_load_controller_launch_description(
     Examples # noqa: D416
     --------
       # Assuming the controller type and controller parameters are known to the controller_manager
-      generate_load_controller_launch_description('joint_state_controller')
+      generate_load_controller_launch_description('joint_state_broadcaster')
 
       # Passing controller type and controller parameter file to load
       generate_load_controller_launch_description(
-        'joint_state_controller',
-        controller_type='joint_state_controller/JointStateController',
+        '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')
         )

controller_manager/doc/userdoc.rst

@@ -11,10 +11,9 @@ Determinism
 -----------
 
 For best performance when controlling hardware you want the controller manager to have as little jitter as possible in the main control loop.
-The normal linux kernel is optimized for computational throughput and therefore is not well suited for hardware control.
-The two easiest kernel options are the `Real-time Ubuntu 22.04 LTS Beta <https://ubuntu.com/blog/real-time-ubuntu-released>`_ or `linux-image-rt-amd64 <https://packages.debian.org/bullseye/linux-image-rt-amd64>`_ on Debian Bullseye.
 
-If you have a realtime kernel installed, the main thread of Controller Manager attempts to configure ``SCHED_FIFO`` with a priority of ``50``.
+Independent of the kernel installed, the main thread of Controller Manager attempts to
+configure ``SCHED_FIFO`` with a priority of ``50``.
 By default, the user does not have permission to set such a high priority.
 To give the user such permissions, add a group named realtime and add the user controlling your robot to this group:
 
@@ -36,6 +35,16 @@ Afterwards, add the following limits to the realtime group in ``/etc/security/li
 
 The limits will be applied after you log out and in again.
 
+The normal linux kernel is optimized for computational throughput and therefore is not well suited for hardware control.
+Alternatives to the standard kernel include
+
+- `Real-time Ubuntu 22.04 LTS Beta <https://ubuntu.com/blog/real-time-ubuntu-released>`_ on Ubuntu 22.04
+- `linux-image-rt-amd64 <https://packages.debian.org/bullseye/linux-image-rt-amd64>`_ on Debian Bullseye
+- lowlatency kernel (``sudo apt install linux-lowlatency``) on any ubuntu
+
+Though installing a realtime-kernel will definitely get the best results when it comes to low
+jitter, using a lowlatency kernel can improve things a lot with being really easy to install.
+
 Parameters
 -----------
 

controller_manager/include/controller_manager/controller_manager.hpp

@@ -71,14 +71,14 @@ class ControllerManager : public rclcpp::Node
     std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
     std::shared_ptr<rclcpp::Executor> executor,
     const std::string & manager_node_name = "controller_manager",
-    const std::string & namespace_ = "",
+    const std::string & node_namespace = "",
     const rclcpp::NodeOptions & options = get_cm_node_options());
 
   CONTROLLER_MANAGER_PUBLIC
   ControllerManager(
     std::shared_ptr<rclcpp::Executor> executor,
     const std::string & manager_node_name = "controller_manager",
-    const std::string & namespace_ = "",
+    const std::string & node_namespace = "",
     const rclcpp::NodeOptions & options = get_cm_node_options());
 
   CONTROLLER_MANAGER_PUBLIC

controller_manager/src/controller_manager.cpp

@@ -255,8 +255,8 @@ rclcpp::NodeOptions get_cm_node_options()
 
 ControllerManager::ControllerManager(
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
-  const std::string & namespace_, const rclcpp::NodeOptions & options)
-: rclcpp::Node(manager_node_name, namespace_, options),
+  const std::string & node_namespace, const rclcpp::NodeOptions & options)
+: rclcpp::Node(manager_node_name, node_namespace, options),
   resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
     update_rate_, this->get_node_clock_interface())),
   diagnostics_updater_(this),
@@ -286,19 +286,19 @@ ControllerManager::ControllerManager(
       "[Deprecated] Passing the robot description parameter directly to the control_manager node "
       "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.");
     init_resource_manager(robot_description_);
+    init_services();
   }
 
   diagnostics_updater_.setHardwareID("ros2_control");
   diagnostics_updater_.add(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
-  init_services();
 }
 
 ControllerManager::ControllerManager(
   std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
-  const std::string & namespace_, const rclcpp::NodeOptions & options)
-: rclcpp::Node(manager_node_name, namespace_, options),
+  const std::string & node_namespace, const rclcpp::NodeOptions & options)
+: rclcpp::Node(manager_node_name, node_namespace, options),
   resource_manager_(std::move(resource_manager)),
   diagnostics_updater_(this),
   executor_(executor),
@@ -313,12 +313,15 @@ ControllerManager::ControllerManager(
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
 
+  if (resource_manager_->is_urdf_already_loaded())
+  {
+    init_services();
+  }
   subscribe_to_robot_description_topic();
 
   diagnostics_updater_.setHardwareID("ros2_control");
   diagnostics_updater_.add(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
-  init_services();
 }
 
 void ControllerManager::subscribe_to_robot_description_topic()
@@ -352,6 +355,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
       return;
     }
     init_resource_manager(robot_description_);
+    init_services();
   }
   catch (std::runtime_error & e)
   {
@@ -948,7 +952,7 @@ controller_interface::return_type ControllerManager::switch_controller(
     {
       RCLCPP_WARN(
         get_logger(),
-        "Controller with name '%s' is not inactive so its following"
+        "Controller with name '%s' is not inactive so its following "
         "controllers do not have to be checked, because it cannot be activated.",
         controller_it->info.name.c_str());
       status = controller_interface::return_type::ERROR;

controller_manager/src/ros2_control_node.cpp

@@ -48,16 +48,14 @@ int main(int argc, char ** argv)
   std::thread cm_thread(
     [cm]()
     {
-      if (realtime_tools::has_realtime_kernel())
+      if (!realtime_tools::configure_sched_fifo(kSchedPriority))
       {
-        if (!realtime_tools::configure_sched_fifo(kSchedPriority))
-        {
-          RCLCPP_WARN(cm->get_logger(), "Could not enable FIFO RT scheduling policy");
-        }
-      }
-      else
-      {
-        RCLCPP_INFO(cm->get_logger(), "RT kernel is recommended for better performance");
+        RCLCPP_WARN(
+          cm->get_logger(),
+          "Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT "
+          "scheduling. See "
+          "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] "
+          "for details.");
       }
 
       // for calculating sleep time

controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp

@@ -33,7 +33,7 @@ TestControllerFailedInit::on_init()
 
 controller_interface::return_type TestControllerFailedInit::init(
   const std::string & /* controller_name */, const std::string & /* urdf */,
-  unsigned int /*cm_update_rate*/, const std::string & /*namespace_*/,
+  unsigned int /*cm_update_rate*/, const std::string & /*node_namespace*/,
   const rclcpp::NodeOptions & /*node_options*/)
 {
   return controller_interface::return_type::ERROR;

controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp

@@ -41,7 +41,7 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac
   CONTROLLER_INTERFACE_PUBLIC
   controller_interface::return_type init(
     const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-    const std::string & namespace_ = "",
+    const std::string & node_namespace = "",
     const rclcpp::NodeOptions & node_options =
       rclcpp::NodeOptions()
         .allow_undeclared_parameters(true)

controller_manager/test/test_controller_manager_hardware_error_handling.cpp

@@ -54,9 +54,9 @@ class TestableControllerManager : public controller_manager::ControllerManager
     std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
     std::shared_ptr<rclcpp::Executor> executor,
     const std::string & manager_node_name = "controller_manager",
-    const std::string & namespace_ = "")
+    const std::string & node_namespace = "")
   : controller_manager::ControllerManager(
-      std::move(resource_manager), executor, manager_node_name, namespace_)
+      std::move(resource_manager), executor, manager_node_name, node_namespace)
   {
   }
 };

controller_manager/test/test_controller_manager_urdf_passing.cpp

@@ -40,9 +40,9 @@ class TestableControllerManager : public controller_manager::ControllerManager
     std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
     std::shared_ptr<rclcpp::Executor> executor,
     const std::string & manager_node_name = "controller_manager",
-    const std::string & namespace_ = "")
+    const std::string & node_namespace = "")
   : controller_manager::ControllerManager(
-      std::move(resource_manager), executor, manager_node_name, namespace_)
+      std::move(resource_manager), executor, manager_node_name, node_namespace)
   {
   }
 };

controller_manager/test/test_controllers_chaining_with_controller_manager.cpp

@@ -94,9 +94,9 @@ class TestableControllerManager : public controller_manager::ControllerManager
     std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
     std::shared_ptr<rclcpp::Executor> executor,
     const std::string & manager_node_name = "controller_manager",
-    const std::string & namespace_ = "")
+    const std::string & node_namespace = "")
   : controller_manager::ControllerManager(
-      std::move(resource_manager), executor, manager_node_name, namespace_)
+      std::move(resource_manager), executor, manager_node_name, node_namespace)
   {
   }
 };