00001
00002 import roslib; roslib.load_manifest('srs_user_tests')
00003
00004 import rospy
00005 from math import fabs, sqrt
00006 import numpy
00007 import tf
00008 import rosbag
00009 import os
00010 import glob
00011 import sys
00012 from geometry_msgs.msg import PoseStamped
00013
00014 class TfEval(object):
00015
00016 def __init__(self, def_frame_1='/map', def_frame_2='/rviz_cam', def_bag_file_dir=''):
00017
00018 self.frame_1 = rospy.get_param('~frame_1', def_frame_1)
00019 self.frame_2 = rospy.get_param('~frame_2', def_frame_2)
00020 self.debug = rospy.get_param('~debug', False)
00021
00022
00023
00024 self.id = rospy.get_param('~id')
00025 self.exp = rospy.get_param('~exp')
00026 self.task = rospy.get_param('~task')
00027 self.cond = rospy.get_param('~cond')
00028 self.path = rospy.get_param('~path')
00029 self.output_file = rospy.get_param('~output_file')
00030
00031 self.start_time = rospy.Time(rospy.get_param('~start_time',0.0))
00032 self.end_time = rospy.Time(rospy.get_param('~end_time',0.0))
00033
00034 self.cache_dur = rospy.Duration(0.0)
00035
00036 self.listener = tf.TransformListener()
00037
00038 self.tfpublisher= rospy.Publisher("tf",tf.msg.tfMessage)
00039
00040 if self.path[-1] != '/':
00041
00042 self.path += '/'
00043
00044 self.bag_file_dir = self.path + self.id + '/' + self.exp + '/' + self.task + '/' + self.cond + '/'
00045
00046 if not os.path.isdir(self.bag_file_dir):
00047
00048 rospy.logerr('Directory (%s) does not exist.',self.bag_file_dir)
00049 sys.exit()
00050
00051 fn = glob.glob(self.bag_file_dir + '*.bag')
00052
00053 if len(fn) == 0:
00054
00055 rospy.logerr('There is no bag file (%s)',self.bag_file_dir)
00056 sys.exit()
00057
00058 if len(fn) > 1:
00059
00060 rospy.logerr("There are more bag files in the directory! Don't know which one to analyze.")
00061 sys.exit()
00062
00063 self.bag_filename = fn[0]
00064
00065 try:
00066
00067 self.bag = rosbag.Bag(self.bag_filename)
00068
00069 except rosbag.ROSBagException:
00070
00071 msg = 'Error on openning bag file (' + self.bag_filename + ')'
00072 rospy.logerr(msg)
00073 rospy.signal_shutdown(msg)
00074 sys.exit()
00075
00076 except rosbag.ROSBagFormatException:
00077
00078 msg = 'Bag file is corrupted.'
00079 rospy.logerr(msg)
00080 rospy.signal_shutdown(msg)
00081 sys.exit()
00082
00083 self.started = False
00084
00085 self.pose = PoseStamped()
00086 self.pose.header.frame_id = self.frame_2
00087 self.pose.header.stamp = rospy.Time(0)
00088 self.pose.pose.position.x = 0.0
00089 self.pose.pose.position.y = 0.0
00090 self.pose.pose.position.z = 0.0
00091
00092 self.pose.pose.orientation.x = 0.0
00093 self.pose.pose.orientation.y = 0.0
00094 self.pose.pose.orientation.z = 0.0
00095 self.pose.pose.orientation.w = 0.0
00096
00097 self.last_pose = None
00098
00099 self.changes = rospy.Duration(0)
00100 self.timer_period = rospy.Duration(0.01)
00101
00102
00103
00104 self.timer_trig = False
00105
00106 if os.path.isfile(self.path + self.output_file):
00107
00108 self.csv = open(self.path + self.output_file,'a')
00109
00110 else:
00111
00112 rospy.loginfo('Creating new CSV file.')
00113 self.csv = open(self.path + self.output_file,'w')
00114 self.csv.write('id;experiment;task;condition;start;end;changes;total;path_len;rotations\n')
00115
00116
00117
00118 rospy.loginfo('Initialized with %s and %s frames.',self.frame_1,self.frame_2)
00119 rospy.loginfo('File to analyze: %s',self.bag_filename)
00120 dur = self.end_time.to_sec() - self.start_time.to_sec()
00121 rospy.loginfo('Start time: %f, end time: %f (dur: %f)',round(self.start_time.to_sec(),2), round(self.end_time.to_sec(),2),round(dur,2))
00122
00123 self.change_occ_time = rospy.Time(0)
00124 self.change_last_time = rospy.Time(0)
00125 self.intgr = 0
00126 self.change = False
00127 self.last_path_len = 0.0
00128
00129
00130 def readBag(self,start,end,start_pose=None):
00131
00132
00133 frames_avail = rospy.Time(0)
00134
00135 frames_checked = False
00136
00137 started = False
00138 last_pose = start_pose
00139 last = None
00140
00141
00142
00143 np = None
00144
00145 path_len = 0.0
00146
00147 start_real = rospy.Time(0)
00148
00149 end_real = start-self.cache_dur
00150
00151 error = False
00152 error_at = rospy.Time(0)
00153
00154 path_len = 0.0
00155
00156 rotations = 0.0
00157
00158 changes = rospy.Duration(0)
00159
00160 if self.debug:
00161
00162 rospy.loginfo('Request for analysis from: %s, to: %s.',str(round(start.to_sec(),2)), str(round(end.to_sec(),2)))
00163
00164 try:
00165
00166 for topic, msg, t in self.bag.read_messages(topics=['/tf'],start_time=start-self.cache_dur,end_time=end):
00167
00168 end_real = t
00169
00170 if rospy.is_shutdown():
00171
00172 break
00173
00174 try:
00175
00176 self.tfpublisher.publish(msg)
00177
00178 except TypeError:
00179
00180 if self.debug:
00181
00182 rospy.logwarn('Strange TF msg.')
00183
00184 pass
00185
00186
00187
00188 if not frames_checked:
00189
00190 if self.listener.frameExists(self.frame_1) and self.listener.frameExists(self.frame_2):
00191
00192 frames_avail = t
00193 frames_checked = True
00194 if self.debug:
00195
00196 rospy.loginfo('Both TF frames are available. Caching TF...')
00197
00198 else:
00199
00200 continue
00201
00202 if not started:
00203 last = t
00204 start_real = t
00205 started = True
00206
00207 if self.debug:
00208
00209 rospy.loginfo('Starting analysis, at %f',round(t.to_sec(),2))
00210
00211 if (t - last >= self.timer_period):
00212
00213 last = t
00214 self.pose.header.stamp = rospy.Time(0)
00215
00216 try:
00217
00218 self.listener.waitForTransform(self.frame_1,self.frame_2,self.pose.header.stamp,rospy.Duration(0.05))
00219
00220 np = self.listener.transformPose(self.frame_1, self.pose)
00221
00222 except Exception, e:
00223
00224 if self.debug:
00225
00226 rospy.logwarn("Can't transform.")
00227
00228 continue
00229
00230 if last_pose is None:
00231
00232 last_pose = np
00233
00234 continue
00235
00236 pdist = sqrt((np.pose.position.x - last_pose.pose.position.x)**2 + (np.pose.position.y - last_pose.pose.position.y)**2 + (np.pose.position.z - last_pose.pose.position.z)**2)
00237 path_len += pdist
00238
00239 (r, p, y) = tf.transformations.euler_from_quaternion([np.pose.orientation.x,
00240 np.pose.orientation.y,
00241 np.pose.orientation.z,
00242 np.pose.orientation.w],axes='sxyz')
00243
00244 (lr, lp, ly) = tf.transformations.euler_from_quaternion([last_pose.pose.orientation.x,
00245 last_pose.pose.orientation.y,
00246 last_pose.pose.orientation.z,
00247 last_pose.pose.orientation.w],axes='sxyz')
00248
00249
00250 dor = fabs(r - lr)
00251 dop = fabs(p - lp)
00252 doy = fabs(y - ly)
00253
00254
00255
00256 if path_len > self.last_path_len or dor > 0.0 or dop > 0.0 or doy > 0.0:
00257
00258 if self.change==False:
00259
00260 self.change_occ_time = t
00261 self.change = True
00262 self.intgr = 0
00263
00264
00265 self.intgr += 1
00266 self.change_last_time = t
00267
00268 rotations += doy
00269 last_pose = np
00270 self.last_path_len = path_len
00271
00272 else:
00273
00274 if self.change == True and (t-self.change_last_time) >= rospy.Duration(2.0):
00275
00276 if self.intgr > 1:
00277
00278 dt = self.change_last_time - self.change_occ_time
00279 changes += dt
00280
00281 if self.debug:
00282
00283 print str(dt.to_sec()) + '; ' + str(self.intgr)
00284
00285 self.change = False
00286
00287
00288 except rosbag.ROSBagFormatException:
00289
00290 if self.debug:
00291
00292 rospy.logerr("Bag file format corrupted around %s.",str(round(end_real.to_sec(),2)))
00293
00294 error = True
00295
00296 except roslib.message.DeserializationError:
00297
00298 if self.debug:
00299
00300 rospy.logerr('Bag file deserialization error around %s.',str(round(end_real.to_sec(),2)))
00301
00302 error = True
00303
00304 except:
00305
00306 if self.debug:
00307
00308 rospy.logerr('Some other error around %s.',str(round(end_real.to_sec(),2)))
00309
00310 error = True
00311
00312 if np is None and last_pose is not None:
00313
00314 np = last_pose
00315
00316 return (error, start_real, end_real, changes, path_len, rotations, np)
00317
00318
00319 def analyze(self):
00320
00321 dur = rospy.Duration(0)
00322 errors = 0
00323 spath_len = 0.0
00324 srotations = 0.0
00325 last_pose = None
00326
00327 (error, start_real, end_real, changes, path_len, rotations, last_pose) = self.readBag(self.start_time, self.end_time, last_pose)
00328
00329 fstart_real = start_real
00330 dur = end_real - start_real
00331 schanges = changes
00332 spath_len = path_len
00333 srotations = rotations
00334
00335 while (error is True and end_real < self.end_time and not rospy.is_shutdown()):
00336
00337 end_real += self.cache_dur + rospy.Duration(0.25)
00338
00339 (error, start_real, end_real, changes, path_len, rotations, last_pose) = self.readBag(end_real, self.end_time, last_pose)
00340
00341 errors += 1
00342
00343 if start_real != rospy.Time(0):
00344
00345 dur += end_real - start_real
00346
00347 schanges += changes
00348 spath_len += path_len
00349 srotations += rotations
00350
00351 rospy.loginfo('Finished (%d errors).',errors)
00352
00353 tot = str(round(dur.to_sec(),2))
00354 ch = str(round(schanges.to_sec(),2))
00355
00356 if fstart_real != rospy.Time(0):
00357
00358 rospy.loginfo('Real start: %s, real end: %s.',str(fstart_real.to_sec()),str(end_real.to_sec()))
00359 rospy.loginfo('Total duration: ' + tot + 's')
00360 rospy.loginfo('Changes:' + ch + 's, path length: ' + str(round(spath_len,2)) + 'm, rotations: ' + str(round(srotations,2)) + 'rads.' )
00361
00362 self.csv.write(self.id + ';' + self.exp + ';' + self.task + ';' + self.cond + ';' + str(fstart_real.to_sec()) + ';' + str(end_real.to_sec()) + ';' + ch + ';' + tot + ';' + str(round(spath_len,2)) + ';' + str(round(srotations,2)) + '\n')
00363
00364 else:
00365
00366 rospy.logwarn("Can't compute anything. One of the frames was probably not available.")
00367
00368
00369 self.csv.close()
00370
00371
00372 if __name__ == '__main__':
00373
00374 rospy.init_node('tf_test_eval')
00375
00376 node = TfEval()
00377
00378 node.analyze()
00379