lpi.py
Go to the documentation of this file.
00001 #
00002 #
00003 # Copyright (c) 2010, Georgia Tech Research Corporation
00004 # All rights reserved.
00005 # 
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Georgia Tech Research Corporation nor the
00014 #       names of its contributors may be used to endorse or promote products
00015 #       derived from this software without specific prior written permission.
00016 # 
00017 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00018 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00021 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00022 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00023 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00024 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00025 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00026 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 #
00028 
00029 # \author Advait Jain (Healthcare Robotics Lab, Georgia Tech.)
00030 
00031 import sys,os
00032 sys.path.append(os.environ['HRLBASEPATH']+'/usr/advait/LPI')
00033 import cam_utm_lpi as cul
00034 
00035 import hrl_lib.util as ut
00036 import hrl_lib.transforms as tr
00037 import mekabot.coord_frames as mcf
00038 import math, numpy as np
00039 import util as uto
00040 
00041 import tilting_hokuyo.processing_3d as p3d
00042 import camera_config as cc
00043 
00044 ## returns selected location in global coord frame.
00045 # @param angle - angle at which to take image, and about which to take
00046 # a 3D scan.
00047 def select_location(c,thok,angle):
00048     thok.servo.move_angle(angle)
00049     cvim = c.get_frame()
00050     cvim = c.get_frame()
00051     cvim = c.get_frame()
00052     im_angle = thok.servo.read_angle()
00053 
00054     tilt_angles = (math.radians(-20)+angle,math.radians(30)+angle)
00055     pos_list,scan_list = thok.scan(tilt_angles,speed=math.radians(10))
00056     m = p3d.generate_pointcloud(pos_list,scan_list,math.radians(-60), math.radians(60),
00057                                 0.0,-0.055)
00058     pts = mcf.utmcam0Tglobal(mcf.globalTthok0(m),im_angle)
00059 
00060     cam_params = cc.camera_parameters['mekabotUTM']
00061     fx = cam_params['focal_length_x_in_pixels']
00062     fy = cam_params['focal_length_y_in_pixels']
00063     cx,cy = cam_params['optical_center_x_in_pixels'],cam_params['optical_center_y_in_pixels']
00064     cam_proj_mat =  np.matrix([[fx, 0, cx],
00065                                [0, fy, cy],
00066                                [0,  0,  1]])
00067 
00068     cvim,pts2d = cul.project_points_in_image(cvim,pts,cam_proj_mat)
00069     cp = cul.get_click_location(cvim)
00070     print 'Clicked location:', cp
00071     if cp == None:
00072         return None
00073     idx = cul.get_index(pts2d.T,cp)
00074     pt3d = pts[:,idx]
00075     pt_interest = mcf.globalTutmcam0(pt3d,im_angle)
00076 
00077     hl_thok0 = mcf.thok0Tglobal(pt_interest)
00078     l1,l2 = 0.0,-0.055
00079     d = {}
00080     d['pt'] = hl_thok0
00081     d['pos_list'],d['scan_list'] = pos_list,scan_list
00082     d['l1'],d['l2'] = l1,l2
00083     d['img'] = uto.cv2np(cvim)
00084     ut.save_pickle(d,'hook_plane_scan_'+ut.formatted_time()+'.pkl')
00085 
00086     return pt_interest
00087 
00088 
00089 if __name__ == '__main__':
00090     import camera
00091     import hokuyo.hokuyo_scan as hs
00092     import tilting_hokuyo.tilt_hokuyo_servo as ths
00093 
00094     hok = hs.Hokuyo('utm',0,flip=True,ros_init_node=True)
00095     thok = ths.tilt_hokuyo('/dev/robot/servo0',5,hok,l1=0.,l2=-0.055)
00096     cam = camera.Camera('mekabotUTM')
00097     for i in range(10):
00098         cam.get_frame()
00099 
00100     pt = select_location(cam,thok)
00101     print 'Selected location in global coordinates:', pt.A1.tolist()
00102 
00103 


2010_icra_epc_pull
Author(s): Advait Jain, Charles C. Kemp (Healthcare Robotics Lab, Georgia Tech)
autogenerated on Wed Nov 27 2013 12:14:43