Skip to content

Commit

Permalink
Create command subscriber only when any command interface is present.…
Browse files Browse the repository at this point in the history
… Update docs.
  • Loading branch information
Wiktor-99 committed Nov 7, 2024
1 parent a8228af commit 7bf2d4e
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 5 deletions.
3 changes: 1 addition & 2 deletions gpio_controllers/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,7 @@ This controller uses the `generate_parameter_library <https://github.com/PickNik
.. generate_parameter_library_details::
../src/gpio_command_controller_parameters.yaml

The controller expects at least one GPIO interface and the corresponding command interface names or state interface.
Command and state interfaces are optional.
The controller expects at least one GPIO interface and the corresponding command interface names or state interface. However, these Command and State interfaces are optional. The controller behaves as a broadcaster when no Command Interface is present, thereby publishing the configured GPIO state interfaces if set, else the one present in the URDF.

.. note::

Expand Down
9 changes: 6 additions & 3 deletions gpio_controllers/src/gpio_command_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -81,9 +81,12 @@ try
return CallbackReturn::ERROR;
}

gpios_command_subscriber_ = get_node()->create_subscription<CmdType>(
"~/commands", rclcpp::SystemDefaultsQoS(),
[this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); });
if (!command_interface_types_.empty())
{
gpios_command_subscriber_ = get_node()->create_subscription<CmdType>(
"~/commands", rclcpp::SystemDefaultsQoS(),
[this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); });
}

gpio_state_publisher_ =
get_node()->create_publisher<StateType>("~/gpio_states", rclcpp::SystemDefaultsQoS());
Expand Down

0 comments on commit 7bf2d4e

Please sign in to comment.