Controller Manager recovery from invalid URDF errors

controller_manager/CMakeLists.txt

@@ -243,6 +243,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

@@ -242,6 +242,16 @@ class ControllerManager : public rclcpp::Node
    */
   rclcpp::Clock::SharedPtr get_trigger_clock() const;
 
+  /**
+   * @brief Returns true if we have a valid robot description, currently based on whether the timer
+   * for waiting on description is still on.
+   */
+  bool has_valid_robot_description() const
+  {
+    return robot_description_notification_timer_ &&
+           !robot_description_notification_timer_->is_canceled();
+  }
+
 protected:
   void init_services();
 

controller_manager/src/controller_manager.cpp

@@ -23,6 +23,7 @@
 #include <vector>
 
 #include "controller_interface/controller_interface_base.hpp"
+#include "controller_manager/controller_manager_parameters.hpp"
 #include "controller_manager_msgs/msg/hardware_component_state.hpp"
 #include "hardware_interface/helpers.hpp"
 #include "hardware_interface/introspection.hpp"
@@ -32,8 +33,6 @@
 #include "rclcpp/version.h"
 #include "rclcpp_lifecycle/state.hpp"
 
-#include "controller_manager/controller_manager_parameters.hpp"
-
 namespace  // utility
 {
 static constexpr const char * kControllerInterfaceNamespace = "controller_interface";
@@ -541,34 +540,56 @@ 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() && !(resource_manager_->get_joint_limiters_imported()) &&
+    params_->enforce_command_limits)
   {
-    if (params_->enforce_command_limits)
+    try
     {
       resource_manager_->import_joint_limiters(robot_description_);
     }
-    init_services();
+    catch (const std::exception & e)
+    {
+      RCLCPP_ERROR(get_logger(), "Error importing joint limiters: %s", e.what());
+    }
+  }
+  else if (!is_resource_manager_initialized())
+  {
+    // The RM failed to initialize after receiving the robot description, or no description was
+    // received at all. This is 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());
+    if (!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");
+        });
+    }
   }
   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 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>(
@@ -589,15 +610,6 @@ void ControllerManager::init_controller_manager()
   diagnostics_updater_.add(
     "Controller Manager Activity", this,
     &ControllerManager::controller_manager_diagnostic_callback);
-
-  INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(
-    this, hardware_interface::DEFAULT_INTROSPECTION_TOPIC,
-    hardware_interface::DEFAULT_REGISTRY_KEY);
-  START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY);
-  INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(
-    this, hardware_interface::CM_STATISTICS_TOPIC, hardware_interface::CM_STATISTICS_KEY);
-  START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::CM_STATISTICS_KEY);
-
   // Add on_shutdown callback to stop the controller manager
   rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
   preshutdown_cb_handle_ =
@@ -675,8 +687,14 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
       "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())
+  {
+    resource_manager_ =
+      std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, get_logger());
+  }
+  else
   {
     RCLCPP_INFO(
       get_logger(),
@@ -688,25 +706,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.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(
-      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.");
+    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(), "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();

controller_manager/test/test_controller_manager_with_resource_manager.cpp

@@ -0,0 +1,139 @@
+// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#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_TRUE(cm.has_valid_robot_description());
+}
+
+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_TRUE(cm.has_valid_robot_description());
+}
+
+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_TRUE(cm.has_valid_robot_description());
+}
+
+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_TRUE(cm.has_valid_robot_description());
+}
+
+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_TRUE(cm.has_valid_robot_description());
+}
+
+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_TRUE(cm.has_valid_robot_description());
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  int result = RUN_ALL_TESTS();
+  return result;
+}