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