From 1f264f5b07053a8567e6a3cf605e28d63de7ee0a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 5 Jan 2024 13:46:20 +0100 Subject: [PATCH 1/3] Initialize the controller manager services after initializing resource manager --- controller_manager/src/controller_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e1cf41f7e4..d154a08b85 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -286,12 +286,12 @@ ControllerManager::ControllerManager( "[Deprecated] Passing the robot description parameter directly to the control_manager node " "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead."); init_resource_manager(robot_description_); + init_services(); } diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); - init_services(); } ControllerManager::ControllerManager( @@ -318,7 +318,6 @@ ControllerManager::ControllerManager( diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); - init_services(); } void ControllerManager::subscribe_to_robot_description_topic() @@ -352,6 +351,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & return; } init_resource_manager(robot_description_); + init_services(); } catch (std::runtime_error & e) { From 345b820fde533e1431489cc61dc858561a344939 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 5 Jan 2024 14:55:59 +0100 Subject: [PATCH 2/3] If the URDF is loaded in resource manager, then initialize the services --- controller_manager/src/controller_manager.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d154a08b85..5328aab69f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -313,6 +313,10 @@ ControllerManager::ControllerManager( RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } + if (resource_manager_->is_urdf_already_loaded()) + { + init_services(); + } subscribe_to_robot_description_topic(); diagnostics_updater_.setHardwareID("ros2_control"); From 7ae58d83eb39efbefbee7624be10e2eaa7a84539 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 5 Jan 2024 15:28:47 +0100 Subject: [PATCH 3/3] fix hardware management srvs test --- .../test/test_hardware_management_srvs.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 0fc7a2f27e..0b4b107ee1 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -84,7 +84,9 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "Unable to initialize resource manager, no robot description found."); } - cm_->init_resource_manager(robot_description); + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); } @@ -383,7 +385,9 @@ class TestControllerManagerHWManagementSrvsWithoutParams "Unable to initialize resource manager, no robot description found."); } - cm_->init_resource_manager(robot_description); + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); } @@ -440,7 +444,9 @@ class TestControllerManagerHWManagementSrvsOldParameters "Unable to initialize resource manager, no robot description found."); } - cm_->init_resource_manager(robot_description); + auto msg = std_msgs::msg::String(); + msg.data = robot_description_; + cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); }