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
00036 '''
00037 A node to check the TF tree
00038
00039 A big chunk of the code was copied from tfwtf, the wtf plugin for tf.
00040 '''
00041
00042 import roslib; roslib.load_manifest('diagnostic_common_diagnostics')
00043 import rospy
00044 import diagnostic_updater as DIAG
00045
00046
00047
00048
00049
00050
00051 import time
00052 import tf.msg
00053
00054
00055 _msgs = []
00056
00057 def rostime_delta(ctx):
00058 deltas = {}
00059 for m, stamp, callerid in _msgs:
00060 for t in m.transforms:
00061 d = t.header.stamp - stamp
00062 secs = d.to_sec()
00063 if abs(secs) > 1.:
00064 if callerid in deltas:
00065 if abs(secs) > abs(deltas[callerid]):
00066 deltas[callerid] = secs
00067 else:
00068 deltas[callerid] = secs
00069
00070 errors = []
00071 for k, v in deltas.iteritems():
00072 errors.append("receiving transform from [%s] that differed from ROS time by %ss"%(k, v))
00073 return errors
00074
00075 def reparenting(ctx):
00076 errors = []
00077 parent_id_map = {}
00078 for m, stamp, callerid in _msgs:
00079 for t in m.transforms:
00080 frame_id = t.child_frame_id
00081 parent_id = t.header.frame_id
00082 if frame_id in parent_id_map and parent_id_map[frame_id] != parent_id:
00083 msg = "reparenting of [%s] to [%s] by [%s]"%(frame_id, parent_id, callerid)
00084 parent_id_map[frame_id] = parent_id
00085 if msg not in errors:
00086 errors.append(msg)
00087 else:
00088 parent_id_map[frame_id] = parent_id
00089 return errors
00090
00091 def cycle_detection(ctx):
00092 max_depth = 100
00093 errors = []
00094 parent_id_map = {}
00095 for m, stamp, callerid in _msgs:
00096 for t in m.transforms:
00097 frame_id = t.child_frame_id
00098 parent_id = t.header.frame_id
00099 parent_id_map[frame_id] = parent_id
00100
00101 for frame in parent_id_map:
00102 frame_list = []
00103 current_frame = frame
00104 count = 0
00105 while count < max_depth + 2:
00106 count = count + 1
00107 frame_list.append(current_frame)
00108 try:
00109 current_frame = parent_id_map[current_frame]
00110 except KeyError:
00111 break
00112 if current_frame == frame:
00113 errors.append("Frame %s is in a loop. It's loop has elements:\n%s"% (frame, " -> ".join(frame_list)))
00114 break
00115
00116
00117 return errors
00118
00119 def multiple_authority(ctx):
00120 errors = []
00121 authority_map = {}
00122 for m, stamp, callerid in _msgs:
00123 for t in m.transforms:
00124 frame_id = t.child_frame_id
00125 parent_id = t.header.frame_id
00126 if frame_id in authority_map and authority_map[frame_id] != callerid:
00127 msg = "node [%s] publishing transform [%s] with parent [%s] already published by node [%s]"%(callerid, frame_id, parent_id, authority_map[frame_id])
00128 authority_map[frame_id] = callerid
00129 if msg not in errors:
00130 errors.append(msg)
00131 else:
00132 authority_map[frame_id] = callerid
00133 return errors
00134
00135 def no_msgs(ctx):
00136 return not _msgs
00137
00138
00139 def _tf_handle(msg):
00140 _msgs.append((msg, rospy.get_rostime(), msg._connection_header['callerid']))
00141
00142
00143
00144
00145
00146
00147 def make_diag_fn(fn, errlvl, errmsg, okmsg="OK"):
00148 '''A diagnostic function generator'''
00149
00150 def diag_fn(stat):
00151 stat.summary(0, okmsg)
00152
00153 res = fn(None)
00154 if isinstance(res, bool):
00155 if res:
00156 stat.summary(errlvl, errmsg)
00157 elif isinstance(res, list):
00158 if len(res)>0:
00159 stat.summary(errlvl, errmsg)
00160 for i,r in enumerate(res):
00161 stat.add("Error %d" % (i+1), r)
00162
00163 return stat
00164
00165 return diag_fn
00166
00167
00168 rospy.init_node('tf_monitor')
00169
00170
00171 diag_updater = DIAG.Updater()
00172 diag_updater.setHardwareID('none')
00173 diag_updater.add('Messaging status', make_diag_fn(no_msgs, DIAG.WARN, 'No tf messages received') )
00174 diag_updater.add('Time status', make_diag_fn(rostime_delta, DIAG.WARN, 'Received out-of-date/future transforms') )
00175 diag_updater.add('Reparenting status', make_diag_fn(reparenting, DIAG.ERROR, 'TF re-parenting contention') )
00176 diag_updater.add('Cycle status', make_diag_fn(cycle_detection, DIAG.ERROR, 'TF cycle detection') )
00177 diag_updater.add('Multiple authority status', make_diag_fn(multiple_authority, DIAG.ERROR, 'TF multiple authority contention') )
00178
00179
00180 while not rospy.is_shutdown():
00181 _msgs = []
00182 sub1 = rospy.Subscriber('/tf', tf.msg.tfMessage, _tf_handle)
00183 time.sleep(1.0)
00184 sub1.unregister()
00185 diag_updater.update()