Controller Manager recovery from invalid URDF errors

controller_manager/CMakeLists.txt

@@ -63,6 +63,7 @@ if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
   find_package(example_interfaces REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
 
   # Plugin Libraries that are built and installed for use in testing
   add_library(test_controller SHARED test/test_controller/test_controller.cpp)
@@ -243,6 +244,15 @@ if(BUILD_TESTING)
     ${controller_manager_msgs_TARGETS}
   )
 
+  ament_add_gtest(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

@@ -242,6 +242,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/package.xml

@@ -36,6 +36,7 @@
   <exec_depend>python3-filelock</exec_depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>hardware_interface_testing</test_depend>
   <test_depend>launch_testing_ros</test_depend>

controller_manager/src/controller_manager.cpp

@@ -541,34 +541,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
+    resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, get_logger());
+    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>(
@@ -674,9 +678,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
+    resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, get_logger());
+  } else 
   {
     RCLCPP_INFO(
       get_logger(),
@@ -688,26 +698,51 @@ 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.node_namespace = this->get_namespace();
   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();
 
@@ -855,6 +890,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
   }
 }
 
+
 void ControllerManager::init_services()
 {
   // TODO(anyone): Due to issues with the MutliThreadedExecutor, this control loop does not rely on

controller_manager/test/test_controller_manager_with_resource_manager.cpp

@@ -0,0 +1,122 @@
+#include "test_controller_manager_with_resource_manager.hpp"
+
+std::shared_ptr<rclcpp::Node> ControllerManagerTest::node_ = nullptr;
+std::unique_ptr<hardware_interface::ResourceManager> ControllerManagerTest::test_resource_manager_ = nullptr;
+std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> ControllerManagerTest::executor_ = nullptr;
+
+using LifecycleCallbackReturn =
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+void ControllerManagerTest::SetUp()
+{
+  if (!rclcpp::ok()) {
+    rclcpp::init(0, nullptr);
+  }
+
+  node_ = std::make_shared<rclcpp::Node>("controller_manager_test_node");
+  auto clock = node_->get_clock();
+  auto logger = node_->get_logger();
+
+  test_resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(clock, logger);
+  executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+}
+
+void ControllerManagerTest::TearDown()
+{
+  node_.reset();
+  test_resource_manager_.reset();
+  executor_.reset();
+  if (rclcpp::ok()) {
+    rclcpp::shutdown();
+  }
+}
+
+TEST_F(ControllerManagerTest, robot_description_callback_handles_urdf_without_hardware_plugin)
+{
+  TestControllerManager cm(std::move(test_resource_manager_), executor_);
+
+  std_msgs::msg::String invalid_urdf_msg;
+  invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_without_hardware_plugin;
+
+  cm.robot_description_callback(invalid_urdf_msg);
+
+  EXPECT_FALSE(cm.is_resource_manager_initialized());
+
+  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+
+}
+
+TEST_F(ControllerManagerTest, robot_description_callback_handles_invalid_urdf)
+{
+  TestControllerManager cm(std::move(test_resource_manager_), executor_);
+
+  std_msgs::msg::String invalid_urdf_msg;
+  invalid_urdf_msg.data = R"(<robot malformed></robot>)";
+
+  cm.robot_description_callback(invalid_urdf_msg);
+
+  EXPECT_FALSE(cm.is_resource_manager_initialized());
+
+  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+
+}
+
+TEST_F(ControllerManagerTest, robot_description_callback_handles_empty_urdf)
+{
+  TestControllerManager cm(std::move(test_resource_manager_), executor_);
+
+  std_msgs::msg::String invalid_urdf_msg;
+  invalid_urdf_msg.data = "";
+
+  cm.robot_description_callback(invalid_urdf_msg);
+
+  EXPECT_FALSE(cm.is_resource_manager_initialized());
+
+  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+}
+
+TEST_F(ControllerManagerTest, robot_description_callback_handles_wrong_plugins)
+{
+  TestControllerManager cm(std::move(test_resource_manager_), executor_);
+
+  std_msgs::msg::String invalid_urdf_msg;
+  invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_with_wrong_plugin;
+
+  cm.robot_description_callback(invalid_urdf_msg);
+
+  EXPECT_FALSE(cm.is_resource_manager_initialized());
+
+  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+}
+
+TEST_F(ControllerManagerTest, robot_description_callback_handles_no_geometry)
+{
+  TestControllerManager cm(std::move(test_resource_manager_), executor_);
+
+  std_msgs::msg::String invalid_urdf_msg;
+  invalid_urdf_msg.data = ros2_control_test_assets::invalid_urdf_no_geometry;
+
+  cm.robot_description_callback(invalid_urdf_msg);
+
+  EXPECT_FALSE(cm.is_resource_manager_initialized());
+
+  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+}
+
+TEST_F(ControllerManagerTest, init_controller_manager_with_invalid_urdf)
+{
+  const std::string invalid_urdf = ros2_control_test_assets::invalid_urdf_with_wrong_plugin;
+
+  TestControllerManager cm(executor_, invalid_urdf, false, "test_controller_manager", "", rclcpp::NodeOptions{});
+
+  EXPECT_FALSE(cm.is_resource_manager_initialized());
+
+  EXPECT_FALSE(cm.get_robot_description_timer()->is_canceled());  
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  int result = RUN_ALL_TESTS();
+  return result;
+}

controller_manager/test/test_controller_manager_with_resource_manager.hpp

@@ -0,0 +1,41 @@
+#ifndef CONTROLLER_MANAGER_TEST_HPP_
+#define CONTROLLER_MANAGER_TEST_HPP_
+
+#include <memory>
+#include <string>
+
+#include "gtest/gtest.h"
+#include "rclcpp/node.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "controller_manager/controller_manager.hpp"
+#include "hardware_interface/resource_manager.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
+
+class TestControllerManager : public controller_manager::ControllerManager
+{
+public:
+  using ControllerManager::ControllerManager;
+
+  // Expose callbacks
+  using ControllerManager::robot_description_callback;
+
+  using ControllerManager::is_resource_manager_initialized;
+
+  using ControllerManager::resource_manager_;
+
+  using ControllerManager::get_robot_description_timer;  
+};
+
+class ControllerManagerTest : public ::testing::Test
+{
+protected:
+  static std::shared_ptr<rclcpp::Node> node_;
+  static std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor_;
+  static std::unique_ptr<hardware_interface::ResourceManager> test_resource_manager_;
+  virtual void SetUp();
+  virtual void TearDown();
+};
+
+#endif