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

(cherry picked from commit ff52562)

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,17 @@ Note that not all controllers have to be restarted, e.g., broadcasters.
 Restarting hardware
 ^^^^^^^^^^^^^^^^^^^^^
 
-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.
+

controller_manager/src/controller_manager.cpp

@@ -1578,6 +1578,7 @@ void ControllerManager::activate_controllers()
 {
   std::vector<ControllerSpec> & rt_controller_list =
     rt_controllers_wrapper_.update_and_get_used_by_rt_list();
+  std::vector<std::string> failed_controllers_command_interfaces;
   for (const auto & controller_name : activate_request_)
   {
     auto found_it = std::find_if(
@@ -1682,10 +1683,16 @@ void ControllerManager::activate_controllers()
     {
       RCLCPP_ERROR(
         get_logger(),
-        "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).",
+        "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;
     }
 
     // if it is a chainable controller, make the reference interfaces available on activation
@@ -1694,6 +1701,18 @@ void ControllerManager::activate_controllers()
       resource_manager_->make_controller_reference_interfaces_available(controller_name);
     }
   }
+  // 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.");
+  }
   // All controllers activated, switching done
   switch_params_.do_switch = false;
 }

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>

controller_manager/test/test_release_interfaces.cpp

@@ -22,6 +22,8 @@
 #include "controller_manager/controller_manager.hpp"
 #include "controller_manager_test_common.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "test_controller/test_controller.hpp"
+#include "test_controller_failed_activate/test_controller_failed_activate.hpp"
 #include "test_controller_with_interfaces/test_controller_with_interfaces.hpp"
 
 using ::testing::_;
@@ -199,3 +201,72 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
       abstract_test_controller2.c->get_state().id());
   }
 }
+
+TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failure)
+{
+  std::string controller_type =
+    test_controller_failed_activate::TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME;
+
+  // Load two controllers of different names
+  std::string controller_name1 = "test_controller1";
+  std::string controller_name2 = "test_controller2";
+  ASSERT_NO_THROW(cm_->load_controller(controller_name1, controller_type));
+  ASSERT_NO_THROW(cm_->load_controller(
+    controller_name2, test_controller_with_interfaces::TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME));
+  ASSERT_EQ(2u, cm_->get_loaded_controllers().size());
+  controller_manager::ControllerSpec abstract_test_controller1 = cm_->get_loaded_controllers()[0];
+  controller_manager::ControllerSpec abstract_test_controller2 = cm_->get_loaded_controllers()[1];
+
+  // Configure controllers
+  ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name1));
+  ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name2));
+
+  ASSERT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    abstract_test_controller1.c->get_lifecycle_state().id());
+  ASSERT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    abstract_test_controller2.c->get_lifecycle_state().id());
+
+  {
+    // Test starting the first controller
+    // test_controller1 activation always fails
+    RCLCPP_INFO(cm_->get_logger(), "Starting controller #1");
+    std::vector<std::string> start_controllers = {controller_name1};
+    std::vector<std::string> stop_controllers = {};
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get());
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      abstract_test_controller1.c->get_lifecycle_state().id());
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      abstract_test_controller2.c->get_lifecycle_state().id());
+  }
+
+  {
+    // Test starting the second controller, interfaces should have been released
+    // test_controller2 always successfully activates
+    RCLCPP_INFO(cm_->get_logger(), "Starting controller #2");
+    std::vector<std::string> start_controllers = {controller_name2};
+    std::vector<std::string> stop_controllers = {};
+    auto switch_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+      start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
+    ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+      << "switch_controller should be blocking until next update cycle";
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+      abstract_test_controller1.c->get_lifecycle_state().id());
+    ASSERT_EQ(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      abstract_test_controller2.c->get_lifecycle_state().id());
+  }
+}

hardware_interface_testing/test/test_components/test_components.xml

@@ -5,6 +5,12 @@
       Test Actuator
     </description>
   </class>
+  <class name="test_actuator_exclusive_interfaces" type="TestActuatorExclusiveInterfaces" base_class_type="hardware_interface::ActuatorInterface">
+    <description>
+      Test Actuator
+    </description>
+  </class>
+
 
   <class name="test_sensor" type="TestSensor" base_class_type="hardware_interface::SensorInterface">
     <description>