Merge branch 'master' into mock_sensor_commands

.github/workflows/galactic-rhel-binary-build.yml

@@ -20,7 +20,7 @@ jobs:
     runs-on: ubuntu-latest
     env:
       ROS_DISTRO: galactic
-    container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel
+    container: ghcr.io/ros-controls/ros:galactic-rhel
     steps:
       - uses: actions/checkout@v3
         with:

.github/workflows/humble-rhel-binary-build.yml

@@ -19,7 +19,7 @@ jobs:
     runs-on: ubuntu-latest
     env:
       ROS_DISTRO: humble
-    container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel
+    container: ghcr.io/ros-controls/ros:humble-rhel
     steps:
       - uses: actions/checkout@v3
         with:

.github/workflows/reusable-industrial-ci-with-cache.yml

@@ -54,7 +54,7 @@ jobs:
     env:
       CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }}
       BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }}
-      CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.os_code_name }}-${{ inputs.ros_repo }}-${{ github.job }}
+      CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }}
     steps:
       - name: Checkout ${{ inputs.ref }} when build is not scheduled
         if: ${{ github.event_name != 'schedule' }}

.github/workflows/rolling-rhel-binary-build.yml

@@ -20,7 +20,7 @@ jobs:
     runs-on: ubuntu-latest
     env:
       ROS_DISTRO: rolling
-    container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel
+    container: ghcr.io/ros-controls/ros:rolling-rhel
     steps:
       - uses: actions/checkout@v3
         with:

controller_interface/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.1.0 (2022-10-05)
+------------------
+* Add docs in export interface configurations for controllers. (`#804 <https://github.com/ros-controls/ros2_control/issues/804>`_)
+* Contributors: Denis Štogl
+
 3.0.0 (2022-09-19)
 ------------------
 

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>3.0.0</version>
+  <version>3.1.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,18 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.1.0 (2022-10-05)
+------------------
+* Don't ask to export reference interface if controller not 'inactive' or 'active' (`#824 <https://github.com/ros-controls/ros2_control/issues/824>`_)
+* Add diagnostics for controllers activity (`#820 <https://github.com/ros-controls/ros2_control/issues/820>`_)
+* Search for controller manager in the same namespace as spawner (`#810 <https://github.com/ros-controls/ros2_control/issues/810>`_)
+* Handle HW errors on read and write in CM by stopping controllers (`#742 <https://github.com/ros-controls/ros2_control/issues/742>`_)
+  Add code for deactivating controller when hardware gets an error on read and write.
+  Fix misleading variable name in the tests.
+  Remove all interface from available list for hardware when an error happens.
+  Do some more variable renaming to the new nomenclature.
+* Contributors: Denis Štogl, Tony Najjar
+
 3.0.0 (2022-09-19)
 ------------------
 

controller_manager/CMakeLists.txt

@@ -9,6 +9,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     ament_index_cpp
     controller_interface
     controller_manager_msgs
+    diagnostic_updater
     hardware_interface
     pluginlib
     rclcpp

controller_manager/include/controller_manager/controller_manager.hpp

@@ -40,6 +40,7 @@
 #include "controller_manager_msgs/srv/switch_controller.hpp"
 #include "controller_manager_msgs/srv/unload_controller.hpp"
 
+#include "diagnostic_updater/diagnostic_updater.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/resource_manager.hpp"
 
@@ -378,6 +379,9 @@ class ControllerManager : public rclcpp::Node
     const std::vector<ControllerSpec> & controllers, int strictness,
     const ControllersListIterator controller_it);
 
+  void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  diagnostic_updater::Updater diagnostics_updater_;
+
   std::shared_ptr<rclcpp::Executor> executor_;
 
   std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterface>> loader_;

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>3.0.0</version>
+  <version>3.1.0</version>
   <description>Description of controller_manager</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>
@@ -14,6 +14,7 @@
   <depend>ament_index_cpp</depend>
   <depend>controller_interface</depend>
   <depend>controller_manager_msgs</depend>
+  <depend>diagnostic_updater</depend>
   <depend>hardware_interface</depend>
   <depend>launch</depend>
   <depend>launch_ros</depend>

controller_manager/src/controller_manager.cpp

