Skip to content

Commit c36f9d2

Browse files
mergify[bot]mergify[bot]
authored
Deprecate prefix_is_for_params of PidROS (backport #431) (#434)
1 parent 44eaf81 commit c36f9d2

File tree

6 files changed

+367
-75
lines changed

6 files changed

+367
-75
lines changed

‎control_toolbox/include/control_toolbox/pid_ros.hpp‎

Lines changed: 73 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ class PidROS
6868
*
6969
*/
7070
template <class NodeT>
71-
explicit PidROS(
71+
[[deprecated("Use overloads with explicit prefixes for params and topics")]] explicit PidROS(
7272
std::shared_ptr<NodeT> node_ptr, std::string prefix = std::string(""),
7373
bool prefix_is_for_params = false)
7474
: PidROS(
@@ -77,14 +77,84 @@ class PidROS
7777
prefix_is_for_params)
7878
{
7979
}
80+
template <class NodeT>
81+
explicit PidROS(std::shared_ptr<NodeT> node_ptr, const std::string & param_prefix)
82+
: PidROS(
83+
node_ptr->get_node_base_interface(), node_ptr->get_node_logging_interface(),
84+
node_ptr->get_node_parameters_interface(), node_ptr->get_node_topics_interface(),
85+
param_prefix, "", false)
86+
{
87+
}
88+
/*!
89+
* \brief Constructor of PidROS class.
90+
*
91+
* The node is passed to this class to handler the ROS parameters, this class allows
92+
* to add a prefix to the pid parameters
93+
*
94+
* \param node Any ROS node
95+
* \param param_prefix prefix to add to the pid parameters.
96+
* \param topic_prefix prefix to add to the state publisher. If it starts with `~/`, topic will be local under the namespace of the node. If it starts with `/` or an alphanumeric character, topic will be in global namespace.
97+
*
98+
*/
99+
template <class NodeT>
100+
explicit PidROS(
101+
std::shared_ptr<NodeT> node_ptr, const std::string & param_prefix,
102+
const std::string & topic_prefix)
103+
: PidROS(
104+
node_ptr->get_node_base_interface(), node_ptr->get_node_logging_interface(),
105+
node_ptr->get_node_parameters_interface(), node_ptr->get_node_topics_interface(),
106+
param_prefix, topic_prefix, true)
107+
{
108+
}
109+
/*!
110+
* \brief Constructor of PidROS class.
111+
*
112+
* The node is passed to this class to handler the ROS parameters, this class allows
113+
* to add a prefix to the pid parameters
114+
*
115+
* \param node Any ROS node
116+
* \param param_prefix prefix to add to the pid parameters.
117+
* \param topic_prefix prefix to add to the state publisher. If it starts with `~/`, topic will be local under the namespace of the node. If it starts with `/` or an alphanumeric character, topic will be in global namespace.
118+
* \param activate_state_publisher If true, the publisher will be enabled after initialization.
119+
*
120+
*/
121+
template <class NodeT>
122+
explicit PidROS(
123+
std::shared_ptr<NodeT> node_ptr, std::string param_prefix, std::string topic_prefix,
124+
bool activate_state_publisher)
125+
: PidROS(
126+
node_ptr->get_node_base_interface(), node_ptr->get_node_logging_interface(),
127+
node_ptr->get_node_parameters_interface(), node_ptr->get_node_topics_interface(),
128+
param_prefix, topic_prefix, activate_state_publisher)
129+
{
130+
}
80131

81-
PidROS(
132+
[[deprecated("Use overloads with explicit prefixes for params and topics")]] PidROS(
82133
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
83134
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
84135
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params,
85136
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
86137
std::string prefix = std::string(""), bool prefix_is_for_params = false);
87138

139+
/*!
140+
* \brief Constructor of PidROS class with node_interfaces
141+
*
142+
* \param node_base Node base interface pointer.
143+
* \param node_logging Node logging interface pointer.
144+
* \param node_params Node parameters interface pointer.
145+
* \param topics_interface Node topics interface pointer.
146+
* \param param_prefix Prefix to add to the PID parameters. This string is not manipulated, i.e., probably should end with `.`.
147+
* \param topic_prefix Prefix to add to the state publisher. This string is not manipulated, i.e., probably should end with `/`. If it starts with `~/`, topic will be local under the namespace of the node. If it starts with `/` or an alphanumeric character, topic will be in global namespace.
148+
* \param activate_state_publisher If true, the publisher will be enabled after initialization.
149+
*/
150+
PidROS(
151+
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
152+
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
153+
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params,
154+
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
155+
const std::string & param_prefix, const std::string & topic_prefix,
156+
bool activate_state_publisher);
157+
88158
/*!
89159
* \brief Initialize the PID controller and set the parameters
90160
* \param p The proportional gain.
@@ -473,7 +543,7 @@ class PidROS
473543
* If not stated explicitly using "/" or "~", prefix is interpreted as global, i.e.,
474544
* "/" will be added in front of topic prefix
475545
*/
476-
void set_prefixes(const std::string & topic_prefix);
546+
[[deprecated]] void set_prefixes(const std::string & topic_prefix);
477547

478548
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_;
479549

‎control_toolbox/src/pid_ros.cpp‎

Lines changed: 61 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,12 @@ PidROS::PidROS(
5454
node_params_(node_params),
5555
topics_interface_(topics_interface)
5656
{
57+
// note: deprecation on templated constructor does not show up
58+
RCLCPP_WARN(
59+
node_logging->get_logger(),
60+
"PidROS constructor with node and prefix is deprecated, use overloads with explicit "
61+
"prefixes for params and topics");
62+
5763
if (prefix_is_for_params)
5864
{
5965
param_prefix_ = prefix;
@@ -97,6 +103,39 @@ PidROS::PidROS(
97103
rt_state_pub_.reset(
98104
new realtime_tools::RealtimePublisher<control_msgs::msg::PidState>(state_pub_));
99105
}
106+
107+
PidROS::PidROS(
108+
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
109+
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
110+
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params,
111+
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface,
112+
const std::string & param_prefix, const std::string & topic_prefix, bool activate_state_publisher)
113+
: topic_prefix_(topic_prefix),
114+
param_prefix_(param_prefix),
115+
node_base_(node_base),
116+
node_logging_(node_logging),
117+
node_params_(node_params),
118+
topics_interface_(topics_interface)
119+
{
120+
// Add a trailing "."
121+
if (!param_prefix_.empty() && param_prefix_.back() != '.')
122+
{
123+
param_prefix_.append(".");
124+
}
125+
// Add a trailing "/"
126+
if (!topic_prefix_.empty() && topic_prefix_.back() != '/')
127+
{
128+
topic_prefix_.append("/");
129+
}
130+
131+
if (activate_state_publisher)
132+
{
133+
state_pub_ = rclcpp::create_publisher<control_msgs::msg::PidState>(
134+
topics_interface_, topic_prefix_ + "pid_state", rclcpp::SensorDataQoS());
135+
rt_state_pub_.reset(
136+
new realtime_tools::RealtimePublisher<control_msgs::msg::PidState>(state_pub_));
137+
}
138+
}
100139
#pragma GCC diagnostic pop
101140

102141
void PidROS::set_prefixes(const std::string & topic_prefix)
@@ -329,7 +368,6 @@ bool PidROS::initialize_from_args(
329368
if (pid_.initialize(p, i, d, u_max, u_min, antiwindup_strat))
330369
{
331370
const Pid::Gains gains = pid_.get_gains();
332-
333371
declare_param(param_prefix_ + "p", rclcpp::ParameterValue(gains.p_gain_));
334372
declare_param(param_prefix_ + "i", rclcpp::ParameterValue(gains.i_gain_));
335373
declare_param(param_prefix_ + "d", rclcpp::ParameterValue(gains.d_gain_));
@@ -352,6 +390,9 @@ bool PidROS::initialize_from_args(
352390
param_prefix_ + "antiwindup_strategy",
353391
rclcpp::ParameterValue(gains.antiwindup_strat_.to_string()));
354392
declare_param(param_prefix_ + "save_i_term", rclcpp::ParameterValue(save_i_term));
393+
declare_param(
394+
param_prefix_ + "activate_state_publisher",
395+
rclcpp::ParameterValue(rt_state_pub_ != nullptr));
355396

356397
set_parameter_event_callback();
357398
return true;
@@ -636,6 +677,25 @@ void PidROS::set_parameter_event_callback()
636677
gains.antiwindup_strat_.error_deadband = parameter.get_value<double>();
637678
changed = true;
638679
}
680+
else if (param_name == param_prefix_ + "activate_state_publisher")
681+
{
682+
if (parameter.get_value<bool>())
683+
{
684+
std::string topic_name = topic_prefix_ + "pid_state";
685+
RCLCPP_INFO(
686+
node_logging_->get_logger(), "Activate publisher: `%s` ...", topic_name.c_str());
687+
state_pub_ = rclcpp::create_publisher<control_msgs::msg::PidState>(
688+
topics_interface_, topic_name, rclcpp::SensorDataQoS());
689+
rt_state_pub_.reset(
690+
new realtime_tools::RealtimePublisher<control_msgs::msg::PidState>(state_pub_));
691+
}
692+
else
693+
{
694+
RCLCPP_INFO(node_logging_->get_logger(), "Deactivate publisher...");
695+
state_pub_.reset();
696+
rt_state_pub_.reset();
697+
}
698+
}
639699
}
640700
catch (const rclcpp::exceptions::InvalidParameterTypeException & e)
641701
{

0 commit comments

Comments
 (0)