Add error recovery for invalid URDFs in controller manager initialization, robot description callback and resource manager constructor

Author: VitezGabriela

controller_manager/include/controller_manager/controller_manager.hpp

@@ -94,6 +94,8 @@ class ControllerManager : public rclcpp::Node
 
   void init_resource_manager(const std::string & robot_description);
 
+  void init_min_resource_manager();
+
   controller_interface::ControllerInterfaceBaseSharedPtr load_controller(
     const std::string & controller_name, const std::string & controller_type);
 
@@ -242,6 +244,17 @@ class ControllerManager : public rclcpp::Node
    */
   rclcpp::Clock::SharedPtr get_trigger_clock() const;
 
+  /**
+ * @brief Return the robot description timer.
+ * 
+ * It can be used to determine whether the Controller Manager 
+ * is currently waiting for a robot description to be published.
+ * 
+ * @return rclcpp::TimerBase::SharedPtr if the timer exists, nullptr otherwise
+ */
+  rclcpp::TimerBase::SharedPtr get_robot_description_timer() const {
+      return robot_description_notification_timer_;
+  }
 protected:
   void init_services();
 

controller_manager/src/controller_manager.cpp

@@ -494,34 +494,38 @@ bool ControllerManager::shutdown_controllers()
 
 void ControllerManager::init_controller_manager()
 {
-  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));
+  if (!resource_manager_ && !robot_description_.empty())
+  {
+    init_resource_manager(robot_description_);
+  }
 
-  // Get parameters needed for RT "update" loop to work
-  if (is_resource_manager_initialized())
+  if (!is_resource_manager_initialized())
   {
-    if (params_->enforce_command_limits)
+    // fallback state
+    init_min_resource_manager();
+    if (!robot_description_notification_timer_)
     {
-      resource_manager_->import_joint_limiters(robot_description_);
+      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");
+        });
     }
-    init_services();
-  }
-  else
+  }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");
-      });
+    RCLCPP_INFO(get_logger(),
+      "Resource Manager has been successfully initialized. Starting Controller Manager "
+      "services...");
+    init_services();
   }
 
+  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));
+
   // 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>(
@@ -627,9 +631,15 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
       get_logger(),
       "ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description.");
     return;
-  }
+  }  
+
   init_resource_manager(robot_description_);
-  if (is_resource_manager_initialized())
+  if (!is_resource_manager_initialized())
+  {
+    // 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
+    init_min_resource_manager();
+  } else 
   {
     RCLCPP_INFO(
       get_logger(),
@@ -641,25 +651,50 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
 
 void ControllerManager::init_resource_manager(const std::string & robot_description)
 {
-  if (params_->enforce_command_limits)
-  {
-    resource_manager_->import_joint_limiters(robot_description_);
-  }
   hardware_interface::ResourceManagerParams params;
   params.robot_description = robot_description;
   params.clock = trigger_clock_;
   params.logger = this->get_logger();
   params.executor = executor_;
   params.update_rate = static_cast<unsigned int>(params_->update_rate);
-  if (!resource_manager_->load_and_initialize_components(params))
+  resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(params, false);
+
+  RCLCPP_INFO_EXPRESSION(
+  get_logger(), params_->enforce_command_limits, "Enforcing command limits is enabled...");
+  if (params_->enforce_command_limits)
   {
-    RCLCPP_WARN(
+    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;
+    }
+  }
+
+  try
+  {
+    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(),
-      "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.");
+      "Exception caught while loading and initializing components: %s", e.what());
     return;
   }
-
+  resource_manager_->set_on_component_state_switch_callback(std::bind(&ControllerManager::publish_activity, this));
+        
   // Get all components and if they are not defined in parameters activate them automatically
   auto components_to_activate = resource_manager_->get_components_status();
 
@@ -807,6 +842,13 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
   }
 }
 
+void ControllerManager::init_min_resource_manager()
+{
+  resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, get_logger());
+  resource_manager_->set_on_component_state_switch_callback(
+      std::bind(&ControllerManager::publish_activity, this));
+}
+
 void ControllerManager::init_services()
 {
   // TODO(anyone): Due to issues with the MutliThreadedExecutor, this control loop does not rely on

hardware_interface/src/resource_manager.cpp

@@ -1406,9 +1406,17 @@ ResourceManager::ResourceManager(
     params.allow_controller_activation_with_inactive_hardware;
   return_failed_hardware_names_on_return_deactivate_write_cycle_ =
     params.return_failed_hardware_names_on_return_deactivate_write_cycle_;
-  if (load)
+  
+ try
   {
-    load_and_initialize_components(params);
+    if (load && !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;
+    }
     if (params.activate_all)
     {
       for (auto const & hw_info : resource_storage_->hardware_info_map_)
@@ -1419,6 +1427,14 @@ ResourceManager::ResourceManager(
       }
     }
   }
+  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;
+  }  
 }
 
 bool ResourceManager::shutdown_components()
@@ -1441,8 +1457,6 @@ bool ResourceManager::shutdown_components()
 bool ResourceManager::load_and_initialize_components(
   const std::string & urdf, const unsigned int update_rate)
 {
-  components_are_loaded_and_initialized_ = true;
-
   resource_storage_->robot_description_ = urdf;
   resource_storage_->cm_update_rate_ = update_rate;
 
@@ -1457,6 +1471,7 @@ bool ResourceManager::load_and_initialize_components(
   const std::string sensor_type = "sensor";
   const std::string actuator_type = "actuator";
 
+  components_are_loaded_and_initialized_ = true;
   std::lock_guard<std::recursive_mutex> resource_guard(resources_lock_);
   std::lock_guard<std::recursive_mutex> limiters_guard(joint_limiters_lock_);
   for (const auto & individual_hardware_info : hardware_info)