diff --git a/README.md b/README.md index bf71f93f0..d274627b7 100644 --- a/README.md +++ b/README.md @@ -83,6 +83,10 @@ The following examples are part of this demo repository: This example shows how to create chained controllers using diff_drive_controller and two pid_controllers to control a differential drive robot. +* Example 17: ["RRBot with Hardware Component that publishes diagnostics"](example_17) + + This example shows how to publish diagnostics from a hardware component using the Executor passed from Controller Manager. + ## Structure The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`. diff --git a/doc/index.rst b/doc/index.rst index 1cbf5006c..1b4ecdfb9 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -88,6 +88,9 @@ Example 15: "Using multiple controller managers" Example 16: "DiffBot with chained controllers" This example shows how to create chained controllers using diff_drive_controller and pid_controllers to control a differential drive robot. +Example 17: "RRBot with Hardware Component that publishes diagnostics" + This example shows how to publish diagnostics from a hardware component using the Executor passed from Controller Manager. + .. _ros2_control_demos_install: ===================== @@ -296,3 +299,4 @@ Examples Example 14: Modular robots with actuators not providing states <../example_14/doc/userdoc.rst> Example 15: Using multiple controller managers <../example_15/doc/userdoc.rst> Example 16: DiffBot with chained controllers <../example_16/doc/userdoc.rst> + Example 17: RRBot with Hardware Component that publishes diagnostics <../example_17/doc/userdoc.rst> diff --git a/example_17/CMakeLists.txt b/example_17/CMakeLists.txt new file mode 100644 index 000000000..deeeb4d17 --- /dev/null +++ b/example_17/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_17 LANGUAGES CXX) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + diagnostic_updater +) + +# Specify the required version of ros2_control +find_package(controller_manager 4.0.0) +# Handle the case where the required version is not found +if(NOT controller_manager_FOUND) + message(FATAL_ERROR "ros2_control version 4.0.0 or higher is required. " + "Are you using the correct branch of the ros2_control_demos repository?") +endif() + +# find dependencies +find_package(backward_ros REQUIRED) +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_17 + SHARED + hardware/rrbot.cpp +) +target_compile_features(ros2_control_demo_example_17 PUBLIC cxx_std_17) +target_include_directories(ros2_control_demo_example_17 PUBLIC +$ +$ +) +target_link_libraries(ros2_control_demo_example_17 PUBLIC + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + diagnostic_updater::diagnostic_updater +) + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_17.xml) + +# INSTALL +install( + DIRECTORY hardware/include/ + DESTINATION include/ros2_control_demo_example_17 +) +install( + DIRECTORY description/launch description/ros2_control description/urdf + DESTINATION share/ros2_control_demo_example_17 +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/ros2_control_demo_example_17 +) +install(TARGETS ros2_control_demo_example_17 + EXPORT export_ros2_control_demo_example_17 + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(example_17_urdf_xacro test/test_urdf_xacro.py) + ament_add_pytest_test(view_example_17_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_17_launch test/test_rrbot_launch.py) + ament_add_pytest_test(run_example_17_launch_cli_direct test/test_rrbot_launch_cli_direct.py) +endif() + + +## EXPORTS +ament_export_targets(export_ros2_control_demo_example_17 HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/example_17/README.md b/example_17/README.md new file mode 100644 index 000000000..2405e75b4 --- /dev/null +++ b/example_17/README.md @@ -0,0 +1,5 @@ +# ros2_control_demo_example_17 + + This example shows how to publish diagnostics from a hardware component using the Executor passed from Controller Manager. + +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_17/doc/userdoc.html). diff --git a/example_17/bringup/config/rrbot_controllers.yaml b/example_17/bringup/config/rrbot_controllers.yaml new file mode 100644 index 000000000..0a658759c --- /dev/null +++ b/example_17/bringup/config/rrbot_controllers.yaml @@ -0,0 +1,15 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + +forward_position_controller: + ros__parameters: + type: forward_command_controller/ForwardCommandController + joints: + - joint1 + - joint2 + interface_name: position diff --git a/example_17/bringup/config/rrbot_forward_position_publisher.yaml b/example_17/bringup/config/rrbot_forward_position_publisher.yaml new file mode 100644 index 000000000..879ad34ab --- /dev/null +++ b/example_17/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, 0.0] + pos3: [-0.785, -0.785] + pos4: [0.0, 0.0] diff --git a/example_17/bringup/config/rrbot_joint_trajectory_publisher.yaml b/example_17/bringup/config/rrbot_joint_trajectory_publisher.yaml new file mode 100644 index 000000000..96de7d843 --- /dev/null +++ b/example_17/bringup/config/rrbot_joint_trajectory_publisher.yaml @@ -0,0 +1,24 @@ +publisher_joint_trajectory_controller: + ros__parameters: + + controller_name: "joint_trajectory_position_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [0.785, 0.785] + pos2: + positions: [0.0, 0.0] + pos3: + positions: [-0.785, -0.785] + pos4: + positions: [0.0, 0.0] + + joints: + - joint1 + - joint2 + + check_starting_point: false + starting_point_limits: + joint1: [-0.1,0.1] + joint2: [-0.1,0.1] diff --git a/example_17/bringup/config/rrbot_jtc.yaml b/example_17/bringup/config/rrbot_jtc.yaml new file mode 100644 index 000000000..2d8e97710 --- /dev/null +++ b/example_17/bringup/config/rrbot_jtc.yaml @@ -0,0 +1,22 @@ +joint_trajectory_position_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + + joints: + - joint1 + - joint2 + + command_interfaces: + - position + + state_interfaces: + - position + + action_monitor_rate: 20.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + interpolate_from_desired_state: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/example_17/bringup/launch/rrbot.launch.py b/example_17/bringup/launch/rrbot.launch.py new file mode 100644 index 000000000..ab5719769 --- /dev/null +++ b/example_17/bringup/launch/rrbot.launch.py @@ -0,0 +1,125 @@ +# 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, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +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( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + + # Initialize Arguments + gui = LaunchConfiguration("gui") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_17"), + "urdf", + "rrbot.urdf.xacro", + ] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_17"), + "config", + "rrbot_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + ) + robot_state_pub_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], + condition=IfCondition(gui), + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["forward_position_controller", "--param-file", robot_controllers], + ) + + # Delay rviz start after `joint_state_broadcaster` + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[rviz_node], + ) + ) + + # Delay start of joint_state_broadcaster after `robot_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + robot_controller_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_controller_spawner, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_17/bringup/launch/test_forward_position_controller.launch.py b/example_17/bringup/launch/test_forward_position_controller.launch.py new file mode 100644 index 000000000..0a20ea996 --- /dev/null +++ b/example_17/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_17"), + "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_17/bringup/launch/test_joint_trajectory_controller.launch.py b/example_17/bringup/launch/test_joint_trajectory_controller.launch.py new file mode 100644 index 000000000..2b66274ff --- /dev/null +++ b/example_17/bringup/launch/test_joint_trajectory_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_17"), + "config", + "rrbot_joint_trajectory_publisher.yaml", + ] + ) + + return LaunchDescription( + [ + Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + name="publisher_joint_trajectory_controller", + parameters=[position_goals], + output="both", + ) + ] + ) diff --git a/example_17/description/launch/view_robot.launch.py b/example_17/description/launch/view_robot.launch.py new file mode 100644 index 000000000..b0a6c48b9 --- /dev/null +++ b/example_17/description/launch/view_robot.launch.py @@ -0,0 +1,111 @@ +# 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.conditions import IfCondition +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( + "gui", + default_value="true", + description="Start Rviz2 and Joint State Publisher gui automatically \ + with this launch file.", + ) + ) + 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") + gui = LaunchConfiguration("gui") + prefix = LaunchConfiguration("prefix") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_17"), "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", + condition=IfCondition(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], + condition=IfCondition(gui), + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_17/description/ros2_control/rrbot.ros2_control.xacro b/example_17/description/ros2_control/rrbot.ros2_control.xacro new file mode 100644 index 000000000..f05f6a8a6 --- /dev/null +++ b/example_17/description/ros2_control/rrbot.ros2_control.xacro @@ -0,0 +1,32 @@ + + + + + + + + ros2_control_demo_example_17/RRBotSystemPositionOnlyHardware + 0 + 3.0 + 100 + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + diff --git a/example_17/description/urdf/rrbot.urdf.xacro b/example_17/description/urdf/rrbot.urdf.xacro new file mode 100644 index 000000000..a15690b31 --- /dev/null +++ b/example_17/description/urdf/rrbot.urdf.xacro @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_17/doc/userdoc.rst b/example_17/doc/userdoc.rst new file mode 100644 index 000000000..7b51f8e30 --- /dev/null +++ b/example_17/doc/userdoc.rst @@ -0,0 +1,251 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_17/doc/userdoc.rst + +.. _ros2_control_demos_example_17_userdoc: + +Example 17: RRBot with Hardware Component that publishes diagnostics +===================================================================== + +This example shows how to publish diagnostics from a hardware component using ROS 2 features available within the ``ros2_control`` framework. + +It is essentially the same as Example 1, but with a modified hardware interface plugin that demonstrates two methods for publishing status information: +1. Using the standard ``diagnostic_updater`` on the default node to publish to the ``/diagnostics`` topic. This is the recommended approach for hardware status reporting. +2. Using the Controller Manager's Executor to add a custom ROS 2 node for publishing to a separate, non-standard topic. + +See the :ref:`Implementation Details of the Diagnostic Publisher ` for more information. + +For *example_17*, the hardware interface plugin is implemented having only one interface. + +* The communication is done using proprietary API to communicate with the robot control box. +* Data for all joints is exchanged at once. +* Examples: KUKA RSI + +The *RRBot* URDF files can be found in the ``description/urdf`` folder. + +.. include:: ../../doc/run_from_docker.rst + +Tutorial steps +-------------------------- +Follow the same basic steps as in Example 1. You can find the details here: +:ref:`Example 1: RRBot System Position Only `. + +This tutorial differs by including a hardware interface that publishes diagnostics using two different mechanisms: + +1. A **default node** is automatically provided to the hardware component. We use the standard ``diagnostic_updater`` library on this node to publish structured diagnostic data. +2. A **custom ROS 2 node** is created inside the hardware interface and added to the executor provided by the controller manager. This demonstrates a more manual approach useful for non-diagnostic topics. + +The nodes and topics: + +- Default node (via ``diagnostic_updater``): + - Is named after the hardware component (e.g., ``/RRBot``). + - Publishes periodically to the standard ``/diagnostics`` topic. + - Uses message type ``diagnostic_msgs/msg/DiagnosticArray``. +- Custom node: + - Is named ``_custom_node`` (e.g., ``/rrbot_custom_node``). + - Publishes on the topic ``/rrbot_custom_status``. + - Uses message type ``std_msgs/msg/String``. + - Sends a message every 2 seconds. + +To check that the nodes are running and diagnostics are published correctly: + +.. tabs:: + + .. group-tab:: Local + + .. code-block:: shell + + # List available nodes + ros2 node list + + # You should see something like: + # /rrbot (the default node) + # /rrbot_custom_node (the custom node) + # /controller_manager + # /robot_state_publisher + + # List topics and confirm diagnostics topics are available + ros2 topic list + + # Confirm message type of the diagnostics topic + ros2 topic info /diagnostics + # Should show: diagnostic_msgs/msg/DiagnosticArray + + # Confirm message type of the custom status topic + ros2 topic info /rrbot_custom_status + # Should show: std_msgs/msg/String + + # Echo the raw messages published by the default node + ros2 topic echo /diagnostics + + # Echo the messages published by the custom node + ros2 topic echo /rrbot_custom_status + + .. group-tab:: Docker + + Enter the running container shell first: + + .. code-block:: shell + + docker exec -it ros2_control_demos ./entrypoint.sh bash + + Then run the same commands inside the container: + + .. code-block:: shell + + ros2 node list + ros2 topic info /diagnostics + ros2 topic info /rrbot_custom_status + ros2 topic echo /diagnostics + ros2 topic echo /rrbot_custom_status + +The echoed messages should look similar to: + +.. code-block:: yaml + + # From /diagnostics (showing the default node's output) + header: + stamp: + sec: 1678886401 + nanosec: 123456789 + status: + - level: 0 + name: "RRBot" + message: "Hardware is OK" + hardware_id: "" + values: [] + --- + +.. code-block:: shell + + # From /rrbot_custom_status (showing the custom node's output) + data: RRBot 'RRBot' custom node is alive at 1751087597.146549 + --- + +.. note:: + + The custom diagnostics node and its timer are created only if the executor is successfully passed to the hardware component. If you don't see the topic or node, ensure the hardware plugin is correctly implemented and that the controller manager is providing an executor. + +.. _diagnostic_publisher_implementation: + +Implementation Details of the Diagnostic Publisher +-------------------------------------------------- + +This example demonstrates the two recommended ways for a hardware component to perform ROS 2 communications: using the standard ``diagnostic_updater`` on the default node, and creating a separate custom node added to the Controller Manager's executor. + +**1. Using the ``diagnostic_updater`` with the Default Node (Standard Method)** + +The ``diagnostic_updater`` library is the standard ROS 2 tool for publishing diagnostics. It automatically handles timer creation and publishing to the ``/diagnostics`` topic, making it the preferred method. The ``HardwareComponentInterface`` provides a ``get_node()`` method to access the default node, which is passed to the updater. + +The key steps are: +1. **Create an Updater**: Instantiate ``diagnostic_updater::Updater``, passing it the default node. The updater internally creates a publisher to ``/diagnostics`` and a periodic timer (default 1 Hz). +2. **Set Hardware ID**: Set a unique identifier for the hardware component. +3. **Add a Diagnostic Task**: Add a function that will be called periodically by the updater's internal timer. This function populates a ``DiagnosticStatusWrapper`` with the current status of the hardware. + +.. code-block:: cpp + + #include "diagnostic_updater/diagnostic_updater.hpp" + + if (get_node()) + { + updater_ = std::make_shared(get_node()); + updater_->setHardwareID(info_.name); + + updater_->add( + info_.name + " Status", this, &RRBotSystemPositionOnlyHardware::produce_diagnostics); + } + + void RRBotSystemPositionOnlyHardware::produce_diagnostics( + diagnostic_updater::DiagnosticStatusWrapper & stat) + { + // Add status summary + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Hardware is OK"); + // Optionally add key-value pairs + // stat.add("voltage", "24.1V"); + } + + +**2. Creating a Custom Node with the Executor (Advanced Method)** + +For non-diagnostic topics or when a separate node identity is required, a hardware component can create its own node and add it to the Controller Manager's executor. + +1. **Receiving the Executor Reference**: The ``on_init`` method of the hardware interface is implemented with an updated signature that accepts ``HardwareComponentInterfaceParams``. This struct contains a weak pointer to the ``ControllerManager``'s executor. + + .. code-block:: cpp + + // Get Weak Pointer to Executor from HardwareComponentInterfaceParams + executor_ = params.executor; + +2. **Safely Accessing the Executor**: Before using the executor, its ``weak_ptr`` must be "locked" into a ``shared_ptr``. This is a crucial safety check to ensure the executor is still valid. + + .. code-block:: cpp + + if (auto locked_executor = executor_.lock()) + { + // ... executor is valid and can be used here ... + } + else + { + return hardware_interface::CallbackReturn::ERROR; + } + +3. **Creating and Adding a Node**: Inside the locked scope, a standard ``rclcpp::Node`` is created. This new node is then added to the ``ControllerManager``'s executor. This allows the node's callbacks (e.g., for timers or subscriptions) to be processed by the same multi-threaded executor. + + .. code-block:: cpp + + // Inside the `if (auto locked_executor = ...)` block + custom_status_node_ = std::make_shared(info_.name + "_custom_node"); + locked_executor->add_node(custom_status_node_->get_node_base_interface()); + + +4. **Publishing Status Messages**: A publisher and a periodic wall timer are created on the new custom node. The timer's callback periodically publishes a status message, demonstrating that the node is alive and running alongside the main control loop. + + .. code-block:: cpp + + // Inside the `if (auto locked_executor = ...)` block + custom_status_publisher_ = + custom_status_node_->create_publisher("rrbot_custom_status", 10); + + custom_status_timer_ = custom_status_node_->create_wall_timer( + 2s, [this](){ /* ... lambda to publish message ... */ }); + +**3. (Extra)Using the Default Node, but with a Custom Publisher** + +This is not implemented in the example, but is also a viable option. + +.. code-block:: cpp + + // Get Default Node added to executor + auto default_node = get_node(); + if (default_node) + { + default_status_publisher_ = default_node->create_publisher("rrbot_default_status", 10); + using namespace std::chrono_literals; + default_status_timer_ = default_node->create_wall_timer(2.5s, [this](){ /* ... */ }); + } + + +This pattern is the recommended approach for hardware components that need to perform their own ROS communications without interfering with the real-time control loop. + +Files used for this demos +------------------------- + +* Launch file: `rrbot.launch.py `__ +* Controllers yaml: + + * `rrbot_controllers.yaml `__ + * `rrbot_jtc.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 `__ + + +Controllers from this demo +-------------------------- + * ``Joint State Broadcaster`` (`ros2_controllers repository `__): :ref:`doc ` + * ``Forward Command Controller`` (`ros2_controllers repository `__): :ref:`doc ` + * ``Joint Trajectory Controller`` (`ros2_controllers repository `__): :ref:`doc ` diff --git a/example_17/hardware/include/ros2_control_demo_example_17/rrbot.hpp b/example_17/hardware/include/ros2_control_demo_example_17/rrbot.hpp new file mode 100644 index 000000000..1afcb7b01 --- /dev/null +++ b/example_17/hardware/include/ros2_control_demo_example_17/rrbot.hpp @@ -0,0 +1,75 @@ +// Copyright 2020 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_17__RRBOT_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_17__RRBOT_HPP_ + +#include +#include +#include + +#include "diagnostic_updater/diagnostic_updater.hpp" +#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/rclcpp.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "std_msgs/msg/string.hpp" + +namespace ros2_control_demo_example_17 +{ +class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware) + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override; + + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Parameters for the RRBot simulation + double hw_start_sec_; + double hw_stop_sec_; + double hw_slowdown_; + + std::shared_ptr updater_; + void produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + + rclcpp::Executor::WeakPtr executor_; + rclcpp::Node::SharedPtr custom_status_node_; + rclcpp::Publisher::SharedPtr custom_status_publisher_; + rclcpp::TimerBase::SharedPtr custom_status_timer_; +}; + +} // namespace ros2_control_demo_example_17 + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_17__RRBOT_HPP_ diff --git a/example_17/hardware/rrbot.cpp b/example_17/hardware/rrbot.cpp new file mode 100644 index 000000000..ae98fd96c --- /dev/null +++ b/example_17/hardware/rrbot.cpp @@ -0,0 +1,263 @@ +// Copyright 2020 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_17/rrbot.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "diagnostic_updater/diagnostic_updater.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_example_17 +{ +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) +{ + if ( + hardware_interface::SystemInterface::on_init(params) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // Get Weak Pointer to Executor from HardwareComponentInterfaceParams + executor_ = params.executor; + + // Ensure that the executor is available before creating the custom status node + if (auto locked_executor = executor_.lock()) + { + std::string name_lower = info_.name; + std::transform( + name_lower.begin(), name_lower.end(), name_lower.begin(), + [](unsigned char c) { return std::tolower(c); }); + + std::string node_name = name_lower + "_custom_node"; + custom_status_node_ = std::make_shared(node_name); + + locked_executor->add_node(custom_status_node_->get_node_base_interface()); + } + + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); + RCLCPP_INFO(get_logger(), "Robot hardware_component update_rate is %dHz", info_.rw_rate); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + 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( + get_logger(), "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( + get_logger(), "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( + get_logger(), "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( + get_logger(), "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; + } + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +void RRBotSystemPositionOnlyHardware::produce_diagnostics( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Hardware is OK"); +} + +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::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(get_logger(), "Configuring ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_state(name, 0.0); + } + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, 0.0); + } + RCLCPP_INFO(get_logger(), "Successfully configured!"); + + // Get Default Node added to executor + if (get_node()) + { + updater_ = std::make_shared(get_node()); + updater_->setHardwareID(info_.name); + + updater_->add( + info_.name + " Status", this, &RRBotSystemPositionOnlyHardware::produce_diagnostics); + } + else + { + RCLCPP_WARN( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Default node is not available. Standard diagnostics will not be published."); + } + + if (custom_status_node_) + { + custom_status_publisher_ = + custom_status_node_->create_publisher("rrbot_custom_status", 10); + + auto custom_timer_callback = [this]() -> void + { + if (custom_status_publisher_) + { + std_msgs::msg::String msg; + msg.data = "RRBot '" + info_.name + "' custom node is alive at " + + std::to_string(custom_status_node_->now().seconds()); + custom_status_publisher_->publish(std::move(msg)); + } + }; + + using namespace std::chrono_literals; + custom_status_timer_ = custom_status_node_->create_wall_timer(2s, custom_timer_callback); + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::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(get_logger(), "Activating ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); + } + // 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 (const auto & [name, descr] : joint_state_interfaces_) + { + set_command(name, get_state(name)); + } + + RCLCPP_INFO(get_logger(), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::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(get_logger(), "Deactivating ...please wait..."); + + for (int i = 0; i < hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); + } + + RCLCPP_INFO(get_logger(), "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 RRBotSystemPositionOnlyHardware::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 + std::stringstream ss; + ss << "Reading states:"; + + for (const auto & [name, descr] : joint_state_interfaces_) + { + // Simulate RRBot's movement + auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_; + set_state(name, new_value); + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << get_state(name) << " for joint '" << name << "'"; + } + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); + // 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 RRBotSystemPositionOnlyHardware::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 + std::stringstream ss; + ss << "Writing commands:"; + + for (const auto & [name, descr] : joint_command_interfaces_) + { + // Simulate sending commands to the hardware + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << get_command(name) << " for joint '" << name << "'"; + } + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); + // 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_17 + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_example_17::RRBotSystemPositionOnlyHardware, + hardware_interface::SystemInterface) diff --git a/example_17/package.xml b/example_17/package.xml new file mode 100644 index 000000000..055eee69b --- /dev/null +++ b/example_17/package.xml @@ -0,0 +1,50 @@ + + + + ros2_control_demo_example_17 + 0.0.0 + Demo package of `ros2_control` hardware for RRBot with Hardware Component that publishes diagnostics + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + Sai Kishor Kothakota + + Apache-2.0 + + Soham Patil + + ament_cmake + ros2_control_cmake + + backward_ros + diagnostic_updater + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + controller_manager + forward_command_controller + joint_state_broadcaster + joint_state_publisher_gui + joint_trajectory_controller + robot_state_publisher + ros2_control_demo_description + ros2_controllers_test_nodes + ros2controlcli + ros2launch + rqt_joint_trajectory_controller + rviz2 + xacro + + ament_cmake_pytest + launch_testing + launch + liburdfdom-tools + rclpy + + + ament_cmake + + diff --git a/example_17/ros2_control_demo_example_17.xml b/example_17/ros2_control_demo_example_17.xml new file mode 100644 index 000000000..3da14cce6 --- /dev/null +++ b/example_17/ros2_control_demo_example_17.xml @@ -0,0 +1,9 @@ + + + + The ros2_control RRbot example using a system hardware interface-type what publishes diagnostics. + + + diff --git a/example_17/test/test_rrbot_launch.py b/example_17/test/test_rrbot_launch.py new file mode 100644 index 000000000..44c8f9edd --- /dev/null +++ b/example_17/test/test_rrbot_launch.py @@ -0,0 +1,155 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import os +import pytest +import unittest +import subprocess + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +import launch_testing.markers +import rclpy +from controller_manager.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_17"), + "launch/rrbot.launch.py", + ) + ), + launch_arguments={"gui": "False"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_node") + + def tearDown(self): + self.node.destroy_node() + + def test_node_start(self, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + + cnames = ["forward_position_controller", "joint_state_broadcaster"] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["joint1", "joint2"]) + + +class TestFixtureCLI(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_node") + + def tearDown(self): + self.node.destroy_node() + + def test_main(self, proc_output): + + # Command to run the CLI + cname = "joint_trajectory_position_controller" + command = [ + "ros2", + "control", + "load_controller", + cname, + os.path.join( + get_package_share_directory("ros2_control_demo_example_17"), + "config/rrbot_jtc.yaml", + ), + ] + subprocess.run(command, check=True) + check_controllers_running(self.node, [cname], state="unconfigured") + check_controllers_running(self.node, ["forward_position_controller"], state="active") + + command = ["ros2", "control", "set_controller_state", cname, "inactive"] + subprocess.run(command, check=True) + check_controllers_running(self.node, [cname], state="inactive") + check_controllers_running(self.node, ["forward_position_controller"], state="active") + + command = [ + "ros2", + "control", + "set_controller_state", + "forward_position_controller", + "inactive", + ] + subprocess.run(command, check=True) + command = ["ros2", "control", "set_controller_state", cname, "active"] + subprocess.run(command, check=True) + check_controllers_running(self.node, ["forward_position_controller"], state="inactive") + check_controllers_running(self.node, [cname], state="active") + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shutdown. +class TestShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/example_17/test/test_rrbot_launch_cli_direct.py b/example_17/test/test_rrbot_launch_cli_direct.py new file mode 100644 index 000000000..fc14d6cb0 --- /dev/null +++ b/example_17/test/test_rrbot_launch_cli_direct.py @@ -0,0 +1,118 @@ +# Copyright (c) 2024 AIT - Austrian Institute of Technology GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Christoph Froehlich + +import os +import pytest +import unittest +import subprocess + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +import launch_testing.markers +import rclpy +from controller_manager.test_utils import check_controllers_running + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_17"), + "launch/rrbot.launch.py", + ) + ), + launch_arguments={"gui": "False"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) + + +class TestFixtureCliDirect(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_node") + + def tearDown(self): + self.node.destroy_node() + + def test_main(self, proc_output): + + # Command to run the CLI + cname = "joint_trajectory_position_controller" + command = [ + "ros2", + "control", + "load_controller", + "--set-state", + "inactive", + cname, + os.path.join( + get_package_share_directory("ros2_control_demo_example_17"), + "config/rrbot_jtc.yaml", + ), + ] + subprocess.run(command, check=True) + check_controllers_running(self.node, [cname], state="inactive") + check_controllers_running(self.node, ["forward_position_controller"], state="active") + + command = [ + "ros2", + "control", + "switch_controllers", + "--activate", + cname, + "--deactivate", + "forward_position_controller", + ] + subprocess.run(command, check=True) + check_controllers_running(self.node, ["forward_position_controller"], state="inactive") + check_controllers_running(self.node, [cname], state="active") + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shutdown. +class TestShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/example_17/test/test_urdf_xacro.py b/example_17/test/test_urdf_xacro.py new file mode 100644 index 000000000..e7f1994f6 --- /dev/null +++ b/example_17/test/test_urdf_xacro.py @@ -0,0 +1,77 @@ +# Copyright (c) 2022 FZI Forschungszentrum Informatik +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Lukas Sackewitz + +import os +import shutil +import subprocess +import tempfile + +from ament_index_python.packages import get_package_share_directory + + +def test_urdf_xacro(): + # General Arguments + description_package = "ros2_control_demo_example_17" + description_file = "rrbot.urdf.xacro" + + description_file_path = os.path.join( + get_package_share_directory(description_package), "urdf", description_file + ) + + (_, tmp_urdf_output_file) = tempfile.mkstemp(suffix=".urdf") + + # Compose `xacro` and `check_urdf` command + xacro_command = ( + f"{shutil.which('xacro')}" f" {description_file_path}" f" > {tmp_urdf_output_file}" + ) + check_urdf_command = f"{shutil.which('check_urdf')} {tmp_urdf_output_file}" + + # Try to call processes but finally remove the temp file + try: + xacro_process = subprocess.run( + xacro_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + + assert xacro_process.returncode == 0, " --- XACRO command failed ---" + + check_urdf_process = subprocess.run( + check_urdf_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + + assert ( + check_urdf_process.returncode == 0 + ), "\n --- URDF check failed! --- \nYour xacro does not unfold into a proper urdf robot description. Please check your xacro file." + + finally: + os.remove(tmp_urdf_output_file) + + +if __name__ == "__main__": + test_urdf_xacro() diff --git a/example_17/test/test_view_robot_launch.py b/example_17/test/test_view_robot_launch.py new file mode 100644 index 000000000..7d37de610 --- /dev/null +++ b/example_17/test/test_view_robot_launch.py @@ -0,0 +1,54 @@ +# Copyright (c) 2022 FZI Forschungszentrum Informatik +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Lukas Sackewitz + +import os +import pytest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_17"), + "launch/view_robot.launch.py", + ) + ), + launch_arguments={"gui": "true"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml index b4845c998..17945244c 100644 --- a/ros2_control_demos/package.xml +++ b/ros2_control_demos/package.xml @@ -30,6 +30,7 @@ ros2_control_demo_example_14 ros2_control_demo_example_15 ros2_control_demo_example_16 + ros2_control_demo_example_17 ament_cmake