Source Code for Module tf.tfwtf

  1  # Software License Agreement (BSD License) 
  2  # 
  3  # Copyright (c) 2009, Willow Garage, Inc. 
  4  # All rights reserved. 
  5  # 
  6  # Redistribution and use in source and binary forms, with or without 
  7  # modification, are permitted provided that the following conditions 
  8  # are met: 
  9  # 
 10  #  * Redistributions of source code must retain the above copyright 
 11  #    notice, this list of conditions and the following disclaimer. 
 12  #  * Redistributions in binary form must reproduce the above 
 13  #    copyright notice, this list of conditions and the following 
 14  #    disclaimer in the documentation and/or other materials provided 
 15  #    with the distribution. 
 16  #  * Neither the name of Willow Garage, Inc. nor the names of its 
 17  #    contributors may be used to endorse or promote products derived 
 18  #    from this software without specific prior written permission. 
 19  # 
 20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
 30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  # POSSIBILITY OF SUCH DAMAGE. 
 32  # 
 33  # Revision $Id$ 
 34   
 35  import time 
 36   
 37  from roswtf.rules import warning_rule, error_rule 
 38  import rosgraph 
 39  import rospy 
 40   
 41  import tf.msg 
 42   
 43   
 44  # global list of messages received 
 45  _msgs = [] 
 46   
 47  ################################################################################ 
 48  # RULES 
 49   
 50 -def rostime_delta(ctx): 
 51      deltas = {} 
 52      for m, stamp, callerid in _msgs: 
 53          for t in m.transforms: 
 54              d = t.header.stamp - stamp 
 55              secs = d.to_sec() 
 56              if abs(secs) > 1.: 
 57                  if callerid in deltas: 
 58                      if abs(secs) > abs(deltas[callerid]): 
 59                          deltas[callerid] = secs 
 60                  else: 
 61                      deltas[callerid]  = secs 
 62   
 63      errors = [] 
 64      for k, v in deltas.iteritems(): 
 65          errors.append("receiving transform from [%s] that differed from ROS time by %ss"%(k, v)) 
 66      return errors 
 67   
 68 -def reparenting(ctx): 
 69      errors = [] 
 70      parent_id_map = {} 
 71      for m, stamp, callerid in _msgs: 
 72          for t in m.transforms: 
 73              frame_id = t.child_frame_id 
 74              parent_id = t.header.frame_id 
 75              if frame_id in parent_id_map and parent_id_map[frame_id] != parent_id: 
 76                  msg = "reparenting of [%s] to [%s] by [%s]"%(frame_id, parent_id, callerid) 
 77                  parent_id_map[frame_id] = parent_id 
 78                  if msg not in errors: 
 79                      errors.append(msg) 
 80              else: 
 81                  parent_id_map[frame_id] = parent_id 
 82      return errors 
 83   
 84 -def cycle_detection(ctx): 
 85      max_depth = 100 
 86      errors = [] 
 87      parent_id_map = {} 
 88      for m, stamp, callerid in _msgs: 
 89          for t in m.transforms: 
 90              frame_id = t.child_frame_id 
 91              parent_id = t.header.frame_id 
 92              parent_id_map[frame_id] = parent_id 
 93   
 94      for frame in parent_id_map: 
 95          frame_list = [] 
 96          current_frame = frame 
 97          count = 0 
 98          while count < max_depth + 2: 
 99              count = count + 1 
100              frame_list.append(current_frame) 
101              try: 
102                  current_frame = parent_id_map[current_frame] 
103              except KeyError: 
104                  break  
105              if current_frame == frame: 
106                  errors.append("Frame %s is in a loop. It's loop has elements:\n%s"% (frame, " -> ".join(frame_list))) 
107                  break 
108               
109               
110      return errors 
111   
112 -def multiple_authority(ctx): 
113      errors = [] 
114      authority_map = {} 
115      for m, stamp, callerid in _msgs: 
116          for t in m.transforms: 
117              frame_id = t.child_frame_id 
118              parent_id = t.header.frame_id  
119              if frame_id in authority_map and authority_map[frame_id] != callerid: 
120                  msg = "node [%s] publishing transform [%s] with parent [%s] already published by node [%s]"%(callerid, frame_id, parent_id, authority_map[frame_id]) 
121                  authority_map[frame_id] = callerid 
122                  if msg not in errors: 
123                      errors.append(msg) 
124              else: 
125                  authority_map[frame_id] = callerid 
126      return errors 
127   
128 -def no_msgs(ctx): 
129      return not _msgs 
130   
131  ################################################################################ 
132  # roswtf PLUGIN 
133   
134  # tf_warnings and tf_errors declare the rules that we actually check 
135                   
136  tf_warnings = [ 
137    (no_msgs, "No tf messages"), 
138    (rostime_delta, "Received out-of-date/future transforms:"),   
139  ] 
140  tf_errors = [ 
141    (reparenting, "TF re-parenting contention:"), 
142    (cycle_detection, "TF cycle detection::"), 
143    (multiple_authority, "TF multiple authority contention:"), 
144  ] 
145   
146  # rospy subscriber callback for /tf 
147 -def _tf_handle(msg): 
148      _msgs.append((msg, rospy.get_rostime(), msg._connection_header['callerid'])) 
149   
150  # @return bool: True if /tf has a publisher 
151 -def is_tf_active(): 
152      master = rosgraph.Master('/tfwtf') 
153      if master is not None: 
154          try: 
155              val = master.getPublishedTopics('/') 
156              if filter(lambda x: x[0] == '/tf', val): 
157                  return True 
158          except: 
159              pass 
160      return False 
161   
162  # roswtf entry point for online checks 
163 -def roswtf_plugin_online(ctx): 
164      # don't run plugin if tf isn't active as these checks take awhile 
165      if not is_tf_active(): 
166          return 
167       
168      print "running tf checks, this will take a second..." 
169      sub1 = rospy.Subscriber('/tf', tf.msg.tfMessage, _tf_handle) 
170      time.sleep(1.0) 
171      sub1.unregister() 
172      print "... tf checks complete"     
173   
174      for r in tf_warnings: 
175          warning_rule(r, r[0](ctx), ctx) 
176      for r in tf_errors: 
177          error_rule(r, r[0](ctx), ctx) 
178   
179       
180  # currently no static checks for tf 
181  #def roswtf_plugin_static(ctx): 
182  #    for r in tf_warnings: 
183  #        warning_rule(r, r[0](ctx), ctx) 
184  #    for r in tf_errors: 
185  #        error_rule(r, r[0](ctx), ctx) 
186