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 rosnode implements the rosnode command-line tool and also provides a
38 library for retrieving ROS Node information.
39 """
40
41 NAME='rosnode'
42 ID = '/rosnode'
43
44 import os
45 import sys
46 import socket
47 import time
48 import xmlrpclib
49
50 from optparse import OptionParser
51
52 import roslib.scriptutil as scriptutil
53
55 """
56 rosnode base exception type
57 """
58 pass
60 """
61 Exceptions for communication-related (i/o) errors, generally due to Master or Node network communication issues.
62 """
63 pass
64
66 code, msg, val = args
67 if code != 1:
68 raise ROSNodeException("remote call failed: %s"%msg)
69 return val
70
71 _caller_apis = {}
73 """
74 @param master: XMLRPC handle to ROS Master
75 @type master: xmlrpclib.ServerProxy
76 @param caller_id: node name
77 @type caller_id: str
78 @return: xmlrpc URI of caller_id
79 @rtype: str
80 @raise ROSNodeIOException: if unable to communicate with master
81 """
82 caller_api = _caller_apis.get(caller_id, None)
83 if not caller_api:
84 try:
85 code, msg, caller_api = master.lookupNode(ID, caller_id)
86 if code != 1:
87 return None
88 else:
89 _caller_apis[caller_id] = caller_api
90 except socket.error:
91 raise ROSNodeIOException("Unable to communicate with master!")
92 return caller_api
93
95 """
96 @param namespace: optional namespace to scope return values by. Namespace must already be resolved.
97 @type namespace: str
98 @return: list of node caller IDs
99 @rtype: [str]
100 @raise ROSNodeIOException: if unable to communicate with master
101 """
102 master = scriptutil.get_master()
103 try:
104 state = _succeed(master.getSystemState(ID))
105 except socket.error:
106 raise ROSNodeIOException("Unable to communicate with master!")
107 nodes = []
108 import itertools
109 if namespace:
110
111 import roslib.names
112 g_ns = roslib.names.make_global_ns(namespace)
113 for s in state:
114 for t, l in s:
115 nodes.extend([n for n in l if n.startswith(g_ns) or n == namespace])
116 else:
117 for s in state:
118 for t, l in s:
119 nodes.extend(l)
120 return list(set(nodes))
121
123 """
124 Find nodes by machine name. This is a very costly procedure as it
125 must do N lookups with the Master, where N is the number of nodes.
126
127 @return: list of nodes on the specified machine
128 @rtype: [str]
129 @raise ROSNodeException: if machine name cannot be resolved to an address
130 @raise ROSNodeIOException: if unable to communicate with master
131 """
132 import urlparse
133
134 master = scriptutil.get_master()
135 try:
136 machine_actual = socket.gethostbyname(machine)
137 except:
138 raise ROSNodeException("cannot resolve machine name [%s] to address"%machine)
139
140
141
142
143 matches = [machine, machine_actual]
144 not_matches = []
145 node_names = get_node_names()
146 retval = []
147 for n in node_names:
148 try:
149 code, msg, uri = master.lookupNode(ID, n)
150
151 if code != 1:
152 continue
153
154 h = urlparse.urlparse(uri).hostname
155 if h in matches:
156 retval.append(n)
157 elif h in not_matches:
158 continue
159 else:
160 r = socket.gethostbyname(h)
161 if r == machine_actual:
162 matches.append(r)
163 retval.append(n)
164 else:
165 not_matches.append(r)
166 except socket.error:
167 raise ROSNodeIOException("Unable to communicate with master!")
168 return retval
169
171 """
172 Call shutdown on the specified nodes
173
174 @return: list of nodes that shutdown was called on successfully and list of failures
175 @rtype: ([str], [str])
176 """
177 master = scriptutil.get_master()
178
179 success = []
180 fail = []
181 tocall = []
182 try:
183
184 for n in node_names:
185 try:
186 uri = _succeed(master.lookupNode(ID, n))
187 tocall.append([n, uri])
188 except:
189 fail.append(n)
190 except socket.error:
191 raise ROSNodeIOException("Unable to communicate with master!")
192
193 for n, uri in tocall:
194
195
196 try:
197 p = xmlrpclib.ServerProxy(uri)
198 _succeed(p.shutdown(ID, 'user request'))
199 except:
200 pass
201 success.append(n)
202
203 return success, fail
204
206 """
207 Subroutine for rosnode_listnodes(). Composes list of strings to print to screen.
208
209 @param namespace: (default None) namespace to scope list to.
210 @type namespace: str
211 @param list_uri: (default False) return uris of nodes instead of names.
212 @type list_uri: bool
213 @param list_all: (default False) return names and uris of nodes as combined strings
214 @type list_all: bool
215 @return: new-line separated string containing list of all nodes
216 @rtype: str
217 """
218 master = scriptutil.get_master()
219 nodes = get_node_names(namespace)
220 nodes.sort()
221 if list_all:
222 return '\n'.join(["%s \t%s"%(get_api_uri(master, n) or 'unknown address', n) for n in nodes])
223 elif list_uri:
224 return '\n'.join([(get_api_uri(master, n) or 'unknown address') for n in nodes])
225 else:
226 return '\n'.join(nodes)
227
229 """
230 Print list of all ROS nodes to screen.
231
232 @param namespace: namespace to scope list to
233 @type namespace: str
234 @param list_uri: print uris of nodes instead of names
235 @type list_uri: bool
236 @param list_all: print node names and uris
237 @param list_all: bool
238 """
239 print _sub_rosnode_listnodes(namespace=namespace, list_uri=list_uri, list_all=list_all)
240
242 """
243 Test connectivity to node by calling its XMLRPC API
244 @param node_name: name of node to ping
245 @type node_name: str
246 @param max_count: number of ping requests to make
247 @type max_count: int
248 @param verbose: print ping information to screen
249 @type verbose: bool
250 @return: True if node pinged
251 @rtype: bool
252 """
253 master = scriptutil.get_master()
254 node_api = get_api_uri(master,node_name)
255 if not node_api:
256 print >> sys.stderr, "cannot ping [%s]: unknown node"%node_name
257 return False
258
259 timeout = 3.
260
261 if verbose:
262 print "pinging %s with a timeout of %ss"%(node_name, timeout)
263 socket.setdefaulttimeout(timeout)
264 node = xmlrpclib.ServerProxy(node_api)
265 lastcall = 0.
266 count = 0
267 acc = 0.
268 try:
269 while True:
270 try:
271 start = time.time()
272 pid = _succeed(node.getPid(ID))
273 end = time.time()
274
275 dur = (end-start)*1000.
276 acc += dur
277 count += 1
278
279 if verbose:
280 print "xmlrpc reply from %s\ttime=%fms"%(node_api, dur)
281
282 except socket.error:
283 print >> sys.stderr, "connection to [%s] timed out"%node_name
284 return False
285 if max_count and count >= max_count:
286 break
287 time.sleep(1.0)
288 except KeyboardInterrupt:
289 pass
290
291 if verbose and count > 1:
292 print "ping average: %fms"%(acc/count)
293 return True
294
296 """
297 Ping all running nodes
298 @return [str], [str]: pinged nodes, un-pingable nodes
299 @raise ROSNodeIOException: if unable to communicate with master
300 """
301 master = scriptutil.get_master()
302 try:
303 state = _succeed(master.getSystemState(ID))
304 except socket.error:
305 raise ROSNodeIOException("Unable to communicate with master!")
306
307 nodes = []
308 import itertools
309 for s in state:
310 for t, l in s:
311 nodes.extend(l)
312 nodes = list(set(nodes))
313 if verbose:
314 print "Will ping the following nodes: \n"+''.join([" * %s\n"%n for n in nodes])
315 pinged = []
316 unpinged = []
317 for node in nodes:
318 if rosnode_ping(node, max_count=1, verbose=verbose):
319 pinged.append(node)
320 else:
321 unpinged.append(node)
322 return pinged, unpinged
323
325 """
326 Remove registrations from ROS Master that do not match blacklist.
327 @param master: XMLRPC handle to ROS Master
328 @type master: xmlrpclib.ServerProxy
329 @param blacklist: list of nodes to scrub
330 @type blacklist: [str]
331 """
332 pubs, subs, srvs = _succeed(master.getSystemState(ID))
333 for n in blacklist:
334 print "Unregistering", n
335 node_api = get_api_uri(master, n)
336 for t, l in pubs:
337 if n in l:
338 _succeed(master.unregisterPublisher(n, t, node_api))
339 for t, l in subs:
340 if n in l:
341 _succeed(master.unregisterSubscriber(n, t, node_api))
342 for s, l in srvs:
343 if n in l:
344 service_api = _succeed(master.lookupService(ID, s))
345 _succeed(master.unregisterService(n, s, service_api))
346
348 """
349 Remove registrations from ROS Master that do not match whitelist.
350 @param master: XMLRPC handle to ROS Master
351 @type master: xmlrpclib.ServerProxy
352 @param whitelist: list of nodes to keep
353 @type whitelist: list of nodes to keep
354 """
355 pubs, subs, srvs = _succeed(master.getSystemState(ID))
356 for t, l in pubs:
357 for n in l:
358 if n not in whitelist:
359 node_api = get_api_uri(master, n)
360 _succeed(master.unregisterPublisher(n, t, node_api))
361 for t, l in subs:
362 for n in l:
363 if n not in whitelist:
364 node_api = get_api_uri(master, n)
365 _succeed(master.unregisterSubscriber(n, t, node_api))
366 for s, l in srvs:
367 for n in l:
368 if n not in whitelist:
369 service_api = _succeed(master.lookupService(ID, s))
370 _succeed(master.unregisterService(n, s, service_api))
371
373 """
374 This is a semi-hidden routine for cleaning up stale node
375 registration information on the ROS Master. The intent is to
376 remove this method once Master TTLs are properly implemented.
377 """
378 pinged, unpinged = rosnode_ping_all()
379 if unpinged:
380 master = scriptutil.get_master()
381 print "Unable to contact the following nodes:"
382 print '\n'.join(' * %s'%n for n in unpinged)
383 print "cleanup will purge all information about these nodes from the master"
384 print "Please type y or n to continue"
385 input = sys.stdin.readline()
386 while not input.strip() in ['y', 'n']:
387 input = sys.stdin.readline()
388 if input.strip() == 'n':
389 print 'aborting'
390 return
391
392 cleanup_master_blacklist(master, unpinged)
393
394 print "done"
395
397 def topic_type(t, pub_topics):
398 matches = [t_type for t_name, t_type in pub_topics if t_name == t]
399 if matches:
400 return matches[0]
401 return 'unknown type'
402
403 master = scriptutil.get_master()
404
405
406 try:
407 state = _succeed(master.getSystemState(ID))
408 pub_topics = _succeed(scriptutil.get_master().getPublishedTopics(ID, '/'))
409 except socket.error:
410 raise ROSNodeIOException("Unable to communicate with master!")
411 pubs = [t for t, l in state[0] if node_name in l]
412 subs = [t for t, l in state[1] if node_name in l]
413 srvs = [t for t, l in state[2] if node_name in l]
414
415 buff = "Node [%s]"%node_name
416 if pubs:
417 buff += "\nPublications: \n"
418 buff += '\n'.join([" * %s [%s]"%(l, topic_type(l, pub_topics)) for l in pubs]) + '\n'
419 else:
420 buff += "\nPublications: None\n"
421 if subs:
422 buff += "\nSubscriptions: \n"
423 buff += '\n'.join([" * %s [%s]"%(l, topic_type(l, pub_topics)) for l in subs]) + '\n'
424 else:
425 buff += "\nSubscriptions: None\n"
426 if srvs:
427 buff += "\nServices: \n"
428 buff += '\n'.join([" * %s"%l for l in srvs]) + '\n'
429 else:
430 buff += "\nServices: None\n"
431
432 return buff
433
435
436 socket.setdefaulttimeout(5.0)
437 node = xmlrpclib.ServerProxy(node_api)
438
439 try:
440 pid = _succeed(node.getPid(ID))
441 buff = "Pid: %s\n"%pid
442
443 businfo = _succeed(node.getBusInfo(ID))
444 if businfo:
445 buff += "Connections:\n"
446 for info in businfo:
447 dest_id = info[1]
448 direction = info[2]
449 transport = info[3]
450 topic = info[4]
451 if len(info) > 5:
452 connected = info[5]
453 else:
454 connected = True
455
456 if connected:
457 buff += " * topic: %s\n"%topic
458
459
460 buff += " * to: %s\n"%dest_id
461 if direction == 'i':
462 buff += " * direction: inbound\n"
463 elif direction == 'o':
464 buff += " * direction: outbound\n"
465 else:
466 buff += " * direction: unknown\n"
467 buff += " * transport: %s\n"%transport
468 except socket.error:
469 raise ROSNodeIOException("Communication with node[%s] failed!"%(node_api))
470 return buff
471
473 """
474 Print information about node, including subscriptions and other debugging information. This will query the node over the network.
475
476 @param node_name: name of ROS node
477 @type node_name: str
478 @raise ROSNodeIOException: if unable to communicate with master
479 """
480 def topic_type(t, pub_topics):
481 matches = [t_type for t_name, t_type in pub_topics if t_name == t]
482 if matches:
483 return matches[0]
484 return 'unknown type'
485
486 master = scriptutil.get_master()
487 node_name = scriptutil.script_resolve_name('rosnode', node_name)
488
489 print '-'*80
490 print get_node_info_description(node_name)
491
492 node_api = get_api_uri(master, node_name)
493 if not node_api:
494 print >> sys.stderr, "cannot contact [%s]: unknown node"%node_name
495 return
496
497 print "\ncontacting node %s ..."%node_api
498
499 print get_node_connection_info_description(node_api)
500
501
502 rosnode_debugnode = rosnode_info
503
505 """
506 Implements rosnode 'list' command.
507 """
508 args = argv[2:]
509 parser = OptionParser(usage="usage: %prog list", prog=NAME)
510 parser.add_option("-u",
511 dest="list_uri", default=False,
512 action="store_true",
513 help="list XML-RPC URIs")
514 parser.add_option("-a","--all",
515 dest="list_all", default=False,
516 action="store_true",
517 help="list all information")
518 (options, args) = parser.parse_args(args)
519 namespace = None
520 if len(args) > 1:
521 parser.error("invalid args: you may only specify one namespace")
522 elif len(args) == 1:
523 namespace = scriptutil.script_resolve_name('rostopic', args[0])
524 rosnode_listnodes(namespace=namespace, list_uri=options.list_uri, list_all=options.list_all)
525
527 """
528 Implements rosnode 'info' command.
529 """
530 args = argv[2:]
531 parser = OptionParser(usage="usage: %prog info node1 [node2...]", prog=NAME)
532 (options, args) = parser.parse_args(args)
533 if not args:
534 parser.error("You must specify at least one node name")
535 for node in args:
536 rosnode_info(node)
537
539 """
540 Implements rosnode 'machine' command.
541
542 @raise ROSNodeException: if user enters in unrecognized machine name
543 """
544 args = argv[2:]
545 parser = OptionParser(usage="usage: %prog machine <machine-name>", prog=NAME)
546 (options, args) = parser.parse_args(args)
547 if len(args) == 0:
548 parser.error("please enter a machine name")
549 elif len(args) > 1:
550 parser.error("please enter only one machine name")
551 nodes = get_nodes_by_machine(args[0])
552 nodes.sort()
553 print '\n'.join(nodes)
554
556 """
557 Implements rosnode 'kill' command.
558
559 @raise ROSNodeException: if user enters in unrecognized nodes
560 """
561 args = argv[2:]
562 parser = OptionParser(usage="usage: %prog kill [node]...", prog=NAME)
563 parser.add_option("-a","--all",
564 dest="kill_all", default=False,
565 action="store_true",
566 help="kill all nodes")
567
568 (options, args) = parser.parse_args(args)
569 if options.kill_all:
570 if args:
571 parser.error("invalid arguments with kill all (-a) option")
572 args = get_node_names()
573 args.sort()
574 elif not args:
575 node_list = get_node_names()
576 node_list.sort()
577 if not node_list:
578 print >> sys.stderr, "No nodes running"
579 return 0
580
581 sys.stdout.write('\n'.join(["%s. %s"%(i+1, n) for i,n in enumerate(node_list)]))
582 sys.stdout.write("\n\nPlease enter the number of the node you wish to kill.\n> ")
583 selection = ''
584 while not selection:
585 selection = sys.stdin.readline().strip()
586 try:
587 selection = int(selection)
588 if selection <= 0:
589 print "ERROR: invalid selection. Please enter a number (ctrl-C to cancel)"
590 except:
591 print "ERROR: please enter a number (ctrl-C to cancel)"
592 sys.stdout.flush()
593 selection = ''
594 args = [node_list[selection - 1]]
595 else:
596
597 args = [scriptutil.script_resolve_name(ID, n) for n in args]
598 node_list = get_node_names()
599 unknown = [n for n in args if not n in node_list]
600 if unknown:
601 raise ROSNodeException("Unknown node(s):\n"+'\n'.join([" * %s"%n for n in unknown]))
602 if len(args) > 1:
603 print "killing:\n"+'\n'.join([" * %s"%n for n in args])
604 else:
605 print "killing %s"%(args[0])
606
607 success, fail = kill_nodes(args)
608 if fail:
609 print >> sys.stderr, "ERROR: Failed to kill:\n"+'\n'.join([" * %s"%n for n in fail])
610 return 1
611 print "killed"
612 return 0
613
615 """
616 Implements rosnode 'cleanup' command.
617 @param argv: command-line args
618 @type argv: [str]
619 """
620 args = argv[2:]
621 parser = OptionParser(usage="usage: %prog cleanup", prog=NAME)
622 (options, args) = parser.parse_args(args)
623 rosnode_cleanup()
624
626 """
627 Implements rosnode 'ping' command.
628 @param argv: command-line args
629 @type argv: [str]
630 """
631 args = argv[2:]
632 parser = OptionParser(usage="usage: %prog ping [options] <node>", prog=NAME)
633 parser.add_option("--all", "-a",
634 dest="ping_all", default=False,
635 action="store_true",
636 help="ping all nodes")
637 parser.add_option("-c",
638 dest="ping_count", default=None, metavar="COUNT",type="int",
639 help="number of pings to send. Not available with --all")
640 (options, args) = parser.parse_args(args)
641
642 node_name = None
643 if not options.ping_all:
644 if not args:
645 try:
646 parser.error("Please enter a node to ping. Available nodes are:\n"+_sub_rosnode_listnodes())
647 except:
648
649 parser.error("Please enter a node to ping")
650 elif len(args) > 1:
651 parser.error("you may only specify one input node")
652 elif len(args) == 1:
653 node_name = scriptutil.script_resolve_name('rosnode', args[0])
654 print "rosnode: node is [%s]"%node_name
655 else:
656 if args:
657 parser.error("Invalid arguments '%s' used with --all"%(' '.join(args)))
658 elif options.ping_count:
659 parser.error("-c may not be used with --all")
660
661 if options.ping_all:
662 rosnode_ping_all(verbose=True)
663 else:
664 rosnode_ping(node_name, verbose=True, max_count=options.ping_count)
665
667 """
668 Prints rosnode usage information.
669 """
670 print """rosnode is a command-line tool for printing information about ROS Nodes.
671
672 Commands:
673 \trosnode ping\ttest connectivity to node
674 \trosnode list\tlist active nodes
675 \trosnode info\tprint information about node
676 \trosnode machine\tlist nodes running on a particular machine
677 \trosnode kill\tkill a running node
678
679 Type rosnode <command> -h for more detailed usage, e.g. 'rosnode ping -h'
680 """
681 sys.exit(os.EX_USAGE)
682
683 -def rosnodemain(argv=None):
684 """
685 Prints rosnode main entrypoint.
686 @param argv: override sys.argv
687 @param argv: [str]
688 """
689 if argv == None:
690 argv = sys.argv
691 if len(argv) == 1:
692 _fullusage()
693 try:
694 command = argv[1]
695 if command == 'ping':
696 sys.exit(_rosnode_cmd_ping(argv) or 0)
697 elif command == 'list':
698 sys.exit(_rosnode_cmd_list(argv) or 0)
699 elif command == 'info':
700 sys.exit(_rosnode_cmd_info(argv) or 0)
701 elif command == 'machine':
702 sys.exit(_rosnode_cmd_machine(argv) or 0)
703 elif command == 'cleanup':
704 sys.exit(_rosnode_cmd_cleanup(argv) or 0)
705 elif command == 'kill':
706 sys.exit(_rosnode_cmd_kill(argv) or 0)
707 else:
708 _fullusage()
709 except socket.error:
710 print >> sys.stderr, "Network communication failed. Most likely failed to communicate with master."
711 except ROSNodeException, e:
712 print >> sys.stderr, "ERROR: "+str(e)
713 except KeyboardInterrupt:
714 pass
715