tilt_hokuyo_servo.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 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 #        if max_angle>math.radians(60.5):
00072 #            print 'tilt_hokuyo_servo.scan: maximum angle is too high, will graze bottom plate of mount. angle:', math.degrees(max_angle)
00073 #            sys.exit()
00074 
00075         self.servo.move_angle(range[0]+np.sign(range[0])*ramp_up_angle)
00076 #        time.sleep(0.05)
00077 #        while(self.servo.is_moving()):
00078 #            continue
00079 
00080         self.servo.move_angle(range[1]+np.sign(range[1])*ramp_up_angle,speed,blocking=False)
00081         #self.servo.move_angle(range[1], speed)
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             #print 'h6', pos
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 # urg mount - l1=0.06, l2=0.05
00134 # utm - l1 = 0.0, l2 = 0.035
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() # to kill the hokuyo thread.
00190 
00191 


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