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<controller_manager::ControllerSpec> & 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<rclcpp::Executor> 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<rclcpp::Time>(
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<std::string> 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 <optional>
#include <string>
#include <vector>
@@ -32,6 +33,9 @@ struct ControllerInfo
/// Controller type.
std::string type;
+ /// Controller param file
+ std::optional<std::string> parameters_file;
+
/// List of claimed interfaces by the controller.
std::vector<std::string> claimed_interfaces;
};