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']}}
