rosping_existence.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004 rosping_existence.py
00005 
00006 check the existence of the ros nodes and publish the result to diagnostics
00007 """
00008 
00009 import rospy
00010 try:
00011     from xmlrpc.client import ServerProxy
00012 except ImportError:
00013     from xmlrpclib import ServerProxy
00014 
00015 from rosnode import *
00016 import sys
00017 import roslaunch
00018 import diagnostic_updater
00019 import diagnostic_msgs
00020 from sound_play.msg import SoundRequest
00021 
00022 def _succeed(args):
00023     code, msg, val = args
00024     if code != 1:
00025         raise ROSNodeException("remote call failed: %s"%msg)
00026     return val
00027 
00028 def ping(node_name, max_count=None, verbose=False):
00029     """
00030     Test connectivity to node by calling its XMLRPC API
00031     @param node_name: name of node to ping
00032     @type  node_name: str
00033     @param max_count: number of ping requests to make
00034     @type  max_count: int
00035     @param verbose: print ping information to screen
00036     @type  verbose: bool
00037     @return: True if node pinged
00038     @rtype: bool
00039     @raise ROSNodeIOException: if unable to communicate with master
00040     """
00041     master = rosgraph.Master(ID)
00042     node_api = get_api_uri(master,node_name)
00043     if not node_api:
00044         # print "cannot ping [%s]: unknown node" % node_name, file=sys.stderr
00045         return False
00046 
00047     timeout = 3.
00048 
00049     if verbose:
00050         print("pinging %s with a timeout of %ss"%(node_name, timeout))
00051     socket.setdefaulttimeout(timeout)
00052     node = ServerProxy(node_api)
00053     lastcall = 0.
00054     count = 0
00055     acc = 0.
00056     try:
00057         while True:
00058             try:
00059                 count += 1
00060                 start = time.time()
00061                 pid = _succeed(node.getPid(ID))
00062                 end = time.time()
00063 
00064                 dur = (end-start)*1000.
00065                 acc += dur
00066                 
00067 
00068                 if verbose:
00069                     print("xmlrpc reply from %s\ttime=%fms"%(node_api, dur))
00070                 # 1s between pings
00071             except socket.error as e:
00072                 # 3786: catch ValueError on unpack as socket.error is not always a tuple
00073                 try:
00074                     # #3659
00075                     errnum, msg = e
00076                     if errnum == -2: #name/service unknown
00077                         p = urlparse.urlparse(node_api)
00078                         #print("ERROR: Unknown host [%s] for node [%s]"%(p.hostname, node_name), file=sys.stderr)
00079                     elif errnum == errno.ECONNREFUSED:
00080                         # check if node url has changed
00081                         new_node_api = get_api_uri(master,node_name, skip_cache=True)
00082                         if not new_node_api:
00083                             #print("cannot ping [%s]: unknown node"%node_name, file=sys.stderr)
00084                             return False
00085                         if new_node_api != node_api:
00086                             if verbose:
00087                                 print("node url has changed from [%s] to [%s], retrying to ping"%(node_api, new_node_api))
00088                             node_api = new_node_api
00089                             node = ServerProxy(node_api)
00090                             continue
00091                          #print("ERROR: connection refused to [%s]"%(node_api), file=sys.stderr)
00092                     else:
00093                         pass
00094                         #print("connection to [%s] timed out"%node_name, file=sys.stderr)
00095                     return False
00096                 except ValueError:
00097                     print("unknown network error contacting node: %s"%(str(e)))
00098             if max_count and count >= max_count:
00099                 break
00100             time.sleep(1.0)
00101     except KeyboardInterrupt:
00102         pass
00103             
00104     if verbose and count > 1:
00105         print("ping average: %fms"%(acc/count))
00106     return True
00107 
00108 def checkNodeExistence(stat):
00109     global nodes, speak_text
00110     result = {}
00111     have_dead = False
00112     for n in nodes:
00113         res = ping(n, max_count = 1, verbose = False)
00114         result[n] = res
00115         if not res:
00116             have_dead = True
00117     if have_dead:
00118         stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
00119                      "dead nodes: " + ", ".join([n for (n, res) 
00120                                                  in result.items() if not res]))
00121         if speak:
00122             sound = SoundRequest()
00123             sound.sound = SoundRequest.SAY
00124             sound.command = SoundRequest.PLAY_ONCE
00125             if speak_text:
00126                 sound.arg = speak_text
00127             else:
00128                 sound.arg = " ".join(nodes).replace("/", "").replace("_", " ") + " is dead"
00129             g_robotsound_pub.publish(sound)
00130     else:
00131         stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK,
00132                      "every node is alive")
00133     for n, res in result.items():
00134         stat.add(n, res)
00135     return stat
00136         
00137 if __name__ == "__main__":
00138     rospy.init_node('rosping_existence')
00139     updater = diagnostic_updater.Updater()
00140     updater.setHardwareID(rospy.get_name())
00141     updater.add("node existence", checkNodeExistence)
00142     argv = rospy.myargv()
00143     # you can specify the list of the launch files
00144     # launch_files = argv[1:]
00145     # config = roslaunch.config.load_config_default(launch_files, 0)
00146     # nodes = [n.namespace + n.name for n in config.nodes]
00147     nodes = argv[1:]
00148     speak = rospy.get_param("~speak", False)
00149     speak_text = rospy.get_param("~speak_text", "")
00150     if speak:
00151         g_robotsound_pub = rospy.Publisher("/robotsound", SoundRequest)
00152     r = rospy.Rate(0.01)
00153     while not rospy.is_shutdown():
00154         updater.update()
00155         r.sleep()
00156     


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Fri Sep 8 2017 03:38:56