Merge branch 'master' into add-simple-joint-limiter

.github/workflows/rosdoc2.yml

@@ -0,0 +1,14 @@
+name: rosdoc2
+
+on:
+  workflow_dispatch:
+  pull_request:
+    paths:
+      - ros2_control/doc/**
+      - ros2_control/rosdoc2.yaml
+      - ros2_control/package.xml
+
+
+jobs:
+  check:
+    uses: ros-controls/ros2_control_ci/.github/workflows/reusable-rosdoc2.yml@master

.pre-commit-config.yaml

@@ -26,6 +26,7 @@ repos:
       - id: check-symlinks
       - id: check-xml
       - id: check-yaml
+        args: ["--allow-multiple-documents"]
       - id: debug-statements
       - id: end-of-file-fixer
       - id: mixed-line-ending
@@ -36,7 +37,7 @@ repos:
 
   # Python hooks
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.15.2
+    rev: v3.16.0
     hooks:
     -   id: pyupgrade
         args: [--py36-plus]
@@ -55,14 +56,14 @@ repos:
         args: ["--line-length=99"]
 
   - repo: https://github.com/pycqa/flake8
-    rev: 7.0.0
+    rev: 7.1.0
     hooks:
     - id: flake8
       args: ["--extend-ignore=E501"]
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v18.1.5
+    rev: v18.1.8
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -132,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.28.4
+    rev: 0.28.6
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.12.0 (2024-07-01)
+-------------------
+
 4.11.0 (2024-05-14)
 -------------------
 * Fix dependencies for source build (`#1533 <https://github.com/ros-controls/ros2_control/issues/1533>`_)

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.11.0</version>
+  <version>4.12.0</version>
   <description>Description of controller_interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_manager/CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.12.0 (2024-07-01)
+-------------------
+* [rqt_controller_manager] Add hardware components (`#1455 <https://github.com/ros-controls/ros2_control/issues/1455>`_)
+* [RM] Rename `load_urdf` method to `load_and_initialize_components` and add error handling there to avoid stack crashing when error happens. (`#1354 <https://github.com/ros-controls/ros2_control/issues/1354>`_)
+* Fix update `period` for the first update after activation (`#1551 <https://github.com/ros-controls/ros2_control/issues/1551>`_)
+* Bump version of pre-commit hooks (`#1556 <https://github.com/ros-controls/ros2_control/issues/1556>`_)
+* Contributors: Christoph Fröhlich, Dr. Denis, github-actions[bot]
+
 4.11.0 (2024-05-14)
 -------------------
 * Add find_package for ament_cmake_gen_version_h (`#1534 <https://github.com/ros-controls/ros2_control/issues/1534>`_)

controller_manager/doc/userdoc.rst

@@ -184,6 +184,21 @@ 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
 
+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.
+
+.. image:: images/rqt_controller_manager.png
+
+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.
+
 Using the Controller Manager in a Process
 -----------------------------------------
 

controller_manager/include/controller_manager/controller_manager.hpp

@@ -193,7 +193,24 @@ class ControllerManager : public rclcpp::Node
   // the executor (see issue #260).
   // rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_;
 
-  // Per controller update rate support
+  /// Interface for external components to check if Resource Manager is initialized.
+  /**
+   * Checks if components in Resource Manager are loaded and initialized.
+   * \returns true if they are initialized, false otherwise.
+   */
+  CONTROLLER_MANAGER_PUBLIC
+  bool is_resource_manager_initialized() const
+  {
+    return resource_manager_ && resource_manager_->are_components_initialized();
+  }
+
+  /// Update rate of the main control loop in the controller manager.
+  /**
+   * Update rate of the main control loop in the controller manager.
+   * The method is used for per-controller update rate support.
+   *
+   * \returns update rate of the controller manager.
+   */
   CONTROLLER_MANAGER_PUBLIC
   unsigned int get_update_rate() const;
 

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.11.0</version>
+  <version>4.12.0</version>
   <description>Description of controller_manager</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_manager/src/controller_manager.cpp

@@ -187,8 +187,8 @@ ControllerManager::ControllerManager(
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
   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())),
+  resource_manager_(
+    std::make_unique<hardware_interface::ResourceManager>(this->get_node_clock_interface())),
   diagnostics_updater_(this),
   executor_(executor),
   loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
@@ -216,7 +216,13 @@ ControllerManager::ControllerManager(
       "[Deprecated] Passing the robot description parameter directly to the control_manager node "
       "is deprecated. Use the 'robot_description' topic from 'robot_state_publisher' instead.");
     init_resource_manager(robot_description_);
