| def hrl_hokuyo.hokuyo_processing.angle_to_index | ( | scan, | |
| angle | |||
| ) | 
returns index corresponding to angle (radians).
Definition at line 70 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.connected_components | ( | p, | |
| dist_threshold | |||
| ) | 
p - 2xN numpy matrix of euclidean points points (indices give neighbours).
    dist_threshold - max distance between two points in the same connected component.
                     typical value is .02 meters
    returns a list of (p1,p2, p1_index, p2_index): (start euclidean point, end euclidean point, start index, end index) 
    p1 and p2 are 2X1 matrices
Definition at line 131 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.coord | ( | x, | |
| y, | |||
| max_dist | |||
| ) | 
hokuyo coord frame to pixel (x,y) - floats
     x,y - NX1 matrices (N points)
     max_dist - dist which will be drawn at row 0
Definition at line 474 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.draw_hokuyo_scan | ( | srf, | |
| scan, | |||
| ang1, | |||
| ang2, | |||
| color, | |||
| reject_zero_ten = True, | |||
| step = 1 | |||
| ) | 
reject_zero_ten - don't show points with 0 or 10. range readings.
    step - set > 1 if you don't want to draw all the points.
Definition at line 490 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.draw_points | ( | srf, | |
| x, | |||
| y, | |||
| color, | |||
| max_dist, | |||
| step = 1 | |||
| ) | 
step - set > 1 if you don't want to draw all the points.
Definition at line 481 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.find_closest_object_point | ( | scan, | |
| pt_interest = np.matrix([0.,0.], | |||
| T, | |||
| min_angle = math.radians(-60), | |||
| max_angle = math.radians(60), | |||
| max_dist = 0.6, | |||
| min_size = 0.01, | |||
| max_size = 0.3 | |||
| ) | 
returns 2x1 matrix - centroid of connected component in hokuyo frame closest to pt_interest
    pt_interest - 2x1 matrix in hokuyo coord frame.
    None if no object found.
Definition at line 192 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.find_door | ( | start_pts_list, | |
| end_pts_list, | |||
| pt_interest = None | |||
| ) | 
returns [p1x,p1y], [p2x,p2y] ([],[] if no door found)
    returns line closest to the pt_interest.
    pt_interest - 2x1 matrix
Definition at line 256 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.find_objects | ( | scan, | |
| max_dist, | |||
| max_size, | |||
| min_size, | |||
| min_angle, | |||
| max_angle, | |||
| connect_dist_thresh, | |||
| all_pts = False | |||
| ) | 
max_dist - objects with centroid greater than max_dist will be ignored. (meters)
    max_size - objects greater than this are ignored. (meters)
    min_size - smaller than this are ignored (meters)
    min_angle, max_angle - part of scan to consider.
    connect_dist_thresh - points in scan greater than this will be treated as separate
                          connected components.
    all_pts == True:
        returns [ np matrix: 2xN1, 2xN2 ...] each matrix consists of all the points in the object.
    all_pts == False:
        returns list of (p,q,centroid): (end points,object centroid (2x1 matrices.))
Definition at line 156 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.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 | |||
| ) | 
start_angle - starting angle for points to be considered (hokuyo coord)
    end_angle - ending angle in hokuyo coord frame.
    scan - object of class HokuyoScan
    reject_zero_ten - ignore points at 0 or 10. meters
            
    returns - 2xN matrix, if get_intensities=True returns (2xN matrix, 1xN matrix)
Definition at line 80 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.hough_lines | ( | xy_map, | |
| save_lines = False | |||
| ) | 
xy_map - 2xN matrix of points.
    returns start_list, end_list. [[p1x,p1y],[p2x,p2y]...],[[q1x,q1y]...]
            [],[] if no lines were found.
Definition at line 356 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.index_to_angle | ( | scan, | |
| index | |||
| ) | 
returns angle (radians) corresponding to index.
Definition at line 75 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.pixel_to_real | ( | x, | |
| y, | |||
| max_dist | |||
| ) | 
pixel to hokuyo
     x,y - NX1 matrices (N points)
     max_dist - dist which will be drawn at row 0
Definition at line 466 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.remove_graze_effect | ( | ranges, | |
| angles, | |||
| skip = 1, | |||
| graze_angle_threshold = math.radians(169.) | |||
| ) | 
ranges,angles - 1xN numpy matrix
    skip - which two rays to consider.
    this function changes ranges.
