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 as BufferType
54 except ImportError:
55 from io import BytesIO as BufferType
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, BufferType(), 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 try:
326 t = get_service_type(s)
327 except ROSServiceIOException:
328 continue
329 if t == service_type:
330 matches.append(s)
331 except socket.error:
332 raise ROSServiceIOException("Unable to communicate with master!")
333 return matches
334
336 """
337 Implements 'rosservice type'
338
339 @param argv: command-line args
340 @type argv: [str]
341 """
342 args = argv[2:]
343 parser = OptionParser(usage="usage: %prog find msg-type", prog=NAME)
344 options, args = parser.parse_args(args)
345 if not len(args):
346 parser.error("please specify a message type")
347 if len(args) > 1:
348 parser.error("you may only specify one message type")
349 print('\n'.join(rosservice_find(args[0])))
350
352 """
353 pkg/typeName -> pkg, typeName -> None
354
355 :param name: package resource name, e.g. 'std_msgs/String', ``str``
356 :returns: package name of resource, ``str``
357 """
358 if not '/' in name:
359 return None
360 return name[:name.find('/')]
361
363 """
364 Get the service class using the name of the service. NOTE: this
365 call results in a probe call to the service.
366 @param service_name: fully-resolved name of service to call
367 @type service_name: str
368 @return: service class
369 @rtype: ServiceDefinition: service class
370 @raise ROSServiceException: if service class cannot be retrieved
371 """
372
373 service_type = get_service_type(service_name)
374 if not service_type:
375
376 srvs = get_service_list()
377 if service_name not in srvs:
378 raise ROSServiceException("Service [%s] is not available."%service_name)
379 else:
380 raise ROSServiceException("Unable to determine type of service [%s]."%service_name)
381
382
383 service_class = roslib.message.get_service_class(service_type)
384
385
386 if service_class and service_type.endswith('Request') and \
387 not hasattr(service_class, "_request_class"):
388 service_type = service_type[:-7]
389 service_class = roslib.message.get_service_class(service_type)
390
391 if service_class is None:
392 pkg = _resource_name_package(service_type)
393 raise ROSServiceException("Unable to load type [%s].\n"%service_type+
394 "Have you typed 'make' in [%s]?"%pkg)
395 return service_class
396
397 -def call_service(service_name, service_args, service_class=None):
398 """
399 Call the specified service_name
400 @param service_name: fully-resolved name of service to call
401 @type service_name: str
402 @param service_args: args to pass to service
403 @type service_args: [any]
404 @param service_class: (optional) service type class. If this
405 argument is provided, it saves a probe call against the service
406 @type service_class: Message class
407 @return: service request, service response
408 @rtype: Message, Message
409 @raise ROSServiceException: if call command cannot be executed
410 """
411
412
413 import std_msgs.msg
414 rospy.init_node('rosservice', anonymous=True)
415
416 if service_class is None:
417 service_class = get_service_class_by_name(service_name)
418 request = service_class._request_class()
419 try:
420 now = rospy.get_rostime()
421 keys = { 'now': now, 'auto': std_msgs.msg.Header(stamp=now) }
422 genpy.message.fill_message_args(request, service_args, keys=keys)
423 except genpy.MessageException as e:
424 def argsummary(args):
425 if type(args) in [tuple, list]:
426 return '\n'.join([' * %s (type %s)'%(a, type(a).__name__) for a in args])
427 else:
428 return ' * %s (type %s)'%(args, type(args).__name__)
429
430 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)))
431 try:
432 return request, rospy.ServiceProxy(service_name, service_class)(request)
433 except rospy.ServiceException as e:
434 raise ROSServiceException(str(e))
435 except genpy.SerializationError 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 except rospy.ROSSerializationException as e:
439 raise ROSServiceException("Unable to send request. One of the fields has an incorrect type:\n"+\
440 " %s\n\nsrv file:\n%s"%(e, rosmsg.get_srv_text(service_class._type)))
441
442 -def _rosservice_call(service_name, service_args, verbose=False, service_class=None):
443 """
444 Implements 'rosservice call'
445 @param service_name: name of service to call
446 @type service_name: str
447 @param service_args: arguments to call service with
448 @type service_args: [args]
449 @param verbose: if True, print extra output
450 @type verbose: bool
451 @param service_class Message class: (optional) service type
452 class. If this argument is provided, it saves a probe call against
453 the service
454 @type service_class: Message class
455 @raise ROSServiceException: if call command cannot be executed
456 """
457 service_name = rosgraph.names.script_resolve_name('rosservice', service_name)
458 request, response = call_service(service_name, service_args, service_class=service_class)
459 if verbose:
460 print(str(request))
461 print('---')
462 print(str(response))
463
465 """
466 Check if service requires arguments
467 @param service_name: name of service being called
468 @type service_name: str
469 @param service_class: (optional) service type class. If this
470 argument is provided, it saves a probe call against the service
471 @type service_class: Message class
472 @return: True if service_name has request arguments
473 @rtype: bool
474 """
475 service_name = rosgraph.names.script_resolve_name('rosservice', service_name)
476 if service_class is None:
477 service_class = get_service_class_by_name(service_name)
478 return len(service_class._request_class.__slots__) > 0
479
481 """
482 Implements 'rosservice args'
483 @param service_name: name of service to get arguments for
484 @type service_name: str
485 @raise ROSServiceException: if call command cannot be executed
486 """
487 print(get_service_args(service_name))
488
490 """
491 Implements 'get service args'
492 @param service_name: name of service to get arguments for
493 @type service_name: str
494 @raise ROSServiceException: if call command cannot be executed
495 """
496 service_name = rosgraph.names.script_resolve_name('rosservice', service_name)
497 service_class = get_service_class_by_name(service_name)
498 return genpy.message.get_printable_message_args(service_class._request_class)
499
500
501
502
504 """
505 Parse command-line arguments for commands that take a service name
506 only. Will cause a system exit if command-line argument parsing
507 fails.
508 @param cmd: command name, e.g. 'type'
509 @type cmd: str
510 @param argv: command-line arguments
511 @type argv: [str]
512 """
513 args = argv[2:]
514 parser = OptionParser(usage="usage: %%prog %s /service"%cmd, prog=NAME)
515 (options, args) = parser.parse_args(args)
516 if len(args) == 0:
517 parser.error("service must be specified")
518 if len(args) > 1:
519 parser.error("you may only specify one input service")
520 return rosgraph.names.script_resolve_name('rosservice', args[0])
521
523 """
524 Parse 'type' command arguments and run command Will cause a system
525 exit if command-line argument parsing fails.
526 @param argv: command-line arguments
527 @type argv: [str]
528 @raise ROSServiceException: if type command cannot be executed
529 """
530 _rosservice_type(_optparse_service_only('type', argv=argv))
531
533 """
534 Parse 'uri' command arguments and run command. Will cause a system
535 exit if command-line argument parsing fails.
536 @param argv: command-line arguments
537 @type argv: [str]
538 @raise ROSServiceException: if uri command cannot be executed
539 """
540 _rosservice_uri(_optparse_service_only('uri', argv=argv))
541
543 """
544 Parse 'node' command arguments and run command. Will cause a system
545 exit if command-line argument parsing fails.
546 @param argv: command-line arguments
547 @type argv: [str]
548 @raise ROSServiceException: if node command cannot be executed
549 """
550 _rosservice_node(_optparse_service_only('node', argv=argv))
551
553 """
554 Parse 'args' command arguments and run command. Will cause a system
555 exit if command-line argument parsing fails.
556 @param argv: command-line arguments
557 @type argv: [str]
558 @raise ROSServiceException: if args command cannot be executed
559 """
560 _rosservice_args(_optparse_service_only('args', argv=argv))
561
563 """
564 Parse 'call' command arguments and run command. Will cause a system
565 exit if command-line argument parsing fails.
566 @param argv: command-line arguments
567 @type argv: [str]
568 @raise ROSServiceException: if call command cannot be executed
569 """
570 try:
571 import yaml
572 except ImportError as e:
573 raise ROSServiceException("Cannot import yaml. Please make sure the pyyaml system dependency is installed")
574
575 args = argv[2:]
576 parser = OptionParser(usage="usage: %prog call /service [args...]", prog=NAME)
577 parser.add_option("-v", dest="verbose", default=False,
578 action="store_true",
579 help="print verbose output")
580 parser.add_option("--wait", dest="wait", default=False,
581 action="store_true",
582 help="wait for service to be advertised")
583
584 (options, args) = parser.parse_args(args)
585 if len(args) == 0:
586 parser.error("service must be specified")
587 service_name = args[0]
588
589 if options.wait:
590
591
592
593 master = _get_master()
594 try:
595 service_uri = master.getPid()
596 except socket.error:
597 raise ROSServiceIOException("Unable to communicate with master!")
598 rospy.wait_for_service(service_name)
599
600
601 service_name = rosgraph.names.script_resolve_name('rosservice', args[0])
602 service_class = get_service_class_by_name(service_name)
603
604
605 service_args = []
606 for arg in args[1:]:
607
608 if arg == '':
609 arg = "''"
610 service_args.append(yaml.safe_load(arg))
611 if not service_args and has_service_args(service_name, service_class=service_class):
612 if sys.stdin.isatty():
613 parser.error("Please specify service arguments")
614 import rostopic
615 for service_args in rostopic.stdin_yaml_arg():
616 if service_args:
617
618 if type(service_args) != list:
619 service_args = [service_args]
620 try:
621 _rosservice_call(service_name, service_args, verbose=options.verbose, service_class=service_class)
622 except ValueError as e:
623 print(str(e), file=sys.stderr)
624 break
625 else:
626 _rosservice_call(service_name, service_args, verbose=options.verbose, service_class=service_class)
627
629 """
630 @return: iterator for next set of service args on stdin. Iterator returns a list of args for each call.
631 @rtype: iterator
632 """
633 import yaml
634 import select
635 loaded = None
636 poll = select.poll()
637 poll.register(sys.stdin, select.POLLIN)
638 try:
639 arg = 'x'
640 while not rospy.is_shutdown() and arg != '\n':
641 buff = ''
642 while arg != '\n' and arg.strip() != '---':
643 val = poll.poll(1.0)
644 if not val:
645 continue
646 arg = sys.stdin.readline() + '\n'
647 if arg.startswith('... logging'):
648
649 continue
650 elif arg.strip() != '---':
651 buff = buff + arg
652 try:
653 loaded = yaml.safe_load(buff.rstrip())
654 except Exception as e:
655 print("Invalid YAML: %s"%str(e), file=sys.stderr)
656 if loaded is not None:
657 yield loaded
658 else:
659
660 return
661
662
663 arg = 'x'
664 except select.error:
665 return
666
668 """
669 Parse 'list' command arguments and run command
670 Will cause a system exit if command-line argument parsing fails.
671 @param argv: command-line arguments
672 @type argv: [str]
673 @raise ROSServiceException: if list command cannot be executed
674 """
675 args = argv[2:]
676 parser = OptionParser(usage="usage: %prog list [/namespace]", prog=NAME)
677 parser.add_option("-n", "--nodes",
678 dest="print_nodes", default=False, action="store_true",
679 help="print nodes that provide service(s)")
680 (options, args) = parser.parse_args(args)
681
682 namespace = None
683 if len(args) == 1:
684 namespace = rosgraph.names.script_resolve_name('rosservice', args[0])
685 elif len(args) > 1:
686 parser.error("you may only specify one input namespace")
687 _rosservice_list(namespace, print_nodes=options.print_nodes)
688
690 """
691 Parse 'info' command arguments and run command
692 Will cause a system exit if command-line argument parsing fails.
693 @param argv: command-line arguments
694 @type argv: [str]
695 @raise ROSServiceException: if list command cannot be executed
696 """
697 args = argv[2:]
698 parser = OptionParser(usage="usage: %prog info /service", prog=NAME)
699 (options, args) = parser.parse_args(args)
700
701 name = None
702 if len(args) == 1:
703 name = rosgraph.names.script_resolve_name('rosservice', args[0])
704 elif len(args) > 1:
705 parser.error("you may only specify one service")
706 elif not len(args):
707 parser.error("you must specify a service name")
708 _rosservice_info(name)
709
711 """Print generic usage for rosservice"""
712 print("""Commands:
713 \trosservice args\tprint service arguments
714 \trosservice call\tcall the service with the provided args
715 \trosservice find\tfind services by service type
716 \trosservice info\tprint information about service
717 \trosservice list\tlist active services
718 \trosservice type\tprint service type
719 \trosservice uri\tprint service ROSRPC uri
720
721 Type rosservice <command> -h for more detailed usage, e.g. 'rosservice call -h'
722 """)
723 sys.exit(getattr(os, 'EX_USAGE', 1))
724
725 -def rosservicemain(argv=sys.argv):
726 """
727 main entry point for rosservice command-line tool
728
729 @param argv: command-line args
730 @type argv: [str]
731 """
732 if len(argv) == 1:
733 _fullusage()
734 try:
735
736 argv = rospy.myargv(argv)
737 command = argv[1]
738 if command == 'list':
739 _rosservice_cmd_list(argv)
740 elif command == 'info':
741 _rosservice_cmd_info(argv)
742 elif command == 'type':
743 _rosservice_cmd_type(argv)
744 elif command == 'uri':
745 _rosservice_cmd_uri(argv)
746 elif command == 'node':
747 _rosservice_cmd_node(argv)
748 elif command == 'call':
749 _rosservice_cmd_call(argv)
750 elif command == 'args':
751 _rosservice_cmd_args(argv)
752 elif command == 'find':
753 _rosservice_cmd_find(argv)
754 else:
755 _fullusage()
756 except socket.error:
757 print("Network communication failed with the master or a node.", file=sys.stderr)
758 sys.exit(1)
759 except ROSServiceException as e:
760 print("ERROR: "+str(e), file=sys.stderr)
761 sys.exit(2)
762 except rosgraph.MasterException as e:
763 print("ERROR: "+str(e), file=sys.stderr)
764 sys.exit(2)
765 except KeyboardInterrupt:
766 pass
767