diff --git a/example_10/CMakeLists.txt b/example_10/CMakeLists.txt index dee817871..3277538a5 100644 --- a/example_10/CMakeLists.txt +++ b/example_10/CMakeLists.txt @@ -16,7 +16,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS pluginlib rclcpp rclcpp_lifecycle - controller_interface ) # Specify the required version of ros2_control @@ -39,12 +38,10 @@ add_library( ros2_control_demo_example_10 SHARED hardware/rrbot.cpp - controllers/gpio_controller.cpp ) target_compile_features(ros2_control_demo_example_10 PUBLIC cxx_std_17) target_include_directories(ros2_control_demo_example_10 PUBLIC $ -$ $ ) ament_target_dependencies( @@ -54,18 +51,12 @@ ament_target_dependencies( # Export hardware plugins pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_10.xml) -# Export controllers -pluginlib_export_plugin_description_file(controller_interface ros2_control_demo_example_10.xml) # INSTALL install( DIRECTORY hardware/include/ DESTINATION include/ros2_control_demo_example_10 ) -install( - DIRECTORY controllers/include/ - DESTINATION include/ros2_control_demo_example_10 -) install( DIRECTORY description/launch description/ros2_control description/urdf DESTINATION share/ros2_control_demo_example_10 diff --git a/example_10/bringup/config/rrbot_controllers.yaml b/example_10/bringup/config/rrbot_controllers.yaml index 3069ab8fa..fe8e7cb72 100644 --- a/example_10/bringup/config/rrbot_controllers.yaml +++ b/example_10/bringup/config/rrbot_controllers.yaml @@ -16,12 +16,14 @@ forward_position_controller: gpio_controller: ros__parameters: - type: ros2_control_demo_example_10/GPIOController - inputs: - - flange_analog_IOs/analog_output1 - - flange_analog_IOs/analog_input1 - - flange_analog_IOs/analog_input2 - - flange_vacuum/vacuum - outputs: - - flange_analog_IOs/analog_output1 - - flange_vacuum/vacuum + type: gpio_controllers/GpioCommandController + gpios: + - flange_analog_IOs + - flange_vacuum + command_interfaces: + flange_analog_IOs: + - interfaces: + - analog_output1 + flange_vacuum: + - interfaces: + - vacuum diff --git a/example_10/controllers/gpio_controller.cpp b/example_10/controllers/gpio_controller.cpp deleted file mode 100644 index 9bd08ec3a..000000000 --- a/example_10/controllers/gpio_controller.cpp +++ /dev/null @@ -1,151 +0,0 @@ -// Copyright 2023 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ros2_control_demo_example_10/gpio_controller.hpp" - -#include - -namespace ros2_control_demo_example_10 -{ -controller_interface::CallbackReturn GPIOController::on_init() -{ - try - { - auto_declare>("inputs", std::vector()); - auto_declare>("outputs", std::vector()); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration GPIOController::command_interface_configuration() const -{ - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - config.names = outputs_; - - return config; -} - -controller_interface::InterfaceConfiguration GPIOController::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - config.names = inputs_; - - return config; -} - -controller_interface::return_type GPIOController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - // send inputs - for (size_t i = 0; i < state_interfaces_.size(); i++) - { - RCLCPP_DEBUG( - get_node()->get_logger(), "%s: (%f)", state_interfaces_[i].get_name().c_str(), - state_interfaces_[i].get_value()); - gpio_msg_.values.at(i) = static_cast(state_interfaces_.at(i).get_value()); - } - gpio_publisher_->publish(gpio_msg_); - - // set outputs - if (!output_cmd_ptr_) - { - // no command received yet - return controller_interface::return_type::OK; - } - if (output_cmd_ptr_->data.size() != command_interfaces_.size()) - { - RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *(get_node()->get_clock()), 1000, - "command size (%zu) does not match number of interfaces (%zu)", output_cmd_ptr_->data.size(), - command_interfaces_.size()); - return controller_interface::return_type::ERROR; - } - - for (size_t i = 0; i < command_interfaces_.size(); i++) - { - command_interfaces_[i].set_value(output_cmd_ptr_->data[i]); - RCLCPP_DEBUG( - get_node()->get_logger(), "%s: (%f)", command_interfaces_[i].get_name().c_str(), - command_interfaces_[i].get_value()); - } - - return controller_interface::return_type::OK; -} - -controller_interface::CallbackReturn GPIOController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - try - { - inputs_ = get_node()->get_parameter("inputs").as_string_array(); - outputs_ = get_node()->get_parameter("outputs").as_string_array(); - - initMsgs(); - - // register publisher - gpio_publisher_ = get_node()->create_publisher( - "~/inputs", rclcpp::SystemDefaultsQoS()); - - // register subscriber - subscription_command_ = get_node()->create_subscription( - "~/commands", rclcpp::SystemDefaultsQoS(), - [this](const CmdType::SharedPtr msg) { output_cmd_ptr_ = msg; }); - } - catch (...) - { - return LifecycleNodeInterface::CallbackReturn::ERROR; - } - return LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -void GPIOController::initMsgs() -{ - gpio_msg_.interface_names = inputs_; - gpio_msg_.values.resize(inputs_.size()); -} - -controller_interface::CallbackReturn GPIOController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - return LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn GPIOController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - try - { - // reset publisher - gpio_publisher_.reset(); - } - catch (...) - { - return LifecycleNodeInterface::CallbackReturn::ERROR; - } - return LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -} // namespace ros2_control_demo_example_10 - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_example_10::GPIOController, controller_interface::ControllerInterface) diff --git a/example_10/controllers/include/ros2_control_demo_example_10/gpio_controller.hpp b/example_10/controllers/include/ros2_control_demo_example_10/gpio_controller.hpp deleted file mode 100644 index 6a1db0249..000000000 --- a/example_10/controllers/include/ros2_control_demo_example_10/gpio_controller.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2023 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ROS2_CONTROL_DEMO_EXAMPLE_10__GPIO_CONTROLLER_HPP_ -#define ROS2_CONTROL_DEMO_EXAMPLE_10__GPIO_CONTROLLER_HPP_ - -#include -#include -#include - -#include "control_msgs/msg/interface_value.hpp" -#include "std_msgs/msg/float64_multi_array.hpp" - -#include "controller_interface/controller_interface.hpp" - -namespace ros2_control_demo_example_10 -{ -using CmdType = std_msgs::msg::Float64MultiArray; - -class GPIOController : public controller_interface::ControllerInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(GPIOController); - - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - - CallbackReturn on_init() override; - -private: - std::vector inputs_; - std::vector outputs_; - -protected: - void initMsgs(); - - // internal commands - std::shared_ptr output_cmd_ptr_; - - // publisher - std::shared_ptr> gpio_publisher_; - control_msgs::msg::InterfaceValue gpio_msg_; - - // subscriber - rclcpp::Subscription::SharedPtr subscription_command_; -}; -} // namespace ros2_control_demo_example_10 - -#endif // ROS2_CONTROL_DEMO_EXAMPLE_10__GPIO_CONTROLLER_HPP_ diff --git a/example_10/doc/userdoc.rst b/example_10/doc/userdoc.rst index d7c442977..60806b589 100644 --- a/example_10/doc/userdoc.rst +++ b/example_10/doc/userdoc.rst @@ -55,36 +55,99 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. .. code-block:: shell - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - gpio_controller [ros2_control_demo_example_10/GPIOController] active - forward_position_controller[forward_command_controller/ForwardCommandController] active + joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active + gpio_controller gpio_controllers/GpioCommandController active + forward_position_controller forward_command_controller/ForwardCommandController active -5. If you get output from above you can subscribe to the ``/gpio_controller/inputs`` topic published by the *GPIO Controller* using ROS 2 CLI interface: +5. If you get output from above you can subscribe to the ``/dynamic_joint_states`` topic published by the *joint_state_broadcaster* using ROS 2 CLI interface: .. code-block:: shell - ros2 topic echo /gpio_controller/inputs + ros2 topic echo /dynamic_joint_states --once + + + This includes not only the state interfaces of the joints but also the GPIO interfaces. + + .. code-block:: shell + + header: + stamp: + sec: 1730670203 + nanosec: 875008879 + frame_id: '' + joint_names: + - joint1 + - joint2 + - flange_vacuum + - flange_analog_IOs + interface_values: + - interface_names: + - position + values: + - 0.0 + - interface_names: + - position + values: + - 0.0 + - interface_names: + - vacuum + values: + - 0.0 + - interface_names: + - analog_input2 + - analog_input1 + - analog_output1 + values: + - 92747888.0 + - 1764536320.0 + - 0.0 + --- + + You can also subscribe to the ``/gpio_controller/gpio_states`` topic published by the *gpio_controller* using ROS 2 CLI interface: .. code-block:: shell - interface_names: - - flange_analog_IOs/analog_output1 - - flange_analog_IOs/analog_input1 - - flange_analog_IOs/analog_input2 - - flange_vacuum/vacuum - values: - - 0.0 - - 1199574016.0 - - 1676318848.0 - - 0.0 + ros2 topic echo /gpio_controller/gpio_states + + which shows the values of the state_interfaces of the configured GPIOs + + .. code-block:: shell -6. Now you can send commands to the *GPIO Controller* using ROS 2 CLI interface: + header: + stamp: + sec: 1731875120 + nanosec: 2015630 + frame_id: '' + interface_groups: + - flange_analog_IOs + - flange_vacuum + interface_values: + - interface_names: + - analog_output1 + - analog_input1 + - analog_input2 + values: + - 0.0 + - 991951680.0 + - 1467646976.0 + - interface_names: + - vacuum + values: + - 0.0 + --- + + +6. Now you can send commands to the *gpio_controller* using ROS 2 CLI interface. You can set a single interface or all at once in one message: .. code-block:: shell - ros2 topic pub /gpio_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.5,0.7]}" + ros2 topic pub /gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues "{interface_groups: [flange_analog_IOs], interface_values: [{interface_names: [analog_output1], values: [0.5]}]}" - You should see a change in the ``/gpio_controller/inputs`` topic and a different output in the terminal where launch file is started, e.g. + ros2 topic pub /gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues "{interface_groups: [flange_vacuum], interface_values: [{interface_names: [vacuum], values: [0.27]}]}" + + ros2 topic pub /gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues "{interface_groups: [flange_vacuum, flange_analog_IOs], interface_values: [{interface_names: [vacuum], values: [0.27]}, {interface_names: [analog_output1], values: [0.5]} ]}" + + You should see a change in the ``/gpio_controller/gpio_states`` topic and a different output in the terminal where launch file is started, e.g. .. code-block:: shell @@ -154,22 +217,34 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. .. code-block:: shell - ros2 topic echo /gpio_controller/inputs + ros2 topic echo /gpio_controller/gpio_states again and you should see that - unless commands are received - the values of the state interfaces are now ``nan`` except for the vacuum interface. .. code-block:: shell - interface_names: - - flange_analog_IOs/analog_output1 - - flange_analog_IOs/analog_input1 - - flange_analog_IOs/analog_input2 - - flange_vacuum/vacuum - values: - - .nan - - .nan - - .nan - - 1.0 + header: + stamp: + sec: 1731875298 + nanosec: 783713170 + frame_id: '' + interface_groups: + - flange_analog_IOs + - flange_vacuum + interface_values: + - interface_names: + - analog_output1 + - analog_input1 + - analog_input2 + values: + - .nan + - .nan + - .nan + - interface_names: + - vacuum + values: + - 1.0 + --- This is, because for the vacuum interface an initial value of ``1.0`` is set in the URDF file. @@ -182,13 +257,7 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. - Call again - - .. code-block:: shell - - ros2 topic pub /gpio_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.5,0.7]}" - - and you will see that the GPIO command interfaces will be mirrored to their respective state interfaces. + Send again topics to ``/gpio_controller/commands`` and you will see that the GPIO command interfaces will be mirrored to their respective state interfaces. Files used for this demos ------------------------- @@ -207,10 +276,9 @@ Files used for this demos + `rrbot.cpp `__ + `generic_system.cpp `__ -- GPIO controller: `gpio_controller.cpp `__ - Controllers from this demo -------------------------- - ``Joint State Broadcaster`` (`ros2_controllers repository `__): :ref:`doc ` - ``Forward Command Controller`` (`ros2_controllers repository `__): :ref:`doc ` +- ``GPIO Command Controller`` (`ros2_controllers repository `__): :ref:`doc ` diff --git a/example_10/hardware/rrbot.cpp b/example_10/hardware/rrbot.cpp index 9c0d2836b..40128a327 100644 --- a/example_10/hardware/rrbot.cpp +++ b/example_10/hardware/rrbot.cpp @@ -108,7 +108,7 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( { RCLCPP_FATAL( get_logger(), "GPIO component %s has '%ld' state interfaces, '%d' expected.", - info_.gpios[0].name.c_str(), info_.gpios[0].state_interfaces.size(), 1); + info_.gpios[1].name.c_str(), info_.gpios[1].state_interfaces.size(), 1); return hardware_interface::CallbackReturn::ERROR; } diff --git a/example_10/package.xml b/example_10/package.xml index 8943501fe..bcf0feae8 100644 --- a/example_10/package.xml +++ b/example_10/package.xml @@ -23,6 +23,7 @@ controller_manager forward_command_controller + gpio_controllers joint_state_broadcaster joint_state_publisher_gui robot_state_publisher @@ -34,9 +35,18 @@ xacro ament_cmake_pytest + controller_manager + forward_command_controller + gpio_controllers + joint_state_broadcaster launch_testing_ros liburdfdom-tools + robot_state_publisher + ros2_control_demo_description ros2_control_demo_testing + ros2_controllers_test_nodes + ros2controlcli + ros2launch xacro diff --git a/example_10/ros2_control_demo_example_10.xml b/example_10/ros2_control_demo_example_10.xml index 1d4c74936..e1fa75cec 100644 --- a/example_10/ros2_control_demo_example_10.xml +++ b/example_10/ros2_control_demo_example_10.xml @@ -6,9 +6,4 @@ The ros2_control RRbot example with GPIO interfaces. - - - This controller publishes the GPIOs. - - diff --git a/ros2_control_demos-not-released.jazzy.repos b/ros2_control_demos-not-released.jazzy.repos index 56f46b6f7..338a77433 100644 --- a/ros2_control_demos-not-released.jazzy.repos +++ b/ros2_control_demos-not-released.jazzy.repos @@ -1 +1,5 @@ repositories: + ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: gpio_controllers diff --git a/ros2_control_demos-not-released.rolling.repos b/ros2_control_demos-not-released.rolling.repos index 56f46b6f7..338a77433 100644 --- a/ros2_control_demos-not-released.rolling.repos +++ b/ros2_control_demos-not-released.rolling.repos @@ -1 +1,5 @@ repositories: + ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers.git + version: gpio_controllers