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