@@ -147,7 +147,8 @@ ControllerManager::ControllerManager(
     kControllerInterfaceNamespace, kControllerInterfaceClassName)),
   chainable_loader_(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
-      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
+      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
+  diagnostics_updater_(this)
 {
   if (!get_parameter("update_rate", update_rate_))
   {
@@ -163,6 +164,9 @@ ControllerManager::ControllerManager(
 
   init_resource_manager(robot_description);
 
+  diagnostics_updater_.setHardwareID("ros2_control");
+  diagnostics_updater_.add(
+    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
   init_services();
 }
 
@@ -177,8 +181,12 @@ ControllerManager::ControllerManager(
     kControllerInterfaceNamespace, kControllerInterfaceClassName)),
   chainable_loader_(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
-      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
+      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
+  diagnostics_updater_(this)
 {
+  diagnostics_updater_.setHardwareID("ros2_control");
+  diagnostics_updater_.add(
+    "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
   init_services();
 }
 
@@ -1307,24 +1315,25 @@ void ControllerManager::list_controllers_srv_cb(
       {
         controller_state.required_state_interfaces = state_interface_config.names;
       }
-    }
-    // check for chained interfaces
-    for (const auto & interface : controller_state.required_command_interfaces)
-    {
-      auto prefix_interface_type_pair = split_command_interface(interface);
-      auto prefix = prefix_interface_type_pair.first;
-      auto interface_type = prefix_interface_type_pair.second;
-      if (controller_chain_map.find(prefix) != controller_chain_map.end())
+      // check for chained interfaces
+      for (const auto & interface : controller_state.required_command_interfaces)
       {
-        controller_chain_map[controller_state.name].insert(prefix);
-        controller_chain_interface_map[controller_state.name].push_back(interface_type);
+        auto prefix_interface_type_pair = split_command_interface(interface);
+        auto prefix = prefix_interface_type_pair.first;
+        auto interface_type = prefix_interface_type_pair.second;
+        if (controller_chain_map.find(prefix) != controller_chain_map.end())
+        {
+          controller_chain_map[controller_state.name].insert(prefix);
+          controller_chain_interface_map[controller_state.name].push_back(interface_type);
+        }
+      }
+      // 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)
+      {
+        controller_state.reference_interfaces.push_back(reference.get_interface_name());
       }
-    }
-    auto references = controllers[i].c->export_reference_interfaces();
-    controller_state.reference_interfaces.reserve(references.size());
-    for (const auto & reference : references)
-    {
-      controller_state.reference_interfaces.push_back(reference.get_interface_name());
     }
     response->controller.push_back(controller_state);
     // keep track of controllers that are part of a chain
@@ -2119,4 +2128,30 @@ controller_interface::return_type ControllerManager::check_preceeding_controller
   return controller_interface::return_type::OK;
 };
 
+void ControllerManager::controller_activity_diagnostic_callback(
+  diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  // lock controllers
+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
+  const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list(guard);
+  bool all_active = true;
+  for (size_t i = 0; i < controllers.size(); ++i)
+  {
+    if (!is_controller_active(controllers[i].c))
+    {
+      all_active = false;
+    }
+    stat.add(controllers[i].info.name, controllers[i].c->get_state().label());
+  }
+
+  if (all_active)
+  {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "All controllers are active");
+  }
+  else
+  {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Not all controllers are active");
+  }
+}
+
 }  // namespace controller_manager

controller_manager/test/test_controller_manager_srvs.cpp

@@ -242,7 +242,7 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
   ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 0u);
   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(result->controller[0].reference_interfaces.size(), 0u);
   ASSERT_EQ(result->controller[0].chain_connections.size(), 0u);
   // check test controller
   ASSERT_EQ(result->controller[1].name, "test_controller_name");

controller_manager_msgs/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.1.0 (2022-10-05)
+------------------
+
 3.0.0 (2022-09-19)
 ------------------
 

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>3.0.0</version>
+  <version>3.1.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,13 @@
 Changelog for package hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.1.0 (2022-10-05)
+------------------
+* Cleanup Resource Manager a bit to increase clarity. (`#816 <https://github.com/ros-controls/ros2_control/issues/816>`_)
+* Handle hardware errors in Resource Manager (`#805 <https://github.com/ros-controls/ros2_control/issues/805>`_)
+  * Add code for deactivating controller when hardware gets an error on read and write.
+* Contributors: Denis Štogl
+
 3.0.0 (2022-09-19)
 ------------------
 

hardware_interface/doc/writing_new_hardware_interface.rst

@@ -3,7 +3,7 @@
 Writing a new hardware interface
 =================================
 
-In ros2_control hardware system components are libraries, dynamically loaded by the controller manager using the `pluginlib <ros.org/wiki/pluginlib>`_ interface.
+In ros2_control hardware system components are libraries, dynamically loaded by the controller manager using the `pluginlib <https://ros.org/wiki/pluginlib>`_ interface.
 The following is a step-by-step guide to create source files, basic tests, and compile rules for a new hardware interface.
 
 1. **Preparing package**
@@ -28,7 +28,7 @@ The following is a step-by-step guide to create source files, basic tests, and c
    1. Take care that you use header guards. ROS2-style is using ``#ifndef`` and ``#define`` preprocessor directives. (For more information on this, a search engine is your friend :) ).
 
    2. Include ``"hardware_interface/$interface_type$_interface.hpp"`` and ``visibility_control.h`` if you are using one.
-      ``$interface_type$`` can be ``Actuator``, ``Sensor`` or ``System`` depending on the type of hardware you are using. for more details about each type check `Hardware Components description <https://ros-controls.github.io/control.ros.org/getting_started.html#hardware-components>`_.
+      ``$interface_type$`` can be ``Actuator``, ``Sensor`` or ``System`` depending on the type of hardware you are using. for more details about each type check `Hardware Components description <https://control.ros.org/master/doc/getting_started/getting_started.html#hardware-components>`_.
 
    3. Define a unique namespace for your hardware_interface. This is usually the package name written in ``snake_case``.
 
@@ -37,7 +37,7 @@ The following is a step-by-step guide to create source files, basic tests, and c
       class HardwareInterfaceName : public hardware_interface::$InterfaceType$Interface
 
    5. Add a constructor without parameters and the following public methods implementing ``LifecycleNodeInterface``: ``on_configure``, ``on_cleanup``, ``on_shutdown``, ``on_activate``, ``on_deactivate``, ``on_error``; and overriding ``$InterfaceType$Interface`` definition: ``on_init``, ``export_state_interfaces``, ``export_command_interfaces``, ``prepare_command_mode_switch`` (optional), ``perform_command_mode_switch`` (optional), ``read``, ``write``.
-   For further explanation of hardware-lifecycle check the `pull request <https://github.com/ros-controls/ros2_control/pull/559/files#diff-2bd171d85b028c1b15b03b27d4e6dcbb87e52f705042bf111840e7a28ab268fc>`_ and for exact definitions of methods check the ``"hardware_interface/$interface_type$_interface.hpp"`` header or `doxygen documentation <http://control.ros.org/api/namespacehardware__interface.html>`_ for *Actuator*, *Sensor* or *System*.
+   For further explanation of hardware-lifecycle check the `pull request <https://github.com/ros-controls/ros2_control/pull/559/files#diff-2bd171d85b028c1b15b03b27d4e6dcbb87e52f705042bf111840e7a28ab268fc>`_ and for exact definitions of methods check the ``"hardware_interface/$interface_type$_interface.hpp"`` header or `doxygen documentation <https://control.ros.org/master/doc/api/namespacehardware__interface.html>`_ for *Actuator*, *Sensor* or *System*.
 
 4. **Adding definitions into source file (.cpp)**
 
@@ -137,4 +137,4 @@ That's it! Enjoy writing great controllers!
 Useful External References
 ---------------------------
 
-- `Templates and scripts for generating controllers shell <https://stoglrobotics.github.io/ros_team_workspace/use-cases/ros2_control/setup_robot_hardware_interface.html>`_ **NOTE**: The script is currently only recommended to use for Foxy, not compatible with the API from Galactic and onwards.
+- `Templates and scripts for generating controllers shell <https://stoglrobotics.github.io/ros_team_workspace/master/use-cases/ros2_control/setup_robot_hardware_interface.html>`_ **NOTE**: The script is currently only recommended to use for Foxy, not compatible with the API from Galactic and onwards.

hardware_interface/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>hardware_interface</name>
-  <version>3.0.0</version>
+  <version>3.1.0</version>
   <description>ros2_control hardware interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

joint_limits/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package joint_limits
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.1.0 (2022-10-05)
+------------------
+
 3.0.0 (2022-09-19)
 ------------------
 

joint_limits/package.xml

@@ -1,6 +1,6 @@
 <package format="3">
   <name>joint_limits</name>
-  <version>3.0.0</version>
+  <version>3.1.0</version>
   <description>Interfaces for handling of joint limits for controllers or hardware.</description>
 
   <maintainer email="[email protected]">Bence Magyar</maintainer>

ros2_control/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package ros2_control
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.1.0 (2022-10-05)
+------------------
+
 3.0.0 (2022-09-19)
 ------------------
 

ros2_control/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ros2_control</name>
-  <version>3.0.0</version>
+  <version>3.1.0</version>
   <description>Metapackage for ROS2 control related packages</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

ros2_control_test_assets/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package ros2_control_test_assets
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.1.0 (2022-10-05)
+------------------
+
 3.0.0 (2022-09-19)
 ------------------
 

ros2_control_test_assets/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>ros2_control_test_assets</name>
-  <version>3.0.0</version>
+  <version>3.1.0</version>
   <description>The package provides shared test resources for ros2_control stack
   </description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>

ros2controlcli/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package ros2controlcli
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.1.0 (2022-10-05)
+------------------
+
 3.0.0 (2022-09-19)
 ------------------
 

ros2controlcli/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>ros2controlcli</name>
-  <version>3.0.0</version>
+  <version>3.1.0</version>
   <description>
     The ROS 2 command line tools for ROS2 Control.
   </description>

ros2controlcli/setup.py

@@ -5,7 +5,7 @@
 
 setup(
     name=package_name,
-    version='3.0.0',
+    version='3.1.0',
     packages=find_packages(exclude=['test']),
     data_files=[
         ('share/' + package_name, ['package.xml']),