-
Notifications
You must be signed in to change notification settings - Fork 270
/
spawner.py
479 lines (424 loc) · 17.8 KB
/
spawner.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
#!/usr/bin/env python3
# Copyright 2021 PAL Robotics S.L.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import argparse
import errno
import os
import sys
import time
import warnings
import yaml
from controller_manager import (
configure_controller,
list_controllers,
load_controller,
switch_controllers,
unload_controller,
)
import rclpy
from rcl_interfaces.msg import Parameter
from rclpy.duration import Duration
from rclpy.node import Node
# @note: The versions conditioning is added here to support the source-compatibility with Humble
# The `get_parameter_value` function is moved to `rclpy.parameter` module from `ros2param.api` module from version 3.6.0
try:
from rclpy.parameter import get_parameter_value
except ImportError:
from ros2param.api import get_parameter_value
from rclpy.signals import SignalHandlerOptions
from ros2param.api import call_set_parameters
# from https://stackoverflow.com/a/287944
class bcolors:
HEADER = "\033[95m"
OKBLUE = "\033[94m"
OKCYAN = "\033[96m"
OKGREEN = "\033[92m"
WARNING = "\033[93m"
FAIL = "\033[91m"
ENDC = "\033[0m"
BOLD = "\033[1m"
UNDERLINE = "\033[4m"
def first_match(iterable, predicate):
return next((n for n in iterable if predicate(n)), None)
def wait_for_value_or(function, node, timeout, default, description):
while node.get_clock().now() < timeout:
if result := function():
return result
node.get_logger().info(
f"Waiting for {description}", throttle_duration_sec=2, skip_first=True
)
time.sleep(0.2)
return default
def combine_name_and_namespace(name_and_namespace):
node_name, namespace = name_and_namespace
return namespace + ("" if namespace.endswith("/") else "/") + node_name
def find_node_and_namespace(node, full_node_name):
node_names_and_namespaces = node.get_node_names_and_namespaces()
return first_match(
node_names_and_namespaces, lambda n: combine_name_and_namespace(n) == full_node_name
)
def has_service_names(node, node_name, node_namespace, service_names):
client_names_and_types = node.get_service_names_and_types_by_node(node_name, node_namespace)
if not client_names_and_types:
return False
client_names, _ = zip(*client_names_and_types)
return all(service in client_names for service in service_names)
def wait_for_controller_manager(node, controller_manager, timeout_duration):
# List of service names from controller_manager we wait for
service_names = (
f"{controller_manager}/configure_controller",
f"{controller_manager}/list_controllers",
f"{controller_manager}/list_controller_types",
f"{controller_manager}/list_hardware_components",
f"{controller_manager}/list_hardware_interfaces",
f"{controller_manager}/load_controller",
f"{controller_manager}/reload_controller_libraries",
f"{controller_manager}/switch_controller",
f"{controller_manager}/unload_controller",
)
# Wait for controller_manager
timeout = node.get_clock().now() + Duration(seconds=timeout_duration)
node_and_namespace = wait_for_value_or(
lambda: find_node_and_namespace(node, controller_manager),
node,
timeout,
None,
f"'{controller_manager}' node to exist",
)
# Wait for the services if the node was found
if node_and_namespace:
node_name, namespace = node_and_namespace
return wait_for_value_or(
lambda: has_service_names(node, node_name, namespace, service_names),
node,
timeout,
False,
f"'{controller_manager}' services to be available",
)
return False
def is_controller_loaded(node, controller_manager, controller_name):
controllers = list_controllers(node, controller_manager).controller
return any(c.name == controller_name for c in controllers)
def get_parameter_from_param_file(controller_name, parameter_file, parameter_name):
with open(parameter_file) as f:
parameters = yaml.safe_load(f)
if controller_name in parameters:
value = parameters[controller_name]
if not isinstance(value, dict) or "ros__parameters" not in value:
raise RuntimeError(
f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {controller_name}"
)
if parameter_name in parameters[controller_name]["ros__parameters"]:
return parameters[controller_name]["ros__parameters"][parameter_name]
else:
return None
def main(args=None):
rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
parser = argparse.ArgumentParser()
parser.add_argument("controller_names", help="List of controllers", nargs="+")
parser.add_argument(
"-c",
"--controller-manager",
help="Name of the controller manager ROS node",
default="controller_manager",
required=False,
)
parser.add_argument(
"-p",
"--param-file",
help="Controller param file to be loaded into controller node before configure",
required=False,
)
parser.add_argument(
"-n", "--namespace", help="Namespace for the controller", default="", required=False
)
parser.add_argument(
"--load-only",
help="Only load the controller and leave unconfigured.",
action="store_true",
required=False,
)
parser.add_argument(
"--inactive",
help="Load and configure the controller, however do not activate them",
action="store_true",
required=False,
)
parser.add_argument(
"-t",
"--controller-type",
help="If not provided it should exist in the controller manager namespace (deprecated)",
default=None,
required=False,
)
parser.add_argument(
"-u",
"--unload-on-kill",
help="Wait until this application is interrupted and unload controller",
action="store_true",
)
parser.add_argument(
"--controller-manager-timeout",
help="Time to wait for the controller manager",
required=False,
default=10,
type=int,
)
parser.add_argument(
"--activate-as-group",
help="Activates all the parsed controllers list together instead of one by one."
" Useful for activating all chainable controllers altogether",
action="store_true",
required=False,
)
parser.add_argument(
"--fallback_controllers",
help="Fallback controllers list are activated as a fallback strategy when the"
" spawned controllers fail. When the argument is provided, it takes precedence over"
" the fallback_controllers list in the param file",
default=None,
nargs="+",
)
command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
args = parser.parse_args(command_line_args)
controller_names = args.controller_names
controller_manager_name = args.controller_manager
controller_namespace = args.namespace
param_file = args.param_file
controller_manager_timeout = args.controller_manager_timeout
if args.controller_type:
warnings.filterwarnings("always")
warnings.warn(
"The '--controller-type' argument is deprecated and will be removed in future releases."
" Declare the controller type parameter in the param file instead.",
DeprecationWarning,
)
if param_file and not os.path.isfile(param_file):
raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
node = Node("spawner_" + controller_names[0])
if not controller_manager_name.startswith("/"):
spawner_namespace = node.get_namespace()
if spawner_namespace != "/":
controller_manager_name = f"{spawner_namespace}/{controller_manager_name}"
else:
controller_manager_name = f"/{controller_manager_name}"
try:
if not wait_for_controller_manager(
node, controller_manager_name, controller_manager_timeout
):
node.get_logger().error(
bcolors.FAIL + "Controller manager not available" + bcolors.ENDC
)
return 1
for controller_name in controller_names:
fallback_controllers = args.fallback_controllers
controller_type = args.controller_type
prefixed_controller_name = controller_name
if controller_namespace:
prefixed_controller_name = controller_namespace + "/" + controller_name
if is_controller_loaded(node, controller_manager_name, prefixed_controller_name):
node.get_logger().warn(
bcolors.WARNING
+ "Controller already loaded, skipping load_controller"
+ bcolors.ENDC
)
else:
if not controller_type and param_file:
controller_type = get_parameter_from_param_file(
controller_name, param_file, "type"
)
if controller_type:
parameter = Parameter()
parameter.name = prefixed_controller_name + ".type"
parameter.value = get_parameter_value(string_value=controller_type)
response = call_set_parameters(
node=node, node_name=controller_manager_name, parameters=[parameter]
)
assert len(response.results) == 1
result = response.results[0]
if result.successful:
node.get_logger().info(
bcolors.OKCYAN
+ 'Set controller type to "'
+ controller_type
+ '" for '
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
else:
node.get_logger().fatal(
bcolors.FAIL
+ 'Could not set controller type to "'
+ controller_type
+ '" for '
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
return 1
if param_file:
parameter = Parameter()
parameter.name = prefixed_controller_name + ".params_file"
parameter.value = get_parameter_value(string_value=param_file)
response = call_set_parameters(
node=node, node_name=controller_manager_name, parameters=[parameter]
)
assert len(response.results) == 1
result = response.results[0]
if result.successful:
node.get_logger().info(
bcolors.OKCYAN
+ 'Set controller params file to "'
+ param_file
+ '" for '
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
else:
node.get_logger().fatal(
bcolors.FAIL
+ 'Could not set controller params file to "'
+ param_file
+ '" for '
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
return 1
if not fallback_controllers and param_file:
fallback_controllers = get_parameter_from_param_file(
controller_name, param_file, "fallback_controllers"
)
if fallback_controllers:
parameter = Parameter()
parameter.name = prefixed_controller_name + ".fallback_controllers"
parameter.value = get_parameter_value(string_value=str(fallback_controllers))
response = call_set_parameters(
node=node, node_name=controller_manager_name, parameters=[parameter]
)
assert len(response.results) == 1
result = response.results[0]
if result.successful:
node.get_logger().info(
bcolors.OKCYAN
+ 'Setting fallback_controllers to ["'
+ ",".join(fallback_controllers)
+ '"] for '
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
else:
node.get_logger().fatal(
bcolors.FAIL
+ 'Could not set fallback_controllers to ["'
+ ",".join(fallback_controllers)
+ '"] for '
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
return 1
ret = load_controller(node, controller_manager_name, controller_name)
if not ret.ok:
node.get_logger().fatal(
bcolors.FAIL
+ "Failed loading controller "
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
return 1
node.get_logger().info(
bcolors.OKBLUE
+ "Loaded "
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
if not args.load_only:
ret = configure_controller(node, controller_manager_name, controller_name)
if not ret.ok:
node.get_logger().error(
bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC
)
return 1
if not args.inactive and not args.activate_as_group:
ret = switch_controllers(
node, controller_manager_name, [], [controller_name], True, True, 5.0
)
if not ret.ok:
node.get_logger().error(
bcolors.FAIL + "Failed to activate controller" + bcolors.ENDC
)
return 1
node.get_logger().info(
bcolors.OKGREEN
+ "Configured and activated "
+ bcolors.BOLD
+ prefixed_controller_name
+ bcolors.ENDC
)
if not args.inactive and args.activate_as_group:
ret = switch_controllers(
node, controller_manager_name, [], controller_names, True, True, 5.0
)
if not ret.ok:
node.get_logger().error(
bcolors.FAIL + "Failed to activate the parsed controllers list" + bcolors.ENDC
)
return 1
node.get_logger().info(
bcolors.OKGREEN
+ "Configured and activated all the parsed controllers list!"
+ bcolors.ENDC
)
if not args.unload_on_kill:
return 0
try:
node.get_logger().info("Waiting until interrupt to unload controllers")
while True:
time.sleep(1)
except KeyboardInterrupt:
if not args.inactive:
node.get_logger().info("Interrupt captured, deactivating and unloading controller")
# TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode
ret = switch_controllers(
node, controller_manager_name, controller_names, [], True, True, 5.0
)
if not ret.ok:
node.get_logger().error(
bcolors.FAIL + "Failed to deactivate controller" + bcolors.ENDC
)
return 1
node.get_logger().info("Deactivated controller")
ret = unload_controller(node, controller_manager_name, controller_name)
if not ret.ok:
node.get_logger().error(
bcolors.FAIL + "Failed to unload controller" + bcolors.ENDC
)
return 1
node.get_logger().info("Unloaded controller")
return 0
finally:
rclpy.shutdown()
if __name__ == "__main__":
warnings.warn(
"'spawner.py' is deprecated, please use 'spawner' (without .py extension)",
DeprecationWarning,
)
ret = main()
sys.exit(ret)