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 import roslib; roslib.load_manifest('hrl_tilting_hokuyo')
00031
00032 import time
00033 import sys, optparse
00034
00035 import numpy as np, math
00036
00037 import hrl_lib.util as ut
00038 import robotis.robotis_servo as rs
00039 import hrl_hokuyo.hokuyo_scan as hs
00040
00041
00042 class tilt_hokuyo():
00043 def __init__(self, dev_name, servo_id, hokuyo, baudrate=57600, l1=0.,l2=0.035, camera_name=None):
00044 ''' dev_name - name of serial device of the servo controller (e.g. '/dev/robot/servo0')
00045 servo_id - 2,3,4 ... (2 to 253)
00046 hokuyo - Hokuyo object.
00047 baudrate - for the servo controller.
00048 camera_name - name of the
00049 '''
00050 self.servo = rs.robotis_servo(dev_name,servo_id,baudrate)
00051
00052 self.hokuyo = hokuyo
00053 self.l1 = l1
00054 self.l2 = l2
00055
00056 def scan(self, range, speed, save_scan=False,avg=1, _max_retries=0):
00057 ''' range - (start,end) in radians
00058 speed - scan speed in radians/sec
00059 save_scan - save a dict of pos_list,scan_list,l1,l2
00060 avg - average scans from the hokuyo.
00061 returns pos_list,scan_list. list of angles and HokuyoScans
00062 '''
00063
00064 ramp_up_angle = math.radians(5)
00065 if abs(range[0])+ramp_up_angle > math.radians(95) or \
00066 abs(range[1])+ramp_up_angle > math.radians(95):
00067 print 'tilt_hokuyo_servo.scan:bad angles- ',math.degrees(range[0]),math.degrees(range[1])
00068
00069 min_angle = min(range[0],range[1])
00070 max_angle = max(range[0],range[1])
00071
00072
00073
00074
00075 self.servo.move_angle(range[0]+np.sign(range[0])*ramp_up_angle)
00076
00077
00078
00079
00080 self.servo.move_angle(range[1]+np.sign(range[1])*ramp_up_angle,speed,blocking=False)
00081
00082 time.sleep(0.05)
00083 t1 = time.time()
00084
00085 pos_list = []
00086 scan_list = []
00087 while self.servo.is_moving():
00088 pos = self.servo.read_angle()
00089
00090 if pos < min_angle or pos > max_angle:
00091 continue
00092 pos_list.append(pos)
00093 plane_scan = self.hokuyo.get_scan(avoid_duplicate=True,remove_graze=True,avg=avg)
00094 scan_list.append(plane_scan)
00095 t2 = time.time()
00096
00097 self.servo.move_angle(0)
00098 if save_scan:
00099 date_name = ut.formatted_time()
00100 dict = {'pos_list': pos_list,'scan_list': scan_list,
00101 'l1': self.l1, 'l2': self.l2}
00102 ut.save_pickle(dict,date_name+'_dict.pkl')
00103
00104 runtime = t2 - t1
00105 expected_number_scans = 19.0 * runtime * (1.0/avg)
00106 scan_threshold = expected_number_scans - expected_number_scans*.2
00107 if len(scan_list) < scan_threshold:
00108 print 'tilt_hokuyo_servo.scan: WARNING! Expected at least %d scans but got only %d scans.' % (expected_number_scans, len(scan_list))
00109 print 'tilt_hokuyo_servo.scan: trying again.. retries:', _max_retries
00110 if _max_retries > 0:
00111 return self.scan(range, speed, save_scan, avg, _max_retries = _max_retries-1)
00112 else:
00113 print 'tilt_hokuyo_servo.scan: returning anyway'
00114
00115 print 'tilt_hokuyo_servo.scan: got %d scans over range %f with speed %f.' % (len(scan_list), (max_angle - min_angle), speed)
00116 return pos_list,scan_list
00117
00118 def scan_around_pt(self,pt,speed=math.radians(5)):
00119 ''' pt - in thok coord frame.
00120 this function scans in a fixed range.
00121 returns pos_lit,scan_list
00122 '''
00123 ang1 = math.radians(40)
00124 ang2 = math.radians(0)
00125
00126 tilt_angles = (ang1,ang2)
00127 pos_list,scan_list = self.scan(tilt_angles,speed=speed)
00128 return pos_list,scan_list
00129
00130
00131 if __name__ == '__main__':
00132
00133
00134
00135
00136 p = optparse.OptionParser()
00137 p.add_option('-d', action='store', type='string', dest='servo_dev_name',
00138 default='/dev/robot/servo0', help='servo device string. [default= /dev/robot/servo0]')
00139 p.add_option('-t', action='store', type='string', dest='hokuyo_type',default='utm',
00140 help='hokuyo_type. urg or utm [default=utm]')
00141 p.add_option('-n', action='store', type='int', dest='hokuyo_number', default=0,
00142 help='hokuyo number. 0,1,2 ... [default=0]')
00143 p.add_option('--save_scan',action='store_true',dest='save_scan',
00144 help='save the scan [dict and cloud]')
00145 p.add_option('--speed', action='store', type='float', dest='scan_speed',
00146 help='scan speed in deg/s.[default=5]',default=5.)
00147 p.add_option('--ang0', action='store', type='float', dest='ang0',
00148 help='starting tilt angle for scan (degrees). [default=20]', default=20.0)
00149 p.add_option('--ang1', action='store', type='float', dest='ang1',
00150 help='ending tilt angle for scan (degrees). default=[-20]', default=-20.0)
00151 p.add_option('--id', action='store', type='int', dest='servo_id', default=2,
00152 help='servo id 1,2 ... [default=2]')
00153 p.add_option('--l2', action='store', type='float', dest='l2', help='l2 (in meters) [0.035 for ElE, -0.055 for mekabot]')
00154 p.add_option('--flip', action='store_true', dest='flip',help='flip the hokuyo scan')
00155
00156
00157 opt, args = p.parse_args()
00158 hokuyo_type = opt.hokuyo_type
00159 hokuyo_number = opt.hokuyo_number
00160 servo_dev_name = opt.servo_dev_name
00161
00162 save_scan = opt.save_scan
00163 scan_speed = math.radians(opt.scan_speed)
00164
00165 ang0 = opt.ang0
00166 ang1 = opt.ang1
00167
00168 servo_id = opt.servo_id
00169 l2 = opt.l2
00170 if l2==None:
00171 print 'please specify l2. do -h for details.'
00172 print 'Exiting...'
00173 sys.exit()
00174
00175 flip = opt.flip
00176
00177 if hokuyo_type == 'utm':
00178 h = hs.Hokuyo('utm',hokuyo_number,flip=flip)
00179 elif hokuyo_type == 'urg':
00180 h = hs.Hokuyo('urg',hokuyo_number,flip=flip)
00181 else:
00182 print 'unknown hokuyo type: ', hokuyo_type
00183
00184 thok = tilt_hokuyo(servo_dev_name,servo_id,h,l1=0.,l2=l2)
00185
00186 tilt_angles = (math.radians(ang0),math.radians(ang1))
00187 pos_list,scan_list = thok.scan(tilt_angles,speed=scan_speed,save_scan=save_scan)
00188
00189 sys.exit()
00190
00191