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 Command-line utility for querying ROS services, along with library
38 calls for similar functionality. The main benefit of the rosservice
39 Python library over the rospy ServiceProxy library is that rosservice
40 supports type-introspection on ROS Services. This allows for both
41 introspecting information about services, as well as using this
42 introspection to dynamically call services.
43 """
44
45 NAME='rosservice'
46
47 import cStringIO
48 import os
49 import sys
50 import socket
51 import struct
52 import threading
53 import time
54
55 import roslib.message
56 import roslib.names
57 import roslib.network
58 import roslib.scriptutil
59 import rospy
60 import rosmsg
61
62 from optparse import OptionParser
63
65 """Base class for rosservice-related exceptions"""
66 pass
67
69 """rosservice related to network I/O failure"""
70 pass
71
73 """
74 Utility that raises a ROSServiceException if ROS XMLRPC command fails
75 @param args: (code, msg, val) ROS XMLRPC call return args
76 @type args: (int, str, XmlRpcValue)
77 @return: value argument from ROS XMLRPC call (third arg of tuple)
78 @rtype: XmlRpcLegal value
79 @raise ROSServiceException: if XMLRPC command does not return a SUCCESS code
80 """
81 code, msg, val = args
82 if code != 1:
83 raise ROSServiceException("remote call failed: %s"%msg)
84 return val
85
87 """
88 Utility for connecting to a service and retrieving the TCPROS
89 headers. Services currently do not declare their type with the
90 master, so instead we probe the service for its headers.
91 @param service_name: name of service
92 @type service_name: str
93 @param service_uri: ROSRPC URI of service
94 @type service_uri: str
95 @return: map of header fields
96 @rtype: dict
97 @raise ROSServiceException: if service has invalid information
98 @raise ROSServiceIOException: if unable to communicate with service
99 """
100 try:
101 dest_addr, dest_port = rospy.parse_rosrpc_uri(service_uri)
102 except:
103 raise ROSServiceException("service [%s] has an invalid RPC URI [%s]"%(service_name, service_uri))
104 s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
105 try:
106 try:
107
108 s.settimeout(5.0)
109 s.connect((dest_addr, dest_port))
110 header = { 'probe':'1', 'md5sum':'*',
111 'callerid':'/rosservice', 'service':service_name}
112 roslib.network.write_ros_handshake_header(s, header)
113 return roslib.network.read_ros_handshake_header(s, cStringIO.StringIO(), 2048)
114 except socket.error:
115 raise ROSServiceIOException("Unable to communicate with service [%s], address [%s]"%(service_name, service_uri))
116 finally:
117 if s is not None:
118 s.close()
119
121 """
122 Get the type of the specified service_name. May print errors to stderr.
123 @param service_name: name of service
124 @type service_name: str
125 @return: type of service or None
126 @rtype: str
127 @raise ROSServiceException: if service information is invalid
128 @raise ROSServiceIOException: if unable to communicate with service
129 """
130 master = roslib.scriptutil.get_master()
131 try:
132 code, msg, service_uri = master.lookupService('/rosservice', service_name)
133 except socket.error:
134 raise ROSServiceIOException("Unable to communicate with master!")
135 if code == -1:
136 return None
137 elif code == 0:
138 raise ROSServiceIOException("Master is malfunctioning: %s"%msg)
139 else:
140 try:
141 return get_service_headers(service_name, service_uri).get('type', None)
142 except socket.error:
143 raise ROSServiceIOException("Unable to communicate with service [%s]! Service address is [%s]"%(service_name, service_uri))
144
146 """
147 Implements 'type' command. Prints service type to stdout. Will
148 system exit with error if service_name is unknown.
149
150 @param service_name: name of service
151 @type service_name: str
152 """
153 service_type = get_service_type(service_name)
154 if service_type is None:
155 print >> sys.stderr, "Unknown service [%s]"%service_name
156 sys.exit(1)
157 else:
158 print service_type
159
161 """
162 Retrieve ROSRPC URI of service.
163
164 @param service_name: name of service to lookup
165 @type service_name: str
166 @return: ROSRPC URI for service_name
167 @rtype: str
168 """
169 try:
170 master = roslib.scriptutil.get_master()
171 code, msg, url = master.lookupService('/rosservice', service_name)
172 if code == 1:
173 return url
174 return None
175 except socket.error:
176 raise ROSServiceIOException("Unable to communicate with master!")
177
179 """
180 Implements rosservice uri command. Will cause system exit with
181 error if service_name is unknown.
182
183 @param service_name: name of service to lookup
184 @type service_name: str
185 @raise ROSServiceIOException: if the I/O issues prevent retrieving service information
186 """
187 uri = get_service_uri(service_name)
188 if uri:
189 print uri
190 else:
191 print >> sys.stderr, "Unknown service: %s"%service_name
192 sys.exit(1)
193
195 """
196 @return: name of node that implements service, or None
197 @rtype: str
198 """
199 srvs = get_service_list(include_nodes=True)
200 s = [s for s in srvs if s[0] == service_name]
201 if s:
202 if s[0][1]:
203 return s[0][1][0]
204 return None
205
207 """
208 Implements rosservice node command. Will cause system exit with error if service is unknown.
209
210 @param service_name: name of service to lookup
211 @type service_name: str
212 @raise ROSServiceIOException: if the I/O issues prevent retrieving service information
213 """
214 n = get_service_node(service_name)
215 if n:
216 print n
217 else:
218 print >> sys.stderr, "Unknown service: %s"%service_name
219 sys.exit(1)
220
222 """
223 Get the list of services
224 @param node: Name of node to get services for or None to return all services
225 @type node: str
226 @param namespace: Namespace to scope services to or None
227 @type namespace: str
228 @param include_nodes: If True, return list will be [service_name, [node]]
229 @type include_nodes: bool
230 @return: if include_nodes, services is service_name,
231 [node]. Otherwise, it is just the service_name
232 @rtype: [services]
233 @raise ROSServiceIOException: if the I/O issues prevent retrieving service information
234 """
235 try:
236 master = roslib.scriptutil.get_master()
237 state = _succeed(master.getSystemState('/rosservice'))
238 srvs = state[2]
239
240
241 if namespace:
242 g_ns = roslib.names.make_global_ns(namespace)
243 srvs = [x for x in srvs if x[0] == namespace or x[0].startswith(g_ns)]
244
245 if include_nodes:
246 if node is None:
247 return srvs
248 else:
249 return [(s, nodelist) for s, nodelist in srvs if node in nodelist]
250 else:
251 if node is None:
252 return [s for s,_ in srvs]
253 else:
254 return [s for s,nodelist in srvs if node in nodelist]
255 except socket.error:
256 raise ROSServiceIOException("Unable to communicate with master!")
257
259 """
260 Implements 'rosservice list'
261 @param namespace: Namespace to limit listing to or None
262 @type namespace: str
263 @param print_nodes: If True, also print nodes providing service
264 @type print_nodes: bool
265 @raise ROSServiceIOException: if the I/O issues prevent retrieving service information
266 """
267 srvs = get_service_list(namespace=namespace, include_nodes=print_nodes)
268
269
270 if print_nodes:
271 import operator
272 srvs.sort(key=operator.itemgetter(0))
273 else:
274 srvs.sort()
275 for s in srvs:
276 if print_nodes:
277 print s[0]+' '+','.join(s[1])
278 else:
279 print s
280
282 """
283 Implements 'rosservice info'. Prints information about a service.
284
285 @param service_name: name of service to get info for
286 @type service_name: str
287 @raise ROSServiceIOException: if the I/O issues prevent retrieving service information
288 """
289 n = get_service_node(service_name)
290 if not n:
291 print >> sys.stderr, "ERROR: unknown service"
292 sys.exit(1)
293 print "Node: %s"%n
294 uri = get_service_uri(service_name)
295 if not uri:
296 print >> sys.stderr, "ERROR: service is no longer available"
297 return
298 print "URI: %s"%uri
299 t = get_service_type(service_name)
300 if not t:
301 print >> sys.stderr, "ERROR: service is no longer available"
302 return
303 print "Type: %s"%t
304 args = get_service_args(service_name)
305 if args is None:
306 print >> sys.stderr, "ERROR: service is no longer available"
307 return
308 print "Args: %s"%args
309
311 """
312 Lookup services by service_type
313 @param service_type: type of service to find
314 @type service_type: str
315 @return: list of service names that use service_type
316 @rtype: [str]
317 """
318 master = roslib.scriptutil.get_master()
319 matches = []
320 try:
321 _, _, services = _succeed(master.getSystemState('/rosservice'))
322 for s, l in services:
323 t = get_service_type(s)
324 if t == service_type:
325 matches.append(s)
326 except socket.error:
327 raise ROSServiceIOException("Unable to communicate with master!")
328 return matches
329
331 """
332 Implements 'rosservice type'
333
334 @param argv: command-line args
335 @type argv: [str]
336 """
337 args = argv[2:]
338 parser = OptionParser(usage="usage: %prog find msg-type", prog=NAME)
339 options, args = parser.parse_args(args)
340 if not len(args):
341 parser.error("please specify a message type")
342 if len(args) > 1:
343 parser.error("you may only specify one message type")
344 print '\n'.join(rosservice_find(args[0]))
345
347 """
348 Get the service class using the name of the service. NOTE: this
349 call results in a probe call to the service.
350 @param service_name: fully-resolved name of service to call
351 @type service_name: str
352 @return: service class
353 @rtype: ServiceDefinition: service class
354 @raise ROSServiceException: if service class cannot be retrieved
355 """
356
357 service_type = get_service_type(service_name)
358 if not service_type:
359
360 srvs = get_service_list()
361 if service_name not in srvs:
362 raise ROSServiceException("Service [%s] is not available."%service_name)
363 else:
364 raise ROSServiceException("Unable to determine type of service [%s]."%service_name)
365
366
367 service_class = roslib.message.get_service_class(service_type)
368
369
370 if service_class and service_type.endswith('Request') and \
371 not hasattr(service_class, "_request_class"):
372 service_type = service_type[:-7]
373 service_class = roslib.message.get_service_class(service_type)
374
375 if service_class is None:
376 pkg = roslib.names.resource_name_package(service_type)
377 raise ROSServiceException("Unable to load type [%s].\n"%service_type+
378 "Have you typed 'make' in [%s]?"%pkg)
379 return service_class
380
381 -def call_service(service_name, service_args, service_class=None):
382 """
383 Call the specified service_name
384 @param service_name: fully-resolved name of service to call
385 @type service_name: str
386 @param service_args: args to pass to service
387 @type service_args: [any]
388 @param service_class: (optional) service type class. If this
389 argument is provided, it saves a probe call against the service
390 @type service_class: Message class
391 @return: service request, service response
392 @rtype: roslib.message.Message, roslib.message.Message
393 @raise ROSServiceException: if call command cannot be executed
394 """
395
396
397 import std_msgs.msg
398 rospy.init_node('rosservice', anonymous=True)
399
400 if service_class is None:
401 service_class = get_service_class_by_name(service_name)
402 request = service_class._request_class()
403 try:
404 now = rospy.get_rostime()
405 keys = { 'now': now, 'auto': std_msgs.msg.Header(stamp=now) }
406 roslib.message.fill_message_args(request, service_args, keys=keys)
407 except roslib.message.ROSMessageException, e:
408 def argsummary(args):
409 if type(args) in [tuple, list]:
410 return '\n'.join([' * %s (type %s)'%(a, type(a).__name__) for a in args])
411 else:
412 return ' * %s (type %s)'%(args, type(args).__name__)
413
414 raise ROSServiceException("Incompatible arguments to call service:\n%s\nProvided arguments are:\n%s\n\nService arguments are: [%s]"%(e, argsummary(service_args), roslib.message.get_printable_message_args(request)))
415 try:
416 return request, rospy.ServiceProxy(service_name, service_class)(request)
417 except rospy.ServiceException, e:
418 raise ROSServiceException(str(e))
419 except roslib.message.SerializationError, e:
420 raise ROSServiceException("Unable to send request. One of the fields has an incorrect type:\n"+\
421 " %s\n\nsrv file:\n%s"%(e, rosmsg.get_srv_text(service_class._type)))
422 except rospy.ROSSerializationException, e:
423 raise ROSServiceException("Unable to send request. One of the fields has an incorrect type:\n"+\
424 " %s\n\nsrv file:\n%s"%(e, rosmsg.get_srv_text(service_class._type)))
425
426 -def _rosservice_call(service_name, service_args, verbose=False, service_class=None):
427 """
428 Implements 'rosservice call'
429 @param service_name: name of service to call
430 @type service_name: str
431 @param service_args: arguments to call service with
432 @type service_args: [args]
433 @param verbose: if True, print extra output
434 @type verbose: bool
435 @param service_class Message class: (optional) service type
436 class. If this argument is provided, it saves a probe call against
437 the service
438 @type service_class: Message class
439 @raise ROSServiceException: if call command cannot be executed
440 """
441 service_name = roslib.scriptutil.script_resolve_name('rosservice', service_name)
442 request, response = call_service(service_name, service_args, service_class=service_class)
443 if verbose:
444 print str(request)
445 print '---'
446 print str(response)
447
449 """
450 Check if service requires arguments
451 @param service_name: name of service being called
452 @type service_name: str
453 @param service_class: (optional) service type class. If this
454 argument is provided, it saves a probe call against the service
455 @type service_class: Message class
456 @return: True if service_name has request arguments
457 @rtype: bool
458 """
459 service_name = roslib.scriptutil.script_resolve_name('rosservice', service_name)
460 if service_class is None:
461 service_class = get_service_class_by_name(service_name)
462 return len(service_class._request_class.__slots__) > 0
463
465 """
466 Implements 'rosservice args'
467 @param service_name: name of service to get arguments for
468 @type service_name: str
469 @raise ROSServiceException: if call command cannot be executed
470 """
471 print get_service_args(service_name)
472
474 """
475 Implements 'get service args'
476 @param service_name: name of service to get arguments for
477 @type service_name: str
478 @raise ROSServiceException: if call command cannot be executed
479 """
480 service_name = roslib.scriptutil.script_resolve_name('rosservice', service_name)
481 service_class = get_service_class_by_name(service_name)
482 return roslib.message.get_printable_message_args(service_class._request_class)
483
484
485
486
488 """
489 Parse command-line arguments for commands that take a service name
490 only. Will cause a system exit if command-line argument parsing
491 fails.
492 @param cmd: command name, e.g. 'type'
493 @type cmd: str
494 @param argv: command-line arguments
495 @type argv: [str]
496 """
497 args = argv[2:]
498 parser = OptionParser(usage="usage: %%prog %s /service"%cmd, prog=NAME)
499 (options, args) = parser.parse_args(args)
500 if len(args) == 0:
501 parser.error("service must be specified")
502 if len(args) > 1:
503 parser.error("you may only specify one input service")
504 return roslib.scriptutil.script_resolve_name('rosservice', args[0])
505
507 """
508 Parse 'type' command arguments and run command Will cause a system
509 exit if command-line argument parsing fails.
510 @param argv: command-line arguments
511 @type argv: [str]
512 @raise ROSServiceException: if type command cannot be executed
513 """
514 _rosservice_type(_optparse_service_only('type', argv=argv))
515
517 """
518 Parse 'uri' command arguments and run command. Will cause a system
519 exit if command-line argument parsing fails.
520 @param argv: command-line arguments
521 @type argv: [str]
522 @raise ROSServiceException: if uri command cannot be executed
523 """
524 _rosservice_uri(_optparse_service_only('uri', argv=argv))
525
527 """
528 Parse 'node' command arguments and run command. Will cause a system
529 exit if command-line argument parsing fails.
530 @param argv: command-line arguments
531 @type argv: [str]
532 @raise ROSServiceException: if node command cannot be executed
533 """
534 _rosservice_node(_optparse_service_only('node', argv=argv))
535
537 """
538 Parse 'args' command arguments and run command. Will cause a system
539 exit if command-line argument parsing fails.
540 @param argv: command-line arguments
541 @type argv: [str]
542 @raise ROSServiceException: if args command cannot be executed
543 """
544 _rosservice_args(_optparse_service_only('args', argv=argv))
545
547 """
548 Parse 'call' command arguments and run command. Will cause a system
549 exit if command-line argument parsing fails.
550 @param argv: command-line arguments
551 @type argv: [str]
552 @raise ROSServiceException: if call command cannot be executed
553 """
554 try:
555 import yaml
556 except ImportError, e:
557 raise ROSServiceException("Cannot import yaml. Please make sure the pyyaml system dependency is installed")
558
559 args = argv[2:]
560 parser = OptionParser(usage="usage: %prog call /service [args...]", prog=NAME)
561 parser.add_option("-v", dest="verbose", default=False,
562 action="store_true",
563 help="print verbose output")
564 parser.add_option("--wait", dest="wait", default=False,
565 action="store_true",
566 help="wait for service to be advertised")
567
568 (options, args) = parser.parse_args(args)
569 if len(args) == 0:
570 parser.error("service must be specified")
571 service_name = args[0]
572
573 if options.wait:
574
575
576
577 master = roslib.scriptutil.get_master()
578 try:
579 code, msg, service_uri = master.getPid('/')
580 except socket.error:
581 raise ROSServiceIOException("Unable to communicate with master!")
582 rospy.wait_for_service(service_name)
583
584
585 service_name = roslib.scriptutil.script_resolve_name('rosservice', args[0])
586 service_class = get_service_class_by_name(service_name)
587
588
589 service_args = []
590 for arg in args[1:]:
591
592 if arg == '':
593 arg = "''"
594 service_args.append(yaml.load(arg))
595 if not service_args and has_service_args(service_name, service_class=service_class):
596 if sys.stdin.isatty():
597 parser.error("Please specify service arguments")
598 import rostopic
599 for service_args in rostopic.stdin_yaml_arg():
600 if service_args:
601
602 if type(service_args) != list:
603 service_args = [service_args]
604 try:
605 _rosservice_call(service_name, service_args, verbose=options.verbose, service_class=service_class)
606 except ValueError, e:
607 print >> sys.stderr, str(e)
608 break
609 else:
610 _rosservice_call(service_name, service_args, verbose=options.verbose, service_class=service_class)
611
613 """
614 @return: iterator for next set of service args on stdin. Iterator returns a list of args for each call.
615 @rtype: iterator
616 """
617 import yaml
618 import select
619 loaded = None
620 poll = select.poll()
621 poll.register(sys.stdin, select.POLLIN)
622 try:
623 arg = 'x'
624 while not rospy.is_shutdown() and arg != '\n':
625 buff = ''
626 while arg != '\n' and arg.strip() != '---':
627 val = poll.poll(1.0)
628 if not val:
629 continue
630 arg = sys.stdin.readline() + '\n'
631 if arg.startswith('... logging'):
632
633 continue
634 elif arg.strip() != '---':
635 buff = buff + arg
636 try:
637 loaded = yaml.load(buff.rstrip())
638 except Exception, e:
639 print >> sys.stderr, "Invalid YAML: %s"%str(e)
640 if loaded is not None:
641 yield loaded
642 else:
643
644 return
645
646
647 arg = 'x'
648 except select.error:
649 return
650
652 """
653 Parse 'list' command arguments and run command
654 Will cause a system exit if command-line argument parsing fails.
655 @param argv: command-line arguments
656 @type argv: [str]
657 @raise ROSServiceException: if list command cannot be executed
658 """
659 args = argv[2:]
660 parser = OptionParser(usage="usage: %prog list [/namespace]", prog=NAME)
661 parser.add_option("-n", "--nodes",
662 dest="print_nodes", default=False, action="store_true",
663 help="print nodes that provide service(s)")
664 (options, args) = parser.parse_args(args)
665
666 namespace = None
667 if len(args) == 1:
668 namespace = roslib.scriptutil.script_resolve_name('rosservice', args[0])
669 elif len(args) > 1:
670 parser.error("you may only specify one input namespace")
671 _rosservice_list(namespace, print_nodes=options.print_nodes)
672
674 """
675 Parse 'info' command arguments and run command
676 Will cause a system exit if command-line argument parsing fails.
677 @param argv: command-line arguments
678 @type argv: [str]
679 @raise ROSServiceException: if list command cannot be executed
680 """
681 args = argv[2:]
682 parser = OptionParser(usage="usage: %prog info /service", prog=NAME)
683 (options, args) = parser.parse_args(args)
684
685 name = None
686 if len(args) == 1:
687 name = roslib.scriptutil.script_resolve_name('rosservice', args[0])
688 elif len(args) > 1:
689 parser.error("you may only specify one service")
690 elif not len(args):
691 parser.error("you must specify a service name")
692 _rosservice_info(name)
693
695 """Print generic usage for rosservice"""
696 print """Commands:
697 \trosservice args\tprint service arguments
698 \trosservice call\tcall the service with the provided args
699 \trosservice find\tfind services by service type
700 \trosservice info\tprint information about service
701 \trosservice list\tlist active services
702 \trosservice type\tprint service type
703 \trosservice uri\tprint service ROSRPC uri
704
705 Type rosservice <command> -h for more detailed usage, e.g. 'rosservice call -h'
706 """
707 sys.exit(os.EX_USAGE)
708
709 -def rosservicemain(argv=sys.argv):
710 """
711 main entry point for rosservice command-line tool
712
713 @param argv: command-line args
714 @type argv: [str]
715 """
716 if len(argv) == 1:
717 _fullusage()
718 try:
719 command = argv[1]
720 if command in 'list':
721 _rosservice_cmd_list(argv)
722 elif command == 'info':
723 _rosservice_cmd_info(argv)
724 elif command == 'type':
725 _rosservice_cmd_type(argv)
726 elif command == 'uri':
727 _rosservice_cmd_uri(argv)
728 elif command == 'node':
729 _rosservice_cmd_node(argv)
730 elif command == 'call':
731 _rosservice_cmd_call(argv)
732 elif command == 'args':
733 _rosservice_cmd_args(argv)
734 elif command == 'find':
735 _rosservice_cmd_find(argv)
736 else:
737 _fullusage()
738 except socket.error:
739 print >> sys.stderr, "Network communication failed with the master or a node."
740 sys.exit(1)
741 except ROSServiceException, e:
742 print >> sys.stderr, "ERROR: "+str(e)
743 sys.exit(2)
744 except KeyboardInterrupt:
745 pass
746