00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
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
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
