From c397e2412a644b1d63d662f7d62439013898b814 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 23 Jan 2024 15:28:35 +0100 Subject: [PATCH] Fix controller parameter loading issue in different cases (#1293) --- .../controller_manager/spawner.py | 68 +++++++++--------- .../controller_manager/controller_manager.hpp | 9 +++ controller_manager/src/controller_manager.cpp | 69 +++++++++++++++---- .../hardware_interface/controller_info.hpp | 4 ++ 4 files changed, 102 insertions(+), 48 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 27823c8cfd..536162612e 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, list_controllers, \ load_controller, switch_controllers, unload_controller @@ -32,7 +30,6 @@ from rclpy.signals import SignalHandlerOptions from ros2param.api import call_set_parameters from ros2param.api import get_parameter_value -from ros2param.api import load_parameter_file # from https://stackoverflow.com/a/287944 @@ -233,6 +230,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( @@ -251,39 +280,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: diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index a1ae11b4cb..b74eb1557b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -378,6 +378,15 @@ class ControllerManager : public rclcpp::Node const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b, const std::vector & controllers); + /** + * @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; + std::shared_ptr executor_; std::shared_ptr> loader_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d4f0b8a15b..4a73414a66 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -569,6 +569,23 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; + // 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 parameters_file parameter exist + const std::string param_name = controller_name + ".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, parameters_file) && !parameters_file.empty()) + { + controller_spec.info.parameters_file = parameters_file; + } + return add_controller_impl(controller_spec); } @@ -1243,8 +1260,9 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co return nullptr; } + const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller); if ( - controller.c->init(controller.info.name, get_namespace()) == + controller.c->init(controller.info.name, get_namespace(), controller_node_options) == controller_interface::return_type::ERROR) { to.clear(); @@ -1253,17 +1271,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); @@ -2484,4 +2491,42 @@ bool ControllerManager::controller_sorting( } }; +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() + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(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 diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 149e0a2f90..61a51a134a 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 parameters_file; + /// List of claimed interfaces by the controller. std::vector claimed_interfaces; };