PID controller handling of parameters in ROS 2

[SammyRamone]

I was switching my code, which uses the PID controller from this project, from ROS1 to ROS2 and ran into the following issue:
When the PID controller is created a reference on the node is taken as a parameter. I assumed that this should be a reference to the node that is using this PID controller. The description of the constructor (https://github.com/ros-controls/control_toolbox/blob/ros2-master/include/control_toolbox/pid_ros.hpp#L58-L61) seemed to suggest that I only need to put it in a correct namespace.
Therefore, I programmed something like this:

std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("node");
node->declare_parameter<double>("pid.p", 0.0);
node->declare_parameter<double>("pid.i", 0.0);
node->declare_parameter<double>("pid.d", 0.0);
node->declare_parameter<double>("pid.i_clamp_max", 0.0);
node->declare_parameter<double>("pid.i_clamp_min", 0.0);
node->declare_parameter<bool>("pid.antiwindup", false);
control_toolbox::PidROS pid = control_toolbox::PidROS(node, "pid");
pid.initPid();
node->declare_parameter<double>("not_related_to_pid", 0.0);
double not_related_to_pid;
node->get_parameter("not_related_to_pid", not_related_to_pid);

Executing this leads to following error:

[Node-4] terminate called after throwing an instance of 'rclcpp::exceptions::InvalidParameterValueException'
[Node-4]   what():  parameter 'not_related_to_pid' could not be set: Invalid parameter

I think, the reason is that the pid controller sets the add_on_set_parameters_callback here:

control_toolbox/src/pid_ros.cpp

Lines 281 to 326 in d7462e9

	PidROS::setParameterEventCallback()
	{
	auto on_parameter_event_callback = [this](const std::vector<rclcpp::Parameter> & parameters) {
	rcl_interfaces::msg::SetParametersResult result;
	result.successful = true;
	/// @note don't use getGains, it's rt
	Pid::Gains gains = pid_.getGains();
	for (auto & parameter : parameters) {
	const std::string param_name = parameter.get_name();
	try {
	if (param_name == "p") {
	gains.p_gain_ = parameter.get_value<double>();
	} else if (param_name == "i") {
	gains.i_gain_ = parameter.get_value<double>();
	} else if (param_name == "d") {
	gains.d_gain_ = parameter.get_value<double>();
	} else if (param_name == "i_clamp_max") {
	gains.i_max_ = parameter.get_value<double>();
	} else if (param_name == "i_clamp_min") {
	gains.i_min_ = parameter.get_value<double>();
	} else if (param_name == "antiwindup") {
	gains.antiwindup_ = parameter.get_value<bool>();
	} else {
	result.successful = false;
	result.reason = "Invalid parameter";
	}
	} catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
	RCLCPP_INFO_STREAM(
	node_logging_->get_logger(), "Please use the right type: " << e.what());
	}
	}
	if (result.successful) {
	/// @note don't call setGains() from inside a callback
	pid_.setGains(gains);
	}
	return result;
	};
	parameter_callback_ =
	node_params_->add_on_set_parameters_callback(
	on_parameter_event_callback);
	}

This is now for the whole node and therefore any other not PID related parameters will result in errors.

I managed to fix this by creating a seperate node for the pid controller.

std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("node");
std::shared_ptr<rclcpp::Node> node_pid = rclcpp::Node::make_shared("pid");
node_pid->declare_parameter<double>("p", 0.0);
node_pid->declare_parameter<double>("i", 0.0);
node_pid->declare_parameter<double>("d", 0.0);
node_pid->declare_parameter<double>("i_clamp_max", 0.0);
node_pid->declare_parameter<double>("i_clamp_min", 0.0);
node_pid->declare_parameter<bool>("antiwindup", false);
control_toolbox::PidROS pid = control_toolbox::PidROS(node_pid, "");
pid.initPid();
node->declare_parameter<double>("not_related_to_pid", 0.0);
double not_related_to_pid;
node->get_parameter("not_related_to_pid", not_related_to_pid);

My question is now: Is this intended behavior? Should every PID controller have its own node?
If yes, I think the description of the constructor should be improved or a clearer error message should be thrown to point out that it needs it own node.

Additionally, I needed to declare the parameters of the controller before calling pid.initPid() to load yaml parameters from a launch file correctly. I think this is because the PID controller only calls the decleration

control_toolbox/src/pid_ros.cpp

Lines 144 to 149 in d7462e9

	declareParam("p", rclcpp::ParameterValue(p));
	declareParam("i", rclcpp::ParameterValue(i));
	declareParam("d", rclcpp::ParameterValue(d));
	declareParam("i_clamp_max", rclcpp::ParameterValue(i_max));
	declareParam("i_clamp_min", rclcpp::ParameterValue(i_min));
	declareParam("antiwindup", rclcpp::ParameterValue(antiwindup));

after trying to get the parameters

control_toolbox/src/pid_ros.cpp

Lines 114 to 118 in d7462e9

	all_params_available &= getDoubleParam(topic_prefix_ + "p", p);
	all_params_available &= getDoubleParam(topic_prefix_ + "i", i);
	all_params_available &= getDoubleParam(topic_prefix_ + "d", d);
	all_params_available &= getDoubleParam(topic_prefix_ + "i_clamp_max", i_max);
	all_params_available &= getDoubleParam(topic_prefix_ + "i_clamp_min", i_min);

[progtologist]

This should be fixed with #125