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