From b8eb0b23a7761356dcf46129c68925b1f5196c78 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 13 Jan 2024 23:19:09 +0100 Subject: [PATCH 1/9] Add optional params_file variable to the ControllerInfo --- .../include/hardware_interface/controller_info.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 149e0a2f90..df6d099ae7 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_ #define HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_ +#include #include #include @@ -32,6 +33,9 @@ struct ControllerInfo /// Controller type. std::string type; + /// Controller param file + std::optional params_file; + /// List of claimed interfaces by the controller. std::vector claimed_interfaces; }; From ae67a26bf39655a158d5e86948ebce626e6ec868 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 13 Jan 2024 23:19:58 +0100 Subject: [PATCH 2/9] declare and retrieve the params_file params into the controller_spec --- controller_manager/src/controller_manager.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e7a2293672..5d0f5be3d9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -574,6 +574,23 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.next_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); + // We have to fetch the params_file at the time of loading the controller, because this way we can + // load them at the creating of the LifeCycleNode and this helps in using the features such as + // read_only params, dynamic maps lists etc + // Now check if the params_file parameter exist + const std::string param_name = controller_name + ".params_file"; + std::string params_file; + + // Check if parameter has been declared + if (!has_parameter(param_name)) + { + declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING); + } + if (get_parameter(param_name, params_file) && !params_file.empty()) + { + controller_spec.info.params_file = params_file; + } + return add_controller_impl(controller_spec); } From 512424df8a62f44e41a180a5224c9e96645c1a94 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 13 Jan 2024 23:21:09 +0100 Subject: [PATCH 3/9] Use the controller NodeOptions to be able to load the params on Node creation --- controller_manager/src/controller_manager.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5d0f5be3d9..fdd2887805 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1317,10 +1317,16 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co return nullptr; } + rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true); + if (controller.info.params_file.has_value()) + { + controller_node_options = controller_node_options.arguments( + {"--ros-args", "--params-file", controller.info.params_file.value()}); + } if ( controller.c->init( - controller.info.name, robot_description_, get_update_rate(), get_namespace()) == - controller_interface::return_type::ERROR) + controller.info.name, robot_description_, get_update_rate(), get_namespace(), + controller_node_options) == controller_interface::return_type::ERROR) { to.clear(); RCLCPP_ERROR( From 8d0b47817ad395a3014b256109b2679b224b001d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 13 Jan 2024 23:28:11 +0100 Subject: [PATCH 4/9] set the params_file parameter if file is parsed to spawner and remove old logic --- .../controller_manager/spawner.py | 68 +++++++++---------- 1 file changed, 32 insertions(+), 36 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 3ca5600997..cf78442856 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,8 +19,6 @@ import sys import time import warnings -import io -from contextlib import redirect_stdout, redirect_stderr from controller_manager import ( configure_controller, @@ -37,7 +35,6 @@ from rclpy.parameter import get_parameter_value from rclpy.signals import SignalHandlerOptions from ros2param.api import call_set_parameters -from ros2param.api import load_parameter_file # from https://stackoverflow.com/a/287944 @@ -266,6 +263,38 @@ def main(args=None): ) return 1 + if param_file: + parameter = Parameter() + parameter.name = prefixed_controller_name + ".params_file" + parameter.value = get_parameter_value(string_value=param_file) + + response = call_set_parameters( + node=node, node_name=controller_manager_name, parameters=[parameter] + ) + assert len(response.results) == 1 + result = response.results[0] + if result.successful: + node.get_logger().info( + bcolors.OKCYAN + + 'Set controller params file to "' + + param_file + + '" for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + else: + node.get_logger().fatal( + bcolors.FAIL + + 'Could not set controller params file to "' + + param_file + + '" for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + return 1 + ret = load_controller(node, controller_manager_name, controller_name) if not ret.ok: node.get_logger().fatal( @@ -284,39 +313,6 @@ def main(args=None): + bcolors.ENDC ) - if param_file: - # load_parameter_file writes to stdout/stderr. Here we capture that and use node logging instead - with redirect_stdout(io.StringIO()) as f_stdout, redirect_stderr( - io.StringIO() - ) as f_stderr: - load_parameter_file( - node=node, - node_name=prefixed_controller_name, - parameter_file=param_file, - use_wildcard=True, - ) - if f_stdout.getvalue(): - node.get_logger().info(bcolors.OKCYAN + f_stdout.getvalue() + bcolors.ENDC) - if f_stderr.getvalue(): - node.get_logger().error(bcolors.FAIL + f_stderr.getvalue() + bcolors.ENDC) - node.get_logger().info( - bcolors.OKCYAN - + 'Loaded parameters file "' - + param_file - + '" for ' - + bcolors.BOLD - + prefixed_controller_name - + bcolors.ENDC - ) - # TODO(destogl): use return value when upstream return value is merged - # ret = - # if ret.returncode != 0: - # Error message printed by ros2 param - # return ret.returncode - node.get_logger().info( - "Loaded " + param_file + " into " + prefixed_controller_name - ) - if not args.load_only: ret = configure_controller(node, controller_manager_name, controller_name) if not ret.ok: From dc8c099ebf92d29caae6bd05c7e850c9da90ecc3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Jan 2024 13:51:24 +0100 Subject: [PATCH 5/9] fix code comments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- controller_manager/src/controller_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fdd2887805..0e010b7eb1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -575,7 +575,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); // We have to fetch the params_file at the time of loading the controller, because this way we can - // load them at the creating of the LifeCycleNode and this helps in using the features such as + // load them at the creation of the LifeCycleNode and this helps in using the features such as // read_only params, dynamic maps lists etc // Now check if the params_file parameter exist const std::string param_name = controller_name + ".params_file"; From a4966825fff952fd3ff8227a6781211cc0620433 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Jan 2024 20:36:08 +0100 Subject: [PATCH 6/9] rename params_file to parameters_file as per PR suggestions --- controller_manager/src/controller_manager.cpp | 16 ++++++++-------- .../hardware_interface/controller_info.hpp | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 0e010b7eb1..eea37ccb91 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -574,21 +574,21 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.next_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); - // We have to fetch the params_file at the time of loading the controller, because this way we can - // load them at the creation of the LifeCycleNode and this helps in using the features such as + // We have to fetch the parameters_file at the time of loading the controller, because this way we + // can load them at the creation of the LifeCycleNode and this helps in using the features such as // read_only params, dynamic maps lists etc - // Now check if the params_file parameter exist + // Now check if the parameters_file parameter exist const std::string param_name = controller_name + ".params_file"; - std::string params_file; + std::string parameters_file; // Check if parameter has been declared if (!has_parameter(param_name)) { declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING); } - if (get_parameter(param_name, params_file) && !params_file.empty()) + if (get_parameter(param_name, parameters_file) && !parameters_file.empty()) { - controller_spec.info.params_file = params_file; + controller_spec.info.parameters_file = parameters_file; } return add_controller_impl(controller_spec); @@ -1318,10 +1318,10 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co } rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true); - if (controller.info.params_file.has_value()) + if (controller.info.parameters_file.has_value()) { controller_node_options = controller_node_options.arguments( - {"--ros-args", "--params-file", controller.info.params_file.value()}); + {"--ros-args", "--params-file", controller.info.parameters_file.value()}); } if ( controller.c->init( diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index df6d099ae7..61a51a134a 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -34,7 +34,7 @@ struct ControllerInfo std::string type; /// Controller param file - std::optional params_file; + std::optional parameters_file; /// List of claimed interfaces by the controller. std::vector claimed_interfaces; From b47b5930b460ef0019ccfa1fcb667e6355a3ecf8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Jan 2024 20:48:46 +0100 Subject: [PATCH 7/9] implement logic to append --ros-args if not defined in the node options arguments --- controller_manager/src/controller_manager.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index eea37ccb91..5a1ac1febb 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1317,12 +1317,22 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co return nullptr; } + auto check_for_element = [](const auto & list, const auto & element) + { return std::find(list.begin(), list.end(), element) != list.end(); }; + rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true); + std::vector node_options_arguments = controller_node_options.arguments(); + const std::string ros_args_arg = "--ros-args"; if (controller.info.parameters_file.has_value()) { - controller_node_options = controller_node_options.arguments( - {"--ros-args", "--params-file", controller.info.parameters_file.value()}); + if (!check_for_element(node_options_arguments, ros_args_arg)) + { + node_options_arguments.push_back(ros_args_arg); + } + node_options_arguments.push_back("--params-file"); + node_options_arguments.push_back(controller.info.parameters_file.value()); } + controller_node_options = controller_node_options.arguments(node_options_arguments); if ( controller.c->init( controller.info.name, robot_description_, get_update_rate(), get_namespace(), From 201fd47c67e3174c76314aa15955586d8273e925 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 21 Jan 2024 20:51:24 +0100 Subject: [PATCH 8/9] implement logic of use_sim_time as arguments to solve #1311 --- controller_manager/src/controller_manager.cpp | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5a1ac1febb..d9550b070e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1332,6 +1332,19 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co node_options_arguments.push_back("--params-file"); node_options_arguments.push_back(controller.info.parameters_file.value()); } + + // ensure controller's `use_sim_time` parameter matches controller_manager's + const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); + if (use_sim_time.as_bool()) + { + if (!check_for_element(node_options_arguments, ros_args_arg)) + { + node_options_arguments.push_back(ros_args_arg); + } + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("use_sim_time:=true"); + } + controller_node_options = controller_node_options.arguments(node_options_arguments); if ( controller.c->init( @@ -1344,17 +1357,6 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co return nullptr; } - // ensure controller's `use_sim_time` parameter matches controller_manager's - const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); - if (use_sim_time.as_bool()) - { - RCLCPP_INFO( - get_logger(), - "Setting use_sim_time=True for %s to match controller manager " - "(see ros2_control#325 for details)", - controller.info.name.c_str()); - controller.c->get_node()->set_parameter(use_sim_time); - } executor_->add_node(controller.c->get_node()->get_node_base_interface()); to.emplace_back(controller); From 604c0eb738bbf96c40aa7b8e4677a518270831d5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 23 Jan 2024 00:27:04 +0100 Subject: [PATCH 9/9] move the NodeOptions logic into a separate method --- .../controller_manager/controller_manager.hpp | 10 +++ controller_manager/src/controller_manager.cpp | 65 ++++++++++--------- 2 files changed, 46 insertions(+), 29 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 44b3c3215a..f5ee6854a1 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -414,6 +414,16 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers); void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + + /** + * @brief determine_controller_node_options - A method that retrieves the controller defined node + * options and adapts them, based on if there is a params file to be loaded or the use_sim_time + * needs to be set + * @param controller - controller info + * @return The node options that will be set to the controller LifeCycleNode + */ + rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const; + diagnostic_updater::Updater diagnostics_updater_; std::shared_ptr executor_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d9550b070e..1bc033fe3f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1317,35 +1317,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co return nullptr; } - auto check_for_element = [](const auto & list, const auto & element) - { return std::find(list.begin(), list.end(), element) != list.end(); }; - - rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true); - std::vector node_options_arguments = controller_node_options.arguments(); - const std::string ros_args_arg = "--ros-args"; - if (controller.info.parameters_file.has_value()) - { - if (!check_for_element(node_options_arguments, ros_args_arg)) - { - node_options_arguments.push_back(ros_args_arg); - } - node_options_arguments.push_back("--params-file"); - node_options_arguments.push_back(controller.info.parameters_file.value()); - } - - // ensure controller's `use_sim_time` parameter matches controller_manager's - const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); - if (use_sim_time.as_bool()) - { - if (!check_for_element(node_options_arguments, ros_args_arg)) - { - node_options_arguments.push_back(ros_args_arg); - } - node_options_arguments.push_back("-p"); - node_options_arguments.push_back("use_sim_time:=true"); - } - - controller_node_options = controller_node_options.arguments(node_options_arguments); + const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller); if ( controller.c->init( controller.info.name, robot_description_, get_update_rate(), get_namespace(), @@ -2628,4 +2600,39 @@ void ControllerManager::controller_activity_diagnostic_callback( } } +rclcpp::NodeOptions ControllerManager::determine_controller_node_options( + const ControllerSpec & controller) const +{ + auto check_for_element = [](const auto & list, const auto & element) + { return std::find(list.begin(), list.end(), element) != list.end(); }; + + rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true); + std::vector node_options_arguments = controller_node_options.arguments(); + const std::string ros_args_arg = "--ros-args"; + if (controller.info.parameters_file.has_value()) + { + if (!check_for_element(node_options_arguments, ros_args_arg)) + { + node_options_arguments.push_back(ros_args_arg); + } + node_options_arguments.push_back("--params-file"); + node_options_arguments.push_back(controller.info.parameters_file.value()); + } + + // ensure controller's `use_sim_time` parameter matches controller_manager's + const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); + if (use_sim_time.as_bool()) + { + if (!check_for_element(node_options_arguments, ros_args_arg)) + { + node_options_arguments.push_back(ros_args_arg); + } + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("use_sim_time:=true"); + } + + controller_node_options = controller_node_options.arguments(node_options_arguments); + return controller_node_options; +} + } // namespace controller_manager