4242 LaunchConfiguration ,
4343 NotSubstitution ,
4444 PathJoinSubstitution ,
45- PythonExpression ,
4645)
4746from launch_ros .actions import Node
4847from launch_ros .parameter_descriptions import ParameterFile
@@ -146,7 +145,7 @@ def launch_setup(context):
146145 )
147146
148147 # Spawn controllers
149- def controller_spawner (controllers , condition , active = True ):
148+ def controller_spawner (controllers , active = True ):
150149 inactive_flags = ["--inactive" ] if not active else []
151150 return Node (
152151 package = "controller_manager" ,
@@ -158,61 +157,28 @@ def controller_spawner(controllers, condition, active=True):
158157 controller_spawner_timeout ,
159158 ]
160159 + inactive_flags
161- + [controllers ],
162- condition = condition ,
160+ + controllers ,
163161 )
164162
165- controllers_active_default = [
163+ controllers_active = [
166164 "joint_state_broadcaster" ,
167165 "io_and_status_controller" ,
168166 "speed_scaling_state_broadcaster" ,
169167 "force_torque_sensor_broadcaster" ,
170168 ]
171- controllers_inactive_default = [
169+ controllers_inactive = [
172170 "scaled_joint_trajectory_controller" ,
173171 "joint_trajectory_controller" ,
174172 "forward_velocity_controller" ,
175173 "forward_position_controller" ,
176174 ]
177- controllers_active_init = PythonExpression (
178- [
179- "'" ,
180- " " .join (controllers_active_default ),
181- "' + ' ' +'" ,
182- initial_joint_controller ,
183- "'" ,
184- ]
185- )
186- controllers_inactive_init = PythonExpression (
187- [
188- "' '.join([c for c in " ,
189- str (controllers_inactive_default ),
190- "if c != '" ,
191- initial_joint_controller ,
192- "'])" ,
193- ]
194- )
195- print (controllers_active_init .perform (context ))
196- print (controllers_inactive_init .perform (context ))
175+ if activate_joint_controller .perform (context ) == "true" :
176+ controllers_active .append (initial_joint_controller .perform (context ))
177+ controllers_inactive .remove (initial_joint_controller .perform (context ))
197178
198179 controller_spawners = [
199- controller_spawner (
200- controllers_active_init , condition = IfCondition (activate_joint_controller )
201- ),
202- controller_spawner (
203- controllers_inactive_init ,
204- condition = IfCondition (activate_joint_controller ),
205- active = False ,
206- ),
207- controller_spawner (
208- " " .join (controllers_active_default ),
209- condition = UnlessCondition (activate_joint_controller ),
210- ),
211- controller_spawner (
212- " " .join (controllers_inactive_default ),
213- condition = UnlessCondition (activate_joint_controller ),
214- active = False ,
215- ),
180+ controller_spawner (controllers_active ),
181+ controller_spawner (controllers_inactive , active = False ),
216182 ]
217183
218184 rsp = IncludeLaunchDescription (
0 commit comments