trans_error_icp.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 #  -------------------------------------------
00005 #  node: trans_error_icp.py
00006 #  author: Marti Morta (mmorta@iri.upc.edu)
00007 #  date: 24 Oct 2013
00008 #  -------------------------------------------
00009 #   Calculates distances using different segway data and saves it in files.
00010 #   The iterations are forward and backward
00011 #   Subscribers
00012 #     scan: LaserScan
00013 #     segway_status: SegwayRMP400Status
00014 #     odom: Odometry
00015 #   Publishers
00016 #     cmd_vel: Twist
00017 
00018 #                                                                                                   Includes
00019 import roslib
00020 roslib.load_manifest('iri_segway_rmp400_odom')
00021 import os
00022 import sys
00023 import rospy
00024 import tf
00025 import math
00026 import time
00027 import numpy
00028 from copy import deepcopy
00029 from geometry_msgs.msg import Twist,Pose
00030 from nav_msgs.msg import Odometry
00031 from sensor_msgs.msg import LaserScan
00032 from iri_segway_rmp_msgs.msg import SegwayRMP400Status
00033 from tf.transformations import euler_from_quaternion,quaternion_from_euler
00034 from iri_laser_icp.srv import GetRelativePose
00035 
00036 #                                                                                                   Parameters
00037 
00038 
00039 #lobal test_name,N,L,vel_lin,vel_ang,kind,M
00040 #test_name = str("test")
00041 #N = int(2)
00042 #L=Pose()
00043 #vel_lin=float(0.3)
00044 #vel_ang=float(-0.3)
00045 #kind=str("line")
00046 #M=int(2)
00047 #                                                                                                   Vars & constants
00048 # Speed
00049 #console colours
00050 SB_='\033[1m' #style bold
00051 CW_='\033[1;37m' #White
00052 CLG_='\033[37m' #Light Gray
00053 CG_='\033[1;30m' #Gray    
00054 CN_='\033[30m' #Black   
00055 CR_='\033[31m' #Red        
00056 CLR_='\033[1;31m' #Light Red  
00057 CV_='\033[32m' #Green      
00058 CLV_='\033[1;32m' #Light Green
00059 CM_='\033[33m' #Brown      
00060 CY_='\033[1;33m' #Yellow     
00061 CB_='\033[34m' #Blue       
00062 CLB_='\033[1;34m' #Light Blue 
00063 CP_='\033[35m' #Purple     
00064 CK_='\033[1;35m' #Pink       
00065 CC_='\033[36m' #Cyan      
00066 CLC_='\033[1;36m' #Light Cyan
00067 _EC='\033[0m' #end color
00068 # global variables
00069 last_t = rospy.Time()
00070 d_fd   = float(0.0)
00071 d_wd   = float(0.0)
00072 d_wv   = float(0.0)
00073 # transform from laser to base link
00074 tfl2b  = [[0.57], [0], [0]] 
00075 
00076 #
00077 #                                                                                                   SEGWAY STATUS CALLBACK 
00078 #                                                                                                   Appends status into list 
00079 #                                                                                                   calculates distances
00080 # 
00081 def callback_status(data):
00082   global d_wv,d_fd,d_wd,lst_status,last_t
00083   d_wd = (data.rmp200[0].left_wheel_displacement+data.rmp200[0].right_wheel_displacement+data.rmp200[1].left_wheel_displacement+data.rmp200[1].right_wheel_displacement)/4  
00084   lst_status.append(data)
00085   #calculat com a odom
00086   current_t = rospy.Time.now()
00087   d_wv += ((data.rmp200[0].left_wheel_velocity + data.rmp200[0].right_wheel_velocity + data.rmp200[1].left_wheel_velocity + data.rmp200[1].right_wheel_velocity)/4)*(current_t-last_t).to_sec()
00088   last_t = current_t
00089   #calculat segons forward displacement (relatiu)
00090   d_fd += (data.rmp200[0].forward_displacement+data.rmp200[1].forward_displacement)/2
00091   #calculat segons despla├žament de les rodes (acumulat)
00092   d_wd = (data.rmp200[0].left_wheel_displacement+data.rmp200[0].right_wheel_displacement+data.rmp200[1].left_wheel_displacement+data.rmp200[1].right_wheel_displacement)/4
00093 
00094 #
00095 #                                                                                                   LASER CALLBACK Saves data 
00096 # 
00097 def callback_laser(data):
00098   global current_scan
00099   current_scan = data;
00100 
00101 #
00102 #                                                                                                   ODOM CALLBACK Saves data
00103 # 
00104 def callback_odom(data):
00105   global current_theta,current_pose
00106   q = [data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w]
00107   current_theta = euler_from_quaternion(q)[2]
00108   current_pose = data.pose.pose
00109   
00110 #
00111 #                                                                                                   LASER TO BASE FRAME 
00112 #                                                                                                   gives a laser framed pose in base frame
00113 # 
00114 def laser_to_base_frame(pose_laser):
00115   dl2b = numpy.array(tfl2b) # transform from laser to base footprint (no rot)
00116   q = [pose_laser.orientation.x, pose_laser.orientation.y, pose_laser.orientation.z, pose_laser.orientation.w]
00117   pose_laser_list = [ pose_laser.position.x, pose_laser.position.y, euler_from_quaternion(q)[2] ]
00118   oicp = numpy.array(pose_laser_list)  # pose given by icp
00119   rot_d = numpy.matrix([[math.cos(pose_laser_list[2]) , -math.sin(pose_laser_list[2]), 0],\
00120                         [math.sin(pose_laser_list[2]), math.cos(pose_laser_list[2]), 0],\
00121                         [0, 0, 1]])
00122   return (dl2b + oicp - rot_d * dl2b).tolist()[0]
00123 
00124 #
00125 #                                                                                                   BASE FRAME TO LASER
00126 #                                                                                                   gives a base framed pose in laser frame
00127 # 
00128 def base_to_laser_frame(pose_robot):
00129   dl2b = numpy.array(tfl2b) # transform from laser to base footprint (no rot)
00130   q = [pose_robot.orientation.x, pose_robot.orientation.y, pose_robot.orientation.z, pose_robot.orientation.w]
00131   pose_list = [ pose_robot.position.x, pose_robot.position.y, euler_from_quaternion(q)[2] ]
00132   oicp = numpy.array(pose_list) # pose given by icp
00133   rot_d = numpy.matrix([[math.cos(pose_list[2]) , -math.sin(pose_list[2]), 0],\
00134                         [math.sin(pose_list[2]), math.cos(pose_list[2]), 0],\
00135                         [0, 0, 1]])
00136   pose_base_list = (-dl2b + oicp + rot_d * dl2b).tolist()[0]
00137   pose_base = Pose()
00138   pose_base.position.x  = pose_base_list[0]
00139   pose_base.position.y  = pose_base_list[1]
00140   q = quaternion_from_euler(0,0,pose_base_list[2])
00141   pose_base.orientation.x = q[0]
00142   pose_base.orientation.y = q[1]
00143   pose_base.orientation.z = q[2]
00144   pose_base.orientation.w = q[3]
00145   return pose_base
00146 
00147 #
00148 #                                                                                                   ICP CLIENT
00149 #                                                                                                   asks for ICP Service 
00150 #                                                                                                   returns a distance
00151 # 
00152 def icp_client(ref,sens,d):
00153   rospy.wait_for_service('iri_laser_icp/get_relative_pose',10)
00154   try:
00155       get_rpose = rospy.ServiceProxy('iri_laser_icp/get_relative_pose', GetRelativePose)
00156       rel_pose = get_rpose(ref,sens,d).pose_rel.pose.pose
00157       d_icp = math.sqrt(rel_pose.position.x**2+rel_pose.position.y**2)
00158       return [d_icp,rel_pose]
00159   except rospy.ServiceException, e:
00160       print CR_+"Service call failed: %s"%e+_EC
00161 
00162 
00163 #
00164 #                                                                                                   PRINT VECTOR STATISTICS
00165 # 
00166 def get_vector_statistics(name,v):
00167     return(name+str("%.5f"%numpy.mean(v, dtype=numpy.float64))  +" "+\
00168                 str("%.5f"%numpy.std(v, dtype=numpy.float64))   +" "+\
00169                 str("%.5f"%numpy.std(v, dtype=numpy.float64)**2)+" "+\
00170                 str("%.5f"%numpy.amin(v))+" "+\
00171                 str("%.5f"%numpy.amax(v))+" ")
00172 
00173 #
00174 #                                                                                                   WRITE DISTANCE FILE
00175 #                                                                                                   saves data from distances into file
00176 # 
00177 def write_and_print_results(test_iterations):
00178   f_name = test_name+"_N"+str(N)+"_L"+str(abs(L.position.x))+"_"+str(int(time.time()))+".txt"
00179   output = open(f_name, 'w')
00180 
00181   sumari = test_name+"\n" \
00182            "Date: "+str(time.asctime(time.localtime(time.time())))+"\n" \
00183            "Kind: "+kind+"\n" \
00184            "L: "+str(L.position.x)+" m"+"\n" \
00185            "N: "+str(N)+"\n" \
00186            "Vel: "+str(vel_lin)+" m/s "
00187   if kind=="turn":
00188     sumari += ", "+str(vel_ang)+" rad/s"+"\n"
00189 
00190   if kind=="line":
00191     raw_data = "d_mp   d_icp    d_wv    d_wd    d_fd\n"
00192     for x in range(len(lst_d_wv)):
00193       raw_data += str("%.5f"%lst_d_mp[x])+" "+str("%.5f"%lst_d_icp[x])+" "+str("%.5f"%lst_d_wv[x])+" "+str("%.5f"%lst_d_wd[x])+" "+str("%.5f"%lst_d_fd[x])+"\n"
00194   elif kind=="turn":
00195     raw_data = "\nd_arc\td_wv\td_wd\td_fd\n"
00196     for x in range(len(lst_d_wv)):
00197          raw_data = str("%.5f"%lst_d_arc[x])+" "+str("%.5f"%lst_d_wv[x])+" "+str("%.5f"%lst_d_wd[x])+" "+str("%.5f"%lst_d_fd[x])+"\n"
00198 
00199   statistics = "          Mean    StdDev  Cov     min     max"+"\n"+\
00200                get_vector_statistics("d_wv     ",lst_d_wv)+"\n"+\
00201                get_vector_statistics("d_wd     ",lst_d_wd)+"\n"+\
00202                get_vector_statistics("d_fd     ",lst_d_fd)+"\n"
00203   if kind=="turn":
00204     statistics += get_vector_statistics("d_arc    ",lst_d_arc)+"\n"
00205   if kind=="line":
00206     statistics += get_vector_statistics("d_icp    ",lst_d_icp)+"\n"+\
00207                   get_vector_statistics("d_mp     ",lst_d_mp)+"\n"+\
00208                   get_vector_statistics("E-icp-wv ",[lst_d_icp[i]-lst_d_wv[i] for i in range(len(lst_d_wv))])+"\n"+\
00209                   get_vector_statistics("E-icp-wd ",[lst_d_icp[i]-lst_d_wd[i] for i in range(len(lst_d_wd))])+"\n"+\
00210                   get_vector_statistics("E-icp-fd ",[lst_d_icp[i]-lst_d_fd[i] for i in range(len(lst_d_fd))])+"\n"+\
00211                   get_vector_statistics("E-mp-wv  ",[lst_d_mp[i]-lst_d_wv[i] for i in range(len(lst_d_wv))])+"\n"+\
00212                   get_vector_statistics("E-mp-wd  ",[lst_d_mp[i]-lst_d_wd[i] for i in range(len(lst_d_wd))])+"\n"+\
00213                   get_vector_statistics("E-mp-fd  ",[lst_d_mp[i]-lst_d_fd[i] for i in range(len(lst_d_fd))])
00214 
00215   output.write(sumari+"\n"+raw_data+"\n"+statistics+"\n")
00216   print("\n"+sumari+"\n"+raw_data+"\n"+statistics+"\n")
00217 
00218   output.close()
00219   print CB_,"> ",f_name," file saved",_EC
00220 
00221 #
00222 #                                                                                                   RESTA entre dues poses
00223 # 
00224 def resta(p2,p1):
00225   p=Pose()
00226   p.position.x = p2.position.x-p1.position.x
00227   p.position.y = p2.position.y-p1.position.y
00228   p.position.z = p2.position.z-p1.position.z
00229   q1 = [p1.orientation.x, p1.orientation.y, p1.orientation.z, p1.orientation.w]
00230   q2 = [p2.orientation.x, p2.orientation.y, p2.orientation.z, p2.orientation.w]
00231   de = euler_from_quaternion(q2)[2] - euler_from_quaternion(q1)[2]
00232   dq = quaternion_from_euler(0,0,de)
00233   p.orientation.x = dq[0]
00234   p.orientation.y = dq[1]
00235   p.orientation.z = dq[2]
00236   p.orientation.w = dq[3]
00237   return p
00238 
00239 
00240 #
00241 #                                                                                                   CALC ARC LENGTH
00242 # 
00243 def calc_arc_length(s):
00244   global arc
00245   d_arc = 0.0
00246   # Calculate arc length
00247   prior_d = base_to_laser_frame( resta(current_pose,old_pose))  # pose previa frame laser
00248   d_icp   = icp_client( first_scan, s, prior_d)[1]   # pose icp    frame laser
00249   d_base_list = laser_to_base_frame( d_icp)                     # pose icp    frame base
00250   if current_theta-old_theta != 0:
00251     d_arc  = d_base_list[0] * (current_theta-old_theta) / math.sin(current_theta-old_theta)
00252     arc   += d_arc
00253   else:
00254     arc += d_base_list[0]
00255   print " > Arc"
00256   print "    d_arc:   ",d_arc
00257   print "    d_theta: ",current_theta-old_theta
00258   print "    d_wv:    ",d_wv
00259 
00260 #
00261 #                                                                                                   MAIN
00262 # 
00263 if __name__ == '__main__':
00264 
00265 
00266   print SB_+CY_+"\nOdometry Translation Error Estimation Node",CR_,">TERROR ESTIMATION<"+_EC
00267 
00268   global test_name,N,L,vel_lin,vel_ang,kind,M,tw_zero,tw,tw_turn
00269   L = Pose()
00270   test_name    = "test" # Name for the files
00271   L.position.x = 1      # Length of the experiment [m]
00272   N            = 2      # Times it will carry the experiment on
00273   M            = 2      # angle division
00274   vel_lin      = 0.5    # Lineal velocity [m/s]
00275   vel_ang      = -0.5    # Angular velocity [rad/s]
00276   kind         = "line" # Type of experiment {line,turn}
00277 
00278   if len(sys.argv)>=2:
00279     test_name = sys.argv[1]
00280   if len(sys.argv)>=3:
00281     N = int(sys.argv[2])
00282   if len(sys.argv)>=4:
00283     L.position.x = float(sys.argv[3])
00284   if len(sys.argv)>=5:
00285     vel_lin = float(sys.argv[4])
00286   if len(sys.argv)>=6:
00287     vel_ang = float(sys.argv[5])
00288   if len(sys.argv)>=7:
00289     kind = sys.argv[6]
00290 
00291   tw_zero           = Twist()
00292   tw                = Twist()
00293   tw.linear.x       = vel_lin
00294   tw_turn           = Twist()
00295   tw_turn.linear.x  = vel_lin
00296   tw_turn.angular.z = vel_ang
00297 
00298   print " > Parameters: "
00299   print CV_,"  ",test_name," N:",N," L:",L.position.x," vlin:",vel_lin," vang:",vel_ang," kind:",kind,_EC
00300 
00301   rospy.init_node('srmp400_terror_icp')
00302  
00303   r = rospy.Rate(10) # 10hz
00304 
00305   global lst_d_mp,lst_d_icp,lst_d_wv,lst_d_wd,lst_d_fd,lst_d_arc,current_scan,current_pose,arc
00306   lst_status = []
00307   lst_d_wv = []
00308   lst_d_wd = []
00309   lst_d_fd = []
00310   lst_d_icp = []
00311   lst_d_mp = []
00312   lst_d_arc = []
00313   current_scan = LaserScan()
00314   current_pose = Pose()
00315   old_theta = 0.0
00316   arc = 0
00317    
00318   # Subscribers & Publishers
00319   rospy.Subscriber('/teo/segway/status', SegwayRMP400Status, callback_status)
00320   rospy.Subscriber('/teo/sensors/front_laser', LaserScan, callback_laser)
00321   rospy.Subscriber('/teo/odomfused', Odometry, callback_odom)
00322   p_v = rospy.Publisher('/teo/segway/cmd_vel_unsafe', Twist)
00323 
00324   i = 0
00325   j = 1
00326   d = 0.0
00327   arc = 0.0
00328   theta_total = abs(vel_ang*L.position.x/vel_lin) #[rad]
00329   no_first_scan = True
00330   first_scan = LaserScan()
00331   last_scan = LaserScan()
00332   prior_d = Pose()
00333   prev_L = Pose()
00334   old_pose = Pose()
00335 
00336   print " > Waiting for the first LaserScan..."
00337 
00338   while i < N and not rospy.is_shutdown():
00339     #                                                                                               Wait until laser is ready
00340     if no_first_scan and len(current_scan.ranges)>0:
00341       print " > Ready ",i+1," of ",N
00342       no_first_scan = False
00343     #                                                                                               Fer un llarg
00344     elif not no_first_scan:
00345       #                                                                                             Arrancada
00346       print CV_,"> Acceleration",_EC
00347       while abs(d_wv) <= 0.5 and not rospy.is_shutdown():
00348         if kind=="line":
00349             p_v.publish(tw)
00350         if kind=="turn":
00351           p_v.publish(tw_turn)
00352       #                                                                                             Set vars
00353       d_wv = d_fd = 0.0
00354       start_d_wd = d_wd
00355       print " > Get first scan"
00356       first_scan = deepcopy(current_scan)
00357       old_pose = deepcopy(current_pose)
00358       #                                                                                             Start getting data
00359       print CV_,"> Start Getting Data",_EC
00360       while abs(d_wv) <= L.position.x and not rospy.is_shutdown():
00361         if   kind=="line":
00362             p_v.publish(tw)
00363         elif kind=="turn":
00364           if abs(d_wv) < L.position.x / float(M) * float(j) :
00365             p_v.publish(tw_turn)
00366           else :
00367             last_scan = deepcopy(current_scan)
00368             calc_arc_length(last_scan)        
00369             # update variables
00370             first_scan = deepcopy(last_scan)
00371             old_theta  = current_theta
00372             old_pose   = current_pose
00373             j += 1
00374       #                                                                                             Stop
00375       if not rospy.is_shutdown():
00376         print " > Get Last scan"
00377         last_scan = deepcopy(current_scan)
00378         print CR_,"> Stop",_EC
00379         p_v.publish(tw_zero)
00380         time.sleep(0.5)
00381         #                                                                                           Calculate ICP & Mid Point
00382         print " > Get ICP and MidPoint distances"
00383         if   kind=="line":
00384           # depends on the direction the ICP needs one or another first guess
00385           prev_L = deepcopy(L)
00386           prev_L.position.x *= -1 if i%2 == 1 else 1
00387           d_icp = icp_client(first_scan, last_scan, prev_L)[0]
00388           # Calculate the difference between scan midpoints
00389           d_mp  = first_scan.ranges[int(len(first_scan.ranges)/2)] - last_scan.ranges[int(len(last_scan.ranges)/2)]
00390         elif kind=="turn":
00391           calc_arc_length(last_scan)
00392         #                                                                                           Update vectors
00393         print " > Append distances"
00394         lst_d_wv.append(abs(d_wv))
00395         lst_d_wd.append(abs(d_wd-start_d_wd))
00396         lst_d_fd.append(abs(d_fd))
00397         if kind=="line":
00398           lst_d_mp.append(abs(d_mp))
00399           lst_d_icp.append(abs(d_icp))
00400         if kind=="turn":
00401           lst_d_arc.append(abs(arc))
00402         #                                                                                           Summary
00403         print CB_,"> Summary Iteration ",i+1," of ",N
00404         print "    first scan: ",first_scan.header.seq
00405         print "    last scan:  ",last_scan.header.seq
00406         print "    d_wv:       ",lst_d_wv[-1]
00407         print "    d_wd:       ",lst_d_wd[-1]
00408         print "    d_fd:       ",lst_d_fd[-1]
00409         if kind=="line":
00410           print "    d_mp:     ",lst_d_mp[-1]
00411           print "    d_icp:    ",lst_d_icp[-1]
00412         if kind=="turn":
00413           print "    d_arc:    ",lst_d_arc[-1]
00414         print _EC
00415         #print " > Saving segway data into file"
00416         #write_status_file(L.position.x,i) 
00417         #                                                                                           Reset
00418         print " > Reset and update vars"
00419         lst_status = []
00420         d_wv = d_fd = 0.0 
00421         if kind=="line":
00422           tw.linear.x  *= -1
00423         elif kind=="turn":
00424           tw_turn.linear.x  *= -1
00425           tw_turn.angular.z *= -1
00426         no_first_scan = True
00427         arc = 0
00428         j = 0
00429         i += 1
00430   
00431   p_v.publish(tw_zero)
00432   
00433   #                                                                                                 Get statistics
00434   if not rospy.is_shutdown():    
00435     print CB_,"> Summary & file save"
00436     write_and_print_results(N)
00437     print "",_EC
00438 
00439   print SB_+CY_+"THE END"+_EC
00440 


teo_apps
Author(s): Marti
autogenerated on Fri Dec 6 2013 22:19:45