00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050 SB_='\033[1m'
00051 CW_='\033[1;37m'
00052 CLG_='\033[37m'
00053 CG_='\033[1;30m'
00054 CN_='\033[30m'
00055 CR_='\033[31m'
00056 CLR_='\033[1;31m'
00057 CV_='\033[32m'
00058 CLV_='\033[1;32m'
00059 CM_='\033[33m'
00060 CY_='\033[1;33m'
00061 CB_='\033[34m'
00062 CLB_='\033[1;34m'
00063 CP_='\033[35m'
00064 CK_='\033[1;35m'
00065 CC_='\033[36m'
00066 CLC_='\033[1;36m'
00067 _EC='\033[0m'
00068
00069 last_t = rospy.Time()
00070 d_fd = float(0.0)
00071 d_wd = float(0.0)
00072 d_wv = float(0.0)
00073
00074 tfl2b = [[0.57], [0], [0]]
00075
00076
00077
00078
00079
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
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
00090 d_fd += (data.rmp200[0].forward_displacement+data.rmp200[1].forward_displacement)/2
00091
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
00096
00097 def callback_laser(data):
00098 global current_scan
00099 current_scan = data;
00100
00101
00102
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
00112
00113
00114 def laser_to_base_frame(pose_laser):
00115 dl2b = numpy.array(tfl2b)
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)
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
00126
00127
00128 def base_to_laser_frame(pose_robot):
00129 dl2b = numpy.array(tfl2b)
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)
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
00149
00150
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
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
00175
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
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
00242
00243 def calc_arc_length(s):
00244 global arc
00245 d_arc = 0.0
00246
00247 prior_d = base_to_laser_frame( resta(current_pose,old_pose))
00248 d_icp = icp_client( first_scan, s, prior_d)[1]
00249 d_base_list = laser_to_base_frame( d_icp)
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
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"
00271 L.position.x = 1
00272 N = 2
00273 M = 2
00274 vel_lin = 0.5
00275 vel_ang = -0.5
00276 kind = "line"
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)
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
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)
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
00340 if no_first_scan and len(current_scan.ranges)>0:
00341 print " > Ready ",i+1," of ",N
00342 no_first_scan = False
00343
00344 elif not no_first_scan:
00345
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
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
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
00370 first_scan = deepcopy(last_scan)
00371 old_theta = current_theta
00372 old_pose = current_pose
00373 j += 1
00374
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
00382 print " > Get ICP and MidPoint distances"
00383 if kind=="line":
00384
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
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
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
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
00416
00417
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
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