hokuyo_processing.py
Go to the documentation of this file.
00001 #
00002 # Copyright (c) 2009, Georgia Tech Research Corporation
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Georgia Tech Research Corporation nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00021 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00022 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00023 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00024 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00025 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 #
00027 
00028 #  \author Advait Jain (Healthcare Robotics Lab, Georgia Tech.)
00029 
00030 
00031 
00032 # functions to use scans from hokuyos.
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 #import util as uto
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 # Need to switch to Willow's OpenCV 2.0 python bindings at some point of time.
00056 # from opencv.cv import *
00057 # from opencv.highgui import *
00058 
00059 from ctypes import POINTER
00060 
00061 MAX_DIST=1.0
00062 # pixel coordinates of origin
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 #------ sigmoid hack to increase ranges of points with small range(<0.3) ----
00093     #    short_range_idxs = np.where(scan.ranges<0.3)
00094     #    scan.ranges[short_range_idxs] = scan.ranges[short_range_idxs]*1.05
00095 
00096 #--- test1
00097 #        scan.ranges = np.multiply(scan.ranges, 1+(1-np.divide(1.,1+np.power(np.e,-5*(scan.ranges-0.3))))*0.1)
00098 
00099 #--- test2
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 #--- test3
00103 #        scan.ranges = np.multiply(scan.ranges, 1+(1-np.divide(1.,1+np.power(np.e,-5*(scan.ranges-0.3))))*0.2)
00104 
00105 #--- test4
00106 #        scan.ranges = np.multiply(scan.ranges, 1+(1-np.divide(1.,1+np.power(np.e,-5*(scan.ranges-0.25))))*0.15)
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) # boundaries of the connected components
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 #    print 'idx_remove_1:', idx_remove_1
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     #idxs = np.where(np.abs(diff_range)<0.04)
00247 #    idxs = np.where(np.abs(diff_range)<0.01)
00248 #    idxs = np.where(np.abs(diff_range)<0.005)
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 #    print 'start_pts_list,end_pts_list',start_pts_list,end_pts_list
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 #    print 'start_pts,end_pts',start_pts.A1.tolist(),end_pts.A1.tolist()
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         # dist - distance of closest point within the line segment
00300         #        or distance of the closest end point.
00301         # keep line segments that are within some distance threshold.
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         # find distance from the robot and select furthest line.
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 #    save_lines=True
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 #    for i in range(3):
00368 #        cvSmooth(img_cv,img_cv,CV_GAUSSIAN,3,3)
00369 #    cvSaveImage('aloha'+str(time.time())+'.png',img_cv)
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 #    cvCanny(img_cv,img_cv,50,200)
00381 #    cvSaveImage('aloha.png',img_cv)
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 #    cvReleaseMemStorage(storage)
00462     return pts_start.T.tolist(),pts_end.T.tolist()
00463 
00464 
00465 #------------- displaying in pygame -----------
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 #    return org_x-(400./max_dist)*y, org_y-(400./max_dist)*x
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 #    pts = get_xy_map(scan, reject_zero_ten=reject_zero_ten)
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     #colors_list = [(200,0,0), (0,255,0), (100,100,0), (100,0,100), (0,100,100)]
00502     n_colors = len(color_list)
00503     cc_list = connected_components(get_xy_map(scan,math.radians(-60),math.radians(60)),0.03)
00504 # draw the connected components as lines
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     #colors_list = [(200,0,0), (0,255,0), (100,100,0), (100,0,100), (0,100,100)]
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 #    pt_interest = np.matrix([0.3,-0.04]).T
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     #-------  to test door finding ----------
00571 #    p_start,p_end = find_door(p_start_list,p_end_list)
00572 #    if p_start == []:
00573 #        return
00574 #    p_start_list,p_end_list = [p_start],[p_end]
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 #------- which things to test ---------
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     # Initialize pygame
00614 #    pygame.init()
00615 
00616     # Open a display
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 #    scan1 = h.get_scan(avoid_duplicate=True)
00636 #    sys.exit()
00637 
00638 #---------- initializations -------------
00639     if test_graze_effect_flag:
00640         sl = tune_graze_effect_init()
00641 
00642 #--------- and now loop -----------
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         # Clear the screen
00651         srf.fill((255,255,255))
00652 
00653         # Draw the urg
00654         pygame.draw.circle(srf, (200,0,0), (org_x,org_y), 10, 0)
00655 
00656         #display all the points
00657         if test_graze_effect_flag or test_show_change_flag:
00658             # here we don't want any filtering on the scan.
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             #draw_hokuyo_scan(srf,scan,math.radians(-70), math.radians(70),color=(0,0,200))
00665             #draw_hokuyo_scan(srf,scan,math.radians(-90), math.radians(90),color=(0,0,200))
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 #        test_connected_comps(srf, scan)
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: # Esc
00693                     loopFlag=False
00694             if widget_clicked == False:
00695                 if e.type==pygame.locals.MOUSEMOTION:
00696                     if e.buttons[0] == 1:
00697                         # left button
00698                         org_x += e.rel[0]
00699                         org_y += e.rel[1]
00700                     if e.buttons[2] == 1:
00701                         # right button
00702                         MAX_DIST *= (1+e.rel[1]*0.01)
00703                         MAX_DIST = max(MAX_DIST,0.1)
00704 
00705         # Try to keep the specified framerate   
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 


hrl_hokuyo
Author(s): Advait Jain, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:55:51