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