From 0662e18a8140783a24dbfe056112a3cf442876e1 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 31 Oct 2024 15:59:22 +0100 Subject: [PATCH] [Example 13] Multi-robot system with lifecycle management (#417) (#606) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add description for r3bot Co-authored-by: Denis Štogl Co-authored-by: bailaC * Remove camera links etc * Initial commit for example_13 from old PRs Co-authored-by: Denis Štogl Co-authored-by: bailaC * Change MD to rst syntax * Update test node syntax * Fix test nodes launch file * Enhance docs * Fix wrench config * Use global joint_state_broadcaster * Add tests * FIx links to files * Add some text * Fix dependencies * Add example13 to workflows+packages * Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota * Update example_13/doc/userdoc.rst Co-authored-by: Sai Kishor Kothakota * Fix rst syntax * Use new ros2controlcli verb * Add initial version for launch_test * Add a more complete test * Fix whitespaces * Fix decimal numbers Co-authored-by: Sai Kishor Kothakota * Fix more decimal numbers Co-authored-by: Sai Kishor Kothakota --------- Co-authored-by: Denis Štogl Co-authored-by: bailaC Co-authored-by: Sai Kishor Kothakota Co-authored-by: Sai Kishor Kothakota (cherry picked from commit d6e730f4e8871e6a66a9339c71f765f2efd36563) Co-authored-by: Christoph Fröhlich --- README.md | 4 +- doc/index.rst | 4 +- example_13/CMakeLists.txt | 38 ++ example_13/README.md | 5 + .../config/three_robots_controllers.yaml | 125 +++++ ...ee_robots_position_command_publishers.yaml | 37 ++ .../bringup/launch/three_robots.launch.py | 215 ++++++++ .../threedofbot.ros2_control.xacro | 42 ++ example_13/description/rviz/three_robots.rviz | 257 ++++++++++ .../description/urdf/three_robots.urdf.xacro | 66 +++ example_13/doc/userdoc.rst | 471 ++++++++++++++++++ example_13/package.xml | 42 ++ example_13/test/test_three_robots_launch.py | 349 +++++++++++++ example_13/test/test_urdf_xacro.py | 77 +++ example_13/test/test_view_robot_launch.py | 54 ++ ros2_control_demo_description/CMakeLists.txt | 5 + ros2_control_demo_description/package.xml | 3 +- .../urdf/threedofbot_description.urdf.xacro | 166 ++++++ ros2_control_demos/package.xml | 1 + 19 files changed, 1957 insertions(+), 4 deletions(-) create mode 100644 example_13/CMakeLists.txt create mode 100644 example_13/README.md create mode 100644 example_13/bringup/config/three_robots_controllers.yaml create mode 100644 example_13/bringup/config/three_robots_position_command_publishers.yaml create mode 100644 example_13/bringup/launch/three_robots.launch.py create mode 100644 example_13/description/ros2_control/threedofbot.ros2_control.xacro create mode 100644 example_13/description/rviz/three_robots.rviz create mode 100644 example_13/description/urdf/three_robots.urdf.xacro create mode 100644 example_13/doc/userdoc.rst create mode 100644 example_13/package.xml create mode 100644 example_13/test/test_three_robots_launch.py create mode 100644 example_13/test/test_urdf_xacro.py create mode 100644 example_13/test/test_view_robot_launch.py create mode 100644 ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro diff --git a/README.md b/README.md index e4157d8f0..452064638 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,9 @@ The following examples are part of this demo repository: The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. -* Example 13: "Multi-robot example (tba.)" +* Example 13: ["Multi-robot system with hardware lifecycle management"](example_13) + + This example shows how to handle multiple robots in a single controller manager instance. * Example 14: ["Modular robots with actuators not providing states and with additional sensors"](example_14) diff --git a/doc/index.rst b/doc/index.rst index 00083458f..9f2957c93 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -76,7 +76,8 @@ Example 11: "CarlikeBot" 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*. -Example 13: "Multi-robot example (tba.)" +Example 13: "Multi-robot system with hardware lifecycle management" + This example shows how to handle multiple robots in a single controller manager instance. Example 14: "Modular robots with actuators not providing states and with additional sensors" The example shows how to implement robot hardware with actuators not providing states and with additional sensors. @@ -276,5 +277,6 @@ Examples Example 10: Industrial robot with GPIO interfaces <../example_10/doc/userdoc.rst> Example 11: CarlikeBot <../example_11/doc/userdoc.rst> Example 12: Controller chaining <../example_12/doc/userdoc.rst> + Example 13: Multiple robots <../example_13/doc/userdoc.rst> 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> diff --git a/example_13/CMakeLists.txt b/example_13/CMakeLists.txt new file mode 100644 index 000000000..0f8ae8643 --- /dev/null +++ b/example_13/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_13 LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# INSTALL +install( + DIRECTORY description/ros2_control description/urdf description/rviz + DESTINATION share/ros2_control_demo_example_13 +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/ros2_control_demo_example_13 +) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(example_13_urdf_xacro test/test_urdf_xacro.py) + ament_add_pytest_test(view_example_13_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_13_launch test/test_three_robots_launch.py) +endif() + +## EXPORTS +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/example_13/README.md b/example_13/README.md new file mode 100644 index 000000000..54b3d9c3a --- /dev/null +++ b/example_13/README.md @@ -0,0 +1,5 @@ +# ros2_control_demo_example_13 + + This example shows how to handle multiple robots in a single controller manager instance. + +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_13/doc/userdoc.html). diff --git a/example_13/bringup/config/three_robots_controllers.yaml b/example_13/bringup/config/three_robots_controllers.yaml new file mode 100644 index 000000000..2a534cade --- /dev/null +++ b/example_13/bringup/config/three_robots_controllers.yaml @@ -0,0 +1,125 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + hardware_components_initial_state: + unconfigured: + # Decide which hardware component will be only loaded + - FakeThreeDofBot + inactive: + # Decide which hardware component will start configured + - RRBotSystemWithSensor + # not listed hardware component should be started immediately + + # Global controllers + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + # RRBot controllers + rrbot_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + rrbot_position_controller: + type: forward_command_controller/ForwardCommandController + + rrbot_external_fts_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + # RRBot with sensor controllers + rrbot_with_sensor_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + rrbot_with_sensor_position_controller: + type: forward_command_controller/ForwardCommandController + + rrbot_with_sensor_fts_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + # ThreeDofBot controllers + threedofbot_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + threedofbot_position_controller: + type: forward_command_controller/ForwardCommandController + + threedofbot_pid_gain_controller: + type: forward_command_controller/ForwardCommandController + +# global joint_state_broadcaster +# joint_state_broadcaster: +# ros__parameters: + # nothing to configure + +# RRBot controllers +rrbot_joint_state_broadcaster: + ros__parameters: + use_local_topics: True + joints: + - rrbot_joint1 + - rrbot_joint2 + interfaces: + - position + +rrbot_position_controller: + ros__parameters: + joints: + - rrbot_joint1 + - rrbot_joint2 + interface_name: position + +rrbot_external_fts_broadcaster: + ros__parameters: + sensor_name: rrbot_tcp_fts_sensor + frame_id: rrbot_tool_link + + +# RRBot with sensor controllers +rrbot_with_sensor_joint_state_broadcaster: + ros__parameters: + use_local_topics: True + joints: + - rrbot_with_sensor_joint1 + - rrbot_with_sensor_joint2 + interfaces: + - position + +rrbot_with_sensor_position_controller: + ros__parameters: + joints: + - rrbot_with_sensor_joint1 + - rrbot_with_sensor_joint2 + interface_name: position + +rrbot_with_sensor_fts_broadcaster: + ros__parameters: + interface_names.force.x: rrbot_with_sensor_tcp_fts_sensor/force.x + interface_names.torque.z: rrbot_with_sensor_tcp_fts_sensor/torque.z + frame_id: rrbot_with_sensor_tool_link + +# ThreeDofBot controllers +threedofbot_joint_state_broadcaster: + ros__parameters: + use_local_topics: True + joints: + - threedofbot_joint1 + - threedofbot_joint2 + - threedofbot_joint3 + interfaces: + - position + - pid_gain + +threedofbot_position_controller: + ros__parameters: + joints: + - threedofbot_joint1 + - threedofbot_joint2 + - threedofbot_joint3 + interface_name: position + +threedofbot_pid_gain_controller: + ros__parameters: + joints: + - threedofbot_joint1 + - threedofbot_joint2 + - threedofbot_joint3 + interface_name: pid_gain diff --git a/example_13/bringup/config/three_robots_position_command_publishers.yaml b/example_13/bringup/config/three_robots_position_command_publishers.yaml new file mode 100644 index 000000000..e06062d1e --- /dev/null +++ b/example_13/bringup/config/three_robots_position_command_publishers.yaml @@ -0,0 +1,37 @@ +rrbot_position_command_publisher: + ros__parameters: + + publish_topic: "/rrbot_position_controller/commands" + wait_sec_between_publish: 5 + + 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] + + +rrbot_with_sensor_position_command_publisher: + ros__parameters: + + publish_topic: "/rrbot_with_sensor_position_controller/commands" + wait_sec_between_publish: 4 + + 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] + + +threedofbot_position_command_publisher: + ros__parameters: + + publish_topic: "/threedofbot_position_controller/commands" + wait_sec_between_publish: 3 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785, 0.785] + pos2: [0.0, 0.0, 0.0] + pos3: [-0.785, -0.785, -0.785] + pos4: [0.0, 0.0, 0.0] diff --git a/example_13/bringup/launch/three_robots.launch.py b/example_13/bringup/launch/three_robots.launch.py new file mode 100644 index 000000000..92a309b48 --- /dev/null +++ b/example_13/bringup/launch/three_robots.launch.py @@ -0,0 +1,215 @@ +# Copyright (c) 2022, 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. +# +# +# Authors: Denis Stogl + +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( + "slowdown", + default_value="50.0", + description="Slowdown factor of the RRbot.", + ) + ) + # Declare arguments + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + + # Initialize Arguments + slowdown = LaunchConfiguration("slowdown") + gui = LaunchConfiguration("gui") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_13"), + "urdf", + "three_robots.urdf.xacro", + ] + ), + " slowdown:=", + slowdown, + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_13"), + "config", + "three_robots_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_13"), "rviz", "three_robots.rviz"] + ) + + position_goals = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_13"), + "config", + "three_robots_position_command_publishers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + ) + 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), + ) + + # Separate robot state publishers for each robot + + # Global joint state broadcaster + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ) + + # RRBot controllers + rrbot_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_joint_state_broadcaster"], + ) + rrbot_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_position_controller"], + ) + # External FTS broadcaster + rrbot_external_fts_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_external_fts_broadcaster"], + ) + + # RRBot controllers + rrbot_with_sensor_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_with_sensor_joint_state_broadcaster"], + ) + rrbot_with_sensor_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "rrbot_with_sensor_position_controller", + "--inactive", + ], + ) + rrbot_with_sensor_fts_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_with_sensor_fts_broadcaster"], + ) + + # ThreeDofBot controllers + threedofbot_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "threedofbot_joint_state_broadcaster", + "-c", + "/controller_manager", + "--inactive", + ], + ) + threedofbot_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["threedofbot_position_controller", "--inactive"], + ) + threedofbot_pid_gain_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["threedofbot_pid_gain_controller", "--inactive"], + ) + + # Command publishers + rrbot_position_command_publisher = Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="rrbot_position_command_publisher", + parameters=[position_goals], + ) + rrbot_with_sensor_position_command_publisher = Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="rrbot_with_sensor_position_command_publisher", + parameters=[position_goals], + ) + threedofbot_position_command_publisher = Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="threedofbot_position_command_publisher", + parameters=[position_goals], + ) + + nodes = [ + control_node, + robot_state_pub_node, + rviz_node, + joint_state_broadcaster_spawner, + rrbot_joint_state_broadcaster_spawner, + rrbot_position_controller_spawner, + rrbot_external_fts_broadcaster_spawner, + rrbot_with_sensor_joint_state_broadcaster_spawner, + rrbot_with_sensor_position_controller_spawner, + rrbot_with_sensor_fts_broadcaster_spawner, + threedofbot_joint_state_broadcaster_spawner, + threedofbot_position_controller_spawner, + threedofbot_pid_gain_controller_spawner, + rrbot_position_command_publisher, + rrbot_with_sensor_position_command_publisher, + threedofbot_position_command_publisher, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_13/description/ros2_control/threedofbot.ros2_control.xacro b/example_13/description/ros2_control/threedofbot.ros2_control.xacro new file mode 100644 index 000000000..95c072a25 --- /dev/null +++ b/example_13/description/ros2_control/threedofbot.ros2_control.xacro @@ -0,0 +1,42 @@ + + + + + + + + mock_components/GenericSystem + + + + + -1 + 1 + + + + + + + + -1 + 1 + + + + + + + + -1 + 1 + + + + + + + + + + diff --git a/example_13/description/rviz/three_robots.rviz b/example_13/description/rviz/three_robots.rviz new file mode 100644 index 000000000..747e1d50e --- /dev/null +++ b/example_13/description/rviz/three_robots.rviz @@ -0,0 +1,257 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Wrench2/Topic1 + Splitter Ratio: 0.5 + Tree Height: 784 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + rrbot_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_with_sensor_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + threedofbot_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: All Bots + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Accept NaN Values: false + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrbot_external_fts_broadcaster/wrench + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Accept NaN Values: true + Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrbot_with_sensor_fts_broadcaster/wrench + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 6.157698154449463 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.23817427456378937 + Y: -0.0642336755990982 + Z: 2.174346685409546 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3603994846343994 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.690403461456299 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000003a2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005c8000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/example_13/description/urdf/three_robots.urdf.xacro b/example_13/description/urdf/three_robots.urdf.xacro new file mode 100644 index 000000000..68911d3e6 --- /dev/null +++ b/example_13/description/urdf/three_robots.urdf.xacro @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_13/doc/userdoc.rst b/example_13/doc/userdoc.rst new file mode 100644 index 000000000..b09fce9b6 --- /dev/null +++ b/example_13/doc/userdoc.rst @@ -0,0 +1,471 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_13/doc/userdoc.rst + +.. _ros2_control_demos_example_13_userdoc: + +Example 13: Multi-robot system with lifecycle management +========================================================== + +This example shows how to include multiple robots in a single controller manager instance. +Additionally, hardware lifecycle management is demonstrated. + +Hardware and interfaces +------------------------- + +- RRBotSystemPositionOnly (auto-start) + + - Command interfaces: + + - rrbot_joint1/position + - rrbot_joint2/position + + - State interfaces: + + - rrbot_joint1/position + - rrbot_joint2/position + +- ExternalRRBotFTSensor (auto-start) + + - State interfaces: + + - rrbot_tcp_fts_sensor/force.x + - rrbot_tcp_fts_sensor/force.y + - rrbot_tcp_fts_sensor/force.z + - rrbot_tcp_fts_sensor/torque.x + - rrbot_tcp_fts_sensor/torque.y + - rrbot_tcp_fts_sensor/torque.z + +- RRBotSystemWithSensor (auto-configure) + + - Command interfaces: + + - rrbot_with_sensor_joint1/position + - rrbot_with_sensor_joint2/position + + - State interfaces: + + - rrbot_with_sensor_joint1/position + - rrbot_with_sensor_joint2/position + - rrbot_with_sensor_tcp_fts_sensor/force.x + - rrbot_with_sensor_tcp_fts_sensor/torque.z + +- ThreeDofBot + + - Command interfaces + + - threedofbot_joint1/position + - threedofbot_joint1/pid_gain + - threedofbot_joint2/position + - threedofbot_joint2/pid_gain + - threedofbot_joint3/position + - threedofbot_joint3/pid_gain + + - State interfaces: + + - threedofbot_joint1/position + - threedofbot_joint1/pid_gain + - threedofbot_joint2/position + - threedofbot_joint2/pid_gain + - threedofbot_joint3/position + - threedofbot_joint3/pid_gain + +Available controllers +------------------------- + +- Global + + - ``joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`` + +- RRBotSystemPositionOnly + + - ``rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`` + - ``rrbot_position_controller[forward_command_controller/ForwardCommandController]`` + +- ExternalRRBotFTSensor + + - ``rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]`` + +- RRBotSystemPositionOnly + + - ``rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`` + - ``rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController]`` + - ``rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]`` + +- FakeThreeDofBot + + - ``threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`` + - ``threedofbot_position_controller[forward_command_controller/ForwardCommandController]`` + - ``threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController]`` + + +Caveats on hardware lifecycling +-------------------------------- + +- There is currently no synchronization between available interface and controllers using them. + This means that you should stop controller before making interfaces they are using unavailable. + If you don't do this and deactivate/cleanup your interface first your computer will catch fire! +- Global Joint State Broadcaster will not broadcast interfaces that become available after it is started. + To solve this restart it manually, for now. During restart TF-transforms are not available. +- There is a possibility that hardware lifecycling (state changes) interfere with the ``update``-loop + if you are trying to start/stop a controller at the same time. + + +Tutorial steps +-------------------------- + +.. include:: ../../doc/run_from_docker.rst + +1. After starting the example with + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_13 three_robots.launch.py + + there should be the following scene: + + - right robot is moving (RRBotSystemPositionOnly - using auto-start) + + - All interfaces are available and position controller is started and receives commands + - all controllers running + + - left robot is standing upright (RRBotWithSensor - using auto-configure) + + - only state interfaces are available therefore it can visualized, but not moved + - only position command controller is not running + + - middle robot is "broken" (FakeThreeDofBot - it is only initialized) + + - no interfaces are available + - all controllers inactive + + Hardware status: + + .. code-block:: shell + + $ ros2 control list_hardware_components + Hardware Component 1 + name: FakeThreeDofBot + type: system + plugin name: mock_components/GenericSystem + state: id=1 label=unconfigured + command interfaces + threedofbot_joint1/position [unavailable] [unclaimed] + threedofbot_joint1/pid_gain [unavailable] [unclaimed] + threedofbot_joint2/position [unavailable] [unclaimed] + threedofbot_joint2/pid_gain [unavailable] [unclaimed] + threedofbot_joint3/position [unavailable] [unclaimed] + threedofbot_joint3/pid_gain [unavailable] [unclaimed] + Hardware Component 2 + name: RRBotSystemWithSensor + type: system + plugin name: ros2_control_demo_hardware/RRBotSystemWithSensorHardware + state: id=2 label=inactive + command interfaces + rrbot_with_sensor_joint1/position [available] [unclaimed] + rrbot_with_sensor_joint2/position [available] [unclaimed] + Hardware Component 3 + name: ExternalRRBotFTSensor + type: sensor + plugin name: ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware + state: id=3 label=active + command interfaces + Hardware Component 4 + name: RRBotSystemPositionOnly + type: system + plugin name: ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + state: id=3 label=active + command interfaces + rrbot_joint1/position [available] [claimed] + rrbot_joint2/position [available] [claimed] + + Controllers status: + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + + +2. Activate ``RRBotWithSensor`` hardware component. Use either the ros2controlcli + + .. code-block:: shell + + ros2 control set_hardware_component_state RRBotSystemWithSensor active + + or call the service manually + + .. code-block:: shell + + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemWithSensor + target_state: + id: 0 + label: active" + + Then activate its position controller via + + .. code-block:: shell + + ros2 control switch_controllers --activate rrbot_with_sensor_position_controller + + Scenario state: + + - right robot is moving + - left robot is moving + - middle robot is "broken" + + Hardware status: ``RRBotSystemWithSensor`` is now in active state + + .. code-block:: shell + + $ ros2 control list_hardware_components + ... + Hardware Component 2 + name: RRBotSystemWithSensor + type: system + plugin name: ros2_control_demo_example_4/RRBotSystemWithSensorHardware + state: id=3 label=active + command interfaces + rrbot_with_sensor_joint1/position [available] [claimed] + rrbot_with_sensor_joint2/position [available] [claimed] + ... + + Controllers status: ``rrbot_with_sensor_position_controller`` is now in active state: + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + + +3. Configure ``FakeThreeDofBot`` and its joint state broadcaster and non-movement command interfaces. Call + + .. code-block:: shell + + ros2 control set_hardware_component_state FakeThreeDofBot inactive + ros2 control switch_controllers --activate threedofbot_joint_state_broadcaster threedofbot_pid_gain_controller + + Scenario state: + + - right robot is moving + - left robot is moving + - middle robot is still "broken" + + Hardware status: ``FakeThreeDofBot`` is in inactive state. + + .. code-block:: shell + + $ ros2 control list_hardware_components + Hardware Component 1 + name: FakeThreeDofBot + type: system + plugin name: mock_components/GenericSystem + state: id=2 label=inactive + command interfaces + threedofbot_joint1/position [available] [unclaimed] + threedofbot_joint1/pid_gain [available] [claimed] + threedofbot_joint2/position [available] [unclaimed] + threedofbot_joint2/pid_gain [available] [claimed] + threedofbot_joint3/position [available] [unclaimed] + threedofbot_joint3/pid_gain [available] [claimed] + ... + + Controllers status, ``threedofbot_joint_state_broadcaster`` and ``threedofbot_pid_gain_controller`` are in active state now: + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + + +4. Restart global joint state broadcaster to broadcast all available states from the framework. First check output to have comparison: + + .. code-block:: shell + + ros2 topic echo /joint_states --once + + Restart: + + .. code-block:: shell + + ros2 control switch_controllers --deactivate joint_state_broadcaster + ros2 control switch_controllers --activate joint_state_broadcaster + + Check output for comparison, now the joint_states of ``threedofbot`` and ``rrbot_with_sensor`` are broadcasted, too. + + .. code-block:: shell + + ros2 topic echo /joint_states --once + + Scenario state (everything is broken during ``joint_state_broadcaster`` restart): + + - right robot is moving + - left robot is moving + - middle robot is now still "standing" + +5. Activate ``FakeThreeDofBot`` and its controller. Call + + .. code-block:: shell + + ros2 control set_hardware_component_state FakeThreeDofBot active + ros2 control switch_controllers --activate threedofbot_position_controller + + Scenario state: + + - right robot is moving + - left robot is moving + - middle robot is moving + + Hardware status: ``FakeThreeDofBot`` is in active state. + + .. code-block:: shell + + $ ros2 control list_hardware_components + Hardware Component 1 + name: FakeThreeDofBot + type: system + plugin name: mock_components/GenericSystem + state: id=3 label=active + command interfaces + threedofbot_joint1/position [available] [claimed] + threedofbot_joint1/pid_gain [available] [claimed] + threedofbot_joint2/position [available] [claimed] + threedofbot_joint2/pid_gain [available] [claimed] + threedofbot_joint3/position [available] [claimed] + threedofbot_joint3/pid_gain [available] [claimed] + ... + + Controllers status (all active now): + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + + +6. Deactivate ``RRBotSystemPositionOnly`` and its position controller (first). Call + + .. code-block:: shell + + ros2 control switch_controllers --deactivate rrbot_position_controller + ros2 control set_hardware_component_state RRBotSystemPositionOnly inactive + + Scenario state: + + - right robot is now "standing" at the last position + - left robot is moving + - middle robot is moving + + Hardware status: ``RRBotSystemPositionOnly`` is in inactive state. + + .. code-block:: shell + + $ ros2 control list_hardware_components + ... + Hardware Component 4 + name: RRBotSystemPositionOnly + type: system + plugin name: ros2_control_demo_example_5/RRBotSystemPositionOnlyHardware + state: id=2 label=inactive + command interfaces + rrbot_joint1/position [available] [unclaimed] + rrbot_joint2/position [available] [unclaimed] + ... + + Controllers status: ``rrbot_position_controller`` is now in inactive state + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] inactive + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + + +7. Set ``RRBotSystemPositionOnly`` in unconfigured state, and deactivate its joint state broadcaster. Also restart global joint state broadcaster. Call + + .. code-block:: shell + + ros2 control switch_controllers --deactivate rrbot_joint_state_broadcaster joint_state_broadcaster + ros2 control set_hardware_component_state RRBotSystemPositionOnly unconfigured + ros2 control switch_controllers --activate joint_state_broadcaster + + Scenario state (everything is broken during ``joint_state_broadcaster`` restart): + + - right robot is standing still. + - left robot is moving + - middle robot is moving + + Controllers status: + + .. code-block:: shell + + $ ros2 control list_controllers + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + rrbot_position_controller[forward_command_controller/ForwardCommandController] inactive + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + +Files used for this demos +------------------------- + +- Launch file: `three_robots.launch.py `__ +- Controllers yaml: `three_robots_controllers.yaml `__ +- URDF file: `three_robots.urdf.xacro `__ + + + Description: `threedofbot_description.urdf.xacro `__ + + ``ros2_control`` tag: `three_robots.ros2_control.xacro `__ +- RViz configuration: `three_robots.rviz `__ + +Controllers from this demo +-------------------------- +- ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ +- ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_13/package.xml b/example_13/package.xml new file mode 100644 index 000000000..0cc8f4a59 --- /dev/null +++ b/example_13/package.xml @@ -0,0 +1,42 @@ + + + + ros2_control_demo_example_13 + 0.0.0 + Demo package of `ros2_control` simulation with multiple robots. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + + Dr.-Ing. Denis Štogl + Christoph Froehlich + + Apache-2.0 + + ament_cmake + + controller_manager + force_torque_sensor_broadcaster + forward_command_controller + joint_state_broadcaster + robot_state_publisher + ros2_control_demo_description + ros2_control_demo_example_4 + ros2_control_demo_example_5 + ros2_controllers_test_nodes + ros2controlcli + ros2launch + rviz2 + xacro + + ament_cmake_pytest + launch_testing_ros + liburdfdom-tools + ros2_control_demo_testing + xacro + + + ament_cmake + + diff --git a/example_13/test/test_three_robots_launch.py b/example_13/test/test_three_robots_launch.py new file mode 100644 index 000000000..8f7460993 --- /dev/null +++ b/example_13/test/test_three_robots_launch.py @@ -0,0 +1,349 @@ +# 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 rclpy.node import Node +from ros2_control_demo_testing.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_13"), + "launch/three_robots.launch.py", + ) + ), + launch_arguments={"gui": "false"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) + + +# This is our first test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +class TestFixture(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = Node("test_node") + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_node_start(self, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_behavior(self, proc_output): + + # --- initial configuration --- + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_position_controller", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + ] + + check_controllers_running(self.node, cnames) + check_if_js_published( + "/joint_states", + [ + "rrbot_with_sensor_joint1", + "rrbot_joint1", + "rrbot_with_sensor_joint2", + "rrbot_joint2", + ], + ) + + # --- activate second robot with its controllers --- + # Command to activate the second robot with its controllers + subprocess.run( + [ + "ros2", + "control", + "set_hardware_component_state", + "RRBotSystemWithSensor", + "active", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--activate", + "rrbot_with_sensor_position_controller", + ] + ) + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_joint_state_broadcaster", + "rrbot_position_controller", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + "rrbot_with_sensor_position_controller", + ] + check_controllers_running(self.node, cnames) + # still the same joint_states + check_if_js_published( + "/joint_states", + [ + "rrbot_with_sensor_joint1", + "rrbot_joint1", + "rrbot_with_sensor_joint2", + "rrbot_joint2", + ], + ) + + # --- configure FakeThreeDofBot its controllers --- + subprocess.run( + [ + "ros2", + "control", + "set_hardware_component_state", + "FakeThreeDofBot", + "inactive", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--activate", + "threedofbot_joint_state_broadcaster", + "threedofbot_pid_gain_controller", + ] + ) + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_joint_state_broadcaster", + "rrbot_position_controller", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + "rrbot_with_sensor_position_controller", + "threedofbot_joint_state_broadcaster", + "threedofbot_pid_gain_controller", + ] + check_controllers_running(self.node, cnames) + # still the same joint_states + check_if_js_published( + "/joint_states", + [ + "rrbot_with_sensor_joint1", + "rrbot_joint1", + "rrbot_with_sensor_joint2", + "rrbot_joint2", + ], + ) + + # --- restart global joint state broadcaster --- + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--deactivate", + "joint_state_broadcaster", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--activate", + "joint_state_broadcaster", + ] + ) + # now the joint_states of threedofbot and rrbot_with_sensor are broadcasted, too. + check_if_js_published( + "/joint_states", + [ + "rrbot_with_sensor_joint1", + "rrbot_joint1", + "rrbot_with_sensor_joint2", + "rrbot_joint2", + "threedofbot_joint1", + "threedofbot_joint2", + "threedofbot_joint3", + ], + ) + + # --- activate FakeThreeDofBot and its controllers --- + subprocess.run( + [ + "ros2", + "control", + "set_hardware_component_state", + "FakeThreeDofBot", + "active", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--activate", + "threedofbot_position_controller", + ] + ) + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_joint_state_broadcaster", + "rrbot_position_controller", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + "rrbot_with_sensor_position_controller", + "threedofbot_joint_state_broadcaster", + "threedofbot_pid_gain_controller", + "threedofbot_position_controller", + ] + check_controllers_running(self.node, cnames) + + # --- deactivate RRBotSystemPositionOnly and its controllers --- + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--deactivate", + "rrbot_position_controller", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "set_hardware_component_state", + "RRBotSystemPositionOnly", + "inactive", + ] + ) + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_joint_state_broadcaster", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + "rrbot_with_sensor_position_controller", + "threedofbot_joint_state_broadcaster", + "threedofbot_pid_gain_controller", + "threedofbot_position_controller", + ] + check_controllers_running(self.node, cnames) + + # --- Set RRBotSystemPositionOnly in unconfigured state, and deactivate its joint state broadcaster. Also restart global joint state broadcaster. --- + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--deactivate", + "rrbot_joint_state_broadcaster", + "joint_state_broadcaster", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "set_hardware_component_state", + "RRBotSystemPositionOnly", + "unconfigured", + ] + ) + subprocess.run( + [ + "ros2", + "control", + "switch_controllers", + "--activate", + "joint_state_broadcaster", + ] + ) + cnames = [ + "joint_state_broadcaster", + "rrbot_external_fts_broadcaster", + "rrbot_with_sensor_joint_state_broadcaster", + "rrbot_with_sensor_fts_broadcaster", + "rrbot_with_sensor_position_controller", + "threedofbot_joint_state_broadcaster", + "threedofbot_pid_gain_controller", + "threedofbot_position_controller", + ] + check_controllers_running(self.node, cnames) + # still all joint_states are published, even if the hardware is in unconfigured state + check_if_js_published( + "/joint_states", + [ + "rrbot_with_sensor_joint1", + "rrbot_joint1", + "rrbot_with_sensor_joint2", + "rrbot_joint2", + "threedofbot_joint1", + "threedofbot_joint2", + "threedofbot_joint3", + ], + ) + + +# TODO(anyone): enable this if shutdown of ros2_control_node does not fail anymore +# @launch_testing.post_shutdown_test() +# # These tests are run after the processes in generate_test_description() have shutdown. +# class TestDescriptionCraneShutdown(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_13/test/test_urdf_xacro.py b/example_13/test/test_urdf_xacro.py new file mode 100644 index 000000000..922daea13 --- /dev/null +++ b/example_13/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_13" + description_file = "three_robots.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_13/test/test_view_robot_launch.py b/example_13/test/test_view_robot_launch.py new file mode 100644 index 000000000..40795e213 --- /dev/null +++ b/example_13/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_13"), + "launch/three_robots.launch.py", + ) + ), + launch_arguments={"gui": "true"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) diff --git a/ros2_control_demo_description/CMakeLists.txt b/ros2_control_demo_description/CMakeLists.txt index ebe8c7f1e..e4d1355cc 100644 --- a/ros2_control_demo_description/CMakeLists.txt +++ b/ros2_control_demo_description/CMakeLists.txt @@ -18,6 +18,11 @@ install( DESTINATION share/${PROJECT_NAME}/carlikebot ) +install( + DIRECTORY r3bot/urdf + DESTINATION share/${PROJECT_NAME}/r3bot +) + install( DIRECTORY r6bot/meshes r6bot/srdf r6bot/urdf r6bot/rviz DESTINATION share/${PROJECT_NAME}/r6bot diff --git a/ros2_control_demo_description/package.xml b/ros2_control_demo_description/package.xml index d43c784f4..5049bd128 100644 --- a/ros2_control_demo_description/package.xml +++ b/ros2_control_demo_description/package.xml @@ -6,13 +6,12 @@ Package with URDF and description files of test robots. Denis Štogl + Christoph Fröhlich Apache-2.0 ament_cmake - joint_state_publisher_gui - robot_state_publisher rviz2 diff --git a/ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro b/ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro new file mode 100644 index 000000000..f7df32f9d --- /dev/null +++ b/ros2_control_demo_description/r3bot/urdf/threedofbot_description.urdf.xacro @@ -0,0 +1,166 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros2_control_demos/package.xml b/ros2_control_demos/package.xml index 68dae5545..32e6925c3 100644 --- a/ros2_control_demos/package.xml +++ b/ros2_control_demos/package.xml @@ -26,6 +26,7 @@ ros2_control_demo_example_10 ros2_control_demo_example_11 ros2_control_demo_example_12 + ros2_control_demo_example_13 ros2_control_demo_example_14 ros2_control_demo_example_15