Merge branch 'humble' into mergify/bp/humble/pr-742

controller_interface/CHANGELOG.rst

@@ -2,6 +2,20 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.33.0 (2023-10-11)
+-------------------
+
+2.32.0 (2023-10-03)
+-------------------
+
+2.31.0 (2023-09-11)
+-------------------
+* add a broadcaster for range sensor (`#1091 <https://github.com/ros-controls/ros2_control/issues/1091>`_) (`#1100 <https://github.com/ros-controls/ros2_control/issues/1100>`_)
+* Contributors: flochre
+
+2.30.0 (2023-08-14)
+-------------------
+
 2.29.0 (2023-07-09)
 -------------------
 

controller_interface/include/semantic_components/range_sensor.hpp

@@ -0,0 +1,60 @@
+// Copyright 2023 flochre
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_
+#define SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_
+
+#include <limits>
+#include <string>
+#include <vector>
+
+#include "semantic_components/semantic_component_interface.hpp"
+#include "sensor_msgs/msg/range.hpp"
+
+namespace semantic_components
+{
+class RangeSensor : public SemanticComponentInterface<sensor_msgs::msg::Range>
+{
+public:
+  explicit RangeSensor(const std::string & name) : SemanticComponentInterface(name, 1)
+  {
+    interface_names_.emplace_back(name_ + "/" + "range");
+  }
+
+  virtual ~RangeSensor() = default;
+
+  /**
+   * Return Range reported by a sensor
+   *
+   * \return value of the range in meters
+   */
+  float get_range() { return state_interfaces_[0].get().get_value(); }
+
+  /// Return Range message with range in meters
+  /**
+   * Constructs and return a Range message from the current values.
+   * \return Range message from values;
+   */
+  bool get_values_as_message(sensor_msgs::msg::Range & message)
+  {
+    // update the message values
+    message.range = get_range();
+
+    return true;
+  }
+};
+
+}  // namespace semantic_components
+
+#endif  // SEMANTIC_COMPONENTS__RANGE_SENSOR_HPP_

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>2.29.0</version>
+  <version>2.33.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,26 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.33.0 (2023-10-11)
+-------------------
+* Export of the get_cm_node_options() from robostack (`#1129 <https://github.com/ros-controls/ros2_control/issues/1129>`_) (`#1130 <https://github.com/ros-controls/ros2_control/issues/1130>`_)
+* Contributors: mergify[bot]
+
+2.32.0 (2023-10-03)
+-------------------
+* Fix multiple calls to export reference interfaces (backport `#1108 <https://github.com/ros-controls/ros2_control/issues/1108>`_) (`#1114 <https://github.com/ros-controls/ros2_control/issues/1114>`_)
+* Contributors: Sai Kishor Kothakota, Dr Denis
+
+2.31.0 (2023-09-11)
+-------------------
+* [Docs] Fix information about activation and deactivation of chainable controllers (`#1104 <https://github.com/ros-controls/ros2_control/issues/1104>`_) (`#1106 <https://github.com/ros-controls/ros2_control/issues/1106>`_)
+* Contributors: mergify[bot]
+
+2.30.0 (2023-08-14)
+-------------------
+* [CM] Fixes the issue with individual controller's update rate (`#1082 <https://github.com/ros-controls/ros2_control/issues/1082>`_) (`#1097 <https://github.com/ros-controls/ros2_control/issues/1097>`_)
+* Contributors: Sai Kishor Kothakota
+
 2.29.0 (2023-07-09)
 -------------------
 * [CM] Make error message after trying to active controller more informative. (`#1066 <https://github.com/ros-controls/ros2_control/issues/1066>`_) (`#1072 <https://github.com/ros-controls/ros2_control/issues/1072>`_)

controller_manager/doc/controller_chaining.rst

