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