tf_monitor.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 # Software License Agreement (BSD License)
5 #
6 # Copyright (c) 2012, Willow Garage, Inc.
7 # All rights reserved.
8 #
9 # Redistribution and use in source and binary forms, with or without
10 # modification, are permitted provided that the following conditions
11 # are met:
12 #
13 # * Redistributions of source code must retain the above copyright
14 # notice, this list of conditions and the following disclaimer.
15 # * Redistributions in binary form must reproduce the above
16 # copyright notice, this list of conditions and the following
17 # disclaimer in the documentation and/or other materials provided
18 # with the distribution.
19 # * Neither the name of the Willow Garage nor the names of its
20 # contributors may be used to endorse or promote products derived
21 # from this software without specific prior written permission.
22 #
23 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 # POSSIBILITY OF SUCH DAMAGE.
35 
36 '''
37 A node to check the TF tree
38 
39 A big chunk of the code was copied from tfwtf, the wtf plugin for tf.
40 '''
41 
42 import roslib; roslib.load_manifest('diagnostic_common_diagnostics')
43 import rospy
44 import diagnostic_updater as DIAG
45 
46 
47 
48 #-------------------------------------------------------------------------------
49 # Copying from tfwtf.py begins here
50 
51 import time
52 import tf.msg
53 
54 # global list of messages received
55 _msgs = []
56 
57 def rostime_delta(ctx):
58  deltas = {}
59  for m, stamp, callerid in _msgs:
60  for t in m.transforms:
61  d = t.header.stamp - stamp
62  secs = d.to_sec()
63  if abs(secs) > 1.:
64  if callerid in deltas:
65  if abs(secs) > abs(deltas[callerid]):
66  deltas[callerid] = secs
67  else:
68  deltas[callerid] = secs
69 
70  errors = []
71  for k, v in deltas.iteritems():
72  errors.append("receiving transform from [%s] that differed from ROS time by %ss"%(k, v))
73  return errors
74 
75 def reparenting(ctx):
76  errors = []
77  parent_id_map = {}
78  for m, stamp, callerid in _msgs:
79  for t in m.transforms:
80  frame_id = t.child_frame_id
81  parent_id = t.header.frame_id
82  if frame_id in parent_id_map and parent_id_map[frame_id] != parent_id:
83  msg = "reparenting of [%s] to [%s] by [%s]"%(frame_id, parent_id, callerid)
84  parent_id_map[frame_id] = parent_id
85  if msg not in errors:
86  errors.append(msg)
87  else:
88  parent_id_map[frame_id] = parent_id
89  return errors
90 
91 def cycle_detection(ctx):
92  max_depth = 100
93  errors = []
94  parent_id_map = {}
95  for m, stamp, callerid in _msgs:
96  for t in m.transforms:
97  frame_id = t.child_frame_id
98  parent_id = t.header.frame_id
99  parent_id_map[frame_id] = parent_id
100 
101  for frame in parent_id_map:
102  frame_list = []
103  current_frame = frame
104  count = 0
105  while count < max_depth + 2:
106  count = count + 1
107  frame_list.append(current_frame)
108  try:
109  current_frame = parent_id_map[current_frame]
110  except KeyError:
111  break
112  if current_frame == frame:
113  errors.append("Frame %s is in a loop. It's loop has elements:\n%s"% (frame, " -> ".join(frame_list)))
114  break
115 
116 
117  return errors
118 
120  errors = []
121  authority_map = {}
122  for m, stamp, callerid in _msgs:
123  for t in m.transforms:
124  frame_id = t.child_frame_id
125  parent_id = t.header.frame_id
126  if frame_id in authority_map and authority_map[frame_id] != callerid:
127  msg = "node [%s] publishing transform [%s] with parent [%s] already published by node [%s]"%(callerid, frame_id, parent_id, authority_map[frame_id])
128  authority_map[frame_id] = callerid
129  if msg not in errors:
130  errors.append(msg)
131  else:
132  authority_map[frame_id] = callerid
133  return errors
134 
135 def no_msgs(ctx):
136  return not _msgs
137 
138 # rospy subscriber callback for /tf
139 def _tf_handle(msg):
140  _msgs.append((msg, rospy.get_rostime(), msg._connection_header['callerid']))
141 
142 
143 # Copying from tfwtf.py stops here
144 #-------------------------------------------------------------------------------
145 
146 
147 def make_diag_fn(fn, errlvl, errmsg, okmsg="OK"):
148  '''A diagnostic function generator'''
149 
150  def diag_fn(stat):
151  stat.summary(0, okmsg)
152 
153  res = fn(None)
154  if isinstance(res, bool):
155  if res:
156  stat.summary(errlvl, errmsg)
157  elif isinstance(res, list):
158  if len(res)>0:
159  stat.summary(errlvl, errmsg)
160  for i,r in enumerate(res):
161  stat.add("Error %d" % (i+1), r)
162 
163  return stat
164 
165  return diag_fn
166 
167 
168 rospy.init_node('tf_monitor')
169 
170 
171 diag_updater = DIAG.Updater()
172 diag_updater.setHardwareID('none')
173 diag_updater.add('Messaging status', make_diag_fn(no_msgs, DIAG.WARN, 'No tf messages received') )
174 diag_updater.add('Time status', make_diag_fn(rostime_delta, DIAG.WARN, 'Received out-of-date/future transforms') )
175 diag_updater.add('Reparenting status', make_diag_fn(reparenting, DIAG.ERROR, 'TF re-parenting contention') )
176 diag_updater.add('Cycle status', make_diag_fn(cycle_detection, DIAG.ERROR, 'TF cycle detection') )
177 diag_updater.add('Multiple authority status', make_diag_fn(multiple_authority, DIAG.ERROR, 'TF multiple authority contention') )
178 
179 
180 while not rospy.is_shutdown():
181  _msgs = []
182  sub1 = rospy.Subscriber('/tf', tf.msg.tfMessage, _tf_handle)
183  time.sleep(1.0)
184  sub1.unregister()
185  diag_updater.update()
def make_diag_fn(fn, errlvl, errmsg, okmsg="OK")
Definition: tf_monitor.py:147


diagnostic_common_diagnostics
Author(s): Brice Rebsamen
autogenerated on Thu Oct 8 2020 03:19:37