Controller Manager recovery from invalid URDF errors (#2775)

Co-authored-by: nibanovic <nikola.banovic@b-robotized.com>
Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
Co-authored-by: Dr. Denis <denis.stogl@b-robotized.com>

Author: 3 people

controller_manager/CMakeLists.txt

@@ -268,6 +268,15 @@ if(BUILD_TESTING)
     ${controller_manager_msgs_TARGETS}
   )
 
+  ament_add_gmock(test_controller_manager_with_resource_manager
+  test/test_controller_manager_with_resource_manager.cpp
+  )
+  target_link_libraries(test_controller_manager_with_resource_manager
+    controller_manager
+    hardware_interface::hardware_interface
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
   find_package(ament_cmake_pytest REQUIRED)
   install(FILES test/test_ros2_control_node.yaml
     DESTINATION test)

controller_manager/include/controller_manager/controller_manager.hpp

@@ -348,10 +348,32 @@ class ControllerManager : public rclcpp::Node
   std::vector<std::string> get_controller_names();
   std::pair<std::string, std::string> split_command_interface(
     const std::string & command_interface);
+
+  /// Initialize controller manager publishers, diagnostics, introspection, and shutdown handling.
   void init_controller_manager();
 
+  /// Initialize controller manager parameters.
+  /**
+   * Declares controller manager parameters, reads them from the generated parameter listener, and
+   * caches values used by the real-time update loop.
+   */
   void initialize_parameters();
 
+  /// Initialize the robot description subscription and wait notification timer.
+  /**
+   * Used when the controller manager starts without a valid robot description, or when a robot
+   * description failed to initialize hardware and the manager should keep waiting for a
+   * replacement.
+   */
+  void init_robot_description_callback();
+
+  /// Set the initial lifecycle state of hardware components.
+  /**
+   * Applies the hardware_components_initial_state parameters after the resource manager has loaded
+   * and initialized components from a valid robot description.
+   */
+  void set_initial_hardware_components_state();
+
   /**
    * Call cleanup to change the given controller lifecycle node to the unconfigured state.
    *
@@ -683,6 +705,8 @@ class ControllerManager : public rclcpp::Node
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
   rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
 
+  bool activate_all_hw_components_ = false;
+
   struct ControllerManagerExecutionTime
   {
     double read_time = 0.0;

controller_manager/src/controller_manager.cpp

@@ -548,25 +548,17 @@ ControllerManager::ControllerManager(
   chainable_loader_(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
       kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
-  robot_description_(urdf)
+  robot_description_(urdf),
+  activate_all_hw_components_(activate_all_hw_components)
 {
   initialize_parameters();
-  hardware_interface::ResourceManagerParams params;
-  params.robot_description = robot_description_;
-  params.clock = trigger_clock_;
-  params.logger = this->get_logger();
-  params.activate_all = activate_all_hw_components;
-  params.update_rate = static_cast<unsigned int>(params_->update_rate);
-  params.executor = executor_;
-  params.node_namespace = node_namespace;
-  params.allow_controller_activation_with_inactive_hardware =
-    params_->defaults.allow_controller_activation_with_inactive_hardware;
-  params.return_failed_hardware_names_on_return_deactivate_write_cycle_ =
-    params_->defaults.deactivate_controllers_on_hardware_self_deactivate;
-  params.handle_exceptions = params_->handle_exceptions;
-  resource_manager_ =
-    std::make_unique<hardware_interface::ResourceManager>(params, !robot_description_.empty());
+  init_resource_manager(urdf);
   init_controller_manager();
+  if (is_resource_manager_initialized())
+  {
+    set_initial_hardware_components_state();
+    init_services();
+  }
 }
 
 ControllerManager::ControllerManager(
@@ -586,7 +578,17 @@ ControllerManager::ControllerManager(
   robot_description_(resource_manager_->get_robot_description())
 {
   initialize_parameters();
-  init_controller_manager();
+  if (is_resource_manager_initialized())
+  {
+    init_controller_manager();
+    set_initial_hardware_components_state();
+    init_services();
+  }
+  else
+  {
+    RCLCPP_FATAL(get_logger(), "The resource manager is not properly initialized");
+    throw std::runtime_error("Resource manager object is not valid. See the FATAL message above.");
+  }
 }
 
 ControllerManager::~ControllerManager()
@@ -633,50 +635,18 @@ bool ControllerManager::shutdown_controllers()
 
 void ControllerManager::init_controller_manager()
 {
+  // Initialized activity publisher and diagnostics
   controller_manager_activity_publisher_ =
     create_publisher<controller_manager_msgs::msg::ControllerManagerActivity>(
       "~/activity", rclcpp::QoS(1).reliable().transient_local());
   rt_controllers_wrapper_.set_on_switch_callback(
     std::bind(&ControllerManager::publish_activity, this));
-  resource_manager_->set_on_component_state_switch_callback(
-    std::bind(&ControllerManager::publish_activity, this));
-
-  // Get parameters needed for RT "update" loop to work
-  if (is_resource_manager_initialized())
+  if (resource_manager_)
   {
-    if (params_->enforce_command_limits)
-    {
-      resource_manager_->import_joint_limiters(robot_description_);
-      RCLCPP_INFO(get_logger(), "Enforcing command limits is enabled...");
-    }
-    else
-    {
-      RCLCPP_INFO(
-        get_logger(),
-        "Enforcing command limits is disabled. Command limits from URDF will be ignored.");
-    }
-    init_services();
-  }
-  else
-  {
-    robot_description_notification_timer_ = create_wall_timer(
-      std::chrono::seconds(1),
-      [&]()
-      {
-        RCLCPP_WARN(
-          get_logger(), "Waiting for data on 'robot_description' topic to finish initialization");
-      });
+    resource_manager_->set_on_component_state_switch_callback(
+      std::bind(&ControllerManager::publish_activity, this));
   }
 
-  // set QoS to transient local to get messages that have already been published
-  // (if robot state publisher starts before controller manager)
-  robot_description_subscription_ = create_subscription<std_msgs::msg::String>(
-    "robot_description", rclcpp::QoS(1).transient_local(),
-    std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1));
-  RCLCPP_INFO(
-    get_logger(), "Subscribing to '%s' topic for robot description.",
-    robot_description_subscription_->get_topic_name());
-
   // Setup diagnostics
   periodicity_stats_.reset();
   diagnostics_updater_.setHardwareID("ros2_control");
@@ -719,6 +689,8 @@ void ControllerManager::init_controller_manager()
         }
         RCLCPP_INFO(get_logger(), "Shutting down the controller manager.");
       }));
+
+  init_robot_description_callback();
 }
 
 void ControllerManager::initialize_parameters()
@@ -758,6 +730,30 @@ void ControllerManager::initialize_parameters()
   }
 }
 
+void ControllerManager::init_robot_description_callback()
+{
+  if (!robot_description_subscription_)
+  {
+    robot_description_subscription_ = create_subscription<std_msgs::msg::String>(
+      "robot_description", rclcpp::QoS(1).transient_local(),
+      std::bind(&ControllerManager::robot_description_callback, this, std::placeholders::_1));
+    RCLCPP_INFO(
+      get_logger(), "Subscribing to '%s' topic for robot description.",
+      robot_description_subscription_->get_topic_name());
+  }
+
+  if (!is_resource_manager_initialized() && !robot_description_notification_timer_)
+  {
+    robot_description_notification_timer_ = create_wall_timer(
+      std::chrono::seconds(1),
+      [&]()
+      {
+        RCLCPP_WARN(
+          get_logger(), "Waiting for data on 'robot_description' topic to finish initialization");
+      });
+  }
+}
+
 void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description)
 {
   RCLCPP_INFO(get_logger(), "Received robot description from topic.");
@@ -768,50 +764,103 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
   {
     RCLCPP_WARN(
       get_logger(),
-      "ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description.");
+      "ResourceManager has already loaded a urdf and is initialized. Ignoring attempt to reload a "
+      "robot description.");
     return;
   }
+
   init_resource_manager(robot_description_);
-  if (is_resource_manager_initialized())
+  if (!is_resource_manager_initialized())
   {
-    RCLCPP_INFO(
-      get_logger(),
-      "Resource Manager has been successfully initialized. Starting Controller Manager "
-      "services...");
-    init_services();
+    // The RM failed to init AFTER we received the description - a critical error.
+    // don't finalize controller manager, instead keep waiting for robot description - fallback
+    // state
+    resource_manager_ =
+      std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, get_logger());
+    return;
   }
+  set_initial_hardware_components_state();
+  RCLCPP_INFO(
+    get_logger(),
+    "Resource Manager has been successfully initialized. Starting Controller Manager "
+    "services...");
+
+  init_services();
 }
 
 void ControllerManager::init_resource_manager(const std::string & robot_description)
 {
+  hardware_interface::ResourceManagerParams params;
+  params.robot_description = robot_description;
+  params.clock = trigger_clock_;
+  params.logger = this->get_logger();
+  params.activate_all = activate_all_hw_components_;
+  params.update_rate = static_cast<unsigned int>(params_->update_rate);
+  params.executor = executor_;
+  params.node_namespace = this->get_namespace();
+  params.allow_controller_activation_with_inactive_hardware =
+    params_->defaults.allow_controller_activation_with_inactive_hardware;
+  params.return_failed_hardware_names_on_return_deactivate_write_cycle_ =
+    params_->defaults.deactivate_controllers_on_hardware_self_deactivate;
+  params.handle_exceptions = params_->handle_exceptions;
+  resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(params, false);
+
+  resource_manager_->set_on_component_state_switch_callback(
+    std::bind(&ControllerManager::publish_activity, this));
+
+  if (robot_description.empty())
+  {
+    return;
+  }
+
   if (params_->enforce_command_limits)
   {
-    resource_manager_->import_joint_limiters(robot_description_);
     RCLCPP_INFO(get_logger(), "Enforcing command limits is enabled...");
+    try
+    {
+      resource_manager_->import_joint_limiters(robot_description);
+    }
+    catch (const std::exception & e)
+    {
+      RCLCPP_ERROR(get_logger(), "Error importing joint limiters: %s", e.what());
+      return;
+    }
   }
   else
   {
     RCLCPP_INFO(
       get_logger(),
-      "Enforcing command limits is disabled. Command limits from URDF will be ignored.");
+      "Enforcing command limits is disabled. Command limits from URDF will be "
+      "ignored.");
   }
-  hardware_interface::ResourceManagerParams params;
-  params.robot_description = robot_description;
-  params.clock = trigger_clock_;
-  params.logger = this->get_logger();
-  params.executor = executor_;
-  params.node_namespace = this->get_namespace();
-  params.update_rate = static_cast<unsigned int>(params_->update_rate);
-  params.handle_exceptions = params_->handle_exceptions;
-  if (!resource_manager_->load_and_initialize_components(params))
+
+  try
   {
-    RCLCPP_WARN(
-      get_logger(),
-      "Could not load and initialize hardware. Please check previous output for more details. "
-      "After you have corrected your URDF, try to publish robot description again.");
+    if (!resource_manager_->load_and_initialize_components(params))
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "Could not load and initialize hardware. Please check previous output for more details. "
+        "After you have corrected your URDF, try to publish robot description again.");
+      return;
+    }
+  }
+  catch (const std::exception & e)
+  {
+    // Other possible errors when loading components
+    RCLCPP_ERROR(
+      get_logger(), "Exception caught while loading and initializing components: %s", e.what());
     return;
   }
 
+  if (robot_description_notification_timer_)
+  {
+    robot_description_notification_timer_->cancel();
+  }
+}
+
+void ControllerManager::set_initial_hardware_components_state()
+{
   // Get all components and if they are not defined in parameters activate them automatically
   auto components_to_activate = resource_manager_->get_components_status();
 
@@ -1019,7 +1068,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
         group_name.c_str());
     }
   }
-
   // Process ungrouped components individually (configure and activate each one)
   for (const auto & component_name : ungrouped_components)
   {
@@ -1030,8 +1078,10 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
     }
   }
 
-  robot_description_notification_timer_->cancel();
-
+  if (robot_description_notification_timer_)
+  {
+    robot_description_notification_timer_->cancel();
+  }
   auto hw_components_info = resource_manager_->get_components_status();
 
   for (const auto & [component_name, component_info] : hw_components_info)

controller_manager/test/controller_manager_test_common.hpp

@@ -66,15 +66,10 @@ class ControllerManagerFixture : public ::testing::Test
     {
       cm_node_options.parameter_overrides(cm_parameters);
     }
+
     cm_ = std::make_shared<CtrlMgr>(
-      std::make_unique<hardware_interface::ResourceManager>(
-        rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()),
-      executor_, TEST_CM_NAME, cm_namespace, cm_node_options);
-    // We want to be able to not pass robot description immediately
-    if (!robot_description_.empty())
-    {
-      pass_robot_description_to_cm_and_rm(robot_description_);
-    }
+      executor_, robot_description_, true, TEST_CM_NAME, cm_namespace, cm_node_options);
+
     time_ = rclcpp::Time(0, 0, cm_->get_trigger_clock()->get_clock_type());
   }
 

controller_manager/test/test_controller_manager_hardware_error_handling.cpp

@@ -61,6 +61,15 @@ class TestableControllerManager : public controller_manager::ControllerManager
       std::move(resource_manager), executor, manager_node_name, node_namespace, node_options)
   {
   }
+  TestableControllerManager(
+    std::shared_ptr<rclcpp::Executor> executor, const std::string & urdf,
+    bool activate_all_hw_components, const std::string & manager_node_name = "controller_manager",
+    const std::string & node_namespace = "",
+    const rclcpp::NodeOptions & options = controller_manager::get_cm_node_options())
+  : ControllerManager(
+      executor, urdf, activate_all_hw_components, manager_node_name, node_namespace, options)
+  {
+  }
 };
 
 class TestControllerManagerWithTestableCM