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 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
00045
00046
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