Fix logic for simpler reviewing

Author: VitezGabriela

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";
@@ -546,7 +545,20 @@ void ControllerManager::init_controller_manager()
     init_resource_manager(robot_description_);
   }
 
-  if (!is_resource_manager_initialized())
+  if (
+    is_resource_manager_initialized() && !(resource_manager_->get_joint_limiters_imported()) &&
+    params_->enforce_command_limits)
+  {
+    try
+    {
+      resource_manager_->import_joint_limiters(robot_description_);
+    }
+    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
@@ -598,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_ =

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -558,6 +558,8 @@ class ResourceManager
 
   const std::string & get_robot_description() const;
 
+  bool get_joint_limiters_imported() const;
+
 protected:
   /// Gets the logger for the resource manager
   /**
@@ -598,6 +600,7 @@ class ResourceManager
 
   // Structure to store read and write status so it is not initialized in the real-time loop
   HardwareReadWriteStatus read_write_status;
+  mutable bool joint_limiters_imported_ = false;
 };
 
 }  // namespace hardware_interface

hardware_interface/src/resource_manager.cpp

@@ -733,7 +733,7 @@ class ResourceStorage
     }
   }
 
-  void import_joint_limiters(const std::vector<HardwareInfo> & hardware_infos)
+  bool import_joint_limiters(const std::vector<HardwareInfo> & hardware_infos)
   {
     for (const auto & hw_info : hardware_infos)
     {
@@ -782,6 +782,7 @@ class ResourceStorage
         joint_limiters_interface_[hw_info.name].insert({joint_name, std::move(limits_interface)});
       }
     }
+    return true;
   }
 
   template <typename T>
@@ -1561,9 +1562,15 @@ void ResourceManager::import_joint_limiters(const std::string & urdf)
 {
   std::lock_guard<std::recursive_mutex> guard(joint_limiters_lock_);
   const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
-  resource_storage_->import_joint_limiters(hardware_info);
+  bool success = resource_storage_->import_joint_limiters(hardware_info);
+  if (success)
+  {
+    joint_limiters_imported_ = true;
+  }
 }
 
+bool ResourceManager::get_joint_limiters_imported() const { return joint_limiters_imported_; }
+
 bool ResourceManager::are_components_initialized() const
 {
   return components_are_loaded_and_initialized_;