-    init_services();
+    if (is_resource_manager_initialized())
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "You have to restart the framework when using robot description from parameter!");
+      init_services();
+    }
   }
 
   diagnostics_updater_.setHardwareID("ros2_control");
@@ -243,7 +249,7 @@ ControllerManager::ControllerManager(
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
 
-  if (resource_manager_->is_urdf_already_loaded())
+  if (is_resource_manager_initialized())
   {
     init_services();
   }
@@ -271,36 +277,32 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
   RCLCPP_INFO(get_logger(), "Received robot description from topic.");
   RCLCPP_DEBUG(
     get_logger(), "'Content of robot description file: %s", robot_description.data.c_str());
-  // TODO(mamueluth): errors should probably be caught since we don't want controller_manager node
-  // to die if a non valid urdf is passed. However, should maybe be fine tuned.
-  try
+  robot_description_ = robot_description.data;
+  if (is_resource_manager_initialized())
   {
-    robot_description_ = robot_description.data;
-    if (resource_manager_->is_urdf_already_loaded())
-    {
-      RCLCPP_WARN(
-        get_logger(),
-        "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
-        "description file.");
-      return;
-    }
-    init_resource_manager(robot_description_);
-    init_services();
+    RCLCPP_WARN(
+      get_logger(),
+      "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
+      "description file.");
+    return;
   }
-  catch (std::runtime_error & e)
+  init_resource_manager(robot_description_);
+  if (is_resource_manager_initialized())
   {
-    RCLCPP_ERROR_STREAM(
-      get_logger(),
-      "The published robot description file (urdf) seems not to be genuine. The following error "
-      "was caught:"
-        << e.what());
+    init_services();
   }
 }
 
 void ControllerManager::init_resource_manager(const std::string & robot_description)
 {
-  // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong...
-  resource_manager_->load_urdf(robot_description);
+  if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_))
+  {
+    RCLCPP_WARN(
+      get_logger(),
+      "Could not load and initialize hardware. Please check previous output for more details. "
+      "After you have corrected your URDF, try to publish robot description again.");
+    return;
+  }
 
   // Get all components and if they are not defined in parameters activate them automatically
   auto components_to_activate = resource_manager_->get_components_status();

controller_manager/test/test_controller_manager_urdf_passing.cpp

@@ -29,11 +29,13 @@ class TestableControllerManager : public controller_manager::ControllerManager
 {
   friend TestControllerManagerWithTestableCM;
 
-  FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called);
-  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
-  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed);
-  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
-  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
+  FRIEND_TEST(
+    TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called);
+  FRIEND_TEST(
+    TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback);
+  FRIEND_TEST(
+    TestControllerManagerWithTestableCM,
+    expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description);
 
 public:
   TestableControllerManager(
@@ -59,29 +61,36 @@ class TestControllerManagerWithTestableCM
   }
 };
 
-TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called)
+TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called)
 {
-  ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
+  ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
 }
 
-TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback)
+TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback)
 {
-  ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
+  ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
   // mimic callback
   auto msg = std_msgs::msg::String();
   msg.data = ros2_control_test_assets::minimal_robot_urdf;
   cm_->robot_description_callback(msg);
-  ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
+  ASSERT_TRUE(cm_->resource_manager_->are_components_initialized());
 }
 
-TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed)
+TEST_P(
+  TestControllerManagerWithTestableCM,
+  expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description)
 {
-  ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
-  // mimic callback
+  ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
+  // wrong urdf
   auto msg = std_msgs::msg::String();
-  msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf;
+  msg.data = ros2_control_test_assets::minimal_uninitializable_robot_urdf;
+  cm_->robot_description_callback(msg);
+  ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
+  // correct urdf
+  msg = std_msgs::msg::String();
+  msg.data = ros2_control_test_assets::minimal_robot_urdf;
   cm_->robot_description_callback(msg);
-  ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
+  ASSERT_TRUE(cm_->resource_manager_->are_components_initialized());
 }
 
 INSTANTIATE_TEST_SUITE_P(

controller_manager_msgs/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.12.0 (2024-07-01)
+-------------------
+
 4.11.0 (2024-05-14)
 -------------------
 

controller_manager_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>controller_manager_msgs</name>
-  <version>4.11.0</version>
+  <version>4.12.0</version>
   <description>Messages and services for the controller manager.</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>