Skip to content

Commit 93dce70

Browse files
committed
Revert "removed logging for when status_publish_rate == 0.0"
This reverts commit 99a2cc4.
1 parent 318f138 commit 93dce70

File tree

2 files changed

+12
-16
lines changed

2 files changed

+12
-16
lines changed

hardware_interface/doc/writing_new_hardware_component.rst

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -212,14 +212,14 @@ The following is a step-by-step guide to create source files, basic tests, and c
212212
return hardware_interface::return_type::OK;
213213
}
214214
215-
* **Enable in URDF**: Finally, to activate the publisher, add the ``status_publish_rate`` parameter to your ``<hardware>`` tag in the URDF. Setting it to 0 disables the feature.
215+
* **Enable in URDF**: Finally, to activate the publisher, add the ``status_publish_rate`` parameter to your ``<hardware>`` tag in the URDF. Setting it to 0.0 disables the feature.
216216

217217
.. code-block:: xml
218218
219219
<ros2_control name="MyHardware" type="system">
220220
<hardware>
221221
<plugin>my_package/MyHardware</plugin>
222-
<param name="status_publish_rate">20</param>
222+
<param name="status_publish_rate">20.0</param>
223223
</hardware>
224224
...
225225
</ros2_control>

hardware_interface/include/hardware_interface/hardware_component_interface.hpp

Lines changed: 10 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -180,33 +180,29 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
180180
params.hardware_info.name.c_str());
181181
}
182182

183-
int publish_rate = 0;
183+
double publish_rate = 0.0;
184184
auto it = info_.hardware_parameters.find("status_publish_rate");
185185
if (it != info_.hardware_parameters.end())
186186
{
187187
try
188188
{
189-
publish_rate = std::stoi(it->second);
190-
191-
if (publish_rate < 0)
192-
{
193-
RCLCPP_ERROR(
194-
get_logger(),
195-
"'status_publish_rate' cannot be negative (got %d). Please fix your hardware "
196-
"parameters.",
197-
publish_rate);
198-
return CallbackReturn::ERROR;
199-
}
189+
publish_rate = hardware_interface::stod(it->second);
200190
}
201191
catch (const std::invalid_argument &)
202192
{
203193
RCLCPP_WARN(
204-
get_logger(), "Invalid 'status_publish_rate' parameter. Using default %.d Hz.",
194+
get_logger(), "Invalid 'status_publish_rate' parameter. Using default %.1f Hz.",
205195
publish_rate);
206196
}
207197
}
208198

209-
if (publish_rate > 0)
199+
if (publish_rate == 0.0)
200+
{
201+
RCLCPP_INFO(
202+
get_logger(),
203+
"`status_publish_rate` is set to 0.0, hardware status publisher will not be created.");
204+
}
205+
else
210206
{
211207
control_msgs::msg::HardwareStatus status_msg_template;
212208
if (init_hardware_status_message(status_msg_template) != CallbackReturn::SUCCESS)

0 commit comments

Comments
 (0)