00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 import roslib
00035 roslib.load_manifest('pr2_computer_monitor')
00036
00037 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00038
00039 import sys
00040 import rospy
00041 import socket
00042 from subprocess import Popen, PIPE
00043
00044 import time
00045
00046 import re
00047
00048 NAME = 'ntp_monitor'
00049
00050 def ntp_monitor(ntp_hostname, offset=500, self_offset=500, diag_hostname = None, error_offset = 5000000):
00051 pub = rospy.Publisher("/diagnostics", DiagnosticArray)
00052 rospy.init_node(NAME, anonymous=True)
00053
00054 hostname = socket.gethostname()
00055 if diag_hostname is None:
00056 diag_hostname = hostname
00057
00058 stat = DiagnosticStatus()
00059 stat.level = 0
00060 stat.name = "NTP offset from "+ diag_hostname + " to " + ntp_hostname
00061 stat.message = "OK"
00062 stat.hardware_id = hostname
00063 stat.values = []
00064
00065 self_stat = DiagnosticStatus()
00066 self_stat.level = DiagnosticStatus.OK
00067 self_stat.name = "NTP self-offset for "+ diag_hostname
00068 self_stat.message = "OK"
00069 self_stat.hardware_id = hostname
00070 self_stat.values = []
00071
00072 while not rospy.is_shutdown():
00073 for st,host,off in [(stat,ntp_hostname,offset), (self_stat, hostname,self_offset)]:
00074 try:
00075 p = Popen(["ntpdate", "-q", host], stdout=PIPE, stdin=PIPE, stderr=PIPE)
00076 res = p.wait()
00077 (o,e) = p.communicate()
00078 except OSError, (errno, msg):
00079 if errno == 4:
00080 break
00081 else:
00082 raise
00083 if (res == 0):
00084 measured_offset = float(re.search("offset (.*),", o).group(1))*1000000
00085
00086 st.level = DiagnosticStatus.OK
00087 st.message = "OK"
00088 st.values = [ KeyValue("Offset (us)", str(measured_offset)),
00089 KeyValue("Offset tolerance (us)", str(off)),
00090 KeyValue("Offset tolerance (us) for Error", str(error_offset)) ]
00091
00092 if (abs(measured_offset) > off):
00093 st.level = DiagnosticStatus.WARN
00094 st.message = "NTP Offset Too High"
00095 if (abs(measured_offset) > error_offset):
00096 st.level = DiagnosticStatus.ERROR
00097 st.message = "NTP Offset Too High"
00098
00099 else:
00100 st.level = DiagnosticStatus.ERROR
00101 st.message = "Error Running ntpupdate. Returned %d" % res
00102 st.values = [ KeyValue("Offset (us)", "N/A"),
00103 KeyValue("Offset tolerance (us)", str(error_offset)),
00104 KeyValue("Offset tolerance (us) for Error", str(off)) ]
00105
00106
00107 msg = DiagnosticArray()
00108 msg.header.stamp = rospy.get_rostime()
00109 msg.status = [stat, self_stat]
00110 pub.publish(msg)
00111 time.sleep(1)
00112
00113 def ntp_monitor_main(argv=sys.argv):
00114 import optparse
00115 parser = optparse.OptionParser(usage="usage: ntp_monitor ntp-hostname []")
00116 parser.add_option("--offset-tolerance", dest="offset_tol",
00117 action="store", default=500,
00118 help="Offset from NTP host", metavar="OFFSET-TOL")
00119 parser.add_option("--error-offset-tolerance", dest="error_offset_tol",
00120 action="store", default=5000000,
00121 help="Offset from NTP host. Above this is error", metavar="OFFSET-TOL")
00122 parser.add_option("--self_offset-tolerance", dest="self_offset_tol",
00123 action="store", default=500,
00124 help="Offset from self", metavar="SELF_OFFSET-TOL")
00125 parser.add_option("--diag-hostname", dest="diag_hostname",
00126 help="Computer name in diagnostics output (ex: 'c1')",
00127 metavar="DIAG_HOSTNAME",
00128 action="store", default=None)
00129 options, args = parser.parse_args(rospy.myargv())
00130
00131 if (len(args) != 2):
00132 parser.error("Invalid arguments. Must have HOSTNAME [args]. %s" % args)
00133
00134
00135 try:
00136 offset = int(options.offset_tol)
00137 self_offset = int(options.self_offset_tol)
00138 error_offset = int(options.error_offset_tol)
00139 except:
00140 parser.error("Offsets must be numbers")
00141
00142 ntp_monitor(args[1], offset, self_offset, options.diag_hostname, error_offset)
00143
00144
00145 if __name__ == "__main__":
00146 try:
00147 ntp_monitor_main(rospy.myargv())
00148 except KeyboardInterrupt: pass
00149 except SystemExit: pass
00150 except:
00151 import traceback
00152 traceback.print_exc()
