def __init__(self, name, bot): # pass in the name of the head component self.bot = bot c = m3t.get_component_config_filename(name) self.config_name = c[:-4] + "_toolbox.yml" try: f = file(self.config_name, "r") self.config = yaml.safe_load(f.read()) except (IOError, EOFError): print "Config file not present:", self.config_name return self.joints = range(7) # lower 7Dof self.joint_names = { "NeckTilt": 0, "NeckPan": 1, "HeadRoll": 2, "HeadTilt": 3, "EyeTilt": 4, "EyePanRight": 5, "EyePanLeft": 6, } fcr = m3t.get_m3_ros_config_path()[-1] + "cameras/" + self.config["right_camera_calibration"] + ".yml" fcl = m3t.get_m3_ros_config_path()[-1] + "cameras/" + self.config["left_camera_calibration"] + ".yml" try: f = file(fcr, "r") cr = yaml.safe_load(f.read()) except (IOError, EOFError): print "Config file not present:", fcr return try: f = file(fcl, "r") cl = yaml.safe_load(f.read()) except (IOError, EOFError): print "Config file not present:", fcl return pr = cr["projection_matrix"]["data"] pl = cl["projection_matrix"]["data"] # extract calibration data for camera. should really do this with ROS image_geometry package self.camera_calib = { "right": { "fx": pr[0], "fy": pr[5], "cx": pr[2], "cy": pr[6], "w": cr["image_width"], "h": cr["image_height"], }, "left": { "fx": pl[0], "fy": pl[5], "cx": pl[2], "cy": pl[6], "w": cl["image_width"], "h": cl["image_height"], }, }
def __get_max_slew_rates_from_config(self): file_name = m3t.get_component_config_filename(self.name) try: f=file(file_name,'r') config= yaml.safe_load(f.read()) except (IOError, EOFError): print 'Config file not present:',file_name return max_slew_rates = [0.0]*self.get_num_dof() for k in config['joint_components']: joint_name = config['joint_components'][k] joint_file_name = m3t.get_component_config_filename(joint_name) try: jf=file(joint_file_name,'r') joint_config= yaml.safe_load(jf.read()) except (IOError, EOFError): print 'Config file not present:', joint_file_name return max_slew_rates[int(k[1:])] = joint_config['param']['max_q_slew_rate'] return max_slew_rates
def __get_max_slew_rates_from_config(self): file_name = m3t.get_component_config_filename(self.name) try: f = file(file_name, 'r') config = yaml.safe_load(f.read()) except (IOError, EOFError): print 'Config file not present:', file_name return max_slew_rates = [0.0] * self.get_num_dof() for k in config['joint_components']: joint_name = config['joint_components'][k] joint_file_name = m3t.get_component_config_filename(joint_name) try: jf = file(joint_file_name, 'r') joint_config = yaml.safe_load(jf.read()) except (IOError, EOFError): print 'Config file not present:', joint_file_name return max_slew_rates[int( k[1:])] = joint_config['param']['max_q_slew_rate'] return max_slew_rates
def __init__(self, name, bot): #pass in the name of the head component self.bot = bot c = m3t.get_component_config_filename(name) self.config_name = c[:-4] + '_toolbox.yml' try: f = file(self.config_name, 'r') self.config = yaml.safe_load(f.read()) except (IOError, EOFError): print 'Config file not present:', self.config_name return self.joints = range(7) #lower 7Dof self.joint_names = { 'NeckTilt': 0, 'NeckPan': 1, 'HeadRoll': 2, 'HeadTilt': 3, 'EyeTilt': 4, 'EyePanRight': 5, 'EyePanLeft': 6 } fcr = m3t.get_m3_ros_config_path( )[-1] + 'cameras/' + self.config['right_camera_calibration'] + '.yml' fcl = m3t.get_m3_ros_config_path( )[-1] + 'cameras/' + self.config['left_camera_calibration'] + '.yml' try: f = file(fcr, 'r') cr = yaml.safe_load(f.read()) except (IOError, EOFError): print 'Config file not present:', fcr return try: f = file(fcl, 'r') cl = yaml.safe_load(f.read()) except (IOError, EOFError): print 'Config file not present:', fcl return pr = cr['projection_matrix']['data'] pl = cl['projection_matrix']['data'] #extract calibration data for camera. should really do this with ROS image_geometry package self.camera_calib={'right':{'fx':pr[0],'fy':pr[5],'cx':pr[2],'cy':pr[6],'w':cr['image_width'],'h':cr['image_height']},\ 'left':{'fx':pl[0],'fy':pl[5],'cx':pl[2],'cy':pl[6] ,'w':cl['image_width'],'h':cl['image_height']}}