diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 64480bfad..393612cd9 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -34,6 +34,7 @@ jobs: ros2_control_demo_example_7 ros2_control_demo_example_8 ros2_control_demo_example_9 + ros2_control_demo_example_10 ros2_control_demo_example_12 vcs-repo-file-url: | diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index da01398fd..934fbf9c4 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -27,6 +27,7 @@ jobs: ros2_control_demo_example_7 ros2_control_demo_example_8 ros2_control_demo_example_9 + ros2_control_demo_example_10 ros2_control_demo_example_12 ament_lint_100: @@ -54,4 +55,5 @@ jobs: ros2_control_demo_example_7 ros2_control_demo_example_8 ros2_control_demo_example_9 + ros2_control_demo_example_10 ros2_control_demo_example_12 diff --git a/README.md b/README.md index 3855e02ac..aa0ff2eda 100644 --- a/README.md +++ b/README.md @@ -31,7 +31,6 @@ The following examples are part of this demo repository: *RRBot* with an integrated sensor. - * Example 5: ["Industrial robots with externally connected sensor"](example_5) *RRBot* with an externally connected sensor. @@ -52,7 +51,9 @@ The following examples are part of this demo repository: Demonstrates how to switch between simulation and hardware. -* Example 10: "RRbot with GPIO interfaces (tba.)" +* Example 10: ["Industrial robot with GPIO interfaces"](example_10) + + *RRBot* with GPIO interfaces. * Example 11: "Car-like robot using steering controller library (tba.)" diff --git a/doc/index.rst b/doc/index.rst index b27e37c0e..d3f959f53 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -67,6 +67,9 @@ Example 8: "Using transmissions" Example 9: "Gazebo Classic" Demonstrates how to switch between simulation and hardware. +Example 10: "GPIO interfaces" + Industrial robot with GPIO interfaces + Example 12: "Controller chaining" The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. @@ -259,4 +262,5 @@ Examples Example 7: Full tutorial with a 6DOF robot <../example_7/doc/userdoc.rst> Example 8: Using transmissions <../example_8/doc/userdoc.rst> Example 9: Gazebo classic <../example_9/doc/userdoc.rst> + Example 10: Industrial robot with GPIO interfaces <../example_10/doc/userdoc.rst> Example 12: Controller chaining <../example_12/doc/userdoc.rst> diff --git a/example_10/CMakeLists.txt b/example_10/CMakeLists.txt new file mode 100644 index 000000000..e2e4dd0d1 --- /dev/null +++ b/example_10/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_10 LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + control_msgs + std_msgs + pluginlib + rclcpp + rclcpp_lifecycle + controller_interface +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + + +## COMPILE +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( + ros2_control_demo_example_10 PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_EXAMPLE_10_BUILDING_DLL") + +# 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 +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/ros2_control_demo_example_10 +) +install(TARGETS ros2_control_demo_example_10 + EXPORT export_ros2_control_demo_example_10 + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) +endif() + +## EXPORTS +ament_export_targets(export_ros2_control_demo_example_10 HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/example_10/README.md b/example_10/README.md new file mode 100644 index 000000000..00e0dc41f --- /dev/null +++ b/example_10/README.md @@ -0,0 +1,5 @@ +# ros2_control_demo_example_10 + + *RRBot* - or ''Revolute-Revolute Manipulator Robot'' - with GPIO interfaces. + +Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_10/doc/userdoc.html). diff --git a/example_10/bringup/config/rrbot_controllers.yaml b/example_10/bringup/config/rrbot_controllers.yaml new file mode 100644 index 000000000..90434b572 --- /dev/null +++ b/example_10/bringup/config/rrbot_controllers.yaml @@ -0,0 +1,31 @@ +controller_manager: + ros__parameters: + update_rate: 1 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + gpio_controller: + type: ros2_control_demo_example_10/GPIOController + + +forward_position_controller: + ros__parameters: + joints: + - joint1 + - joint2 + interface_name: position + +gpio_controller: + ros__parameters: + 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 diff --git a/example_10/bringup/config/rrbot_forward_position_publisher.yaml b/example_10/bringup/config/rrbot_forward_position_publisher.yaml new file mode 100644 index 000000000..d0fd6330a --- /dev/null +++ b/example_10/bringup/config/rrbot_forward_position_publisher.yaml @@ -0,0 +1,11 @@ +publisher_forward_position_controller: + ros__parameters: + + wait_sec_between_publish: 5 + publish_topic: "/forward_position_controller/commands" + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0, 0] + pos3: [-0.785, -0.785] + pos4: [0, 0] diff --git a/example_10/bringup/launch/rrbot.launch.py b/example_10/bringup/launch/rrbot.launch.py new file mode 100644 index 000000000..9326d4690 --- /dev/null +++ b/example_10/bringup/launch/rrbot.launch.py @@ -0,0 +1,97 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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. + + +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_10"), + "urdf", + "rrbot.urdf.xacro", + ] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_10"), + "config", + "rrbot_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["forward_position_controller", "-c", "/controller_manager"], + ) + + gpio_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["gpio_controller", "-c", "/controller_manager"], + ) + + # Delay start of robot_controller after `joint_state_broadcaster` + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + joint_state_broadcaster_spawner, + gpio_controller_spawner, + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + ] + + return LaunchDescription(nodes) diff --git a/example_10/bringup/launch/test_forward_position_controller.launch.py b/example_10/bringup/launch/test_forward_position_controller.launch.py new file mode 100644 index 000000000..3605c3446 --- /dev/null +++ b/example_10/bringup/launch/test_forward_position_controller.launch.py @@ -0,0 +1,41 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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. + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + position_goals = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_1"), + "config", + "rrbot_forward_position_publisher.yaml", + ] + ) + + return LaunchDescription( + [ + Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="publisher_forward_position_controller", + parameters=[position_goals], + output="both", + ) + ] + ) diff --git a/example_10/controllers/gpio_controller.cpp b/example_10/controllers/gpio_controller.cpp new file mode 100644 index 000000000..9bd08ec3a --- /dev/null +++ b/example_10/controllers/gpio_controller.cpp @@ -0,0 +1,151 @@ +// 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 new file mode 100644 index 000000000..cf582bc21 --- /dev/null +++ b/example_10/controllers/include/ros2_control_demo_example_10/gpio_controller.hpp @@ -0,0 +1,78 @@ +// 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 "ros2_control_demo_example_10/visibility_control.h" +#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); + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + 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/description/launch/view_robot.launch.py b/example_10/description/launch/view_robot.launch.py new file mode 100644 index 000000000..bc294c87f --- /dev/null +++ b/example_10/description/launch/view_robot.launch.py @@ -0,0 +1,99 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description="Description package with robot URDF/xacro files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="rrbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + + # Initialize Arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_10"), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rrbot/rviz", "rrbot.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ) + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_10/description/ros2_control/rrbot.ros2_control.xacro b/example_10/description/ros2_control/rrbot.ros2_control.xacro new file mode 100644 index 000000000..e12bad4f7 --- /dev/null +++ b/example_10/description/ros2_control/rrbot.ros2_control.xacro @@ -0,0 +1,43 @@ + + + + + + + + ros2_control_demo_example_10/RRBotSystemWithGPIOHardware + 0 + 3.0 + 100 + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + + + + + + + + + diff --git a/example_10/description/urdf/rrbot.urdf.xacro b/example_10/description/urdf/rrbot.urdf.xacro new file mode 100644 index 000000000..c941ac0c9 --- /dev/null +++ b/example_10/description/urdf/rrbot.urdf.xacro @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_10/doc/userdoc.rst b/example_10/doc/userdoc.rst new file mode 100644 index 000000000..a7e9f6f82 --- /dev/null +++ b/example_10/doc/userdoc.rst @@ -0,0 +1,114 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_10/doc/userdoc.rst + +.. _ros2_control_demos_example_10_userdoc: + +Example 10: Industrial robot with GPIO interfaces +=============================================================== + +This demo shows how to interact with GPIO interfaces. + +The *RRBot* URDF files can be found in the ``description/urdf`` folder. + +1. To check that *RRBot* descriptions are working properly use following launch commands + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_10 view_robot.launch.py + + +2. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_10 rrbot.launch.py + + The launch file loads and starts the robot hardware and controllers. + +3. Check if the hardware interface loaded properly, by opening another terminal and executing + + .. code-block:: console + + ros2 control list_hardware_interfaces + + .. code-block:: + + command interfaces + flange_analog_IOs/analog_output1 [available] [claimed] + flange_vacuum/vacuum [available] [claimed] + joint1/position [available] [claimed] + joint2/position [available] [claimed] + state interfaces + flange_analog_IOs/analog_input1 + flange_analog_IOs/analog_input2 + flange_analog_IOs/analog_output1 + flange_vacuum/vacuum + joint1/position + joint2/position + + In contrast to the *RRBot* of example_1, you see in addition to the joints now also GPIO interfaces. + +4. Check if controllers are running by + + .. code-block:: shell + + ros2 control list_controllers + + .. 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 + +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: + + .. code-block:: shell + + ros2 topic echo /gpio_controller/inputs + + .. 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 + +6. Now you can send commands to the *GPIO Controller* using ROS 2 CLI interface: + + .. code-block:: shell + + ros2 topic pub /gpio_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.5,0.7]}" + + 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. + + .. code-block:: shell + + [RRBotSystemWithGPIOHardware]: Got command 0.5 for GP output 0! + [RRBotSystemWithGPIOHardware]: Got command 0.7 for GP output 1! + + +Files used for this demos +------------------------- + +- Launch file: `rrbot.launch.py `__ +- Controllers yaml: `rrbot_controllers.yaml `__ +- URDF file: `rrbot.urdf.xacro `__ + + + Description: `rrbot_description.urdf.xacro `__ + + ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ + +- RViz configuration: `rrbot.rviz `__ + +- Hardware interface plugin: `rrbot.cpp `__ +- GPIO controller: `gpio_controller.cpp `__ + + +Controllers from this demo +-------------------------- +- ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ +- ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp new file mode 100644 index 000000000..9f6b2c62f --- /dev/null +++ b/example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp @@ -0,0 +1,80 @@ +// 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__RRBOT_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_10__RRBOT_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "ros2_control_demo_example_10/visibility_control.h" + +namespace ros2_control_demo_example_10 +{ +class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemWithGPIOHardware); + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Parameters for the RRBot simulation + + // Store the command and state interfaces for the simulated robot + std::vector hw_commands_; + std::vector hw_states_; + std::vector hw_gpio_in_; + std::vector hw_gpio_out_; +}; + +} // namespace ros2_control_demo_example_10 + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_10__RRBOT_HPP_ diff --git a/example_10/hardware/include/ros2_control_demo_example_10/visibility_control.h b/example_10/hardware/include/ros2_control_demo_example_10/visibility_control.h new file mode 100644 index 000000000..5fbd35996 --- /dev/null +++ b/example_10/hardware/include/ros2_control_demo_example_10/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2021 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_10__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_DEMO_EXAMPLE_10__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROS2_CONTROL_DEMO_EXAMPLE_10_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_10_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_10_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DEMO_EXAMPLE_10_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_DEMO_EXAMPLE_10_BUILDING_DLL +#define ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_10_EXPORT +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_10_IMPORT +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_10_LOCAL +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_10_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_10_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_10_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_10_LOCAL +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_10_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_10__VISIBILITY_CONTROL_H_ diff --git a/example_10/hardware/rrbot.cpp b/example_10/hardware/rrbot.cpp new file mode 100644 index 000000000..d8e9bd84a --- /dev/null +++ b/example_10/hardware/rrbot.cpp @@ -0,0 +1,280 @@ +// 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/rrbot.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_example_10 +{ +hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // RRBotSystemPositionOnly has exactly one state and command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), + "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), + "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + } + + // RRBotSystemWithGPIOHardware has exactly two GPIO components + if (info_.gpios.size() != 2) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), + "RRBotSystemWithGPIOHardware has '%ld' GPIO components, '%d' expected.", info_.gpios.size(), + 2); + return hardware_interface::CallbackReturn::ERROR; + } + // with exactly 1 command interface + for (int i = 0; i < 2; i++) + { + if (info_.gpios[i].command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), + "GPIO component %s has '%ld' command interfaces, '%d' expected.", + info_.gpios[i].name.c_str(), info_.gpios[i].command_interfaces.size(), 1); + return hardware_interface::CallbackReturn::ERROR; + } + } + // and 3/1 state interfaces, respectively + if (info_.gpios[0].state_interfaces.size() != 3) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), + "GPIO component %s has '%ld' state interfaces, '%d' expected.", info_.gpios[0].name.c_str(), + info_.gpios[0].state_interfaces.size(), 3); + return hardware_interface::CallbackReturn::ERROR; + } + if (info_.gpios[1].state_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), + "GPIO component %s has '%ld' state interfaces, '%d' expected.", info_.gpios[0].name.c_str(), + info_.gpios[0].state_interfaces.size(), 1); + return hardware_interface::CallbackReturn::ERROR; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Configuring ...please wait..."); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // reset values always when configuring hardware + std::fill(hw_states_.begin(), hw_states_.end(), 0); + std::fill(hw_commands_.begin(), hw_commands_.end(), 0); + std::fill(hw_gpio_in_.begin(), hw_gpio_in_.end(), 0); + std::fill(hw_gpio_out_.begin(), hw_gpio_out_.end(), 0); + + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully configured!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +RRBotSystemWithGPIOHardware::export_state_interfaces() +{ + std::vector state_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "State interfaces:"); + hw_gpio_in_.resize(4); + size_t ct = 0; + for (size_t i = 0; i < info_.gpios.size(); i++) + { + for (auto state_if : info_.gpios.at(i).state_interfaces) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.gpios.at(i).name, state_if.name, &hw_gpio_in_[ct++])); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s", + info_.gpios.at(i).name.c_str(), state_if.name.c_str()); + } + } + + return state_interfaces; +} + +std::vector +RRBotSystemWithGPIOHardware::export_command_interfaces() +{ + std::vector command_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + } + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Command interfaces:"); + hw_gpio_out_.resize(2); + size_t ct = 0; + for (size_t i = 0; i < info_.gpios.size(); i++) + { + for (auto command_if : info_.gpios.at(i).command_interfaces) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.gpios.at(i).name, command_if.name, &hw_gpio_out_[ct++])); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s", + info_.gpios.at(i).name.c_str(), command_if.name.c_str()); + } + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Activating ...please wait..."); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // command and state should be equal when starting + for (uint i = 0; i < hw_states_.size(); i++) + { + hw_commands_[i] = hw_states_[i]; + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully deactivated!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type RRBotSystemWithGPIOHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Reading..."); + + for (uint i = 0; i < hw_states_.size(); i++) + { + // Simulate RRBot's movement + hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]); + } + + // mirror GPIOs back + hw_gpio_in_[0] = hw_gpio_out_[0]; + hw_gpio_in_[3] = hw_gpio_out_[1]; + // random inputs + unsigned int seed = time(NULL) + 1; + hw_gpio_in_[1] = static_cast(rand_r(&seed)); + seed = time(NULL) + 2; + hw_gpio_in_[2] = static_cast(rand_r(&seed)); + + for (uint i = 0; i < hw_gpio_in_.size(); i++) + { + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Read %.1f from GP input %d!", + hw_gpio_in_[i], i); + } + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully read!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type RRBotSystemWithGPIOHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Writing..."); + + for (uint i = 0; i < hw_gpio_out_.size(); i++) + { + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Got command %.1f for GP output %d!", + hw_gpio_out_[i], i); + } + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully written!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +} // namespace ros2_control_demo_example_10 + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_example_10::RRBotSystemWithGPIOHardware, hardware_interface::SystemInterface) diff --git a/example_10/package.xml b/example_10/package.xml new file mode 100644 index 000000000..d1a211eb7 --- /dev/null +++ b/example_10/package.xml @@ -0,0 +1,40 @@ + + + + ros2_control_demo_example_10 + 0.0.0 + Demo package of `ros2_control` hardware for RRbot with GPIO interfaces. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + + Christoph Froehlich + + Apache-2.0 + + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + controller_manager + forward_command_controller + joint_state_broadcaster + joint_state_publisher_gui + robot_state_publisher + ros2_control_demo_description + ros2_controllers_test_nodes + ros2controlcli + ros2launch + rviz2 + xacro + + ament_cmake_gtest + + + ament_cmake + + diff --git a/example_10/ros2_control_demo_example_10.xml b/example_10/ros2_control_demo_example_10.xml new file mode 100644 index 000000000..1d4c74936 --- /dev/null +++ b/example_10/ros2_control_demo_example_10.xml @@ -0,0 +1,14 @@ + + + + The ros2_control RRbot example with GPIO interfaces. + + + + + This controller publishes the GPIOs. + + + diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml index f5fbf4a9a..79ada4f7a 100644 --- a/ros2_control_demos/package.xml +++ b/ros2_control_demos/package.xml @@ -22,6 +22,7 @@ ros2_control_demo_example_6 ros2_control_demo_example_8 ros2_control_demo_example_9 + ros2_control_demo_example_10 ament_cmake