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
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 @return: True if node pinged
304 @rtype: bool
305 @raise ROSNodeIOException: if unable to communicate with master
306 """
307 master = rosgraph.Master(ID)
308 node_api = get_api_uri(master,node_name)
309 if not node_api:
310 print("cannot ping [%s]: unknown node"%node_name, file=sys.stderr)
311 return False
312
313 timeout = 3.
314
315 if verbose:
316 print("pinging %s with a timeout of %ss"%(node_name, timeout))
317 socket.setdefaulttimeout(timeout)
318 node = ServerProxy(node_api)
319 lastcall = 0.
320 count = 0
321 acc = 0.
322 try:
323 while True:
324 try:
325 start = time.time()
326 pid = _succeed(node.getPid(ID))
327 end = time.time()
328
329 dur = (end-start)*1000.
330 acc += dur
331 count += 1
332
333 if verbose:
334 print("xmlrpc reply from %s\ttime=%fms"%(node_api, dur))
335
336 except socket.error as e:
337
338 try:
339
340 errnum, msg = e.args
341 if errnum == -2:
342 p = urlparse.urlparse(node_api)
343 print("ERROR: Unknown host [%s] for node [%s]"%(p.hostname, node_name), file=sys.stderr)
344 elif errnum == errno.ECONNREFUSED:
345
346 new_node_api = get_api_uri(master,node_name, skip_cache=True)
347 if not new_node_api:
348 print("cannot ping [%s]: unknown node"%node_name, file=sys.stderr)
349 return False
350 if new_node_api != node_api:
351 if verbose:
352 print("node url has changed from [%s] to [%s], retrying to ping"%(node_api, new_node_api))
353 node_api = new_node_api
354 node = ServerProxy(node_api)
355 continue
356 print("ERROR: connection refused to [%s]"%(node_api), file=sys.stderr)
357 else:
358 print("connection to [%s] timed out"%node_name, file=sys.stderr)
359 return False
360 except ValueError:
361 print("unknown network error contacting node: %s"%(str(e)))
362 if max_count and count >= max_count:
363 break
364 time.sleep(1.0)
365 except KeyboardInterrupt:
366 pass
367
368 if verbose and count > 1:
369 print("ping average: %fms"%(acc/count))
370 return True
371
373 """
374 Ping all running nodes
375 @return [str], [str]: pinged nodes, un-pingable nodes
376 @raise ROSNodeIOException: if unable to communicate with master
377 """
378 master = rosgraph.Master(ID)
379 try:
380 state = master.getSystemState()
381 except socket.error:
382 raise ROSNodeIOException("Unable to communicate with master!")
383
384 nodes = []
385 for s in state:
386 for t, l in s:
387 nodes.extend(l)
388 nodes = list(set(nodes))
389 if verbose:
390 print("Will ping the following nodes: \n"+''.join([" * %s\n"%n for n in nodes]))
391 pinged = []
392 unpinged = []
393 for node in nodes:
394 if rosnode_ping(node, max_count=1, verbose=verbose):
395 pinged.append(node)
396 else:
397 unpinged.append(node)
398 return pinged, unpinged
399
401 """
402 Remove registrations from ROS Master that match blacklist.
403 @param master: rosgraph Master instance
404 @type master: rosgraph.Master
405 @param blacklist: list of nodes to scrub
406 @type blacklist: [str]
407 """
408 pubs, subs, srvs = master.getSystemState()
409 for n in blacklist:
410 print("Unregistering", n)
411 node_api = get_api_uri(master, n)
412 for t, l in pubs:
413 if n in l:
414 master_n = rosgraph.Master(n)
415 master_n.unregisterPublisher(t, node_api)
416 for t, l in subs:
417 if n in l:
418 master_n = rosgraph.Master(n)
419 master_n.unregisterSubscriber(t, node_api)
420 for s, l in srvs:
421 if n in l:
422 service_api = master.lookupService(s)
423 master_n = rosgraph.Master(n)
424 master_n.unregisterService(s, service_api)
425
427 """
428 Remove registrations from ROS Master that do not match whitelist.
429 @param master: rosgraph Master instance
430 @type master: rosgraph.Master
431 @param whitelist: list of nodes to keep
432 @type whitelist: list of nodes to keep
433 """
434 pubs, subs, srvs = master.getSystemState()
435 for t, l in pubs:
436 for n in l:
437 if n not in whitelist:
438 node_api = get_api_uri(master, n)
439 master_n = rosgraph.Master(n)
440 master_n.unregisterPublisher(t, node_api)
441 for t, l in subs:
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.unregisterSubscriber(t, node_api)
447 for s, l in srvs:
448 for n in l:
449 if n not in whitelist:
450 service_api = master.lookupService(s)
451 master_n = rosgraph.Master(n)
452 master_n.unregisterService(s, service_api)
453
455 """
456 This is a semi-hidden routine for cleaning up stale node
457 registration information on the ROS Master. The intent is to
458 remove this method once Master TTLs are properly implemented.
459 """
460 pinged, unpinged = rosnode_ping_all()
461 if unpinged:
462 master = rosgraph.Master(ID)
463 print("Unable to contact the following nodes:")
464 print('\n'.join(' * %s'%n for n in unpinged))
465 print("Warning: these might include alive and functioning nodes, e.g. in unstable networks.")
466 print("Cleanup will purge all information about these nodes from the master.")
467 print("Please type y or n to continue:")
468 input = sys.stdin.readline()
469 while not input.strip() in ['y', 'n']:
470 input = sys.stdin.readline()
471 if input.strip() == 'n':
472 print('aborting')
473 return
474
475 cleanup_master_blacklist(master, unpinged)
476
477 print("done")
478
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 = rosgraph.Master(ID)
487
488
489 try:
490 state = master.getSystemState()
491 pub_topics = master.getPublishedTopics('/')
492 except socket.error:
493 raise ROSNodeIOException("Unable to communicate with master!")
494 pubs = sorted([t for t, l in state[0] if node_name in l])
495 subs = sorted([t for t, l in state[1] if node_name in l])
496 srvs = sorted([t for t, l in state[2] if node_name in l])
497
498 buff = "Node [%s]"%node_name
499 if pubs:
500 buff += "\nPublications: \n"
501 buff += '\n'.join([" * %s [%s]"%(l, topic_type(l, pub_topics)) for l in pubs]) + '\n'
502 else:
503 buff += "\nPublications: None\n"
504 if subs:
505 buff += "\nSubscriptions: \n"
506 buff += '\n'.join([" * %s [%s]"%(l, topic_type(l, pub_topics)) for l in subs]) + '\n'
507 else:
508 buff += "\nSubscriptions: None\n"
509 if srvs:
510 buff += "\nServices: \n"
511 buff += '\n'.join([" * %s"%l for l in srvs]) + '\n'
512 else:
513 buff += "\nServices: None\n"
514
515 return buff
516
518
519 socket.setdefaulttimeout(5.0)
520 node = ServerProxy(node_api)
521 system_state = master.getSystemState()
522
523 try:
524 pid = _succeed(node.getPid(ID))
525 buff = "Pid: %s\n"%pid
526
527 businfo = _succeed(node.getBusInfo(ID))
528 if businfo:
529 buff += "Connections:\n"
530 for info in businfo:
531 dest_id = info[1]
532 direction = info[2]
533 transport = info[3]
534 topic = info[4]
535 if len(info) > 5:
536 connected = info[5]
537 else:
538 connected = True
539
540 if connected:
541 buff += " * topic: %s\n"%topic
542
543
544 buff += " * to: %s\n"%lookup_uri(master, system_state, topic, dest_id)
545 if direction == 'i':
546 buff += " * direction: inbound\n"
547 elif direction == 'o':
548 buff += " * direction: outbound\n"
549 else:
550 buff += " * direction: unknown\n"
551 buff += " * transport: %s\n"%transport
552 except socket.error:
553 raise ROSNodeIOException("Communication with node[%s] failed!"%(node_api))
554 return buff
555
557 """
558 Print information about node, including subscriptions and other debugging information. This will query the node over the network.
559
560 @param node_name: name of ROS node
561 @type node_name: str
562 @raise ROSNodeIOException: if unable to communicate with master
563 """
564 def topic_type(t, pub_topics):
565 matches = [t_type for t_name, t_type in pub_topics if t_name == t]
566 if matches:
567 return matches[0]
568 return 'unknown type'
569
570 master = rosgraph.Master(ID)
571 node_name = rosgraph.names.script_resolve_name('rosnode', node_name)
572
573 print('-'*80)
574 print(get_node_info_description(node_name))
575
576 node_api = get_api_uri(master, node_name)
577 if not node_api:
578 print("cannot contact [%s]: unknown node"%node_name, file=sys.stderr)
579 return
580
581 if not quiet:
582 print("\ncontacting node %s ..." % node_api)
583 print(get_node_connection_info_description(node_api, master))
584
585
586 rosnode_debugnode = rosnode_info
587
589 """
590 Implements rosnode 'list' command.
591 """
592 args = argv[2:]
593 parser = OptionParser(usage="usage: %prog list", prog=NAME)
594 parser.add_option("-u",
595 dest="list_uri", default=False,
596 action="store_true",
597 help="list XML-RPC URIs")
598 parser.add_option("-a","--all",
599 dest="list_all", default=False,
600 action="store_true",
601 help="list all information")
602 (options, args) = parser.parse_args(args)
603 namespace = None
604 if len(args) > 1:
605 parser.error("invalid args: you may only specify one namespace")
606 elif len(args) == 1:
607 namespace = rosgraph.names.script_resolve_name('rostopic', args[0])
608 rosnode_listnodes(namespace=namespace, list_uri=options.list_uri, list_all=options.list_all)
609
611 """
612 Implements rosnode 'info' command.
613 """
614 args = argv[2:]
615 parser = OptionParser(usage="usage: %prog info [options] node1 [node2...]",
616 prog=NAME)
617 parser.add_option("-q", "--quiet",
618 dest="quiet", default=False,
619 action="store_true",
620 help="Prints only basic information such as pubs/subs and does not contact nodes for more information")
621 (options, args) = parser.parse_args(args)
622 if not args:
623 parser.error("You must specify at least one node name")
624 for node in args:
625 rosnode_info(node, options.quiet)
626
628 """
629 Implements rosnode 'machine' command.
630
631 @raise ROSNodeException: if user enters in unrecognized machine name
632 """
633 args = argv[2:]
634 parser = OptionParser(usage="usage: %prog machine [machine-name]", prog=NAME)
635 (options, args) = parser.parse_args(args)
636 if len(args) > 1:
637 parser.error("please enter only one machine name")
638 elif len(args) == 0:
639 machines = get_machines_by_nodes()
640 machines.sort()
641 print('\n'.join(machines))
642 else:
643 nodes = get_nodes_by_machine(args[0])
644 nodes.sort()
645 print('\n'.join(nodes))
646
648 """
649 Implements rosnode 'kill' command.
650
651 @raise ROSNodeException: if user enters in unrecognized nodes
652 """
653 args = argv[2:]
654 parser = OptionParser(usage="usage: %prog kill [node]...", prog=NAME)
655 parser.add_option("-a","--all",
656 dest="kill_all", default=False,
657 action="store_true",
658 help="kill all nodes")
659
660 (options, args) = parser.parse_args(args)
661 if options.kill_all:
662 if args:
663 parser.error("invalid arguments with kill all (-a) option")
664 args = get_node_names()
665 args.sort()
666 elif not args:
667 node_list = get_node_names()
668 node_list.sort()
669 if not node_list:
670 print("No nodes running", file=sys.stderr)
671 return 0
672
673 sys.stdout.write('\n'.join(["%s. %s"%(i+1, n) for i,n in enumerate(node_list)]))
674 sys.stdout.write("\n\nPlease enter the number of the node you wish to kill.\n> ")
675 selection = ''
676 while not selection:
677 selection = sys.stdin.readline().strip()
678 try:
679 selection = int(selection)
680 if selection <= 0:
681 print("ERROR: invalid selection. Please enter a number (ctrl-C to cancel)")
682 except:
683 print("ERROR: please enter a number (ctrl-C to cancel)")
684 sys.stdout.flush()
685 selection = ''
686 args = [node_list[selection - 1]]
687 else:
688
689 args = [rosgraph.names.script_resolve_name(ID, n) for n in args]
690 node_list = get_node_names()
691 unknown = [n for n in args if not n in node_list]
692 if unknown:
693 raise ROSNodeException("Unknown node(s):\n"+'\n'.join([" * %s"%n for n in unknown]))
694 if len(args) > 1:
695 print("killing:\n"+'\n'.join([" * %s"%n for n in args]))
696 else:
697 print("killing %s"%(args[0]))
698
699 success, fail = kill_nodes(args)
700 if fail:
701 print("ERROR: Failed to kill:\n"+'\n'.join([" * %s"%n for n in fail]), file=sys.stderr)
702 return 1
703 print("killed")
704 return 0
705
707 """
708 Implements rosnode 'cleanup' command.
709 @param argv: command-line args
710 @type argv: [str]
711 """
712 args = argv[2:]
713 parser = OptionParser(usage="usage: %prog cleanup", prog=NAME)
714 (options, args) = parser.parse_args(args)
715 rosnode_cleanup()
716
718 """
719 Implements rosnode 'ping' command.
720 @param argv: command-line args
721 @type argv: [str]
722 """
723 args = argv[2:]
724 parser = OptionParser(usage="usage: %prog ping [options] <node>", prog=NAME)
725 parser.add_option("--all", "-a",
726 dest="ping_all", default=False,
727 action="store_true",
728 help="ping all nodes")
729 parser.add_option("-c",
730 dest="ping_count", default=None, metavar="COUNT",type="int",
731 help="number of pings to send. Not available with --all")
732 (options, args) = parser.parse_args(args)
733
734 node_name = None
735 if not options.ping_all:
736 if not args:
737 try:
738 parser.error("Please enter a node to ping. Available nodes are:\n"+_sub_rosnode_listnodes())
739 except:
740
741 parser.error("Please enter a node to ping")
742 elif len(args) > 1:
743 parser.error("you may only specify one input node")
744 elif len(args) == 1:
745 node_name = rosgraph.names.script_resolve_name('rosnode', args[0])
746 print("rosnode: node is [%s]"%node_name)
747 else:
748 if args:
749 parser.error("Invalid arguments '%s' used with --all"%(' '.join(args)))
750 elif options.ping_count:
751 parser.error("-c may not be used with --all")
752
753 if options.ping_all:
754 rosnode_ping_all(verbose=True)
755 else:
756 rosnode_ping(node_name, verbose=True, max_count=options.ping_count)
757
759 """
760 Prints rosnode usage information.
761 @param return_error whether to exit with error code os.EX_USAGE
762 """
763 print("""rosnode is a command-line tool for printing information about ROS Nodes.
764
765 Commands:
766 \trosnode ping\ttest connectivity to node
767 \trosnode list\tlist active nodes
768 \trosnode info\tprint information about node
769 \trosnode machine\tlist nodes running on a particular machine or list machines
770 \trosnode kill\tkill a running node
771 \trosnode cleanup\tpurge registration information of unreachable nodes
772
773 Type rosnode <command> -h for more detailed usage, e.g. 'rosnode ping -h'
774 """)
775 if return_error:
776 sys.exit(getattr(os, 'EX_USAGE', 1))
777 else:
778 sys.exit(0)
779
780 -def rosnodemain(argv=None):
781 """
782 Prints rosnode main entrypoint.
783 @param argv: override sys.argv
784 @param argv: [str]
785 """
786 if argv == None:
787 argv = sys.argv
788 if len(argv) == 1:
789 _fullusage()
790 try:
791 command = argv[1]
792 if command == 'ping':
793 sys.exit(_rosnode_cmd_ping(argv) or 0)
794 elif command == 'list':
795 sys.exit(_rosnode_cmd_list(argv) or 0)
796 elif command == 'info':
797 sys.exit(_rosnode_cmd_info(argv) or 0)
798 elif command == 'machine':
799 sys.exit(_rosnode_cmd_machine(argv) or 0)
800 elif command == 'cleanup':
801 sys.exit(_rosnode_cmd_cleanup(argv) or 0)
802 elif command == 'kill':
803 sys.exit(_rosnode_cmd_kill(argv) or 0)
804 elif command == '--help':
805 _fullusage(False)
806 else:
807 _fullusage()
808 except socket.error:
809 print("Network communication failed. Most likely failed to communicate with master.", file=sys.stderr)
810 sys.exit(1)
811 except rosgraph.MasterError as e:
812 print("ERROR: "+str(e), file=sys.stderr)
813 sys.exit(1)
814 except ROSNodeException as e:
815 print("ERROR: "+str(e), file=sys.stderr)
816 sys.exit(1)
817 except KeyboardInterrupt:
818 pass
819