[RM] Rename load_urdf method to load_and_initialize_components an…

…d add error handling there to avoid stack crashing when error happens. (#1354)

controller_manager/include/controller_manager/controller_manager.hpp

@@ -193,7 +193,24 @@ class ControllerManager : public rclcpp::Node
   // the executor (see issue #260).
   // rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_;
 
-  // Per controller update rate support
+  /// Interface for external components to check if Resource Manager is initialized.
+  /**
+   * Checks if components in Resource Manager are loaded and initialized.
+   * \returns true if they are initialized, false otherwise.
+   */
+  CONTROLLER_MANAGER_PUBLIC
+  bool is_resource_manager_initialized() const
+  {
+    return resource_manager_ && 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

@@ -187,8 +187,8 @@ ControllerManager::ControllerManager(
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
   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())),
+  resource_manager_(
+    std::make_unique<hardware_interface::ResourceManager>(this->get_node_clock_interface())),
   diagnostics_updater_(this),
   executor_(executor),
   loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
@@ -216,7 +216,13 @@ ControllerManager::ControllerManager(
       "[Deprecated] Passing the robot description parameter directly to the control_manager node "
       "is deprecated. Use the 'robot_description' topic from 'robot_state_publisher' instead.");
     init_resource_manager(robot_description_);
-    init_services();
+    if (is_resource_manager_initialized())
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "You have to restart the framework when using robot description from parameter!");
+      init_services();
+    }
   }
 
   diagnostics_updater_.setHardwareID("ros2_control");
@@ -243,7 +249,7 @@ ControllerManager::ControllerManager(
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
 
-  if (resource_manager_->is_urdf_already_loaded())
+  if (is_resource_manager_initialized())
   {
     init_services();
   }
@@ -271,36 +277,32 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
   RCLCPP_INFO(get_logger(), "Received robot description from topic.");
   RCLCPP_DEBUG(
     get_logger(), "'Content of robot description file: %s", robot_description.data.c_str());
-  // TODO(mamueluth): errors should probably be caught since we don't want controller_manager node
-  // to die if a non valid urdf is passed. However, should maybe be fine tuned.
-  try
+  robot_description_ = robot_description.data;
+  if (is_resource_manager_initialized())
   {
-    robot_description_ = robot_description.data;
-    if (resource_manager_->is_urdf_already_loaded())
-    {
-      RCLCPP_WARN(
-        get_logger(),
-        "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
-        "description file.");
-      return;
-    }
-    init_resource_manager(robot_description_);
-    init_services();
+    RCLCPP_WARN(
+      get_logger(),
+      "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
+      "description file.");
+    return;
   }
-  catch (std::runtime_error & e)
+  init_resource_manager(robot_description_);
+  if (is_resource_manager_initialized())
   {
-    RCLCPP_ERROR_STREAM(
-      get_logger(),
-      "The published robot description file (urdf) seems not to be genuine. The following error "
-      "was caught:"
-        << e.what());
+    init_services();
   }
 }
 
 void ControllerManager::init_resource_manager(const std::string & robot_description)
 {
-  // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong...
-  resource_manager_->load_urdf(robot_description);
+  if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_))
+  {
+    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;
+  }
 
   // Get all components and if they are not defined in parameters activate them automatically
   auto components_to_activate = resource_manager_->get_components_status();

controller_manager/test/test_controller_manager_urdf_passing.cpp

@@ -29,11 +29,13 @@ class TestableControllerManager : public controller_manager::ControllerManager
 {
   friend TestControllerManagerWithTestableCM;
 
-  FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called);
-  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
-  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed);
-  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
-  FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback);
+  FRIEND_TEST(
+    TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called);
+  FRIEND_TEST(
+    TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback);
+  FRIEND_TEST(
+    TestControllerManagerWithTestableCM,
+    expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description);
 
 public:
   TestableControllerManager(
@@ -59,29 +61,36 @@ class TestControllerManagerWithTestableCM
   }
 };
 
-TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called)
+TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called)
 {
-  ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
+  ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
 }
 
-TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback)
+TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback)
 {
-  ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
+  ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
   // mimic callback
   auto msg = std_msgs::msg::String();
   msg.data = ros2_control_test_assets::minimal_robot_urdf;
   cm_->robot_description_callback(msg);
-  ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
+  ASSERT_TRUE(cm_->resource_manager_->are_components_initialized());
 }
 
-TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed)
+TEST_P(
+  TestControllerManagerWithTestableCM,
+  expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description)
 {
-  ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
-  // mimic callback
+  ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
+  // wrong urdf
   auto msg = std_msgs::msg::String();
-  msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf;
+  msg.data = ros2_control_test_assets::minimal_uninitializable_robot_urdf;
+  cm_->robot_description_callback(msg);
+  ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
+  // correct urdf
+  msg = std_msgs::msg::String();
+  msg.data = ros2_control_test_assets::minimal_robot_urdf;
   cm_->robot_description_callback(msg);
-  ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded());
+  ASSERT_TRUE(cm_->resource_manager_->are_components_initialized());
 }
 
 INSTANTIATE_TEST_SUITE_P(

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -49,8 +49,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 {
 public:
   /// Default constructor for the Resource Manager.
-  ResourceManager(
-    unsigned int update_rate = 100,
+  explicit ResourceManager(
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
 
   /// Constructor for the Resource Manager.
@@ -59,18 +58,16 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * hardware components listed within as well as populate their respective
    * state and command interfaces.
    *
-   * If the interfaces ought to be validated, the constructor throws an exception
-   * in case the URDF lists interfaces which are not available.
-   *
    * \param[in] urdf string containing the URDF.
-   * \param[in] validate_interfaces boolean argument indicating whether the exported
-   * interfaces ought to be validated. Defaults to true.
    * \param[in] activate_all boolean argument indicating if all resources should be immediately
    * activated. Currently used only in tests.
+   * \param[in] update_rate Update rate of the controller manager to calculate calling frequency
+   * of async components.
+   * \param[in] clock_interface reference to the clock interface of the CM node for getting time
+   * used for triggering async components.
    */
   explicit ResourceManager(
-    const std::string & urdf, bool validate_interfaces = true, bool activate_all = false,
-    unsigned int update_rate = 100,
+    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;
@@ -79,29 +76,26 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 
   /// Load resources from on a given URDF.
   /**
-   * The resource manager can be post initialized with a given URDF.
+   * The resource manager can be post-initialized with a given URDF.
    * This is mainly used in conjunction with the default constructor
    * in which the URDF might not be present at first initialization.
    *
    * \param[in] urdf string containing the URDF.
-   * \param[in] validate_interfaces boolean argument indicating whether the exported
-   * interfaces ought to be validated. Defaults to true.
-   * \param[in] load_and_initialize_components boolean argument indicating whether to load and
-   * initialize the components present in the parsed URDF. Defaults to true.
+   * \param[in] update_rate update rate of  the main control loop, i.e., of the controller manager.
+   * \returns false if URDF validation has failed.
    */
-  void load_urdf(
-    const std::string & urdf, bool validate_interfaces = true,
-    bool load_and_initialize_components = true);
+  virtual bool load_and_initialize_components(
+    const std::string & urdf, const unsigned int update_rate = 100);
 
   /**
-   * @brief if the resource manager load_urdf(...) function has been called this returns true.
-   * We want to permit to load the urdf later on but we currently don't want to permit multiple
-   * calls to load_urdf (reloading/loading different urdf).
+   * @brief if the resource manager load_and_initialize_components(...) function has been called
+   * this returns true. We want to permit to loading the urdf later on, but we currently don't want
+   * to permit multiple calls to load_and_initialize_components (reloading/loading different urdf).
    *
-   * @return true if resource manager's load_urdf() has been already called.
-   * @return false if resource manager's load_urdf() has not been yet called.
+   * @return true if the resource manager has successfully loaded and initialized the components
+   * @return false if the resource manager doesn't have any components loaded and initialized.
    */
-  bool is_urdf_already_loaded() const;
+  bool are_components_initialized() const;
 
   /// Claim a state interface given its key.
   /**
@@ -413,23 +407,24 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    */
   bool state_interface_exists(const std::string & key) const;
 
+protected:
+  bool components_are_loaded_and_initialized_ = false;
+
+  mutable std::recursive_mutex resource_interfaces_lock_;
+  mutable std::recursive_mutex claimed_command_interfaces_lock_;
+  mutable std::recursive_mutex resources_lock_;
+
 private:
-  void validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
+  bool validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
 
   void release_command_interface(const std::string & key);
 
   std::unordered_map<std::string, bool> claimed_command_interface_map_;
 
-  mutable std::recursive_mutex resource_interfaces_lock_;
-  mutable std::recursive_mutex claimed_command_interfaces_lock_;
-  mutable std::recursive_mutex resources_lock_;
-
   std::unique_ptr<ResourceStorage> resource_storage_;
 
   // Structure to store read and write status so it is not initialized in the real-time loop
   HardwareReadWriteStatus read_write_status;
-
-  bool is_urdf_loaded__ = false;
 };
 
 }  // namespace hardware_interface