Definition at line 211 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.remove_graze_effect_scan | ( | scan, | |
| graze_angle_threshold = math.radians(169.) | |||
| ) | 
changes scan
Definition at line 232 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.subtract_scans | ( | scan2, | |
| scan1, | |||
| threshold = 0.01 | |||
| ) | 
Definition at line 237 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.test_connected_comps | ( | srf, | |
| scan | |||
| ) | 
Definition at line 500 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.test_find_closest_object_point | ( | srf, | |
| scan | |||
| ) | 
Definition at line 523 of file hokuyo_processing.py.
Definition at line 554 of file hokuyo_processing.py.
Definition at line 564 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.test_find_objects | ( | srf, | |
| scan | |||
| ) | 
Definition at line 510 of file hokuyo_processing.py.
Definition at line 534 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.tune_graze_effect_update | ( | sl, | |
| srf, | |||
| scan | |||
| ) | 
Definition at line 538 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.xy_map_to_cv_image | ( | xy_map, | |
| m_per_pixel, | |||
| dilation_count = 0, | |||
| padding = 10 | |||
| ) | 
Definition at line 351 of file hokuyo_processing.py.
| def hrl_hokuyo.hokuyo_processing.xy_map_to_np_image | ( | xy_map, | |
| m_per_pixel, | |||
| dilation_count = 0, | |||
| padding = 50 | |||
| ) | 
returns binary numpy image. (255 for occupied
    pixels, 0 for unoccupied)
    2d array
Definition at line 323 of file hokuyo_processing.py.
| tuple hrl_hokuyo::hokuyo_processing::ang_range = opt.ang_range | 
Definition at line 603 of file hokuyo_processing.py.
Definition at line 601 of file hokuyo_processing.py.
| tuple hrl_hokuyo::hokuyo_processing::clk = pygame.time.Clock() | 
Definition at line 622 of file hokuyo_processing.py.
00001 [(255,255,0),(255,0,0),(0,255,255),(0,255,0),(0,0,255),(0,100,100),(100,100,0), 00002 (100,0,100),(100,200,100),(200,100,100),(100,100,200),(100,0,200),(0,200,100), 00003 (0,0,0),(0,100,200),(200,0,100),(100,0,100),(255,152,7) ]
Definition at line 65 of file hokuyo_processing.py.
| int hrl_hokuyo::hokuyo_processing::default = 360 | 
Definition at line 596 of file hokuyo_processing.py.
Definition at line 668 of file hokuyo_processing.py.
Definition at line 687 of file hokuyo_processing.py.
| hrl_hokuyo::hokuyo_processing::flip = opt.flip | 
Definition at line 602 of file hokuyo_processing.py.
| int hrl_hokuyo::hokuyo_processing::fps = 100 | 
Definition at line 620 of file hokuyo_processing.py.
00001 hs.Hokuyo('utm',hokuyo_number,start_angle=-ang_range, 00002 end_angle=ang_range,flip=flip)
Definition at line 626 of file hokuyo_processing.py.
| string hrl_hokuyo::hokuyo_processing::help = 'hokuyo_type. urg or utm' | 
Definition at line 587 of file hokuyo_processing.py.
| hrl_hokuyo::hokuyo_processing::hokuyo_number = opt.hokuyo_number | 
Definition at line 600 of file hokuyo_processing.py.
| hrl_hokuyo::hokuyo_processing::hokuyo_type = opt.hokuyo_type | 
Definition at line 599 of file hokuyo_processing.py.
Definition at line 621 of file hokuyo_processing.py.
| tuple hrl_hokuyo::hokuyo_processing::MAX_DIST = 1.0 | 
Definition at line 61 of file hokuyo_processing.py.
Definition at line 672 of file hokuyo_processing.py.
Definition at line 645 of file hokuyo_processing.py.
| tuple hrl_hokuyo::hokuyo_processing::p = optparse.OptionParser() | 
Definition at line 585 of file hokuyo_processing.py.
| tuple hrl_hokuyo::hokuyo_processing::pts = get_xy_map(diff_scan,math.radians(-40), math.radians(40),reject_zero_ten=True) | 
Definition at line 670 of file hokuyo_processing.py.
| tuple hrl_hokuyo::hokuyo_processing::scan = h.get_scan(avoid_duplicate=True, avg=avg_number, remove_graze=False) | 
Definition at line 659 of file hokuyo_processing.py.
| hrl_hokuyo::hokuyo_processing::scan_prev = h.get_scan(avoid_duplicate=True, avg=avg_number, remove_graze=False) | 
Definition at line 644 of file hokuyo_processing.py.
Definition at line 640 of file hokuyo_processing.py.
| tuple hrl_hokuyo::hokuyo_processing::srf = pygame.display.set_mode((640,480)) | 
Definition at line 617 of file hokuyo_processing.py.
Definition at line 609 of file hokuyo_processing.py.
Definition at line 611 of file hokuyo_processing.py.
Definition at line 608 of file hokuyo_processing.py.
Definition at line 607 of file hokuyo_processing.py.
Definition at line 610 of file hokuyo_processing.py.
Definition at line 648 of file hokuyo_processing.py.