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 Implementation of the rosparam as well as a library for modifying the
37 state of the ROS Parameter Server using YAML files.
38 """
39
40 from __future__ import print_function
41
42 NAME = 'rosparam'
43
44
45
46
47
48 NS = '_ns'
49
50 import base64
51 import math
52 import os
53 import re
54 import sys
55 import socket
56 try:
57 from xmlrpc.client import Binary
58 except ImportError:
59 from xmlrpclib import Binary
60
61 from optparse import OptionParser
62
63 import yaml
64
65 import rosgraph
66 from rosgraph.names import script_resolve_name, ns_join, get_ros_namespace, make_caller_id, make_global_ns, GLOBALNS
67
69 """
70 rosparam base exception type
71 """
72 pass
74 """
75 Exception for communication-based (i/o) errors.
76 """
77 pass
78
79
80
82 """
83 Adds a pyyaml serializer to handle xmlrpclib.Binary objects
84 """
85 data = base64.b64encode(data.data)
86 return loader.represent_scalar(u'tag:yaml.org,2002:binary', data, style='|')
87
89 return loader.represent_scalar(u'#', data)
90
92 """
93 Overrides pyaml's constructor for binary data. Wraps binary data in
94 xmlrpclib.Binary container instead of straight string
95 representation.
96 """
97 return Binary(loader.construct_yaml_binary(node))
98
99
100 yaml.add_representer(Binary,represent_xml_binary)
101 yaml.add_constructor(u'tag:yaml.org,2002:binary', construct_yaml_binary)
102
104 """
105 python-yaml utility for converting rad(num) into float value
106 """
107 value = loader.construct_scalar(node).strip()
108 exprvalue = value.replace('pi', 'math.pi')
109 if exprvalue.startswith("rad("):
110 exprvalue = exprvalue[4:-1]
111 try:
112 return float(eval(exprvalue))
113 except SyntaxError as e:
114 raise RosParamException("invalid radian expression: %s"%value)
115
117 """
118 python-yaml utility for converting deg(num) into float value
119 """
120 value = loader.construct_scalar(node)
121 exprvalue = value
122 if exprvalue.startswith("deg("):
123 exprvalue = exprvalue.strip()[4:-1]
124 try:
125 return float(exprvalue) * math.pi / 180.0
126 except ValueError:
127 raise RosParamException("invalid degree value: %s"%value)
128
129
130
131
133 """
134 :returns: caller ID for rosparam ROS client calls, ``str``
135 """
136 return make_caller_id('rosparam-%s'%os.getpid())
137
139 """
140 Print contents of param dictionary to screen
141 """
142 if type(params) == dict:
143 for k, v in params.items():
144 if type(v) == dict:
145 print_params(v, ns_join(ns, k))
146 else:
147 print("%s=%s"%(ns_join(ns, k), v))
148 else:
149 print(params)
150
151
152
153 -def load_file(filename, default_namespace=None, verbose=False):
154 """
155 Load the YAML document from the specified file
156
157 :param filename: name of filename, ``str``
158 :param default_namespace: namespace to load filename into, ``str``
159 :returns [(dict, str)...]: list of parameter dictionary and
160 corresponding namespaces for each YAML document in the file
161 :raises: :exc:`RosParamException`: if unable to load contents of filename
162 """
163 if not filename or filename == '-':
164 f = sys.stdin
165 if verbose:
166 print("reading parameters from stdin")
167 return load_str(f.read(), filename, default_namespace=default_namespace, verbose=verbose)
168 else:
169 if not os.path.isfile(filename):
170 raise RosParamException("file [%s] does not exist"%filename)
171 if verbose:
172 print("reading parameters from [%s]"%filename)
173 with open(filename, 'r') as f:
174 return load_str(f.read(), filename, default_namespace=default_namespace, verbose=verbose)
175
176 -def load_str(str, filename, default_namespace=None, verbose=False):
177 """
178 Load the YAML document as a string
179
180 :param filename: name of filename, only used for debugging, ``str``
181 :param default_namespace: namespace to load filename into, ``str``
182 :param str: YAML text, ``str``
183 :returns: list of parameter dictionary and
184 corresponding namespaces for each YAML document in the file, ``[(dict, str)...]``
185 """
186 paramlist = []
187 default_namespace = default_namespace or get_ros_namespace()
188 for doc in yaml.load_all(str):
189 if NS in doc:
190 ns = ns_join(default_namespace, doc.get(NS, None))
191 if verbose:
192 print("reading parameters into namespace [%s]"%ns)
193 del doc[NS]
194 else:
195 ns = default_namespace
196 paramlist.append((doc, ns))
197 return paramlist
198
199
200
201
203 return rosgraph.Master(_get_caller_id())
204
206 """
207 Download a parameter from Parameter Server
208
209 :param param: parameter name to retrieve from parameter
210 server. If param is a parameter namespace, entire parameter
211 subtree will be downloaded, ``str``
212 """
213 try:
214 return get_param_server().getParam(param)
215 except socket.error:
216 raise RosParamIOException("Unable to communicate with master!")
217
218
220 """
221 Pretty print get value
222 :param value: value to print
223 :param indent: indent level, used for recursive calls, ``str``
224 """
225 keys = list(value.keys())
226 keys.sort()
227 for k in keys:
228 v = value[k]
229 if type(v) == dict:
230 print("%s%s:"%(indent, k))
231 _pretty_print(v, indent+' ')
232 elif type(v) == str:
233 if '\n' in v:
234 print(indent+'%s: |'%k)
235 for l in v.split('\n'):
236 print(indent+' '+l)
237 else:
238 print("%s%s: %s"%(indent, k, v))
239 else:
240 dump = yaml.dump(v)
241
242
243
244
245
246 if dump.endswith('\n...\n'):
247 dump = dump[:-4]
248
249 sys.stdout.write("%s%s: %s"%(indent, k, dump))
250
252 """
253 Download a parameter tree and print to screen
254 :param param: parameter name to retrieve from Parameter
255 Server. If param is a parameter namespace, entire parameter
256 subtree will be downloaded, ``str``
257 """
258
259 if verbose:
260 print("getting parameter [%s]"%param)
261 try:
262 val = get_param(param)
263 except rosgraph.masterapi.Error as e:
264 raise RosParamException(str(e))
265 if pretty and type(val) in [dict, str]:
266 if type(val) == dict:
267 _pretty_print(val)
268 else:
269 if '\n' in val:
270 print('|')
271 for l in val.split('\n'):
272 print(' '+l)
273 else:
274 print(val)
275 else:
276 dump = yaml.dump(val)
277
278
279
280
281
282 if dump.endswith('\n...\n'):
283 dump = dump[:-5]
284
285
286 sys.stdout.write("%s\n"%(dump))
287
289 """
290 Download a parameter tree from the Parameter Server and store in a yaml file
291
292 :param filename: name of file to save YAML representation, ``str``
293 :param param: name of parameter/namespace to dump, ``str``
294 :param verbose: print verbose output for debugging, ``bool``
295 """
296 tree = get_param(param)
297 if verbose:
298 print_params(tree, param)
299 if not filename:
300 f = sys.stdout
301 yaml.dump(tree, f)
302 else:
303 f = open(filename, 'w')
304 try:
305 yaml.dump(tree, f)
306 finally:
307 f.close()
308
309
311 """
312 Delete a parameter from the Parameter Server
313
314 :param param: parameter name, ``str``
315 :param verbose: print verbose output for debugging, ``bool``
316 """
317 try:
318 if param == GLOBALNS:
319
320
321
322 get_param_server().setParam(GLOBALNS, {})
323 if verbose:
324 print("deleted ENTIRE parameter server")
325 else:
326 get_param_server().deleteParam(param)
327 if verbose:
328 print("deleted parameter [%s]"%param)
329 except socket.error:
330 raise RosParamIOException("Unable to communicate with master!")
331
332
333
335 """
336 Set param on the Parameter Server. Unlike L{set_param()}, this
337 takes in a Python value to set instead of YAML.
338
339 :param param: parameter name, ``str``
340 :param value XmlRpcLegalValue: value to upload, ``XmlRpcLegalValue``
341 """
342 if type(value) == dict:
343
344
345 for k, v in value.items():
346
347 if isinstance(k, str):
348 set_param_raw(ns_join(param, k), v, verbose=verbose)
349 else:
350 raise RosParamException("YAML dictionaries must have string keys. Invalid dictionary is:\n%s"%value)
351 else:
352 try:
353 expected_type = long
354 except NameError :
355 expected_type = int
356
357 if type(value) == expected_type:
358 if value > sys.maxsize:
359 raise RosParamException("Overflow: Parameter Server integers must be 32-bit signed integers:\n\t-%s <= value <= %s"%(maxint - 1, maxint))
360
361 try:
362 get_param_server().setParam(param, value)
363 except socket.error:
364 raise RosParamIOException("Unable to communicate with master!")
365 if verbose:
366 print("set parameter [%s] to [%s]"%(param, value))
367
369 """
370 Set param on the ROS parameter server using a YAML value.
371
372 :param param: parameter name, ``str``
373 :param value: yaml-encoded value, ``str``
374 """
375 set_param_raw(param, yaml.load(value), verbose=verbose)
376
378 """
379 Upload params to the Parameter Server
380 :param values: key/value dictionary, where keys are parameter names and values are parameter values, ``dict``
381 :param ns: namespace to load parameters into, ``str``
382 """
383 if ns == '/' and not type(values) == dict:
384 raise RosParamException("global / can only be set to a dictionary")
385 if verbose:
386 print_params(values, ns)
387 set_param_raw(ns, values)
388
389
390
391
393 """
394 Get list of parameters in ns
395
396 :param ns: namespace to match, ``str``
397 """
398 try:
399 ns = make_global_ns(ns)
400 names = get_param_server().getParamNames()
401 names.sort()
402 return [n for n in names if n.startswith(ns)]
403 except socket.error:
404 raise RosParamIOException("Unable to communicate with master!")
405
406
407
409 """
410 Process command line for rosparam get/dump, e.g.::
411 rosparam get param
412 rosparam dump file.yaml [namespace]
413
414 :param cmd: command ('get' or 'dump'), ``str``
415 :param argv: command-line args, ``str``
416 """
417
418 if cmd == 'dump':
419 parser = OptionParser(usage="usage: %prog dump [options] file [namespace]", prog=NAME)
420 elif cmd == 'get':
421 parser = OptionParser(usage="usage: %prog get [options] parameter", prog=NAME)
422 parser.add_option("-p", dest="pretty", default=False,
423 action="store_true", help="pretty print. WARNING: not YAML-safe")
424
425 parser.add_option("-v", dest="verbose", default=False,
426 action="store_true", help="turn on verbose output")
427 options, args = parser.parse_args(argv[2:])
428
429 arg = None
430 ns = ''
431
432 if len(args) == 0:
433 if cmd == 'get':
434 parser.error("invalid arguments. Please specify a parameter name")
435 elif len(args) == 1:
436 arg = args[0]
437 elif len(args) == 2 and cmd == 'dump':
438 arg = args[0]
439 ns = args[1]
440 else:
441 parser.error("too many arguments")
442
443 if cmd == 'get':
444 _rosparam_cmd_get_param(script_resolve_name(NAME, arg), pretty=options.pretty, verbose=options.verbose)
445 else:
446 if options.verbose:
447 print("dumping namespace [%s] to file [%s]"%(ns, arg))
448 dump_params(arg, script_resolve_name(NAME, ns), verbose=options.verbose)
449
451
452
453
454
455 args = []
456 optparse_args = []
457 skip = False
458 for s in argv[2:]:
459 if s.startswith('-'):
460 if s in ['-t', '--textfile', '-b', '--binfile']:
461 skip = True
462 optparse_args.append(s)
463 elif skip:
464 parser.error("-t and --textfile options require an argument")
465 elif len(s) > 1 and ord(s[1]) >= ord('0') and ord(s[1]) <= ord('9'):
466 args.append(s)
467 else:
468 optparse_args.append(s)
469 else:
470 if skip:
471 skip = False
472 optparse_args.append(s)
473 else:
474 args.append(s)
475 options, _ = parser.parse_args(optparse_args)
476 return options, args
477
478
480 """
481 Process command line for rosparam set/load, e.g.::
482 rosparam load file.yaml [namespace]
483 rosparam set param value
484
485 :param cmd: command name, ``str``
486 :param argv: command-line args, ``str``
487 """
488 if cmd == 'load':
489 parser = OptionParser(usage="usage: %prog load [options] file [namespace]", prog=NAME)
490 elif cmd == 'set':
491 parser = OptionParser(usage="usage: %prog set [options] parameter value", prog=NAME)
492 parser.add_option("-t", "--textfile", dest="text_file", default=None,
493 metavar="TEXT_FILE", help="set parameters to contents of text file")
494 parser.add_option("-b", "--binfile", dest="bin_file", default=None,
495 metavar="BINARY_FILE", help="set parameters to contents of binary file")
496
497 parser.add_option("-v", dest="verbose", default=False,
498 action="store_true", help="turn on verbose output")
499 if cmd == 'set':
500 options, args = _set_optparse_neg_args(parser, argv)
501 if options.text_file and options.bin_file:
502 parser.error("you may only specify one of --textfile or --binfile")
503 else:
504 options, args = parser.parse_args(argv[2:])
505
506 arg2 = None
507 if len(args) == 0:
508 if cmd == 'load':
509 parser.error("invalid arguments. Please specify a file name or - for stdin")
510 elif cmd == 'set':
511 parser.error("invalid arguments. Please specify a parameter name")
512 elif len(args) == 1:
513 arg = args[0]
514 if cmd == 'set' and not (options.text_file or options.bin_file):
515 parser.error("invalid arguments. Please specify a parameter value")
516 elif len(args) == 2:
517 arg = args[0]
518 arg2 = args[1]
519 else:
520 parser.error("too many arguments")
521
522 if cmd == 'set':
523 name = script_resolve_name(NAME, arg)
524
525 if options.text_file:
526 if not os.path.isfile(options.text_file):
527 parser.error("file '%s' does not exist"%(options.text_file))
528 with open(options.text_file) as f:
529 arg2 = f.read()
530 set_param_raw(name, arg2, verbose=options.verbose)
531 elif options.bin_file:
532 with open(options.bin_file, 'rb') as f:
533 arg2 = Binary(f.read())
534 set_param_raw(name, arg2, verbose=options.verbose)
535 else:
536
537
538
539
540
541 if arg2 == '':
542 arg2 = '!!str'
543 set_param(name, arg2, verbose=options.verbose)
544 else:
545 paramlist = load_file(arg, default_namespace=script_resolve_name(NAME, arg2), verbose=options.verbose)
546 for params,ns in paramlist:
547 upload_params(ns, params, verbose=options.verbose)
548
550 """
551 Process command line for rosparam set/load, e.g.::
552 rosparam load file.yaml [namespace]
553 rosparam set param value
554
555 :param argv: command-line args, ``str``
556 """
557 parser = OptionParser(usage="usage: %prog list [namespace]", prog=NAME)
558 options, args = parser.parse_args(argv[2:])
559
560 ns = GLOBALNS
561 if len(args) == 1:
562 ns = script_resolve_name(NAME, args[0])
563 elif len(args) == 2:
564 parser.error("too many arguments")
565
566 print('\n'.join(list_params(ns)))
567
568
570 """
571 Process command line for rosparam delete, e.g.::
572 rosparam delete param
573
574 :param cmd: command name, ``str``
575 :param argv: command-line args, ``str``
576 """
577 parser = OptionParser(usage="usage: %prog delete [options] parameter", prog=NAME)
578 parser.add_option("-v", dest="verbose", default=False,
579 action="store_true", help="turn on verbose output")
580 options, args = parser.parse_args(argv[2:])
581
582 arg2 = None
583 if len(args) == 0:
584 parser.error("invalid arguments. Please specify a parameter name")
585 elif len(args) == 1:
586 arg = args[0]
587 else:
588 parser.error("too many arguments")
589
590 try:
591 delete_param(script_resolve_name(NAME, arg), verbose=options.verbose)
592 except rosgraph.masterapi.Error as e:
593 raise RosParamException(str(e))
594
596 """
597 Prints rosparam usage
598 """
599 print("""rosparam is a command-line tool for getting, setting, and deleting parameters from the ROS Parameter Server.
600
601 Commands:
602 \trosparam set\tset parameter
603 \trosparam get\tget parameter
604 \trosparam load\tload parameters from file
605 \trosparam dump\tdump parameters to file
606 \trosparam delete\tdelete parameter
607 \trosparam list\tlist parameter names
608 """)
609 sys.exit(0)
610
611 -def yamlmain(argv=None):
612 """
613 Command-line main routine. Loads in one or more input files
614
615 :param argv: command-line arguments or None to use sys.argv, ``[str]``
616 """
617 if argv is None:
618 argv = sys.argv
619 if len(argv) == 1:
620 _fullusage()
621 try:
622 command = argv[1]
623 if command in ['get', 'dump']:
624 _rosparam_cmd_get_dump(command, argv)
625 elif command in ['set', 'load']:
626 _rosparam_cmd_set_load(command, argv)
627 elif command in ['delete']:
628 _rosparam_cmd_delete(argv)
629 elif command == 'list':
630 _rosparam_cmd_list(argv)
631 else:
632 _fullusage()
633 except RosParamException as e:
634 print("ERROR: "+str(e), file=sys.stderr)
635 sys.exit(1)
636
637
638
639 yaml.add_constructor(u'!radians', construct_angle_radians)
640 yaml.add_constructor(u'!degrees', construct_angle_degrees)
641
642
643 pattern = re.compile(r'^deg\([^\)]*\)$')
644 yaml.add_implicit_resolver(u'!degrees', pattern, first="deg(")
645 pattern = re.compile(r'^rad\([^\)]*\)$')
646 yaml.add_implicit_resolver(u'!radians', pattern, first="rad(")
647