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: diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index cf1e959e1f..532e8909c5 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 5973d651ac..768df4a2ff 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -577,6 +577,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 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); } @@ -1303,8 +1320,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(); @@ -1313,17 +1331,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); @@ -2609,4 +2616,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 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; };