test_eval_tf.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
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         # /logs/$(arg id)/$(arg exp)/$(arg task)/$(arg cond)/
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         #rospy.Timer(self.timer_period, self.timer)
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         #end_real = rospy.Time(0)
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                     #rospy.sleep(0.001)
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: #and (t >= frames_avail + self.cache_dur):
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): # we don't want to do calculations each for iteration
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') # is this order correct??
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                         # if there is any change in position/orientation, lets do some stuff
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                             #cnt = 0
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): #cnt > 50: # 20
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: #and dt != rospy.Duration(0.0):
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 # first start real
00330         dur = end_real - start_real
00331         schanges = changes # sum of 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         


srs_user_tests
Author(s): SRS team
autogenerated on Mon Oct 6 2014 08:53:11