From 9f743c18fc54a480f261feed00c9b4d01f6350a7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 5 Aug 2023 18:35:33 +0000 Subject: [PATCH 1/3] Change yaml to current controller_manager syntax --- .../config/three_robots_controllers.yaml | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/ros2_control_demo_bringup/config/three_robots_controllers.yaml b/ros2_control_demo_bringup/config/three_robots_controllers.yaml index 2e3cbec4..fe3a2d6c 100644 --- a/ros2_control_demo_bringup/config/three_robots_controllers.yaml +++ b/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: From 13daf09500ec3f33c1b2df08653cf54500f7a27f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 5 Aug 2023 18:38:11 +0000 Subject: [PATCH 2/3] Fix syntax of current cli and scripts --- README.md | 135 ++++++++++++------ .../launch/three_robots.launch.py | 8 +- 2 files changed, 94 insertions(+), 49 deletions(-) diff --git a/README.md b/README.md index 5bd7e25b..68a96be6 100644 --- a/README.md +++ b/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): diff --git a/ros2_control_demo_bringup/launch/three_robots.launch.py b/ros2_control_demo_bringup/launch/three_robots.launch.py index 108207d1..b4e17f2e 100644 --- a/ros2_control_demo_bringup/launch/three_robots.launch.py +++ b/ros2_control_demo_bringup/launch/three_robots.launch.py @@ -140,7 +140,7 @@ def generate_launch_description(): "rrbot_with_sensor_position_controller", "-c", "/controller_manager", - "--stopped", + "--inactive", ], ) rrbot_with_sensor_fts_broadcaster_spawner = Node( @@ -157,18 +157,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 From 9fac0836375a2a904c5645b9f0acc6b1241ae0ca Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 5 Aug 2023 18:51:18 +0000 Subject: [PATCH 3/3] Cleanup launch file --- ros2_control_demo_bringup/launch/three_robots.launch.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ros2_control_demo_bringup/launch/three_robots.launch.py b/ros2_control_demo_bringup/launch/three_robots.launch.py index b4e17f2e..5ea9bf84 100644 --- a/ros2_control_demo_bringup/launch/three_robots.launch.py +++ b/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")