Fix exclusive hardware control mode switching on controller failed activation (#1522)

(cherry picked from commit ff52562)

# Conflicts:
#	controller_manager/doc/userdoc.rst
#	controller_manager/src/controller_manager.cpp
#	controller_manager/test/test_release_interfaces.cpp

Author: saikishor

controller_manager/CMakeLists.txt

@@ -162,13 +162,28 @@ if(BUILD_TESTING)
     DESTINATION lib
   )
 
+  add_library(test_controller_failed_activate SHARED
+  test/test_controller_failed_activate/test_controller_failed_activate.cpp
+  )
+  target_link_libraries(test_controller_failed_activate PUBLIC
+    controller_manager
+  )
+  target_compile_definitions(test_controller_failed_activate PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
+  pluginlib_export_plugin_description_file(
+    controller_interface test/test_controller_failed_activate/test_controller_failed_activate.xml)
+  install(
+    TARGETS test_controller_failed_activate
+    DESTINATION lib
+  )
+
   ament_add_gmock(test_release_interfaces
     test/test_release_interfaces.cpp
     APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
   )
   target_link_libraries(test_release_interfaces
     controller_manager
     test_controller_with_interfaces
+    test_controller_failed_activate
     ros2_control_test_assets::ros2_control_test_assets
   )
 

controller_manager/doc/userdoc.rst

@@ -356,6 +356,84 @@ Note that not all controllers have to be restarted, e.g., broadcasters.
 Restarting hardware
 ^^^^^^^^^^^^^^^^^^^^^
 
+<<<<<<< HEAD
 If hardware gets restarted then you should go through its lifecycle again.
 This can be simply achieved by returning ``ERROR`` from ``write`` and ``read`` methods of interface implementation.
 **NOT IMPLEMENTED YET - PLEASE STOP/RESTART ALL CONTROLLERS MANUALLY FOR NOW** The controller manager detects that and stops all the controllers that are commanding that hardware and restarts broadcasters that are listening to its states.
+=======
+If hardware gets restarted then you should go through its lifecycle again in order to reconfigure and export the interfaces
+
+Hardware and Controller Errors
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+If the hardware during it's ``read`` or ``write`` method returns ``return_type::ERROR``, the controller manager will stop all controllers that are using the hardware's command and state interfaces.
+Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller. In future, the controller manager will try to start any fallback controllers if available.
+
+Factors that affect Determinism
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+When run under the conditions determined in the above section, the determinism is assured up to the limitations of the hardware and the real-time kernel. However, there are some situations that can affect determinism:
+
+* When a controller fails to activate, the controller_manager will call the methods ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` to stop the started interfaces. These calls can cause jitter in the main control loop.
+
+Support for Asynchronous Updates
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+For some applications, it is desirable to run a controller at a lower frequency than the controller manager's update rate. For instance, if the ``update_rate`` for the controller manager is 100Hz, the sum of the execution times of all controllers' ``update`` calls and hardware components ``read`` and ``write`` calls must be below 10ms. If one controller requires 15ms of execution time, it cannot be executed synchronously without affecting the overall system update rate. Running a controller asynchronously can be beneficial in this scenario.
+
+The async update support is transparent to each controller implementation. A controller can be enabled for asynchronous updates by setting the ``is_async`` parameter to ``true``. The controller manager will load the controller accordingly. For example:
+
+.. code-block:: yaml
+
+    controller_manager:
+      ros__parameters:
+        update_rate: 100  # Hz
+        ...
+
+    example_async_controller:
+      ros__parameters:
+        type: example_controller/ExampleAsyncController
+        is_async: true
+        update_rate: 20  # Hz
+        ...
+
+will result in the controller being loaded and configured to run at 20Hz, while the controller manager runs at 100Hz. The description of the parameters can be found in the `Common Controller Parameters <https://control.ros.org/master/doc/ros2_controllers/doc/controllers_index.html#common-controller-parameters>`_ section of the ros2_controllers documentation.
+
+Scheduling Behavior
+----------------------
+From a design perspective, the controller manager functions as a scheduler that triggers updates for asynchronous controllers during the control loop.
+
+In this case, the ``ControllerInterfaceBase`` calls ``AsyncFunctionHandler`` to handle the actual ``update`` callback of the controller, which is the same mechanism used by the resource manager to support read/write operations for asynchronous hardware. When a controller is configured to run asynchronously, the controller interface creates an async handler during the controller's configuration and binds it to the controller's update method. The async handler thread created by the controller interface has either the same thread priority as the controller manager or the priority specified by the ``thread_priority`` parameter. When triggered by the controller manager, the async handler evaluates if the previous trigger is successfully finished and then calls the update method.
+
+If the update takes significant time and another update is triggered while the previous update is still running, the result of the previous update will be used. When this situation occurs, the controller manager will print a missing update cycle message, informing the user that they need to lower their controller's frequency as the computation is taking longer than initially estimated, as shown in the following example:
+
+.. code-block:: console
+
+   [ros2_control_node-1] [WARN] [1741626670.311533972] [example_async_controller]: The controller missed xx update cycles out of yy total triggers.
+
+If the async controller's update method throws an unhandled exception, the controller manager will handle it the same way as the synchronous controllers, deactivating the controller. It will also print an error message, similar to the following:
+
+.. code-block:: console
+
+  [ros2_control_node-1] [ERROR] [1741629098.352771957] [AsyncFunctionHandler]: AsyncFunctionHandler: Exception caught in the async callback thread!
+  ...
+  [ros2_control_node-1] [ERROR] [1741629098.352874151] [controller_manager]: Caught exception of type : St13runtime_error while updating controller
+  [ros2_control_node-1] [ERROR] [1741629098.352940701] [controller_manager]: Deactivating controllers : [example_async_controller] as their update resulted in an error!
+
+Monitoring and Tuning
+----------------------
+
+ros2_control ``controller_interface`` has a ``ControllerUpdateStats`` structure which can be used to monitor the controller update rate and the missed update cycles. The data is published to the ``/diagnostics`` topic. This can be used to fine tune the controller update rate.
+
+
+Different Clocks used by Controller Manager
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The controller manager internally uses the following two different clocks for a non-simulation setup:
+
+- ``RCL_ROS_TIME``: This clock is used mostly in the non-realtime loops.
+- ``RCL_STEADY_TIME``: This clock is used mostly in the realtime loops for the ``read``, ``update``, and ``write`` loops. However, when the controller manager is used in a simulation environment, the ``RCL_ROS_TIME`` clock is used for triggering the ``read``, ``update``, and ``write`` loops.
+
+The ``time`` argument in the ``read`` and ``write`` methods of the hardware components is of type ``RCL_STEADY_TIME``, as most of the hardware expects the time to be monotonic and not affected by the system time changes. However, the ``time`` argument in the ``update`` method of the controller is of type ``RCL_ROS_TIME`` as the controller is the one that interacts with other nodes or topics to receive the commands or publish the state. This ``time`` argument can be used by the controllers to validate the received commands or to publish the state at the correct timestamp.
+The ``period`` argument in the ``read``, ``update`` and ``write`` methods is calculated using the trigger clock of type ``RCL_STEADY_TIME`` so it is always monotonic.
+
+The reason behind using different clocks is to avoid the issues related to the affect of system time changes in the realtime loops. The ``ros2_control_node`` now also detects the overruns caused by the system time changes and longer execution times of the controllers and hardware components. The controller manager will print a warning message if the controller or hardware component misses the update cycle due to the system time changes or longer execution times.
+>>>>>>> ff52562 (Fix exclusive hardware control mode switching on controller failed activation (#1522))

controller_manager/src/controller_manager.cpp

@@ -1576,9 +1576,14 @@ void ControllerManager::switch_chained_mode(
 
 void ControllerManager::activate_controllers()
 {
+<<<<<<< HEAD
   std::vector<ControllerSpec> & rt_controller_list =
     rt_controllers_wrapper_.update_and_get_used_by_rt_list();
   for (const auto & controller_name : activate_request_)
+=======
+  std::vector<std::string> failed_controllers_command_interfaces;
+  for (const auto & controller_name : controllers_to_activate)
+>>>>>>> ff52562 (Fix exclusive hardware control mode switching on controller failed activation (#1522))
   {
     auto found_it = std::find_if(
       rt_controller_list.begin(), rt_controller_list.end(),
@@ -1677,6 +1682,7 @@ void ControllerManager::activate_controllers()
     }
     controller->assign_interfaces(std::move(command_loans), std::move(state_loans));
 
+<<<<<<< HEAD
     const auto new_state = controller->get_node()->activate();
     if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
     {
@@ -1686,6 +1692,41 @@ void ControllerManager::activate_controllers()
         controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(),
         hardware_interface::lifecycle_state_names::ACTIVE,
         lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+=======
+    auto new_state = controller->get_lifecycle_state();
+    try
+    {
+      found_it->periodicity_statistics->reset();
+      found_it->execution_time_statistics->reset();
+      new_state = controller->get_node()->activate();
+    }
+    catch (const std::exception & e)
+    {
+      RCLCPP_ERROR(
+        get_logger(), "Caught exception of type : %s while activating the controller '%s': %s",
+        typeid(e).name(), controller_name.c_str(), e.what());
+    }
+    catch (...)
+    {
+      RCLCPP_ERROR(
+        get_logger(), "Caught unknown exception while activating the controller '%s'",
+        controller_name.c_str());
+    }
+    if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    {
+      RCLCPP_ERROR(
+        get_logger(),
+        "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d). Releasing "
+        "interfaces!",
+        controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(),
+        hardware_interface::lifecycle_state_names::ACTIVE,
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      controller->release_interfaces();
+      failed_controllers_command_interfaces.insert(
+        failed_controllers_command_interfaces.end(), command_interface_names.begin(),
+        command_interface_names.end());
+      continue;
+>>>>>>> ff52562 (Fix exclusive hardware control mode switching on controller failed activation (#1522))
     }
 
     // if it is a chainable controller, make the reference interfaces available on activation
@@ -1694,8 +1735,23 @@ void ControllerManager::activate_controllers()
       resource_manager_->make_controller_reference_interfaces_available(controller_name);
     }
   }
+<<<<<<< HEAD
   // All controllers activated, switching done
   switch_params_.do_switch = false;
+=======
+  // Now prepare and perform the stop interface switching as this is needed for exclusive
+  // interfaces
+  if (
+    !failed_controllers_command_interfaces.empty() &&
+    (!resource_manager_->prepare_command_mode_switch({}, failed_controllers_command_interfaces) ||
+     !resource_manager_->perform_command_mode_switch({}, failed_controllers_command_interfaces)))
+  {
+    RCLCPP_ERROR(
+      get_logger(),
+      "Error switching back the interfaces in the hardware when the controller activation "
+      "failed.");
+  }
+>>>>>>> ff52562 (Fix exclusive hardware control mode switching on controller failed activation (#1522))
 }
 
 void ControllerManager::activate_controllers_asap()

controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp

@@ -0,0 +1,66 @@
+// Copyright 2021 Department of Engineering Cybernetics, NTNU.
+//
+// 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.
+
+#include "test_controller_failed_activate.hpp"
+
+#include <memory>
+#include <string>
+
+#include "lifecycle_msgs/msg/transition.hpp"
+
+namespace test_controller_failed_activate
+{
+TestControllerFailedActivate::TestControllerFailedActivate()
+: controller_interface::ControllerInterface()
+{
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerFailedActivate::on_init()
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type TestControllerFailedActivate::update(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+  return controller_interface::return_type::OK;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerFailedActivate::on_configure(const rclcpp_lifecycle::State & /*previous_state&*/)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerFailedActivate::on_activate(const rclcpp_lifecycle::State & /*previous_state&*/)
+{
+  // Simply simulate a controller that can not be activated
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerFailedActivate::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+}  // namespace test_controller_failed_activate
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  test_controller_failed_activate::TestControllerFailedActivate,
+  controller_interface::ControllerInterface)

controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp

@@ -0,0 +1,67 @@
+// Copyright 2020 Department of Engineering Cybernetics, NTNU
+//
+// 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 TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_
+#define TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "controller_manager/controller_manager.hpp"
+
+namespace test_controller_failed_activate
+{
+// Corresponds to the name listed within the pluginglib xml
+constexpr char TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME[] =
+  "controller_manager/test_controller_failed_activate";
+// Corresponds to the command interface to claim
+constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint2/velocity";
+class TestControllerFailedActivate : public controller_interface::ControllerInterface
+{
+public:
+  TestControllerFailedActivate();
+
+  virtual ~TestControllerFailedActivate() = default;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override
+  {
+    return controller_interface::InterfaceConfiguration{
+      controller_interface::interface_configuration_type::INDIVIDUAL,
+      {TEST_CONTROLLER_COMMAND_INTERFACE}};
+  }
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override
+  {
+    return controller_interface::InterfaceConfiguration{
+      controller_interface::interface_configuration_type::NONE};
+  }
+
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override;
+
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+};
+
+}  // namespace test_controller_failed_activate
+
+#endif  // TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_

controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml

@@ -0,0 +1,9 @@
+<library path="test_controller_failed_activate">
+
+  <class name="controller_manager/test_controller_failed_activate" type="test_controller_failed_activate::TestControllerFailedActivate" base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Controller used for testing
+    </description>
+  </class>
+
+</library>