tf_monitor.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 # Software License Agreement (BSD License)
00005 #
00006 # Copyright (c) 2012, Willow Garage, Inc.
00007 # All rights reserved.
00008 #
00009 # Redistribution and use in source and binary forms, with or without
00010 # modification, are permitted provided that the following conditions
00011 # are met:
00012 #
00013 #  * Redistributions of source code must retain the above copyright
00014 #    notice, this list of conditions and the following disclaimer.
00015 #  * Redistributions in binary form must reproduce the above
00016 #    copyright notice, this list of conditions and the following
00017 #    disclaimer in the documentation and/or other materials provided
00018 #    with the distribution.
00019 #  * Neither the name of the Willow Garage nor the names of its
00020 #    contributors may be used to endorse or promote products derived
00021 #    from this software without specific prior written permission.
00022 #
00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 # POSSIBILITY OF SUCH DAMAGE.
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 # Copying from tfwtf.py begins here
00050 
00051 import time
00052 import tf.msg
00053 
00054 # global list of messages received
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 # rospy subscriber callback for /tf
00139 def _tf_handle(msg):
00140     _msgs.append((msg, rospy.get_rostime(), msg._connection_header['callerid']))
00141 
00142 
00143 # Copying from tfwtf.py stops here
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()


diagnostic_common_diagnostics
Author(s): Brice Rebsamen
autogenerated on Sun Oct 5 2014 23:27:39