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