tf_interface.py
Go to the documentation of this file.
00001 #***********************************************************
00002 #* Software License Agreement (BSD License)
00003 #*
00004 #*  Copyright (c) 2009, Willow Garage, Inc.
00005 #*  All rights reserved.
00006 #*
00007 #*  Redistribution and use in source and binary forms, with or without
00008 #*  modification, are permitted provided that the following conditions
00009 #*  are met:
00010 #*
00011 #*   * Redistributions of source code must retain the above copyright
00012 #*     notice, this list of conditions and the following disclaimer.
00013 #*   * Redistributions in binary form must reproduce the above
00014 #*     copyright notice, this list of conditions and the following
00015 #*     disclaimer in the documentation and/or other materials provided
00016 #*     with the distribution.
00017 #*   * Neither the name of Willow Garage, Inc. nor the names of its
00018 #*     contributors may be used to endorse or promote products derived
00019 #*     from this software without specific prior written permission.
00020 #*
00021 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 #*  POSSIBILITY OF SUCH DAMAGE.
00033 #* 
00034 #* Author: Eitan Marder-Eppstein
00035 #***********************************************************
00036 import roslib; roslib.load_manifest('tf2_visualization')
00037 import rospy
00038 
00039 import os
00040 import yaml
00041 import subprocess
00042 
00043 from tf2_msgs.srv import FrameGraph
00044 import tf2_ros
00045 import tf2
00046 
00047 import rosgraph.masterapi
00048 
00049 class TFInterface(object):
00050     def __init__(self, use_listener=True):
00051         self.buffer = tf2_ros.Buffer()
00052         
00053         self.selected_parent = None
00054         self.selected_child = None
00055         self.data = None
00056         self.data_timestamp = None
00057         self.namespace = 'local'
00058 
00059         if use_listener:
00060             self.listener = tf2_ros.TransformListener(self.buffer)
00061             rospy.wait_for_service('~tf2_frames')
00062             self.srv = rospy.ServiceProxy('~tf2_frames', FrameGraph, persistent=True)
00063             self.master = rosgraph.masterapi.Master(rospy.get_name())
00064         else:
00065             self.listener = None
00066             self.srv = None
00067             self.master = None
00068     
00069     def register_srv(self, namespace):
00070         if namespace == self.namespace:
00071             return
00072         
00073         if namespace == 'local':
00074             rospy.wait_for_service('~tf2_frames')
00075             self.srv = rospy.ServiceProxy('~tf2_frames', FrameGraph, persistent=True)
00076         else:
00077             rospy.wait_for_service(namespace + '/tf2_frames')
00078             self.srv = rospy.ServiceProxy(namespace + '/tf2_frames', FrameGraph, persistent=True)
00079             self.namespace = namespace
00080 
00081     def find_tf_namespaces(self):
00082         ns = []
00083         
00084         if self.master:
00085             services = self.master.getSystemState()[2]
00086             for name, providers in services:
00087                 #we don't want to list our own node
00088                 if name.find(rospy.get_name() + '/') == 0:
00089                     continue
00090                 index = name.rfind('tf2_frames')
00091                 if index > 0:
00092                     ns.append(name[:index-1])
00093             ns.sort()
00094         
00095         return ns
00096 
00097     def clear_detail(self):
00098         self.selected_parent = None
00099         self.selected_child = None
00100 
00101     def set_detail(self, detail):
00102         selected = detail.split('-')
00103         self.selected_parent = selected[0]
00104         self.selected_child = selected[1]
00105 
00106     def set_data_timestamp(self, timestamp):
00107         self.data_timestamp = timestamp
00108 
00109     def update_data(self, namespace, from_file=False):
00110         if not from_file:
00111             if namespace == 'local':
00112                 frame_yaml = self.buffer.all_frames_as_yaml()
00113             else:
00114                 self.register_srv(namespace)
00115                 frame_yaml = self.srv().frame_yaml
00116 
00117             if self.listener is not None:
00118                 self.data_timestamp = rospy.Time.now()
00119 
00120             self.data = yaml.load(frame_yaml)
00121         else:
00122             self.data = self._load_yaml_file(namespace)
00123 
00124     def _load_yaml_file(self, filename):
00125         if not filename:
00126             return
00127 
00128         with open(filename, 'r') as f:
00129             data = yaml.load(f)
00130         return data
00131 
00132     def save_yaml(self, full_name):
00133         if not full_name:
00134             return
00135 
00136         filename = full_name.rstrip('.tf')
00137         with open(filename+'.tf', 'w') as f:
00138             f.write(yaml.dump(self.data))
00139 
00140     def save_pdf(self, full_name):
00141         if not full_name:
00142             return
00143 
00144         filename = full_name.rstrip('.pdf')
00145         with open(filename+'.gv', 'w') as f:
00146             f.write(self._generate_dot(self.data, full_info=True))
00147         subprocess.Popen(('dot -Tpdf ' + filename + '.gv -o ' + filename + '.pdf').split(' ')).communicate()
00148         subprocess.Popen(('rm -rf ' + filename + '.gv').split(' ')).communicate()
00149 
00150     def get_dot(self):
00151         return self._generate_dot(self.data)
00152 
00153     def get_info(self):
00154         if self.selected_child:
00155             return self.data[self.selected_child]
00156         return ""
00157 
00158     def get_echo_string(self, target, source):
00159         try:
00160             msg = self.buffer.lookup_transform(target, source, rospy.Time())
00161             echo = 'Time: ' + str(msg.header.stamp) + '\n'
00162             echo += 'Target: ' + msg.header.frame_id + '\n'
00163             echo += 'Source: ' + msg.child_frame_id + '\n'
00164             echo += 'Translation: \n' + str(msg.transform.translation) + '\n'
00165             echo += 'Rotation Quaternion: \n' + str(msg.transform.rotation) + '\n'
00166             return echo
00167         except tf2.TransformException as e:
00168             return str(e)
00169 
00170     def get_frame_list(self):
00171         if not self.data:
00172             return []
00173 
00174         l = self.data.keys()
00175 
00176         #we also need to find the root of the tree
00177         for el in self.data:
00178             map = self.data[el]
00179             if not map['parent'] in self.data:
00180                 l.append(map['parent'])
00181 
00182         l.sort()
00183         return l
00184 
00185     def _generate_dot(self, data, full_info=False):
00186         if not data:
00187             return 'digraph G { "No tf data received" }'
00188 
00189         dot = 'digraph G {\n'
00190         for el in data: 
00191             map = data[el]
00192             node_color = 'white'
00193             arrow_color = 'black'
00194             if el == self.selected_parent:
00195                 node_color = 'green'
00196             if el == self.selected_child:
00197                 node_color = 'green'
00198                 arrow_color = 'green'
00199             dot += "\"" + el + "\""
00200             dot += '['
00201             dot += 'shape=ellipse style=filled fillcolor="' + node_color + '" '
00202             dot += 'URL="' + map['parent'] + '-' + el + '"] '
00203             dot += '"'+map['parent']+'" -> "'+el+'" [color="' + arrow_color + '"]'
00204             if full_info:
00205                 dot += '[label=" '
00206                 dot += 'Broadcaster: '+map['broadcaster']+'\\n'
00207                 dot += 'Average rate: '+str(map['rate'])+'\\n'
00208                 dot += 'Buffer length: '+str(map['buffer_length'])+'\\n' 
00209                 dot += 'Most recent transform: '+str(map['most_recent_transform'])+'\\n'
00210                 dot += 'Oldest transform: '+str(map['oldest_transform'])+'\\n'
00211                 dot += '"];\n'
00212             if not map['parent'] in data:
00213                 root = map['parent']
00214                 if map['parent'] == self.selected_parent:
00215                     dot += "\"" + root + "\"" + '[fillcolor="green" style="filled"]'
00216         dot += 'edge [style=invis];\n'
00217         dot += ' subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";\n'
00218         dot += '"Recorded at time: '+str(self.data_timestamp)+'"[ shape=plaintext ] ;\n'
00219         dot += '}->"'+root+'";\n}'
00220 
00221         return dot
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


tf2_visualization
Author(s): Wim Meeussen
autogenerated on Mon Aug 19 2013 11:37:23