Skip to content

Commit

Permalink
[CM] Fix controller parameter loading issue in different cases (#1293) (
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor authored Jan 25, 2024
1 parent e341696 commit 75ff347
Show file tree
Hide file tree
Showing 4 changed files with 100 additions and 48 deletions.
68 changes: 32 additions & 36 deletions controller_manager/controller_manager/spawner.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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

Expand Down Expand Up @@ -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(
Expand All @@ -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:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
66 changes: 54 additions & 12 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down Expand Up @@ -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();
Expand All @@ -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);

Expand Down Expand Up @@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#ifndef HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_
#define HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_

#include <optional>
#include <string>
#include <vector>

Expand All @@ -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;
};
Expand Down

0 comments on commit 75ff347

Please sign in to comment.