From bb08ec272c87ba2caa29e173643bff50de626887 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 25 Dec 2023 15:09:33 +0000 Subject: [PATCH] Initial commit for example_13 from old PRs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl Co-authored-by: bailaC --- README.md | 2 +- example_13/CMakeLists.txt | 34 ++ example_13/README.md | 328 +++++++++++++++++ .../config/three_robots_controllers.yaml | 119 ++++++ ...ee_robots_position_command_publishers.yaml | 37 ++ .../bringup/launch/three_robots.launch.py | 225 ++++++++++++ .../threedofbot.ros2_control.xacro | 42 +++ example_13/description/rviz/three_robots.rviz | 340 ++++++++++++++++++ .../description/urdf/three_robots.urdf.xacro | 66 ++++ example_13/package.xml | 37 ++ 10 files changed, 1229 insertions(+), 1 deletion(-) 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/package.xml diff --git a/README.md b/README.md index 62370d87e..971b98830 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ 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 lifecycle management: adjust controllers at runtime"](example_13) * Example 14: ["Modular robots with actuators not providing states and with additional sensors"](example_14) diff --git a/example_13/CMakeLists.txt b/example_13/CMakeLists.txt new file mode 100644 index 000000000..4241a273b --- /dev/null +++ b/example_13/CMakeLists.txt @@ -0,0 +1,34 @@ +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_gtest REQUIRED) +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..a2cbd3904 --- /dev/null +++ b/example_13/README.md @@ -0,0 +1,328 @@ +### Example XX: "Multi-robot system with tools" + +- Launch file: [three_robots.launch.py](ros2_control_demo_bringup/launch/three_robots.launch.py) +- Controllers: [three_robots_controllers.yaml](ros2_control_demo_bringup/config/three_robots_controllers.yaml) +- URDF: [three_robots.urdf.xacro](ros2_control_demo_bringup/config/three_robots.urdf.xacro) +- ros2_control URDF: [three_robots.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/three_robots.ros2_control.xacro) + + +**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**: +- `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. + + +Notes: + 1. After starting the example 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: (`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: (`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` and its position controller. Call + ``` + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemWithSensor + target_state: + id: 0 + label: active" + 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 in state active + Controllers status, `rrbot_with_sensor_position_controller` is now active: + ``` + $ 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 + ``` + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: FakeThreeDofBot + target_state: + id: 0 + label: 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. + Controllers status, `threedofbot_joint_state_broadcaster` and `threedofbot_pid_gain_controller` are in active state now: + ``` + $ 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 + ``` + + 1. Restart global joint state broadcaster to broadcast all available states from the framework. + First check output to have comparison: + ``` + ros2 topic echo /joint_states --once + ``` + Restart: + ``` + 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. + ``` + 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" + + 1. Activate `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call + ``` + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: FakeThreeDofBot + target_state: + id: 0 + label: 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. + Controllers status (all active now): + ``` + $ 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 + ``` + + 1. Deactivate `RRBotSystemPositionOnly` and its position controller (first). Call + ``` + ros2 control switch_controllers --deactivate rrbot_position_controller + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: inactive" + ``` + + Scenario state: + - right robot is "standing" + - left robot is moving + - middle robot is moving + + Hardware status: `RRBotSystemPositionOnly` is in inactive state. + Controllers status: `rrbot_position_controller` is now in inactive state + ``` + $ 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 + ``` + + 1. Set `RRBotSystemPositionOnly` in unconfigured state, and deactivate its joint state broadcaster. + Also restart global joint state broadcaster. Call + ``` + ros2 control switch_controllers --deactivate rrbot_joint_state_broadcaster joint_state_broadcaster + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: unconfigured" + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: finalized" + ros2 control switch_controllers --activate joint_state_broadcaster + ``` + + Scenario state (everything is broken during `joint_state_broadcaster` restart): + - right robot is "broken" + - left robot is moving + - middle robot is moving + + Hardware status: + Controllers status: (`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 + ``` 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..fe3a2d6ca --- /dev/null +++ b/example_13/bringup/config/three_robots_controllers.yaml @@ -0,0 +1,119 @@ +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 + + +# RRBot controllers +rrbot_joint_state_broadcaster: + ros__parameters: + 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: tool_link + + +# RRBot with sensor controllers +rrbot_with_sensor_joint_state_broadcaster: + ros__parameters: + 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: tool_link + + +# ThreeDofBot controllers +threedofbot_joint_state_broadcaster: + ros__parameters: + 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..512931a1f --- /dev/null +++ b/example_13/bringup/config/three_robots_position_command_publishers.yaml @@ -0,0 +1,37 @@ +rrbot_position_command_publisher: + ros__parameters: + + controller_name: "rrbot_position_controller" + wait_sec_between_publish: 5 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0, 0] + pos3: [-0.785, -0.785] + pos4: [0, 0] + + +rrbot_with_sensor_position_command_publisher: + ros__parameters: + + controller_name: "rrbot_with_sensor_position_controller" + wait_sec_between_publish: 4 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0, 0] + pos3: [-0.785, -0.785] + pos4: [0, 0] + + +threedofbot_position_command_publisher: + ros__parameters: + + controller_name: "threedofbot_position_controller" + wait_sec_between_publish: 3 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785, 0.785] + pos2: [0, 0, 0] + pos3: [-0.785, -0.785, -0.785] + pos4: [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..c9c731974 --- /dev/null +++ b/example_13/bringup/launch/three_robots.launch.py @@ -0,0 +1,225 @@ +# 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.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 = [ + DeclareLaunchArgument( + "slowdown", + default_value="50.0", + description="Slowdown factor of the RRbot.", + ) + ] + + # Initialize Arguments + slowdown = LaunchConfiguration("slowdown") + + # 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"), + "bringup", + "config", + "three_robots_position_command_publishers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + remappings=[ + ("/joint_state_broadcaster/joint_states", "joint_states"), + ], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + 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], + ) + + # 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", "--controller-manager", "/controller_manager"], + ) + + # RRBot controllers + rrbot_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_joint_state_broadcaster", "-c", "/controller_manager"], + ) + rrbot_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_position_controller", "-c", "/controller_manager"], + ) + # External FTS broadcaster + rrbot_external_fts_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_external_fts_broadcaster", "-c", "/controller_manager"], + ) + + # RRBot controllers + rrbot_with_sensor_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_with_sensor_joint_state_broadcaster", "-c", "/controller_manager"], + ) + rrbot_with_sensor_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "rrbot_with_sensor_position_controller", + "-c", + "/controller_manager", + "--inactive", + ], + ) + rrbot_with_sensor_fts_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["rrbot_with_sensor_fts_broadcaster", "-c", "/controller_manager"], + ) + + # 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", "-c", "/controller_manager", "--inactive"], + ) + threedofbot_pid_gain_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["threedofbot_pid_gain_controller", "-c", "/controller_manager", "--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], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + 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], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + threedofbot_position_command_publisher = Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="threedofbot_position_command_publisher", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + 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..aa589c99f --- /dev/null +++ b/example_13/description/rviz/three_robots.rviz @@ -0,0 +1,340 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 1110 + - 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_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_hokuyo_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_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_with_sensor_hokuyo_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_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + threedofbot_hokuyo_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 + Name: All Bots + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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: /rrobt_robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RRBotSystemPositionOnly + TF Prefix: rrbot_ + Update Interval: 0 + Value: false + Visual Enabled: 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: /rrbot_with_sensor_robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RRBotWithSensor + TF Prefix: rrbot_with_sensor_ + Update Interval: 0 + Value: false + Visual Enabled: 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: /threedofbot_robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: ThreeDofBot + TF Prefix: threedofbot_ + Update Interval: 0 + Value: false + Visual Enabled: 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 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /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 + 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: 1381 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004fafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000004fa000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004fafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000044000004fa000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004fa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 30 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/package.xml b/example_13/package.xml new file mode 100644 index 000000000..6f6640f25 --- /dev/null +++ b/example_13/package.xml @@ -0,0 +1,37 @@ + + + + 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 + + rrbot_description + r3bot_description + ros2_control_demo_example_4 + ros2_control_demo_example_5 + rviz2 + xacro + ros2_controllers_test_nodes + robot_state_publisher + + force_torque_sensor_broadcaster + forward_command_controller + joint_state_broadcaster + joint_trajectory_controller + joint_state_publisher_gui + + + ament_cmake + +