[Example 13] Multi-robot system with lifecycle management

ros2_control_demo_bringup/config/three_robots_controllers.yaml

@@ -0,0 +1,118 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    # Decide which hardware component should be started immediately
+    autostart_components:
+      - RRBotSystemPositionOnly
+      - ExternalRRBotFTSensor
+    # Decide which hardware component should start configured
+    autoconfigure_components:
+      - RRBotSystemWithSensor
+
+    # 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

ros2_control_demo_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]

ros2_control_demo_bringup/launch/three_robots.launch.py

@@ -0,0 +1,225 @@
+# Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+#
+# 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 = []
+    declared_arguments.append(
+        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("rrbot_description"),
+                    "urdf",
+                    "three_robots.urdf.xacro",
+                ]
+            ),
+            " slowdown:=",
+            slowdown,
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("ros2_control_demo_bringup"),
+            "config",
+            "three_robots_controllers.yaml",
+        ]
+    )
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("rrbot_description"), "config", "three_robots.rviz"]
+    )
+
+    position_goals = PathJoinSubstitution(
+        [
+            FindPackageShare("ros2_control_demo_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",
+            "--stopped",
+        ],
+    )
+    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",
+            "--stopped",
+        ],
+    )
+    threedofbot_position_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["threedofbot_position_controller", "-c", "/controller_manager", "--stopped"],
+    )
+    threedofbot_pid_gain_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["threedofbot_pid_gain_controller", "-c", "/controller_manager", "--stopped"],
+    )
+
+    # Command publishers
+    rrbot_position_command_publisher = Node(
+        package="ros2_control_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_control_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_control_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)