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 import roslib
00035 roslib.load_manifest('hrl_hokuyo')
00036
00037 import pygame
00038 import pygame.locals
00039 import hokuyo_scan as hs
00040
00041 import hrl_lib.transforms as tr
00042 import hrl_lib.util as ut
00043
00044 import math, numpy as np
00045 import scipy.ndimage as ni
00046 import pylab as pl
00047
00048 import sys, time
00049 import optparse
00050 import copy
00051
00052 import pygame_utils as pu
00053
00054
00055
00056
00057
00058
00059 from ctypes import POINTER
00060
00061 MAX_DIST=1.0
00062
00063 org_x,org_y = 320,240+150
00064
00065 color_list = [(255,255,0),(255,0,0),(0,255,255),(0,255,0),(0,0,255),(0,100,100),(100,100,0),
00066 (100,0,100),(100,200,100),(200,100,100),(100,100,200),(100,0,200),(0,200,100),
00067 (0,0,0),(0,100,200),(200,0,100),(100,0,100),(255,152,7) ]
00068
00069
00070 def angle_to_index(scan, angle):
00071 ''' returns index corresponding to angle (radians).
00072 '''
00073 return int(min(scan.n_points-1, max(0.,int(round((angle-scan.start_angle)/scan.angular_res)))))
00074
00075 def index_to_angle(scan, index):
00076 ''' returns angle (radians) corresponding to index.
00077 '''
00078 return scan.start_angle+index*scan.angular_res
00079
00080 def get_xy_map(scan,start_angle=math.radians(-360),end_angle=math.radians(360),reject_zero_ten=True,max_dist=np.Inf,min_dist=-np.Inf,sigmoid_hack=False, get_intensities=False):
00081 """ start_angle - starting angle for points to be considered (hokuyo coord)
00082 end_angle - ending angle in hokuyo coord frame.
00083 scan - object of class HokuyoScan
00084 reject_zero_ten - ignore points at 0 or 10. meters
00085
00086 returns - 2xN matrix, if get_intensities=True returns (2xN matrix, 1xN matrix)
00087 """
00088 start_index = angle_to_index(scan,start_angle)
00089 end_index = angle_to_index(scan,end_angle)
00090
00091 if sigmoid_hack:
00092
00093
00094
00095
00096
00097
00098
00099
00100 scan.ranges = np.multiply(scan.ranges, 1+(1-np.divide(1.,1+np.power(np.e,-5*(scan.ranges-0.2))))*0.20)
00101
00102
00103
00104
00105
00106
00107
00108
00109 xy_map = np.matrix(np.row_stack((np.multiply(scan.ranges,np.cos(scan.angles)),
00110 np.multiply(scan.ranges,np.sin(scan.angles))*-1)))
00111
00112 sub_range = scan.ranges[:,start_index:end_index+1]
00113 keep_condition = np.multiply(sub_range<=max_dist,sub_range>=min_dist)
00114
00115 if reject_zero_ten == True:
00116 keep_condition = np.multiply(keep_condition,np.multiply(sub_range > 0.01,sub_range <= 10.))
00117
00118 xy_map = xy_map[:,start_index:end_index+1]
00119
00120 idxs = np.where(keep_condition)
00121 xy_map = np.row_stack((xy_map[idxs],xy_map[idxs[0]+1,idxs[1]]))
00122
00123 if get_intensities == True:
00124 intensities = scan.intensities[:,start_index:end_index+1]
00125 intensities = intensities[idxs]
00126 return xy_map, intensities
00127 else:
00128 return xy_map
00129
00130
00131 def connected_components(p, dist_threshold):
00132 ''' p - 2xN numpy matrix of euclidean points points (indices give neighbours).
00133 dist_threshold - max distance between two points in the same connected component.
00134 typical value is .02 meters
00135 returns a list of (p1,p2, p1_index, p2_index): (start euclidean point, end euclidean point, start index, end index)
00136 p1 and p2 are 2X1 matrices
00137 '''
00138 nPoints = p.shape[1]
00139 q = p[:,0:nPoints-1]
00140 r = p[:,1:nPoints]
00141 diff = r-q
00142 dists = ut.norm(diff).T
00143 idx = np.where(dists>dist_threshold)
00144
00145 end_list = idx[0].A1.tolist()
00146 end_list.append(nPoints-1)
00147
00148 cc_list = []
00149 start = 0
00150 for i in end_list:
00151 cc_list.append((p[:,start], p[:,i], start, i))
00152 start = i+1
00153
00154 return cc_list
00155
00156 def find_objects(scan, max_dist, max_size, min_size, min_angle, max_angle,
00157 connect_dist_thresh, all_pts=False):
00158 ''' max_dist - objects with centroid greater than max_dist will be ignored. (meters)
00159 max_size - objects greater than this are ignored. (meters)
00160 min_size - smaller than this are ignored (meters)
00161 min_angle, max_angle - part of scan to consider.
00162 connect_dist_thresh - points in scan greater than this will be treated as separate
00163 connected components.
00164
00165 all_pts == True:
00166 returns [ np matrix: 2xN1, 2xN2 ...] each matrix consists of all the points in the object.
00167 all_pts == False:
00168 returns list of (p,q,centroid): (end points,object centroid (2x1 matrices.))
00169 '''
00170 xy_map = get_xy_map(scan,min_angle,max_angle)
00171 cc_list = connected_components(xy_map,connect_dist_thresh)
00172 object_list = []
00173 all_pts_list = []
00174 for i,(p,q,p_idx,q_idx) in enumerate(cc_list):
00175 object_pts = xy_map[:,p_idx:q_idx+1]
00176 centroid = object_pts.sum(1)/(q_idx-p_idx+1)
00177 size = np.linalg.norm(p-q)
00178 if size>max_size:
00179 continue
00180 if size<min_size:
00181 continue
00182 if np.linalg.norm(centroid) > max_dist:
00183 continue
00184 object_list.append((p,q,centroid))
00185 all_pts_list.append(object_pts)
00186
00187 if all_pts == True:
00188 return all_pts_list
00189 else:
00190 return object_list
00191
00192 def find_closest_object_point(scan, pt_interest=np.matrix([0.,0.]).T, min_angle=math.radians(-60),
00193 max_angle=math.radians(60),max_dist=0.6,min_size=0.01,max_size=0.3):
00194 ''' returns 2x1 matrix - centroid of connected component in hokuyo frame closest to pt_interest
00195 pt_interest - 2x1 matrix in hokuyo coord frame.
00196 None if no object found.
00197 '''
00198 obj_list = find_objects(scan,max_dist,max_size,min_size,min_angle,max_angle,
00199 connect_dist_thresh=0.02, all_pts=True)
00200
00201 if obj_list == []:
00202 return None
00203
00204 min_dist_list = []
00205 for pts in obj_list:
00206 min_dist_list.append(np.min(ut.norm(pts-pt_interest)))
00207
00208 min_idx = np.argmin(np.matrix(min_dist_list))
00209 return obj_list[min_idx].mean(1)
00210
00211 def remove_graze_effect(ranges, angles, skip=1, graze_angle_threshold=math.radians(169.)):
00212 ''' ranges,angles - 1xN numpy matrix
00213 skip - which two rays to consider.
00214 this function changes ranges.
00215 '''
00216 nPoints = ranges.shape[1]
00217 p = ranges[:,0:nPoints-(1+skip)]
00218 q = ranges[:,(1+skip):nPoints]
00219 d_mat = np.abs(p-q)
00220 angles_diff = np.abs(angles[:,(1+skip):nPoints]-angles[:,0:nPoints-(1+skip)])
00221 l_mat = np.max(np.row_stack((p,q)),0)
00222 l_mat = np.multiply(l_mat,np.sin(angles_diff))/math.sin(graze_angle_threshold)
00223
00224 thresh_exceed = d_mat>l_mat
00225 l1_greater = p>q
00226 idx_remove_1 = np.where(np.all(np.row_stack((thresh_exceed,l1_greater)),0))
00227 idx_remove_2 = np.where(np.all(np.row_stack((thresh_exceed,1-l1_greater)),0))
00228
00229 p[idx_remove_1] = 1000.
00230 q[idx_remove_2] = 1000.
00231
00232 def remove_graze_effect_scan(scan, graze_angle_threshold=math.radians(169.)):
00233 ''' changes scan
00234 '''
00235 remove_graze_effect(scan.ranges,scan.angles,1,graze_angle_threshold)
00236
00237 def subtract_scans(scan2,scan1,threshold=0.01):
00238 if scan1.ranges.shape != scan2.ranges.shape:
00239 print 'hokuyo_processing.subtract_scans: the two scan.ranges have different shapes.'
00240 print 'remember to pass remove_graze_effect = False'
00241 print 'Exiting...'
00242 sys.exit()
00243
00244 diff_range = scan2.ranges-scan1.ranges
00245 idxs = np.where(np.abs(diff_range)<threshold)
00246
00247
00248
00249 hscan = hs.HokuyoScan(scan2.hokuyo_type,scan2.angular_res,
00250 scan2.max_range,scan2.min_range,
00251 scan2.start_angle,scan2.end_angle)
00252 hscan.ranges = copy.copy(scan2.ranges)
00253 hscan.ranges[idxs] = 0.
00254 return hscan
00255
00256 def find_door(start_pts_list,end_pts_list,pt_interest=None):
00257 ''' returns [p1x,p1y], [p2x,p2y] ([],[] if no door found)
00258 returns line closest to the pt_interest.
00259 pt_interest - 2x1 matrix
00260 '''
00261 if start_pts_list == []:
00262 return [],[]
00263
00264 start_pts = np.matrix(start_pts_list).T
00265 end_pts = np.matrix(end_pts_list).T
00266 line_vecs = end_pts-start_pts
00267 line_vecs_ang = np.arctan2(line_vecs[1,:],line_vecs[0,:])
00268
00269 idxs = np.where(np.add(np.multiply(line_vecs_ang>math.radians(45),
00270 line_vecs_ang<math.radians(135)),
00271 np.multiply(line_vecs_ang<math.radians(-45),
00272 line_vecs_ang>math.radians(-135))
00273 ) > 0
00274 )[1].A1.tolist()
00275
00276 if idxs == []:
00277 return [],[]
00278 start_pts = start_pts[:,idxs]
00279 end_pts = end_pts[:,idxs]
00280
00281 if pt_interest == None:
00282 print 'hokuyo_processing.find_door: pt_interest in None so returning the longest line.'
00283 length = ut.norm(end_pts-start_pts)
00284 longest_line_idx = np.argmax(length)
00285 vec_door = (end_pts-start_pts)[:,longest_line_idx]
00286 return start_pts[:,longest_line_idx].A1.tolist(),end_pts[:,longest_line_idx].A1.tolist()
00287 else:
00288 v = end_pts-start_pts
00289 q_dot_v = pt_interest.T*v
00290 p1_dot_v = np.sum(np.multiply(start_pts,v),0)
00291 v_dot_v = ut.norm(v)
00292 lam = np.divide((q_dot_v-p1_dot_v),v_dot_v)
00293 r = start_pts + np.multiply(lam,v)
00294 dist = ut.norm(pt_interest-r)
00295 edge_idxs = np.where(np.multiply(lam>1.,lam<0.))[1].A1.tolist()
00296 min_end_dist = np.minimum(ut.norm(start_pts-pt_interest),ut.norm(end_pts-pt_interest))
00297 dist[:,edge_idxs] = min_end_dist[:,edge_idxs]
00298
00299
00300
00301
00302
00303 keep_idxs = np.where(dist<0.5)[1].A1.tolist()
00304 if len(keep_idxs) == 0:
00305 return [],[]
00306
00307 start_pts = start_pts[:,keep_idxs]
00308 end_pts = end_pts[:,keep_idxs]
00309
00310
00311 p_robot = np.matrix([0.,0.]).T
00312 v = end_pts-start_pts
00313 q_dot_v = p_robot.T*v
00314 p1_dot_v = np.sum(np.multiply(start_pts,v),0)
00315 v_dot_v = ut.norm(v)
00316 lam = np.divide((q_dot_v-p1_dot_v),v_dot_v)
00317 r = start_pts + np.multiply(lam,v)
00318 dist = ut.norm(p_robot-r)
00319
00320 door_idx = np.argmax(dist)
00321 return start_pts[:,door_idx].A1.tolist(),end_pts[:,door_idx].A1.tolist()
00322
00323 def xy_map_to_np_image(xy_map,m_per_pixel,dilation_count=0,padding=50):
00324 ''' returns binary numpy image. (255 for occupied
00325 pixels, 0 for unoccupied)
00326 2d array
00327 '''
00328 min_x = np.min(xy_map[0,:])
00329 max_x = np.max(xy_map[0,:])
00330 min_y = np.min(xy_map[1,:])
00331 max_y = np.max(xy_map[1,:])
00332 br = np.matrix([min_x,min_y]).T
00333
00334 n_x = int(round((max_x-min_x)/m_per_pixel)) + padding
00335 n_y = int(round((max_y-min_y)/m_per_pixel)) + padding
00336 img = np.zeros((n_x+padding,n_y+padding),dtype='int')
00337 occupied_pixels = np.matrix([n_x,n_y]).T - np.round((xy_map-br)/m_per_pixel).astype('int')
00338
00339 if dilation_count == 0:
00340 img[(occupied_pixels[0,:],occupied_pixels[1,:])] = 255
00341 else:
00342 img[(occupied_pixels[0,:],occupied_pixels[1,:])] = 1
00343 connect_structure = np.empty((3,3),dtype='int')
00344 connect_structure[:,:] = 1
00345 img = ni.binary_closing(img,connect_structure,iterations=dilation_count)
00346 img = ni.binary_dilation(img,connect_structure,iterations=1)
00347 img = img*255
00348
00349 return img,n_x,n_y,br
00350
00351 def xy_map_to_cv_image(xy_map,m_per_pixel,dilation_count=0,padding=10):
00352 np_im,n_x,n_y,br = xy_map_to_np_image(xy_map,m_per_pixel,dilation_count,padding)
00353 cv_im = uto.np2cv(np_im)
00354 return cv_im,n_x,n_y,br
00355
00356 def hough_lines(xy_map,save_lines=False):
00357 ''' xy_map - 2xN matrix of points.
00358 returns start_list, end_list. [[p1x,p1y],[p2x,p2y]...],[[q1x,q1y]...]
00359 [],[] if no lines were found.
00360 '''
00361
00362 m_per_pixel = 0.005
00363 img_cv,n_x,n_y,br = xy_map_to_cv_image(xy_map,m_per_pixel,dilation_count=1,padding=50)
00364
00365 time_str = str(time.time())
00366
00367
00368
00369
00370
00371 storage = cvCreateMemStorage(0)
00372 method = CV_HOUGH_PROBABILISTIC
00373 rho = max(int(round(0.01/m_per_pixel)),1)
00374 rho = 1
00375 theta = math.radians(1)
00376 min_line_length = int(0.3/m_per_pixel)
00377 max_gap = int(0.1/m_per_pixel)
00378 n_points_thresh = int(0.2/m_per_pixel)
00379
00380
00381
00382 lines = cvHoughLines2(img_cv, storage, method, rho, theta, n_points_thresh, min_line_length, max_gap)
00383
00384 if lines.total == 0:
00385 return [],[]
00386 pts_start = np.zeros((2, lines.total))
00387 pts_end = np.zeros((2, lines.total))
00388
00389 if save_lines:
00390 color_dst = cvCreateImage( cvGetSize(img_cv), 8, 3 )
00391 cvCvtColor( img_cv, color_dst, CV_GRAY2BGR )
00392
00393 n_lines = lines.total
00394 for idx, line in enumerate(lines.asarrayptr(POINTER(CvPoint))):
00395 pts_start[0, idx] = line[0].y
00396 pts_start[1, idx] = line[0].x
00397 pts_end[0, idx] = line[1].y
00398 pts_end[1, idx] = line[1].x
00399
00400 if save_lines:
00401 pts_start_pixel = pts_start
00402 pts_end_pixel = pts_end
00403
00404 pts_start = (np.matrix([n_x,n_y]).T - pts_start)*m_per_pixel + br
00405 pts_end = (np.matrix([n_x,n_y]).T - pts_end)*m_per_pixel + br
00406
00407 along_vec = pts_end - pts_start
00408 along_vec = along_vec/ut.norm(along_vec)
00409 ang_vec = np.arctan2(-along_vec[0,:],along_vec[1,:])
00410
00411 res_list = []
00412 keep_indices = []
00413
00414 for i in range(n_lines):
00415 ang = ang_vec[0,i]
00416 if ang>math.radians(90):
00417 ang = ang - math.radians(180)
00418 if ang<math.radians(-90):
00419 ang = ang + math.radians(180)
00420
00421 rot_mat = tr.Rz(ang)[0:2,0:2]
00422 st = rot_mat*pts_start[:,i]
00423 en = rot_mat*pts_end[:,i]
00424 pts = rot_mat*xy_map
00425 x_all = pts[0,:]
00426 y_all = pts[1,:]
00427 min_x = min(st[0,0],en[0,0]) - 0.1
00428 max_x = max(st[0,0],en[0,0]) + 0.1
00429 min_y = min(st[1,0],en[1,0]) + 0.01
00430 max_y = max(st[1,0],en[1,0]) - 0.01
00431
00432 keep = np.multiply(np.multiply(x_all>min_x,x_all<max_x),
00433 np.multiply(y_all>min_y,y_all<max_y))
00434
00435 xy_sub = xy_map[:,np.where(keep)[1].A1.tolist()]
00436 if xy_sub.shape[1] == 0:
00437 continue
00438
00439 a,b,res = uto.fitLine_highslope(xy_sub[0,:].T, xy_sub[1,:].T)
00440 if res<0.0002:
00441 res_list.append(res)
00442 keep_indices.append(i)
00443
00444 if keep_indices == []:
00445 return [],[]
00446
00447 pts_start = pts_start[:,keep_indices]
00448 pts_end = pts_end[:,keep_indices]
00449
00450 print 'number of lines:', len(keep_indices)
00451
00452 if save_lines:
00453 ut.save_pickle(res_list,'residual_list_'+time_str+'.pkl')
00454 for i, idx in enumerate(keep_indices):
00455 s = pts_start_pixel[:,idx]
00456 e = pts_end_pixel[:,idx]
00457 cvLine(color_dst, cvPoint(int(s[1]),int(s[0])), cvPoint(int(e[1]),int(e[0])), CV_RGB(*(color_list[i])),
00458 3, 8)
00459 cvSaveImage('lines_'+time_str+'.png',color_dst)
00460
00461
00462 return pts_start.T.tolist(),pts_end.T.tolist()
00463
00464
00465
00466 def pixel_to_real(x,y, max_dist):
00467 ''' pixel to hokuyo
00468 x,y - NX1 matrices (N points)
00469 max_dist - dist which will be drawn at row 0
00470 '''
00471 return (org_y-y)*max_dist/400.,(org_x-x)*max_dist/400.
00472
00473
00474 def coord(x,y, max_dist):
00475 '''hokuyo coord frame to pixel (x,y) - floats
00476 x,y - NX1 matrices (N points)
00477 max_dist - dist which will be drawn at row 0
00478 '''
00479 return org_x-(400./max_dist)*y, org_y-(400./max_dist)*x
00480
00481 def draw_points(srf,x,y,color,max_dist,step=1):
00482 ''' step - set > 1 if you don't want to draw all the points.
00483 '''
00484 if len(x.A1) == 0:
00485 return
00486 x_pixel, y_pixel = coord(x.T,y.T,max_dist)
00487 for i in range(0,x_pixel.shape[0],step):
00488 pygame.draw.circle(srf, color, (int(x_pixel[i,0]+0.5), int(y_pixel[i,0]+0.5)), 2, 0)
00489
00490 def draw_hokuyo_scan(srf, scan, ang1, ang2, color, reject_zero_ten=True,step=1):
00491 ''' reject_zero_ten - don't show points with 0 or 10. range readings.
00492 step - set > 1 if you don't want to draw all the points.
00493 '''
00494 pts = get_xy_map(scan, ang1, ang2, reject_zero_ten=reject_zero_ten)
00495
00496 max_dist = MAX_DIST
00497 draw_points(srf,pts[0,:],pts[1,:],color,max_dist,step=step)
00498
00499
00500 def test_connected_comps(srf, scan):
00501
00502 n_colors = len(color_list)
00503 cc_list = connected_components(get_xy_map(scan,math.radians(-60),math.radians(60)),0.03)
00504
00505 for i,(p,q,p_idx,q_idx) in enumerate(cc_list):
00506 c1,c2 = p.A1.tolist(),q.A1.tolist()
00507 c1,c2 = coord(c1[0],c1[1], max_dist=MAX_DIST), coord(c2[0],c2[1], max_dist=MAX_DIST)
00508 pygame.draw.line(srf,color_list[i%n_colors],c1,c2,2)
00509
00510 def test_find_objects(srf, scan):
00511 obj_list = find_objects(scan, max_dist=0.6, max_size=0.3, min_size=0.01,
00512 min_angle=math.radians(-60), max_angle=math.radians(60),
00513 connect_dist_thresh=0.02)
00514 print 'number of objects:', len(obj_list)
00515
00516 for i,(p,q,c) in enumerate(obj_list):
00517 if i>4:
00518 break
00519 c1,c2 = p.A1.tolist(),q.A1.tolist()
00520 c1,c2 = coord(c1[0],c1[1], max_dist=MAX_DIST), coord(c2[0],c2[1], max_dist=MAX_DIST)
00521 pygame.draw.line(srf,color_list[i],c1,c2,2)
00522
00523 def test_find_closest_object_point(srf, scan):
00524 pt_interest = np.matrix([0.,0.]).T
00525
00526 p = find_closest_object_point(scan, pt_interest)
00527 if p == None:
00528 return
00529 c1,c2 = coord(p[0,0],p[1,0], max_dist=MAX_DIST)
00530 pygame.draw.circle(srf, (0,200,0), (c1,c2), 3, 0)
00531 c1,c2 = coord(pt_interest[0,0],pt_interest[1,0], max_dist=MAX_DIST)
00532 pygame.draw.circle(srf, (200,200,0), (c1,c2), 3, 0)
00533
00534 def tune_graze_effect_init():
00535 sl = pu.Slider((340, 20), 10)
00536 return sl
00537
00538 def tune_graze_effect_update(sl, srf, scan):
00539 sl.update()
00540 val = sl.value/255.
00541 angle = 160+val*20
00542
00543 remove_graze_effect_scan(scan,graze_angle_threshold=math.radians(angle))
00544 draw_hokuyo_scan(srf,scan,math.radians(-90), math.radians(90),color=(200,0,0),reject_zero_ten=False)
00545 points_removed = np.where(np.matrix(scan.ranges)>10.)[0].shape[1]
00546
00547 sl.set_text('angle: %.2f, points_removed: %d'%(angle, points_removed))
00548 sl.render(srf)
00549 if sl.clicked:
00550 return True
00551 else:
00552 return False
00553
00554 def test_find_handle_init():
00555 fr = np.matrix([1.28184669,0.05562259]).T
00556 bl = np.matrix([1.19585711,-0.06184923]).T
00557 max_dist = MAX_DIST
00558 x_fr, y_fr = coord(fr[0,0],fr[1,0],max_dist)
00559 x_bl, y_bl = coord(bl[0,0],bl[1,0],max_dist)
00560 pygame.draw.rect(srf,(200,0,200),pygame.Rect(x_bl,y_bl,x_fr-x_bl,y_fr-y_bl),1)
00561 cv_handle_template = create_handle_template(dict['scan'],dict['bk_lt'],dict['fr_rt'])
00562
00563
00564 def test_find_lines():
00565 pts = get_xy_map(scan,math.radians(-60),math.radians(60))
00566 p_start_list,p_end_list = hough_lines(pts)
00567 if p_start_list == []:
00568 return
00569
00570
00571
00572
00573
00574
00575
00576 n_colors = len(color_list)
00577
00578 for i,(p1,p2) in enumerate(zip(p_start_list,p_end_list)):
00579 c1,c2 = coord(p1[0],p1[1], max_dist=MAX_DIST), coord(p2[0],p2[1], max_dist=MAX_DIST)
00580 pygame.draw.line(srf,color_list[i%n_colors],c1,c2,2)
00581
00582
00583 if __name__ == '__main__':
00584
00585 p = optparse.OptionParser()
00586 p.add_option('-t', action='store', type='string', dest='hokuyo_type',
00587 help='hokuyo_type. urg or utm')
00588 p.add_option('-a', action='store', type='int', dest='avg',
00589 help='number of scans to average', default=1)
00590 p.add_option('-n', action='store', type='int', dest='hokuyo_number',
00591 help='hokuyo number. 0,1,2 ...')
00592 p.add_option('-f', action='store_true', dest='flip',
00593 help='flip the hokuyo scan')
00594 p.add_option('--ang_range', type='float', dest='ang_range',
00595 help='max angle of the ray to display (degrees)',
00596 default=360.)
00597
00598 opt, args = p.parse_args()
00599 hokuyo_type = opt.hokuyo_type
00600 hokuyo_number = opt.hokuyo_number
00601 avg_number = opt.avg
00602 flip = opt.flip
00603 ang_range = opt.ang_range
00604 ang_range = math.radians(ang_range)
00605
00606
00607 test_graze_effect_flag = False
00608 test_find_objects_flag = False
00609 test_find_closest_object_point_flag = False
00610 test_show_change_flag = False
00611 test_find_lines_flag = False
00612
00613
00614
00615
00616
00617 srf = pygame.display.set_mode((640,480))
00618 pygame.display.set_caption(hokuyo_type+' '+str(hokuyo_number))
00619
00620 fps = 100
00621 loopFlag = True
00622 clk = pygame.time.Clock()
00623
00624
00625 if hokuyo_type == 'utm':
00626 h = hs.Hokuyo('utm',hokuyo_number,start_angle=-ang_range,
00627 end_angle=ang_range,flip=flip)
00628 elif hokuyo_type == 'urg':
00629 h = hs.Hokuyo('urg',hokuyo_number,flip=flip)
00630 else:
00631 print 'unknown hokuyo type: ', hokuyo_type
00632 print 'Exiting...'
00633 sys.exit()
00634
00635
00636
00637
00638
00639 if test_graze_effect_flag:
00640 sl = tune_graze_effect_init()
00641
00642
00643 if test_show_change_flag:
00644 scan_prev = h.get_scan(avoid_duplicate=True, avg=avg_number, remove_graze=False)
00645 n_ch_pts_list = []
00646 while loopFlag:
00647
00648 widget_clicked = False
00649
00650
00651 srf.fill((255,255,255))
00652
00653
00654 pygame.draw.circle(srf, (200,0,0), (org_x,org_y), 10, 0)
00655
00656
00657 if test_graze_effect_flag or test_show_change_flag:
00658
00659 scan = h.get_scan(avoid_duplicate=True, avg=avg_number, remove_graze=False)
00660 else:
00661 scan = h.get_scan(avoid_duplicate=True, avg=avg_number, remove_graze=True)
00662
00663 if test_show_change_flag == False:
00664
00665
00666 draw_hokuyo_scan(srf,scan,math.radians(-135),math.radians(135),color=(0,0,200))
00667 else:
00668 diff_scan = subtract_scans(scan,scan_prev)
00669 scan_prev = scan
00670 pts = get_xy_map(diff_scan,math.radians(-40), math.radians(40),reject_zero_ten=True)
00671 n_ch_pts_list.append(pts.shape[1])
00672 max_dist = MAX_DIST
00673 draw_points(srf,pts[0,:],pts[1,:],color=(0,200,0),max_dist=max_dist)
00674
00675
00676 if test_find_objects_flag:
00677 test_find_objects(srf, scan)
00678 if test_find_closest_object_point_flag:
00679 test_find_closest_object_point(srf, scan)
00680 if test_graze_effect_flag:
00681 widget_clicked |= tune_graze_effect_update(sl,srf, scan)
00682 if test_find_lines_flag:
00683 test_find_lines()
00684
00685 pygame.display.flip()
00686
00687 events = pygame.event.get()
00688 for e in events:
00689 if e.type==pygame.locals.QUIT:
00690 loopFlag=False
00691 if e.type==pygame.locals.KEYDOWN:
00692 if e.key == 27:
00693 loopFlag=False
00694 if widget_clicked == False:
00695 if e.type==pygame.locals.MOUSEMOTION:
00696 if e.buttons[0] == 1:
00697
00698 org_x += e.rel[0]
00699 org_y += e.rel[1]
00700 if e.buttons[2] == 1:
00701
00702 MAX_DIST *= (1+e.rel[1]*0.01)
00703 MAX_DIST = max(MAX_DIST,0.1)
00704
00705
00706 clk.tick(fps)
00707 if test_show_change_flag:
00708 ut.save_pickle(n_ch_pts_list,ut.formatted_time()+'_ch_pts_list.pkl')
00709
00710
00711
00712
00713