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