class CalibrationManager:
def __init__(self, robot_description, urdf_xml, joint_names):
self.lock = threading.Lock()
self.urdf_xml = urdf_xml
self.publish_list = {}
self.tf_listener = tf.TransformListener()
self.config = {'sensors': {'rectified_cams': {}, 'chains': {}, 'tilting_lasers': {}}, 'checkerboards': {}, 'transforms': {}}
self.joint_names = joint_names
self.robot_params = UrdfParams(robot_description, self.config)
self.updated = False
self.sub = rospy.Subscriber('camera_calibration', CameraCalibration, self.cal_cb)
def is_updated(self):
return self.updated
def cal_cb(self, msg):
with self.lock:
self.publish_list = {}
for pose, camera in zip(msg.camera_pose, msg.camera_id):
camera = camera.strip("/")
if not camera in self.publish_list and camera in self.joint_names:
joint = self.robot_params.urdf.joint_map[self.joint_names[camera]]
chain = self.robot_params.urdf.get_chain(joint.child, camera)
self.publish_list[camera] = CameraTransformator(joint, chain, self.tf_listener)
print "New pose for ", camera, "\n", pose
self.publish_list[camera].set_pose(pose)
self.updated = True
def update(self):
with self.lock:
transforms = {}
for link_name, joint_name in self.joint_names.iteritems():
transforms[joint_name] = self.publish_list[link_name].get_transform()
self.config['transforms'] = transforms
self.robot_params.configure(self.robot_params.get_clean_urdf(), self.config)
new_urdf = update_urdf(self.robot_params.get_clean_urdf(), self.robot_params)
# write out to URDF
outfile = open(self.urdf_xml, 'w')
rospy.loginfo('Writing model updates to %s', self.urdf_xml)
outfile.write( new_urdf.to_xml_string() )
outfile.close()
# updating the description
rospy.loginfo("Setting the new robot description")
rospy.set_param("robot_description", new_urdf.to_xml_string())
self.updated = False
def setUp(self):
config = yaml.load(open(rospy.get_param('config_file')))
self.robot_params = UrdfParams(rospy.get_param('robot_description'),
config)
rospy.wait_for_service('fk', 3.0)
self._fk_ref = rospy.ServiceProxy('fk', FkTest)
def __init__(self, robot_description, urdf_xml, joint_names):
self.lock = threading.Lock()
self.urdf_xml = urdf_xml
self.publish_list = {}
self.tf_listener = tf.TransformListener()
self.config = {'sensors': {'rectified_cams': {}, 'chains': {}, 'tilting_lasers': {}}, 'checkerboards': {}, 'transforms': {}}
self.joint_names = joint_names
self.robot_params = UrdfParams(robot_description, self.config)
self.updated = False
self.sub = rospy.Subscriber('camera_calibration', CameraCalibration, self.cal_cb)
def __init__(self, robot_description, urdf_xml, joint_names):
self.lock = threading.Lock()
self.urdf_xml = urdf_xml
self.publish_list = {}
self.tf_listener = tf.TransformListener()
self.config = {
'sensors': {
'rectified_cams': {},
'chains': {},
'tilting_lasers': {}
},
'checkerboards': {},
'transforms': {}
}
self.joint_names = joint_names
self.robot_params = UrdfParams(robot_description, self.config)
self.updated = False
self.sub = rospy.Subscriber('camera_calibration', CameraCalibration,
self.cal_cb)
def loadSystem():
robot_description = get_robot_description(
'/tmp/pr2_calibration/cal_measurements.bag')
outfile = open(raw_xml, 'w')
outfile.write(robot_description)
outfile.close()
config = yaml.load('''
sensors:
chains: {}
rectified_cams: {}
tilting_lasers: {}
transforms: {}
checkerboards: {}
''')
return UrdfParams(robot_description, config)
def loadSystem1():
urdf = '''
<robot>
<link name="base_link"/>
<joint name="j0" type="fixed">
<origin xyz="10 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="j0_link"/>
</joint>
<link name="j0_link"/>
<joint name="j1" type="revolute">
<axis xyz="0 0 1"/>
<origin xyz="1 0 0" rpy="0 0 0"/>
<parent link="j0_link"/>
<child link="j1_link"/>
</joint>
<link name="j1_link"/>
<joint name="j2" type="revolute">
<axis xyz="0 0 1"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="j1_link"/>
<child link="j2_link"/>
</joint>
<link name="j2_link"/>
<joint name="j3" type="fixed">
<origin xyz="0 0 20" rpy="0 0 0"/>
<parent link="j2_link"/>
<child link="j3_link"/>
</joint>
<link name="j3_link"/>
</robot>
'''
config = yaml.load('''
sensors:
chains:
chain1:
root: j0_link
tip: j2_link
cov:
joint_angles: [0.001, 0.001]
gearing: [1.0, 1.0]
rectified_cams: {}
tilting_lasers: {}
transforms: {}
checkerboards: {}
''')
return UrdfParams(urdf, config)
def loadSystem():
urdf = '''
<robot>
<link name="base_link"/>
<joint name="j0" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="j0_link"/>
</joint>
<link name="j0_link"/>
<joint name="j1" type="revolute">
<axis xyz="0 0 1"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="j0_link"/>
<child link="j1_link"/>
</joint>
<link name="j1_link"/>
<joint name="j2" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="j1_link"/>
<child link="j2_link"/>
</joint>
<link name="j2_link"/>
</robot>
'''
config = yaml.load('''
sensors:
chains: {}
rectified_cams: {}
tilting_lasers:
laserA:
sensor_id: laserA
joint: j1
frame_id: j2_link
gearing: 1
cov:
bearing: 1
range: 1
tilt: 1
transforms: {}
checkerboards: {}
''')
return config["sensors"], UrdfParams(urdf, config)
def loadSystem():
urdf = '''
<robot>
<link name="base_link"/>
<joint name="j0" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="j0_link"/>
</joint>
<link name="j0_link"/>
<joint name="j1" type="revolute">
<axis xyz="0 0 1"/>
<origin xyz="1 0 0" rpy="0 0 0"/>
<parent link="j0_link"/>
<child link="j1_link"/>
</joint>
<link name="j1_link"/>
<joint name="j2" type="revolute">
<axis xyz="0 0 1"/>
<origin xyz="1 0 0" rpy="0 0 0"/>
<parent link="j1_link"/>
<child link="j2_link"/>
</joint>
<link name="j2_link"/>
<joint name="j3" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="j2_link"/>
<child link="j3_link"/>
</joint>
<link name="j3_link"/>
</robot>
'''
config = yaml.load('''
sensors:
chains:
chainA:
sensor_id: chainA
root: j0_link
tip: j1_link
cov:
joint_angles: [1]
gearing: [1.0]
chainB:
sensor_id: chainB
root: j0_link
tip: j2_link
cov:
joint_angles: [1, 1]
gearing: [1.0, 1.0]
rectified_cams: {}
tilting_lasers: {}
transforms:
chainA_cb: [0, 0, 0, 0, 0, 0]
chainB_cb: [0, 0, 0, 0, 0, 0]
checkerboards:
boardA:
corners_x: 2
corners_y: 2
spacing_x: 1
spacing_y: 1
''')
return config["sensors"]["chains"], UrdfParams(urdf, config)
# Check if we can write to all of our output files
output_filenames = [calibrated_xml]
for suffix in [".yaml", "_poses.yaml", "_cov.txt"]:
output_filenames += [output_dir + "/" + cur_step["output_filename"] + suffix for cur_step in step_list]
valid_list = [check_file_permissions(curfile) for curfile in output_filenames];
permissions_valid = all(valid_list)
if not permissions_valid:
print "Invalid file permissions. You need to be able to write to the following files:"
print "\n".join([" - " + cur_file for cur_file,cur_valid in zip(output_filenames, valid_list) if not cur_valid])
sys.exit(-1)
# Generate robot parameters
robot_description = get_robot_description(bag_filename)
robot_params = UrdfParams(robot_description, config)
# Load all the sensors from the bagfile and config file
for cur_step in step_list:
print ""
print "*"*30
print "Beginning [%s]" % cur_step["name"]
# Need to load only the sensors that we're interested in
cur_sensors = load_requested_sensors(all_sensors_dict, cur_step['sensors'])
# Load all the sensors from bag
multisensors = get_multisensors(bag_filename, cur_sensors, sample_skip_list)
# Display sensor count statistics
print "Executing step with the following Sensors:"
# Check if we can write to all of our output files
output_filenames = [calibrated_xml]
for suffix in [".yaml", "_poses.yaml", "_cov.txt"]:
output_filenames += [output_dir + "/" + cur_step["output_filename"] + suffix for cur_step in step_list]
valid_list = [check_file_permissions(curfile) for curfile in output_filenames];
permissions_valid = all(valid_list)
if not permissions_valid:
print "Invalid file permissions. You need to be able to write to the following files:"
print "\n".join([" - " + cur_file for cur_file,cur_valid in zip(output_filenames, valid_list) if not cur_valid])
sys.exit(-1)
# Generate robot parameters
robot_description = get_robot_description(bag_filename)
robot_params = UrdfParams(robot_description, config)
if robot_params == "":
print bag_filename, " does not have robot_description, exitting.."
sys.exit(-1)
# Load all the sensors from the bagfile and config file
for cur_step in step_list:
print ""
print "*"*30
print "Beginning [%s]" % cur_step["name"]
# Need to load only the sensors that we're interested in
cur_sensors = load_requested_sensors(all_sensors_dict, cur_step['sensors'])
# Load all the sensors from bag
"Could not find namespace [%s/%s]. Please populate this namespace with sensors.",
(config_param_name, sensors_name))
sys.exit(1)
all_sensors_dict = est_helpers.build_sensor_defs(config[sensors_name])
all_sensor_types = list(
set([x['sensor_type'] for x in all_sensors_dict.values()]))
# Load all the calibration steps.
step_list = est_helpers.load_calibration_steps(config["cal_steps"])
# Only process the last step
cur_step = step_list[-1]
# Load the resulting system definition
system_def_dict = yaml.load(
open(output_dir + "/" + cur_step["output_filename"] + ".yaml"))
system_def = UrdfParams(robot_description, system_def_dict)
cb_poses = yaml.load(
open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml"))
free_dict = yaml.load(cur_step["free_params"])
sample_skip_list = rospy.get_param('calibration_skip_list', [])
scatter_list = []
marker_count = 0
label_list = list()
for cur_loop in loop_list:
marker_count += 1
sensor_defs = est_helpers.load_requested_sensors(
all_sensors_dict, [cur_loop['cam'], cur_loop['3d']])
multisensors = get_multisensors(bag_filename, sensor_defs,
sample_skip_list)
class CalibrationManager:
def __init__(self, robot_description, urdf_xml, joint_names):
self.lock = threading.Lock()
self.urdf_xml = urdf_xml
self.publish_list = {}
self.tf_listener = tf.TransformListener()
self.config = {
'sensors': {
'rectified_cams': {},
'chains': {},
'tilting_lasers': {}
},
'checkerboards': {},
'transforms': {}
}
self.joint_names = joint_names
self.robot_params = UrdfParams(robot_description, self.config)
self.updated = False
self.sub = rospy.Subscriber('camera_calibration', CameraCalibration,
self.cal_cb)
def is_updated(self):
return self.updated
def cal_cb(self, msg):
with self.lock:
self.publish_list = {}
for pose, camera in zip(msg.camera_pose, msg.camera_id):
camera = camera.strip("/")
if not camera in self.publish_list and camera in self.joint_names:
joint = self.robot_params.urdf.joint_map[
self.joint_names[camera]]
chain = self.robot_params.urdf.get_chain(
joint.child, camera)
self.publish_list[camera] = CameraTransformator(
joint, chain, self.tf_listener)
print "New pose for ", camera, "\n", pose
self.publish_list[camera].set_pose(pose)
self.updated = True
def update(self):
with self.lock:
transforms = {}
for link_name, joint_name in self.joint_names.iteritems():
transforms[joint_name] = self.publish_list[
link_name].get_transform()
self.config['transforms'] = transforms
self.robot_params.configure(self.robot_params.get_clean_urdf(),
self.config)
new_urdf = update_urdf(self.robot_params.get_clean_urdf(),
self.robot_params)
# write out to URDF
outfile = open(self.urdf_xml, 'w')
rospy.loginfo('Writing model updates to %s', self.urdf_xml)
outfile.write(new_urdf.to_xml_string())
outfile.close()
# updating the description
rospy.loginfo("Setting the new robot description")
rospy.set_param("robot_description", new_urdf.to_xml_string())
self.updated = False