Fix multi-robot demo (partially)

README.md

@@ -596,7 +596,7 @@ ros2 topic echo /fts_broadcaster/wrench
   - `rrbot_position_controller[forward_command_controller/ForwardCommandController]`
 
 - ExternalRRBotFTSensor
-  - `external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]`
+  - `rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]`
 
 - RRBotSystemPositionOnly
   - `rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]`
@@ -630,37 +630,45 @@ Notes:
        - no interfaces are available
        - all controllers inactive
 
-     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] 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
-      ```
+     Hardware status: (`ros2 control list_hardware_components`)
 
-  1. 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
+     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]
      ```
 
-     Scenario state:
-     - right robot is moving
-     - left robot is moving
-     - middle robot is "broken"
-
-     Hardware status:
      Controllers status: (`ros2 control list_controllers`)
       ```
       joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
@@ -669,13 +677,44 @@ Notes:
       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
+      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
       ```
 
-  1. Configure `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call
+  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
@@ -691,9 +730,10 @@ Notes:
      - middle robot is still "broken"
 
 
-     Hardware status:
-     Controllers status: (`ros2 control list_controllers`)
+     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
@@ -709,22 +749,22 @@ Notes:
   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
+     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 to for comparison
+     Check output for comparison, now the joint_states of `threedofbot` and `rrbot_with_sensor` are broadcasted, too.
      ```
-     ros2 topic echo /joint_states
+     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 still "standing"
+     - middle robot is now still "standing"
 
   1. Activate `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call
      ```
@@ -741,9 +781,10 @@ Notes:
      - left robot is moving
      - middle robot is moving
 
-     Hardware status:
-     Controllers status: (`ros2 control list_controllers`)
+     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
@@ -771,9 +812,10 @@ Notes:
      - left robot is moving
      - middle robot is moving
 
-     Hardware status:
-     Controllers status: (`ros2 control list_controllers`)
+     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
@@ -786,18 +828,21 @@ Notes:
       threedofbot_position_controller[forward_command_controller/ForwardCommandController] active
       ```
 
-  1. Cleanup `RRBotSystemPositionOnly` and its joint state broadcaster.
+  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_position_controller joint_state_broadcaster
+     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/cleanup_hardware_component controller_manager_msgs/srv/CleanupHardwareComponent "{name: RRBotSystemPositionOnly}"
-     ros2 control switch_controllers --start joint_state_broadcaster
+     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):

ros2_control_demo_bringup/config/three_robots_controllers.yaml

@@ -2,13 +2,14 @@ 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
+    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:

ros2_control_demo_bringup/launch/three_robots.launch.py

@@ -1,4 +1,4 @@
-# Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
+# 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.
@@ -25,14 +25,13 @@
 
 def generate_launch_description():
     # Declare arguments
-    declared_arguments = []
-    declared_arguments.append(
+    declared_arguments = [
         DeclareLaunchArgument(
             "slowdown",
             default_value="50.0",
             description="Slowdown factor of the RRbot.",
         )
-    )
+    ]
 
     # Initialize Arguments
     slowdown = LaunchConfiguration("slowdown")
@@ -140,7 +139,7 @@ def generate_launch_description():
             "rrbot_with_sensor_position_controller",
             "-c",
             "/controller_manager",
-            "--stopped",
+            "--inactive",
         ],
     )
     rrbot_with_sensor_fts_broadcaster_spawner = Node(
@@ -157,18 +156,18 @@ def generate_launch_description():
             "threedofbot_joint_state_broadcaster",
             "-c",
             "/controller_manager",
-            "--stopped",
+            "--inactive",
         ],
     )
     threedofbot_position_controller_spawner = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["threedofbot_position_controller", "-c", "/controller_manager", "--stopped"],
+        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", "--stopped"],
+        arguments=["threedofbot_pid_gain_controller", "-c", "/controller_manager", "--inactive"],
     )
 
     # Command publishers