From b32a5952b4e57f31d0e1a286a7395d848b9b3c67 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Tue, 23 Jan 2024 14:50:38 +0100
Subject: [PATCH] Load the URDF to the resource_manager before parsing it to CM
  (#262) (#266)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* Load the URDF to the resource_manager before parsing it to CM constructor (fixes https://github.com/ros-controls/ros2_control/issues/1299)

(cherry picked from commit f5baf71c4c7cb3c0a0af52f988c107b356c95ed0)

Co-authored-by: Sai Kishor Kothakota <saisastra3@gmail.com>
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
---
 gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
index 1642792e..82b7ba77 100644
--- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
+++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
@@ -275,6 +275,13 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
   std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
     std::make_unique<hardware_interface::ResourceManager>();
 
+  try {
+    resource_manager_->load_urdf(urdf_string, false, false);
+  } catch (...) {
+    // This error should be normal as the resource manager is not supposed to load and initialize
+    // them
+    RCLCPP_ERROR(impl_->model_nh_->get_logger(), "Error initializing URDF to resource manager!");
+  }
   try {
     impl_->robot_hw_sim_loader_.reset(
       new pluginlib::ClassLoader<gazebo_ros2_control::GazeboSystemInterface>(