[RM] Rename load_urdf method to load_and_initialize_components and add error handling there to avoid stack crashing when error happens.

controller_manager/include/controller_manager/controller_manager.hpp

@@ -203,6 +203,13 @@ class ControllerManager : public rclcpp::Node
     return resource_manager_->are_components_initialized();
   }
 
+  /// Update rate of the main control loop in the controller manager.
+  /**
+   * Update rate of the main control loop in the controller manager.
+   * The method is used for per-controller update rate support.
+   *
+   * \returns update rate of the controller manager.
+   */
   CONTROLLER_MANAGER_PUBLIC
   unsigned int get_update_rate() const;
 

controller_manager/src/controller_manager.cpp

@@ -188,7 +188,7 @@ ControllerManager::ControllerManager(
   const std::string & node_namespace, const rclcpp::NodeOptions & options)
 : rclcpp::Node(manager_node_name, node_namespace, options),
   resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
-    update_rate_, this->get_node_clock_interface())),
+    this->get_node_clock_interface())),
   diagnostics_updater_(this),
   executor_(executor),
   loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
@@ -295,7 +295,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
 
 void ControllerManager::init_resource_manager(const std::string & robot_description)
 {
-  if (!resource_manager_->load_and_initialize_components(robot_description))
+  if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_))
   {
     RCLCPP_WARN(
       get_logger(),

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -50,7 +50,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 public:
   /// Default constructor for the Resource Manager.
   ResourceManager(
-    unsigned int update_rate = 100,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
 
   /// Constructor for the Resource Manager.
@@ -68,8 +67,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * used for triggering async components.
    */
   explicit ResourceManager(
-    const std::string & urdf, bool activate_all = false, unsigned int update_rate = 100,
-    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
+    const std::string & urdf, bool activate_all = false, const unsigned int update_rate = 100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
 
   ResourceManager(const ResourceManager &) = delete;
 
@@ -82,9 +80,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * in which the URDF might not be present at first initialization.
    *
    * \param[in] urdf string containing the URDF.
+   * \param[in] update_rate update rate of  the main control loop, i.e., of the controller manager.
    * \returns false if URDF validation has failed.
    */
-  bool load_and_initialize_components(const std::string & urdf);
+  virtual bool load_and_initialize_components(const std::string & urdf, const unsigned int update_rate = 100);
 
   /**
    * @brief if the resource manager load_and_initialize_components(...) function has been called

hardware_interface/src/resource_manager.cpp

@@ -79,12 +79,10 @@ class ResourceStorage
   // TODO(VX792): Change this when HW ifs get their own update rate,
   // because the ResourceStorage really shouldn't know about the cm's parameters
   ResourceStorage(
-    unsigned int update_rate = 100,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr)
   : actuator_loader_(pkg_name, actuator_interface_name),
     sensor_loader_(pkg_name, sensor_interface_name),
     system_loader_(pkg_name, system_interface_name),
-    cm_update_rate_(update_rate),
     clock_interface_(clock_interface)
   {
   }
@@ -762,24 +760,23 @@ class ResourceStorage
 
   // Update rate of the controller manager, and the clock interface of its node
   // Used by async components.
-  unsigned int cm_update_rate_;
+  unsigned int cm_update_rate_ = 100;
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
 };
 
 ResourceManager::ResourceManager(
-  unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
-: resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
+: resource_storage_(std::make_unique<ResourceStorage>(clock_interface))
 {
 }
 
 ResourceManager::~ResourceManager() = default;
 
 ResourceManager::ResourceManager(
-  const std::string & urdf, bool activate_all, unsigned int update_rate,
-  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
-: resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
+  const std::string & urdf, bool activate_all, const unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
+: resource_storage_(std::make_unique<ResourceStorage>(clock_interface))
 {
-  load_and_initialize_components(urdf);
+  load_and_initialize_components(urdf, update_rate);
 
   if (activate_all)
   {
@@ -793,10 +790,12 @@ ResourceManager::ResourceManager(
 }
 
 // CM API: Called in "callback/slow"-thread
-bool ResourceManager::load_and_initialize_components(const std::string & urdf)
+bool ResourceManager::load_and_initialize_components(const std::string & urdf, const unsigned int update_rate)
 {
   components_are_loaded_and_initialized_ = true;
 
+  resource_storage_->cm_update_rate_ = update_rate;
+
   const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
 
   const std::string system_type = "system";