From 0dfbd3dfe160d8ea0e8403955a794f94766e4443 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Thu, 4 Jan 2024 16:55:25 +0100 Subject: [PATCH 1/5] Use portable version for string-to-double conversion (#1257) --- .../hardware_interface/component_parser.hpp | 3 -- .../hardware_interface/lexical_casts.hpp | 52 +++++++++++++++++++ hardware_interface/src/component_parser.cpp | 31 +++++------ .../src/mock_components/generic_system.cpp | 18 ++----- 4 files changed, 68 insertions(+), 36 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/lexical_casts.hpp diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 5112f7927e..d5d999cca8 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -33,8 +33,5 @@ namespace hardware_interface HARDWARE_INTERFACE_PUBLIC std::vector parse_control_resources_from_urdf(const std::string & urdf); -HARDWARE_INTERFACE_PUBLIC -bool parse_bool(const std::string & bool_string); - } // namespace hardware_interface #endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_ diff --git a/hardware_interface/include/hardware_interface/lexical_casts.hpp b/hardware_interface/include/hardware_interface/lexical_casts.hpp new file mode 100644 index 0000000000..e0333756f4 --- /dev/null +++ b/hardware_interface/include/hardware_interface/lexical_casts.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 ros2_control Development Team +// +// 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. + +#ifndef HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ +#define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ + +#include +#include +#include +#include + +namespace hardware_interface +{ + +/** \brief Helper function to convert a std::string to double in a locale-independent way. + \throws std::invalid_argument if not a valid number + * from + https://github.com/ros-planning/srdfdom/blob/ad17b8d25812f752c397a6011cec64aeff090c46/src/model.cpp#L53 +*/ +double stod(const std::string & s) +{ + // convert from string using no locale + std::istringstream stream(s); + stream.imbue(std::locale::classic()); + double result; + stream >> result; + if (stream.fail() || !stream.eof()) + { + throw std::invalid_argument("Failed converting string to real number"); + } + return result; +} + +bool parse_bool(const std::string & bool_string) +{ + return bool_string == "true" || bool_string == "True"; +} + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_ diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 8e9b6bf16b..4f67d3e8b6 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -23,6 +23,7 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lexical_casts.hpp" namespace { @@ -128,26 +129,23 @@ double get_parameter_value_or( { while (params_it) { - // Fill the map with parameters - const auto tag_name = params_it->Name(); - if (strcmp(tag_name, parameter_name) == 0) + try { - const auto tag_text = params_it->GetText(); - if (tag_text) + // Fill the map with parameters + const auto tag_name = params_it->Name(); + if (strcmp(tag_name, parameter_name) == 0) { - // Parse and return double value if there is no parsing error - double result_value; - const auto parse_result = - std::from_chars(tag_text, tag_text + std::strlen(tag_text), result_value); - if (parse_result.ec == std::errc()) + const auto tag_text = params_it->GetText(); + if (tag_text) { - return result_value; + return hardware_interface::stod(tag_text); } - - // Parsing failed - exit loop and return default value - break; } } + catch (const std::exception & e) + { + return default_value; + } params_it = params_it->NextSiblingElement(); } @@ -616,9 +614,4 @@ std::vector parse_control_resources_from_urdf(const std::string & return hardware_info; } -bool parse_bool(const std::string & bool_string) -{ - return bool_string == "true" || bool_string == "True"; -} - } // namespace hardware_interface diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index f4aee6c8a6..22d8aa573c 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -26,22 +26,12 @@ #include #include "hardware_interface/component_parser.hpp" +#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rcutils/logging_macros.h" namespace mock_components { -double parse_double(const std::string & text) -{ - double result_value; - const auto parse_result = std::from_chars(text.data(), text.data() + text.size(), result_value); - if (parse_result.ec == std::errc()) - { - return result_value; - } - - return 0.0; -} CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info) { @@ -123,7 +113,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i it = info_.hardware_parameters.find("position_state_following_offset"); if (it != info_.hardware_parameters.end()) { - position_state_following_offset_ = parse_double(it->second); + position_state_following_offset_ = hardware_interface::stod(it->second); it = info_.hardware_parameters.find("custom_interface_with_following_offset"); if (it != info_.hardware_parameters.end()) { @@ -169,7 +159,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i auto param_it = joint.parameters.find("multiplier"); if (param_it != joint.parameters.end()) { - mimic_joint.multiplier = parse_double(joint.parameters.at("multiplier")); + mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier")); } mimic_joints_.push_back(mimic_joint); } @@ -696,7 +686,7 @@ void GenericSystem::initialize_storage_vectors( // Check the initial_value param is used if (!interface.initial_value.empty()) { - states[index][i] = parse_double(interface.initial_value); + states[index][i] = hardware_interface::stod(interface.initial_value); } } } From 64c9e2ab9636c98e0383e6dba4fb431f8e0948d5 Mon Sep 17 00:00:00 2001 From: Maximilian Schik Date: Thu, 4 Jan 2024 16:57:08 +0100 Subject: [PATCH 2/5] [ResourceManager] adds test for uninitialized hardware (#1243) --- .../test/test_components/test_actuator.cpp | 10 ++++ .../test/test_components/test_components.xml | 18 +++++++ .../test/test_components/test_sensor.cpp | 10 ++++ .../test/test_components/test_system.cpp | 10 ++++ .../test/test_resource_manager.cpp | 40 +++++++++++++++ .../ros2_control_test_assets/descriptions.hpp | 51 +++++++++++++++++++ 6 files changed, 139 insertions(+) diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 7cf55b50d3..2f606b74cb 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -120,5 +120,15 @@ class TestActuator : public ActuatorInterface double max_velocity_command_ = 0.0; }; +class TestUnitilizableActuator : public TestActuator +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + ActuatorInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuator, hardware_interface::ActuatorInterface) diff --git a/hardware_interface/test/test_components/test_components.xml b/hardware_interface/test/test_components/test_components.xml index 235fec668f..f029d3dd37 100644 --- a/hardware_interface/test/test_components/test_components.xml +++ b/hardware_interface/test/test_components/test_components.xml @@ -17,4 +17,22 @@ Test System + + + + Test Unitilizable Actuator + + + + + + Test Unitilizable Sensor + + + + + + Test Unitilizable System + + diff --git a/hardware_interface/test/test_components/test_sensor.cpp b/hardware_interface/test/test_components/test_sensor.cpp index ae31c73436..4811244138 100644 --- a/hardware_interface/test/test_components/test_sensor.cpp +++ b/hardware_interface/test/test_components/test_sensor.cpp @@ -55,5 +55,15 @@ class TestSensor : public SensorInterface double velocity_state_ = 0.0; }; +class TestUnitilizableSensor : public TestSensor +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + SensorInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableSensor, hardware_interface::SensorInterface) diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 3326cb893b..bc7a75df6f 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -123,5 +123,15 @@ class TestSystem : public SystemInterface double configuration_command_ = 0.0; }; +class TestUnitilizableSystem : public TestSystem +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + SystemInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableSystem, hardware_interface::SystemInterface) diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 8246cc207d..84519c20fa 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -69,6 +69,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); + FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation); TestableResourceManager() : hardware_interface::ResourceManager() {} @@ -154,6 +155,45 @@ TEST_F(ResourceManagerTest, post_initialization_with_urdf) ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); } +TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) +{ + // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a + // runtime exception is thrown + TestableResourceManager rm; + ASSERT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), + std::runtime_error); +} + +TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) +{ + // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, + // the interface should not show up + TestableResourceManager rm; + EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + + // test actuator + EXPECT_FALSE(rm.state_interface_exists("joint1/position")); + EXPECT_FALSE(rm.state_interface_exists("joint1/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint1/position")); + EXPECT_FALSE(rm.command_interface_exists("joint1/max_velocity")); + + // test sensor + EXPECT_FALSE(rm.state_interface_exists("sensor1/velocity")); + + // test system + EXPECT_FALSE(rm.state_interface_exists("joint2/position")); + EXPECT_FALSE(rm.state_interface_exists("joint2/velocity")); + EXPECT_FALSE(rm.state_interface_exists("joint2/acceleration")); + EXPECT_FALSE(rm.command_interface_exists("joint2/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint2/max_acceleration")); + EXPECT_FALSE(rm.state_interface_exists("joint3/position")); + EXPECT_FALSE(rm.state_interface_exists("joint3/velocity")); + EXPECT_FALSE(rm.state_interface_exists("joint3/acceleration")); + EXPECT_FALSE(rm.command_interface_exists("joint3/velocity")); + EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration")); +} + TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) { // we validate the results manually diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 05ffad00d6..a2f6195c67 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -176,6 +176,55 @@ const auto hardware_resources = )"; +const auto unitilizable_hardware_resources = + R"( + + + test_unitilizable_actuator + + + + + + + + + + + test_unitilizable_sensor + 2 + 2 + + + + + + + + test_unitilizable_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + const auto hardware_resources_missing_state_keys = R"( @@ -406,6 +455,8 @@ const auto diffbot_urdf = const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); +const auto minimal_unitilizable_robot_urdf = + std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail); const auto minimal_robot_missing_state_keys_urdf = std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) + From 2d82de84ba0ce746ecc6d3db58b0e12b2533bd2c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 8 Jan 2024 11:09:54 +0100 Subject: [PATCH 3/5] Fix rqt controller manager crash on ros2_control restart (#1273) * add service_timeout to the other service methods * catch RuntimeErrors when the service is no longer available --- .../controller_manager_services.py | 54 ++++++++++++++----- .../controller_manager.py | 26 +++++---- 2 files changed, 56 insertions(+), 24 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 5fe78c69b8..62b350f7d3 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -47,57 +47,75 @@ def service_caller(node, service_name, service_type, request, service_timeout=10 raise RuntimeError(f"Exception while calling service: {future.exception()}") -def configure_controller(node, controller_manager_name, controller_name): +def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = ConfigureController.Request() request.name = controller_name return service_caller( - node, f"{controller_manager_name}/configure_controller", ConfigureController, request + node, + f"{controller_manager_name}/configure_controller", + ConfigureController, + request, + service_timeout, ) -def list_controllers(node, controller_manager_name): +def list_controllers(node, controller_manager_name, service_timeout=10.0): request = ListControllers.Request() return service_caller( - node, f"{controller_manager_name}/list_controllers", ListControllers, request + node, + f"{controller_manager_name}/list_controllers", + ListControllers, + request, + service_timeout, ) -def list_controller_types(node, controller_manager_name): +def list_controller_types(node, controller_manager_name, service_timeout=10.0): request = ListControllerTypes.Request() return service_caller( - node, f"{controller_manager_name}/list_controller_types", ListControllerTypes, request + node, + f"{controller_manager_name}/list_controller_types", + ListControllerTypes, + request, + service_timeout, ) -def list_hardware_components(node, controller_manager_name): +def list_hardware_components(node, controller_manager_name, service_timeout=10.0): request = ListHardwareComponents.Request() return service_caller( node, f"{controller_manager_name}/list_hardware_components", ListHardwareComponents, request, + service_timeout, ) -def list_hardware_interfaces(node, controller_manager_name): +def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0): request = ListHardwareInterfaces.Request() return service_caller( node, f"{controller_manager_name}/list_hardware_interfaces", ListHardwareInterfaces, request, + service_timeout, ) -def load_controller(node, controller_manager_name, controller_name): +def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = LoadController.Request() request.name = controller_name return service_caller( - node, f"{controller_manager_name}/load_controller", LoadController, request + node, + f"{controller_manager_name}/load_controller", + LoadController, + request, + service_timeout, ) -def reload_controller_libraries(node, controller_manager_name, force_kill): +def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0): request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -105,10 +123,13 @@ def reload_controller_libraries(node, controller_manager_name, force_kill): f"{controller_manager_name}/reload_controller_libraries", ReloadControllerLibraries, request, + service_timeout, ) -def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state): +def set_hardware_component_state( + node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0 +): request = SetHardwareComponentState.Request() request.name = component_name request.target_state = lifecyle_state @@ -117,6 +138,7 @@ def set_hardware_component_state(node, controller_manager_name, component_name, f"{controller_manager_name}/set_hardware_component_state", SetHardwareComponentState, request, + service_timeout, ) @@ -143,9 +165,13 @@ def switch_controllers( ) -def unload_controller(node, controller_manager_name, controller_name): +def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = UnloadController.Request() request.name = controller_name return service_caller( - node, f"{controller_manager_name}/unload_controller", UnloadController, request + node, + f"{controller_manager_name}/unload_controller", + UnloadController, + request, + service_timeout, ) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 4d3c247e5b..7ca1f6d8c3 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -172,16 +172,22 @@ def _list_controllers(self): @rtype [str] """ # Add loaded controllers first - controllers = list_controllers(self._node, self._cm_name).controller - - # Append potential controller configs found in the node's parameters - for name in _get_parameter_controller_names(self._node, self._cm_name): - add_ctrl = all(name != ctrl.name for ctrl in controllers) - if add_ctrl: - type_str = _get_controller_type(self._node, self._cm_name, name) - uninit_ctrl = ControllerState(name=name, type=type_str) - controllers.append(uninit_ctrl) - return controllers + try: + controllers = list_controllers( + self._node, self._cm_name, 2.0 / self._cm_update_freq + ).controller + + # Append potential controller configs found in the node's parameters + for name in _get_parameter_controller_names(self._node, self._cm_name): + add_ctrl = all(name != ctrl.name for ctrl in controllers) + if add_ctrl: + type_str = _get_controller_type(self._node, self._cm_name, name) + uninit_ctrl = ControllerState(name=name, type=type_str) + controllers.append(uninit_ctrl) + return controllers + except RuntimeError as e: + print(e) + return [] def _show_controllers(self): table_view = self._widget.table_view From 672480dfd674c31436878dd52c5fac086dd99ee9 Mon Sep 17 00:00:00 2001 From: bailaC Date: Mon, 8 Jan 2024 16:44:49 +0530 Subject: [PATCH 4/5] Issue 695: Changing 'namespace_' variables to 'node_namespace' to make it more explicit (#1239) --- .../controller_interface/controller_interface_base.hpp | 2 +- controller_interface/src/controller_interface_base.cpp | 5 +++-- .../test/test_controller_with_options.hpp | 4 ++-- .../include/controller_manager/controller_manager.hpp | 4 ++-- controller_manager/src/controller_manager.cpp | 8 ++++---- .../test_controller_failed_init.cpp | 2 +- .../test_controller_failed_init.hpp | 2 +- .../test_controller_manager_hardware_error_handling.cpp | 4 ++-- .../test/test_controller_manager_urdf_passing.cpp | 4 ++-- .../test_controllers_chaining_with_controller_manager.cpp | 4 ++-- 10 files changed, 20 insertions(+), 19 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index bbc45c8583..2e953ee932 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -115,7 +115,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & namespace_ = "", + const std::string & node_namespace = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true)); /// Custom configure method to read additional parameters for controller-nodes diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 84cd0d5e2b..560feff5f9 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -26,10 +26,11 @@ namespace controller_interface { return_type ControllerInterfaceBase::init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & namespace_, const rclcpp::NodeOptions & node_options) + const std::string & node_namespace, const rclcpp::NodeOptions & node_options) { node_ = std::make_shared( - controller_name, namespace_, node_options, false); // disable LifecycleNode service interfaces + controller_name, node_namespace, node_options, + false); // disable LifecycleNode service interfaces urdf_ = urdf; try diff --git a/controller_interface/test/test_controller_with_options.hpp b/controller_interface/test/test_controller_with_options.hpp index 1221536784..1416a7d2df 100644 --- a/controller_interface/test/test_controller_with_options.hpp +++ b/controller_interface/test/test_controller_with_options.hpp @@ -41,13 +41,13 @@ class ControllerWithOptions : public controller_interface::ControllerInterface controller_interface::return_type init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & namespace_ = "", + const std::string & node_namespace = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true)) override { - ControllerInterface::init(controller_name, urdf, cm_update_rate, namespace_, node_options); + ControllerInterface::init(controller_name, urdf, cm_update_rate, node_namespace, node_options); switch (on_init()) { diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 79a9861463..44b3c3215a 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -71,14 +71,14 @@ class ControllerManager : public rclcpp::Node std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "", + const std::string & node_namespace = "", const rclcpp::NodeOptions & options = get_cm_node_options()); CONTROLLER_MANAGER_PUBLIC ControllerManager( std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "", + const std::string & node_namespace = "", const rclcpp::NodeOptions & options = get_cm_node_options()); CONTROLLER_MANAGER_PUBLIC diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e1cf41f7e4..270af53fb3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -255,8 +255,8 @@ rclcpp::NodeOptions get_cm_node_options() ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, - const std::string & namespace_, const rclcpp::NodeOptions & options) -: rclcpp::Node(manager_node_name, namespace_, options), + const std::string & node_namespace, const rclcpp::NodeOptions & options) +: rclcpp::Node(manager_node_name, node_namespace, options), resource_manager_(std::make_unique( update_rate_, this->get_node_clock_interface())), diagnostics_updater_(this), @@ -297,8 +297,8 @@ ControllerManager::ControllerManager( ControllerManager::ControllerManager( std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name, - const std::string & namespace_, const rclcpp::NodeOptions & options) -: rclcpp::Node(manager_node_name, namespace_, options), + const std::string & node_namespace, const rclcpp::NodeOptions & options) +: rclcpp::Node(manager_node_name, node_namespace, options), resource_manager_(std::move(resource_manager)), diagnostics_updater_(this), executor_(executor), diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp index 6a6f954e87..a106d5cbdf 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp @@ -33,7 +33,7 @@ TestControllerFailedInit::on_init() controller_interface::return_type TestControllerFailedInit::init( const std::string & /* controller_name */, const std::string & /* urdf */, - unsigned int /*cm_update_rate*/, const std::string & /*namespace_*/, + unsigned int /*cm_update_rate*/, const std::string & /*node_namespace*/, const rclcpp::NodeOptions & /*node_options*/) { return controller_interface::return_type::ERROR; diff --git a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp index 28cb48ab6c..315b0745b0 100644 --- a/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp +++ b/controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp @@ -41,7 +41,7 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac CONTROLLER_INTERFACE_PUBLIC controller_interface::return_type init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, - const std::string & namespace_ = "", + const std::string & node_namespace = "", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .allow_undeclared_parameters(true) diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index f12ab9e96b..4c800d41c2 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -54,9 +54,9 @@ class TestableControllerManager : public controller_manager::ControllerManager std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "") + const std::string & node_namespace = "") : controller_manager::ControllerManager( - std::move(resource_manager), executor, manager_node_name, namespace_) + std::move(resource_manager), executor, manager_node_name, node_namespace) { } }; diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 102cbdfbd4..5c2ebd14f6 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -40,9 +40,9 @@ class TestableControllerManager : public controller_manager::ControllerManager std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "") + const std::string & node_namespace = "") : controller_manager::ControllerManager( - std::move(resource_manager), executor, manager_node_name, namespace_) + std::move(resource_manager), executor, manager_node_name, node_namespace) { } }; diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 0ebddd85e0..eadca39756 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -94,9 +94,9 @@ class TestableControllerManager : public controller_manager::ControllerManager std::unique_ptr resource_manager, std::shared_ptr executor, const std::string & manager_node_name = "controller_manager", - const std::string & namespace_ = "") + const std::string & node_namespace = "") : controller_manager::ControllerManager( - std::move(resource_manager), executor, manager_node_name, namespace_) + std::move(resource_manager), executor, manager_node_name, node_namespace) { } }; From bdad0091946097542cb0397fbdf8130bfac42b3a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 8 Jan 2024 12:18:16 +0100 Subject: [PATCH 5/5] Initialize the controller manager services after initializing resource manager (#1271) --- controller_manager/src/controller_manager.cpp | 8 ++++++-- .../test/test_hardware_management_srvs.cpp | 12 +++++++++--- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 270af53fb3..e7a2293672 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( @@ -313,12 +313,15 @@ 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"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); - init_services(); } void ControllerManager::subscribe_to_robot_description_topic() @@ -352,6 +355,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & return; } init_resource_manager(robot_description_); + init_services(); } catch (std::runtime_error & e) { 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(); }