@@ -0,0 +1,85 @@
+:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_manager/doc/controller_chaining.rst
+
+.. _controller_chaining:
+
+Controller Chaining / Cascade Control
+======================================
+
+This document proposes a minimal-viable-implementation of serial controller chaining as described in `Chaining Controllers design document <https://github.com/ros-controls/roadmap/blob/master/design_drafts/controller_chaining.md>`__.
+Cascade control is a specific type of controller chaining.
+
+
+Scope of the Document and Background Knowledge
+-------------------------------------------------------
+
+This approach focuses only on serial chaining of controllers and tries to reuse existing mechanisms for it.
+It focuses on `inputs and outputs of a controller <https://github.com/ros-controls/roadmap/blob/master/design_drafts/controller_chaining.md#input--outputs-of-a-controller>`__ and their management in the controller manager.
+The concept of `controller groups <https://github.com/ros-controls/roadmap/blob/master/design_drafts/controller_chaining.md#controller-group>`__ will be introduced only for clarity reasons, and its only meaning is that controllers in that group can be updated in arbitrary order.
+This doesn't mean that the controller groups as described `in the controller chaining document <https://github.com/ros-controls/roadmap/blob/master/design_drafts/controller_chaining.md#controller-group>`__ will not be introduced and used in the future.
+Nevertheless, the author is convinced that this would add only unnecessary complexity at this stage, although in the long term they *could* provide clearer structure and interfaces.
+
+Motivation, Purpose and Use
+---------------------------------
+
+To describe the intent of this document, lets focus on the simple yet sufficient example `Example 2 from 'controllers_chaining' design docs  <https://github.com/ros-controls/roadmap/blob/master/design_drafts/controller_chaining.md#example-2>`__:
+
+.. image:: images/chaining_example2.png
+  :alt: Example2
+
+
+In this example, we want to chain 'position_tracking' controller with 'diff_drive_controller' and two PID controllers.
+Let's now imagine a use-case where we don't only want to run all those controllers as a group, but also flexibly add preceding steps.
+This means the following:
+
+  1. When a robot is started, we want to check if motor velocity control is working properly and therefore only PID controllers are activated.
+     At this stage we can control the input of PID controller also externally using topics.
+     However, these controllers also provide virtual interfaces, so we can chain them.
+  2. Then "diff_drive_controller" is activated and attaches itself to the virtual input interfaces of PID controllers.
+     PID controllers also get informed that they are working in chained mode and therefore disable their external interface through subscriber.
+     Now we check if kinematics of differential robot is running properly.
+  3. After that, "position_tracking" can be activated and attached to "diff_drive_controller" that disables its external interfaces.
+  4. If any of the controllers is deactivated, also all preceding controllers are deactivated.
+
+
+Implementation
+--------------
+
+A Controller Base-Class: ChainableController
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+A ``ChainableController`` extends ``ControllerInterface`` class with ``virtual InterfaceConfiguration input_interface_configuration() const = 0`` method.
+This method should implement for each controller that **can be preceded** by another controller exporting all the input interfaces.
+For simplicity reasons, it is assumed for now that controller's all input interfaces are used.
+Therefore, do not try to implement any exclusive combinations of input interfaces, but rather write multiple controllers if you need exclusivity.
+
+The ``ChainableController`` base class implements ``void set_chained_mode(bool activate)`` that sets an internal flag that a controller is used by another controller (in chained mode) and calls ``virtual void on_set_chained_mode(bool activate) = 0`` that implements controller's specific actions when chained modes is activated or deactivated, e.g., deactivating subscribers.
+
+As an example, PID controllers export one virtual interface ``pid_reference`` and stop their subscriber ``<controller_name>/pid_reference`` when used in chained mode.  'diff_drive_controller' controller exports list of virtual interfaces  ``<controller_name>/v_x``, ``<controller_name>/v_y``, and ``<controller_name>/w_z``, and stops subscribers from topics ``<controller_name>/cmd_vel`` and ``<controller_name>/cmd_vel_unstamped``. Its publishers can continue running.
+
+Inner Resource Management
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+After configuring a chainable controller, controller manager calls ``input_interface_configuration`` method and takes ownership over controller's input interfaces.
+This is the same process as done by ``ResourceManager`` and hardware interfaces.
+Controller manager maintains "claimed" status of interface in a vector (the same as done in ``ResourceManager``).
+
+
+Activation and Deactivation Chained Controllers
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Chained controllers must be activated and deactivated together or in the proper order.
+This means you must first activate all following controllers to have the preceding one activated.
+For the deactivation there is the inverse rule - all preceding controllers have to be deactivated before the following controller is deactivated.
+One can also think of it as an actual chain, you can not add a chain link or break the chain in the middle.
+
+
+Debugging outputs
+----------------------------
+
+Flag ``unavailable`` on reference interface does not provide much information about anything at the moment. So don't get confused by it. The reason we have it are internal implementation reasons irelevant for the usage.
+
+
+Closing remarks
+----------------------------
+
+- Maybe addition of the new controller's type ``ChainableController`` is not necessary. It would also be feasible to add implementation of ``input_interface_configuration()`` method into ``ControllerInterface`` class with default result ``interface_configuration_type::NONE``.

controller_manager/include/controller_manager/controller_manager.hpp

@@ -55,7 +55,7 @@ namespace controller_manager
 {
 using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
 
-rclcpp::NodeOptions get_cm_node_options();
+CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options();
 
 class ControllerManager : public rclcpp::Node
 {

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>2.29.0</version>
+  <version>2.33.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

@@ -1362,11 +1362,17 @@ void ControllerManager::list_controllers_srv_cb(
         }
       }
       // check reference interfaces only if controller is inactive or active
-      auto references = controllers[i].c->export_reference_interfaces();
-      controller_state.reference_interfaces.reserve(references.size());
-      for (const auto & reference : references)
+      if (controllers[i].c->is_chainable())
       {
-        controller_state.reference_interfaces.push_back(reference.get_interface_name());
+        auto references =
+          resource_manager_->get_controller_reference_interface_names(controllers[i].info.name);
+        controller_state.reference_interfaces.reserve(references.size());
+        for (const auto & reference : references)
+        {
+          const std::string prefix_name = controllers[i].c->get_node()->get_name();
+          const std::string interface_name = reference.substr(prefix_name.size() + 1);
+          controller_state.reference_interfaces.push_back(interface_name);
+        }
       }
     }
     response->controller.push_back(controller_state);

