add to support restart operations in switch_controller by specifying …

…the same controller name in the start/stop requests

controller_manager/CMakeLists.txt

@@ -72,6 +72,17 @@ if(BUILD_TESTING)
     DESTINATION lib
   )
 
+  add_library(test_controller_with_command SHARED
+    test/test_controller_with_command/test_controller_with_command.cpp)
+  target_link_libraries(test_controller_with_command PUBLIC controller_manager)
+  target_compile_definitions(test_controller_with_command PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
+  pluginlib_export_plugin_description_file(
+    controller_interface test/test_controller_with_command/test_controller_with_command.xml)
+  install(
+    TARGETS test_controller_with_command
+    DESTINATION lib
+  )
+
   add_library(test_chainable_controller SHARED
     test/test_chainable_controller/test_chainable_controller.cpp
   )
@@ -123,6 +134,16 @@ if(BUILD_TESTING)
     ros2_control_test_assets::ros2_control_test_assets
   )
 
+  ament_add_gmock(test_restart_controller
+    test/test_restart_controller.cpp
+    APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
+  )
+  target_link_libraries(test_restart_controller
+    controller_manager
+    test_controller_with_command
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
   ament_add_gmock(test_controllers_chaining_with_controller_manager
     test/test_controllers_chaining_with_controller_manager.cpp
   )

controller_manager/src/controller_manager.cpp

@@ -975,12 +975,14 @@ controller_interface::return_type ControllerManager::switch_controller(
     // 'from' chained mode
     propagate_deactivation_of_chained_mode(controllers);
 
-    if (const auto result = check_activate_requests(controllers, strictness);
+    // we need to check the requests in the order of deactivate and activate to consider the case
+    // where the controller will be restarted is included in the (de)activate request
+    if (const auto result = check_deactivate_requests(controllers, strictness);
         result != CheckDeActivateRequestResult::OK)
     {
       return result;
     }
-    if (const auto result = check_deactivate_requests(controllers, strictness);
+    if (const auto result = check_activate_requests(controllers, strictness);
         result != CheckDeActivateRequestResult::OK)
     {
       return result;
@@ -1362,9 +1364,16 @@ void ControllerManager::deactivate_controllers(
         const auto new_state = controller->get_node()->deactivate();
         controller->release_interfaces();
 
-        // if it is a chainable controller, make the reference interfaces unavailable on
-        // deactivation
-        if (controller->is_chainable())
+        // If it is a chainable controller, make the reference interfaces unavailable on
+        // deactivation.
+        // However, if it will be activated asap for restart due to subsequent processes in
+        // manage_switch, the reference_interface needs to remain available to ensure the success of
+        // the switch_chained_mode process, so this case is an exception.
+        const bool will_be_restarted_asap =
+          switch_params_.do_switch && switch_params_.activate_asap &&
+          (std::find(activate_request_.begin(), activate_request_.end(), controller_name) !=
+           activate_request_.end());
+        if (controller->is_chainable() && !will_be_restarted_asap)
         {
           resource_manager_->make_controller_reference_interfaces_unavailable(controller_name);
         }
@@ -2052,8 +2061,11 @@ void ControllerManager::manage_switch()
 
   deactivate_controllers(rt_controller_list, deactivate_request_);
 
-  switch_chained_mode(to_chained_mode_request_, true);
+  // When the same controller is specified for both 'from' and 'to' for restarting a controller, it
+  // is necessary to switch in the order 'from' then 'to', in order to disable the chained mode
+  // once and then enable it again.
   switch_chained_mode(from_chained_mode_request_, false);
+  switch_chained_mode(to_chained_mode_request_, true);
 
   // activate controllers once the switch is fully complete
   if (!switch_params_.activate_asap)
@@ -2365,24 +2377,28 @@ controller_interface::return_type ControllerManager::check_following_controllers
       return controller_interface::return_type::ERROR;
     }
 
+    const bool following_will_be_activated =
+      std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) !=
+      activate_request_.end();
+    const bool following_will_be_deactivated =
+      std::find(
+        deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) !=
+      deactivate_request_.end();
+
     if (is_controller_active(following_ctrl_it->c))
     {
-      // will following controller be deactivated?
-      if (
-        std::find(
-          deactivate_request_.begin(), deactivate_request_.end(), following_ctrl_it->info.name) !=
-        deactivate_request_.end())
+      // will following controller be deactivated and will not be restarted?
+      if (following_will_be_deactivated && !following_will_be_activated)
       {
         RCLCPP_WARN(
-          get_logger(), "The following controller with name '%s' will be deactivated.",
+          get_logger(),
+          "The following controller with name '%s' will be deactivated and will not be restarted.",
           following_ctrl_it->info.name.c_str());
         return controller_interface::return_type::ERROR;
       }
     }
     // check if following controller will not be activated
-    else if (
-      std::find(activate_request_.begin(), activate_request_.end(), following_ctrl_it->info.name) ==
-      activate_request_.end())
+    else if (!following_will_be_activated)
     {
       RCLCPP_WARN(
         get_logger(),
@@ -2551,12 +2567,22 @@ controller_interface::return_type ControllerManager::check_preceeding_controller
         continue;
       }
 
-      // check if preceding controller will be activated
-      if (
-        is_controller_inactive(preceding_ctrl_it->c) &&
+      const bool following_will_be_activated =
+        std::find(activate_request_.begin(), activate_request_.end(), controller_it->info.name) !=
+        activate_request_.end();
+      const bool preceding_will_be_deactivated =
+        std::find(
+          deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) !=
+        deactivate_request_.end();
+      const bool preceding_will_be_activated =
         std::find(
           activate_request_.begin(), activate_request_.end(), preceding_ctrl_it->info.name) !=
-          activate_request_.end())
+        activate_request_.end();
+      const bool preceding_will_be_restarted =
+        preceding_will_be_deactivated && preceding_will_be_activated;
+
+      // check if preceding controller will be activated
+      if (is_controller_inactive(preceding_ctrl_it->c) && preceding_will_be_activated)
       {
         RCLCPP_WARN(
           get_logger(),
@@ -2566,11 +2592,7 @@ controller_interface::return_type ControllerManager::check_preceeding_controller
         return controller_interface::return_type::ERROR;
       }
       // check if preceding controller will not be deactivated
-      else if (
-        is_controller_active(preceding_ctrl_it->c) &&
-        std::find(
-          deactivate_request_.begin(), deactivate_request_.end(), preceding_ctrl_it->info.name) ==
-          deactivate_request_.end())
+      else if (is_controller_active(preceding_ctrl_it->c) && !preceding_will_be_deactivated)
       {
         RCLCPP_WARN(
           get_logger(),
@@ -2579,6 +2601,19 @@ controller_interface::return_type ControllerManager::check_preceeding_controller
           controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str());
         return controller_interface::return_type::ERROR;
       }
+      // check if only following controller will not be restarted despite preceding will not be
+      // restarted
+      else if (
+        is_controller_active(preceding_ctrl_it->c) && !following_will_be_activated &&
+        preceding_will_be_restarted)
+      {
+        RCLCPP_WARN(
+          get_logger(),
+          "Could not deactivate and restart controller with name '%s' because "
+          "preceding controller with name '%s' is active and will not be restarted.",
+          controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str());
+        return controller_interface::return_type::ERROR;
+      }
       // TODO(destogl): this should be discussed how to it the best - just a placeholder for now
       // else if (
       //  strictness ==

controller_manager/test/test_chainable_controller/test_chainable_controller.cpp

@@ -134,6 +134,8 @@ CallbackReturn TestChainableController::on_configure(
 CallbackReturn TestChainableController::on_activate(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  ++activate_calls;
+
   if (!is_in_chained_mode())
   {
     auto msg = rt_command_ptr_.readFromRT();
@@ -143,6 +145,13 @@ CallbackReturn TestChainableController::on_activate(
   return CallbackReturn::SUCCESS;
 }
 
+CallbackReturn TestChainableController::on_deactivate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  ++deactivate_calls;
+  return CallbackReturn::SUCCESS;
+}
+
 CallbackReturn TestChainableController::on_cleanup(
   const rclcpp_lifecycle::State & /*previous_state*/)
 {

controller_manager/test/test_chainable_controller/test_chainable_controller.hpp

@@ -57,6 +57,9 @@ class TestChainableController : public controller_interface::ChainableController
   CONTROLLER_MANAGER_PUBLIC
   CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
 
+  CONTROLLER_MANAGER_PUBLIC
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
   CONTROLLER_MANAGER_PUBLIC
   CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
 
@@ -80,7 +83,12 @@ class TestChainableController : public controller_interface::ChainableController
   CONTROLLER_MANAGER_PUBLIC
   void set_reference_interface_names(const std::vector<std::string> & reference_interface_names);
 
-  size_t internal_counter;
+  size_t internal_counter = 0;
+
+  // Variable where we store when activate or deactivate was called
+  size_t activate_calls = 0;
+  size_t deactivate_calls = 0;
+
   controller_interface::InterfaceConfiguration cmd_iface_cfg_;
   controller_interface::InterfaceConfiguration state_iface_cfg_;
   std::vector<std::string> reference_interface_names_;

controller_manager/test/test_controller/test_controller.cpp

@@ -101,6 +101,18 @@ CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*pr
   return CallbackReturn::SUCCESS;
 }
 
+CallbackReturn TestController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  ++activate_calls;
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn TestController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  ++deactivate_calls;
+  return CallbackReturn::SUCCESS;
+}
+
 CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/)
 {
   if (simulate_cleanup_failure)

controller_manager/test/test_controller/test_controller.hpp

@@ -56,6 +56,12 @@ class TestController : public controller_interface::ControllerInterface
   CONTROLLER_MANAGER_PUBLIC
   CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
 
+  CONTROLLER_MANAGER_PUBLIC
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  CONTROLLER_MANAGER_PUBLIC
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
   CONTROLLER_MANAGER_PUBLIC
   CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
 
@@ -69,6 +75,11 @@ class TestController : public controller_interface::ControllerInterface
   const std::string & getRobotDescription() const;
 
   unsigned int internal_counter = 0;
+
+  // Variable where we store when activate or deactivate was called
+  size_t activate_calls = 0;
+  size_t deactivate_calls = 0;
+
   bool simulate_cleanup_failure = false;
   // Variable where we store when cleanup was called, pointer because the controller
   // is usually destroyed after cleanup

controller_manager/test/test_controller_with_command/test_controller_with_command.cpp

@@ -0,0 +1,74 @@
+// Copyright 2024 Tokyo Robotics Inc.
+//
+// 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_with_command.hpp"
+
+#include <memory>
+#include <string>
+
+#include "lifecycle_msgs/msg/transition.hpp"
+
+namespace test_controller_with_command
+{
+TestControllerWithCommand::TestControllerWithCommand() : controller_interface::ControllerInterface()
+{
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerWithCommand::on_init()
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type TestControllerWithCommand::update(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+  return controller_interface::return_type::OK;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerWithCommand::on_configure(const rclcpp_lifecycle::State & /*previous_state&*/)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerWithCommand::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  simulate_command = SIMULATE_COMMAND_ACTIVATE_VALUE;
+  ++activate_calls;
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerWithCommand::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  simulate_command = SIMULATE_COMMAND_DEACTIVATE_VALUE;
+  ++deactivate_calls;
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerWithCommand::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+}  // namespace test_controller_with_command
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  test_controller_with_command::TestControllerWithCommand,
+  controller_interface::ControllerInterface)