diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 295f5df4d4..08cebdad09 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -289,6 +289,7 @@ void ControllerManager::init_controller_manager() // Get parameters needed for RT "update" loop to work if (is_resource_manager_initialized()) { + resource_manager_->import_joint_limiters(robot_description_); init_services(); } else @@ -370,6 +371,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & get_logger(), "Resource Manager has been successfully initialized. Starting Controller Manager " "services..."); + resource_manager_->import_joint_limiters(robot_description_); init_services(); } } diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index bcd53ad541..cd759a96e4 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -89,6 +89,12 @@ class ResourceManager virtual bool load_and_initialize_components( const std::string & urdf, const unsigned int update_rate = 100); + /** + * @brief Import joint limiters from the URDF. + * @param urdf string containing the URDF. + */ + void import_joint_limiters(const std::string & 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 diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index a076e90a62..e2f1110206 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1160,7 +1160,6 @@ bool ResourceManager::load_and_initialize_components( resource_storage_->cm_update_rate_ = update_rate; auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); - resource_storage_->import_joint_limiters(hardware_info); // Set the update rate for all hardware components for (auto & hw : hardware_info) { @@ -1235,6 +1234,12 @@ bool ResourceManager::load_and_initialize_components( return components_are_loaded_and_initialized_; } +void ResourceManager::import_joint_limiters(const std::string & urdf) +{ + const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + resource_storage_->import_joint_limiters(hardware_info); +} + bool ResourceManager::are_components_initialized() const { return components_are_loaded_and_initialized_; diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 37065ccf01..424d54e882 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -2165,6 +2165,7 @@ class ResourceManagerTestCommandLimitEnforcement : public ResourceManagerTest { rm = std::make_shared( node_, ros2_control_test_assets::minimal_robot_urdf, false); + rm->import_joint_limiters(ros2_control_test_assets::minimal_robot_urdf); activate_components(*rm); cm_update_rate_ = 100u; // The default value inside