controller_manager/test/test_controller_manager_srvs.cpp

@@ -269,7 +269,9 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
   ASSERT_EQ(result->controller[0].is_chainable, true);
   ASSERT_EQ(result->controller[0].is_chained, false);
   ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u);
-  ;
+  ASSERT_EQ("joint1/position", result->controller[0].reference_interfaces[0]);
+  ASSERT_EQ("joint1/velocity", result->controller[0].reference_interfaces[1]);
+
   ASSERT_EQ(result->controller[0].chain_connections.size(), 0u);
   // check test controller
   ASSERT_EQ(result->controller[1].state, "inactive");

controller_manager_msgs/CHANGELOG.rst

@@ -2,6 +2,18 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.33.0 (2023-10-11)
+-------------------
+
+2.32.0 (2023-10-03)
+-------------------
+
+2.31.0 (2023-09-11)
+-------------------
+
+2.30.0 (2023-08-14)
+-------------------
+
 2.29.0 (2023-07-09)
 -------------------
 

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>2.29.0</version>
+  <version>2.33.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>

hardware_interface/CHANGELOG.rst

@@ -2,6 +2,24 @@
 Changelog for package hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.33.0 (2023-10-11)
+-------------------
+* [MockHardware] Added dynamic simulation functionality. (`#1028 <https://github.com/ros-controls/ros2_control/issues/1028>`_) (`#1125 <https://github.com/ros-controls/ros2_control/issues/1125>`_)
+* Contributors: mergify[bot]
+
+2.32.0 (2023-10-03)
+-------------------
+* Add GPIO tag description to docs (`#1109 <https://github.com/ros-controls/ros2_control/issues/1109>`_) (`#1120 <https://github.com/ros-controls/ros2_control/issues/1120>`_)
+* Contributors: Christoph Froehlich
+
+2.31.0 (2023-09-11)
+-------------------
+
+2.30.0 (2023-08-14)
+-------------------
+* Add checks if hardware is initialized. (backport `#1054 <https://github.com/ros-controls/ros2_control/issues/1054>`_) (`#1081 <https://github.com/ros-controls/ros2_control/issues/1081>`_)
+* Contributors: Denis Stogl
+
 2.29.0 (2023-07-09)
 -------------------
 

hardware_interface/doc/hardware_components_userdoc.rst

@@ -14,20 +14,21 @@ Guidelines and Best Practices
 .. toctree::
    :titlesonly:
 
-   Writing a Hardware Interface <writing_new_hardware_interface.rst>
+   Hardware Interface Types <hardware_interface_types_userdoc.rst>
+   Writing a Hardware Component <writing_new_hardware_component.rst>
 
 
 Handling of errors that happen during read() and write() calls
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``write()`` methods of a hardware interface, ``on_error(previous_state)`` method will be called to handle any error that happened.
+If ``hardware_interface::return_type::ERROR`` is returned from ``read()`` or ``write()`` methods of a hardware_interface class, ``on_error(previous_state)`` method will be called to handle any error that happened.
 
 Error handling follows the `node lifecycle <https://design.ros2.org/articles/node_lifecycle.html>`_.
 If successful ``CallbackReturn::SUCCESS`` is returned and hardware is again in ``UNCONFIGURED``  state, if any ``ERROR`` or ``FAILURE`` happens the hardware ends in ``FINALIZED`` state and can not be recovered.
 The only option is to reload the complete plugin, but there is currently no service for this in the Controller Manager.
 
-Migration from Foxy to Galactic
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Migration from Foxy to newer versions
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle.
 The following list shows a set of quick changes to port existing hardware components to Galactic:
@@ -36,20 +37,29 @@ The following list shows a set of quick changes to port existing hardware compon
 
 2. If using BaseInterface as base class then you should remove it. Specifically, change:
 
-hardware_interface::BaseInterface<hardware_interface::[Actuator|Sensor|System]Interface> to hardware_interface::[Actuator|Sensor|System]Interface
+  .. code-block:: c++
+
+    hardware_interface::BaseInterface<hardware_interface::[Actuator|Sensor|System]Interface>
+
+  to
+
+  .. code-block:: c++
+
+    hardware_interface::[Actuator|Sensor|System]Interface
 
 3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp``
 
 4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary
 
-5. replace first three lines in ``on_init`` to:
+5. replace first three lines in ``on_init`` to
+
+  .. code-block:: c++
 
-.. code-block:: c++
+    if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS)
+    {
+      return CallbackReturn::ERROR;
+    }
 
-   if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS)
-   {
-     return CallbackReturn::ERROR;
-   }
 
 6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;``