|
1 | 1 | from launch import LaunchDescription |
2 | | -from launch.actions import RegisterEventHandler |
3 | | -from launch.event_handlers import OnExecutionComplete, OnProcessStart |
4 | 2 | from launch.substitutions import LaunchConfiguration |
5 | 3 | from lbr_bringup.description import LBRDescriptionMixin |
6 | 4 | from lbr_bringup.ros2_control import LBRROS2ControlMixin |
@@ -31,45 +29,16 @@ def generate_launch_description() -> LaunchDescription: |
31 | 29 | ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False) |
32 | 30 | ld.add_action(ros2_control_node) |
33 | 31 |
|
34 | | - # controllers on ros2 control node start |
35 | | - joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( |
36 | | - controller="joint_state_broadcaster" |
37 | | - ) |
38 | | - estimated_wrench_interface = LBRROS2ControlMixin.node_controller_spawner( |
39 | | - controller="estimated_wrench_interface" |
40 | | - ) |
41 | | - lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( |
42 | | - controller="lbr_state_broadcaster" |
43 | | - ) |
44 | | - |
45 | | - preceding_controllers_event_handler = RegisterEventHandler( |
46 | | - OnProcessStart( |
47 | | - target_action=ros2_control_node, |
48 | | - on_start=[ |
49 | | - joint_state_broadcaster, |
50 | | - estimated_wrench_interface, |
51 | | - lbr_state_broadcaster, |
52 | | - ], |
53 | | - ) |
54 | | - ) |
55 | | - ld.add_action(preceding_controllers_event_handler) |
56 | | - |
57 | | - # controllers on estimated wrench interface |
58 | | - force_torque_broadcaster = LBRROS2ControlMixin.node_controller_spawner( |
59 | | - controller="force_torque_broadcaster" |
60 | | - ) |
61 | | - controller = LBRROS2ControlMixin.node_controller_spawner( |
62 | | - controller=LaunchConfiguration("ctrl") |
63 | | - ) |
64 | | - |
65 | | - controller_event_handler = RegisterEventHandler( |
66 | | - OnExecutionComplete( |
67 | | - target_action=estimated_wrench_interface, # estimated wrench interface is chained and exposes external wrench estimate state interfaces |
68 | | - on_completion=[ |
69 | | - force_torque_broadcaster, |
70 | | - controller, |
71 | | - ], |
| 32 | + # spawn controllers |
| 33 | + ld.add_action( |
| 34 | + LBRROS2ControlMixin.node_controller_spawner( |
| 35 | + controllers=[ |
| 36 | + "joint_state_broadcaster", |
| 37 | + "estimated_wrench_interface", |
| 38 | + "lbr_state_broadcaster", |
| 39 | + "force_torque_broadcaster", |
| 40 | + LaunchConfiguration("ctrl"), |
| 41 | + ] |
72 | 42 | ) |
73 | 43 | ) |
74 | | - ld.add_action(controller_event_handler) |
75 | 44 | return ld |
0 commit comments