dotcode_tf.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2008, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 
00033 from __future__ import with_statement, print_function
00034 
00035 import time
00036 import rospy
00037 import yaml
00038 
00039 
00040 class RosTfTreeDotcodeGenerator(object):
00041 
00042     def __init__(self, initial_listen_duration=1):
00043         """
00044         :param initial_listen_duration: how many secs to listen to tf initially.
00045         """
00046         self.last_drawargs = None
00047         self.dotcode = None
00048         self.firstcall = True
00049         self.listen_duration = initial_listen_duration
00050         self.rank = None
00051         self.rankdir = None
00052         self.ranksep = None
00053         self.graph = None
00054         self.dotcode_factory = None
00055 
00056     def generate_dotcode(self,
00057                          dotcode_factory,
00058                          tf2_frame_srv,
00059                          timer=rospy.Time,
00060                          yaml_parser=yaml,
00061                          rank='same',   # None, same, min, max, source, sink
00062                          ranksep=0.2,   # vertical distance between layers
00063                          rankdir='TB',  # direction of layout (TB top > bottom, LR left > right)
00064                          force_refresh=False):
00065         """
00066         :param force_refresh: if False, may return same dotcode as last time
00067         """
00068         if self.firstcall is True:
00069             self.firstcall = False
00070             force_refresh = True
00071 
00072         drawing_args = {
00073             'dotcode_factory': dotcode_factory,
00074             "rank": rank,
00075             "rankdir": rankdir,
00076             "ranksep": ranksep}
00077 
00078         selection_changed = False
00079         if self.last_drawargs != drawing_args:
00080             selection_changed = True
00081             self.last_drawargs = drawing_args
00082 
00083             self.dotcode_factory = dotcode_factory
00084             self.rank = rank
00085             self.rankdir = rankdir
00086             self.ranksep = ranksep
00087 
00088         # generate new dotcode
00089         if force_refresh or self.dotcode is None or selection_changed:
00090             if self.listen_duration > 0:
00091                 time.sleep(self.listen_duration)
00092             # no need to listen more once we've listened for 1 sec?
00093             self.listen_duration = 0
00094 
00095             yaml_data = tf2_frame_srv().frame_yaml
00096             data = yaml_parser.load(yaml_data)
00097             self.graph = self.generate(data, timer.now().to_sec())
00098             self.dotcode = self.dotcode_factory.create_dot(self.graph)
00099 
00100         return self.dotcode
00101 
00102     def generate(self, data, timestamp):
00103         graph = self.dotcode_factory.get_graph(rank=self.rank,
00104                                                rankdir=self.rankdir,
00105                                                ranksep=self.ranksep)
00106 
00107         if data is None or len(data) == 0:
00108             self.dotcode_factory.add_node_to_graph(graph, "No tf data received")
00109             return graph
00110 
00111         for frame_dict in data:
00112             tf_frame_values = data[frame_dict]
00113             if not tf_frame_values['parent'] in data:
00114                 root = tf_frame_values['parent']
00115             self.dotcode_factory.add_node_to_graph(graph,
00116                                                    str(tf_frame_values['parent']),
00117                                                    shape='ellipse')
00118             self.dotcode_factory.add_node_to_graph(
00119                 graph, frame_dict, shape='ellipse')
00120 
00121             edge_label = '"Broadcaster: %s\\n' % str(tf_frame_values['broadcaster'])
00122             edge_label += 'Average rate: %s\\n' % str(tf_frame_values['rate'])
00123             edge_label += 'Buffer length: %s\\n' % str(tf_frame_values['buffer_length'])
00124             edge_label += 'Most recent transform: %s\\n' % str(tf_frame_values['most_recent_transform'])
00125             edge_label += 'Oldest transform: %s"' % str(tf_frame_values['oldest_transform'])
00126             self.dotcode_factory.add_edge_to_graph(graph,
00127                                                    str(tf_frame_values['parent']),
00128                                                    frame_dict,
00129                                                    label=edge_label)
00130 
00131         # create legend before root node
00132         legend_label = '"Recorded at time: %s"' % str(timestamp)
00133         self.dotcode_factory.add_node_to_graph(graph, legend_label)
00134         self.dotcode_factory.add_edge_to_graph(graph,
00135                                                legend_label,
00136                                                root,
00137                                                style='invis')
00138 
00139         # dot += ' subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";\n'
00140         # dot += '"Recorded at time: '+str(rospy.Time.now().to_sec())+'"[ shape=plaintext ] ;\n'
00141         # dot += '}->"'+root+'"[style=invis];\n}'
00142         return graph


rqt_tf_tree
Author(s): Thibault Kruse
autogenerated on Wed Feb 6 2019 03:16:16