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
00031
00032
00033
00034
00035
00036 import roslib; roslib.load_manifest('calibration_estimation')
00037
00038 import sys
00039 import rospy
00040 import time
00041 import numpy
00042 import rosbag
00043 import yaml
00044 import os.path
00045 import numpy
00046 import math
00047
00048 import stat
00049 import os
00050
00051 from numpy import matrix
00052
00053 from calibration_estimation.cal_bag_helpers import *
00054 from calibration_estimation.urdf_params import UrdfParams
00055 from calibration_estimation.sensors.multi_sensor import MultiSensor
00056 from calibration_estimation.opt_runner import opt_runner
00057
00058 from calibration_estimation.single_transform import angle_axis_to_RPY, RPY_to_angle_axis
00059
00060 def usage():
00061 rospy.logerr("Not enough arguments")
00062 print "Usage:"
00063 print " ./proto1.py [bagfile] [output_dir]"
00064 sys.exit(0)
00065
00066
00067 def build_sensor_defs(sensors):
00068 '''
00069 Given a list of sensor definition dictionaries, merge them into a single dictionary
00070 '''
00071 all_sensors_dict = dict()
00072
00073
00074 for cur_sensor_type, cur_sensor_list in sensors.items():
00075 if cur_sensor_type in ['checkerboards']:
00076 continue
00077 for cur_sensor_name, cur_sensor in cur_sensor_list.items():
00078 cur_sensor['sensor_id'] = cur_sensor_name
00079
00080 if cur_sensor['sensor_id'] in all_sensors_dict.keys():
00081 rospy.logwarn("Loading a duplicate [%s]. Overwriting previous" % cur_sensor['sensor_id'])
00082 all_sensors_dict[cur_sensor['sensor_id']] = cur_sensor
00083 all_sensors_dict[cur_sensor['sensor_id']]['sensor_type'] = cur_sensor_type
00084
00085 print "The following error sensors have been loaded:\n"
00086
00087 all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
00088
00089 for cur_sensor_type in all_sensor_types:
00090 print " %s sensors" % cur_sensor_type
00091 cur_sensor_ids = [cur_sensor_id for cur_sensor_id,cur_sensor in all_sensors_dict.items() if cur_sensor['sensor_type'] == cur_sensor_type]
00092 cur_sensor_ids.sort()
00093 for cur_sensor_id in cur_sensor_ids:
00094 print " - %s" % cur_sensor_id
00095 print ""
00096
00097 return all_sensors_dict
00098
00099 def load_calibration_steps(steps_dict):
00100
00101 step_keys = steps_dict.keys()
00102 step_keys.sort()
00103 step_list = []
00104 for step_name in step_keys:
00105
00106 steps_dict[step_name]["name"] = step_name
00107 step_list.append(steps_dict[step_name])
00108 print "Loaded the calibration steps to execute in the following order:"
00109 for cur_step in step_list:
00110 print " - %s" % cur_step['name']
00111 return step_list
00112
00113
00114
00115 def check_file_permissions(filename):
00116
00117 if not os.path.isfile(filename):
00118 return True
00119
00120 statinfo = os.stat(filename);
00121
00122
00123 if (os.geteuid() == statinfo.st_uid):
00124 return (os.stat(filename).st_mode & stat.S_IWUSR) > 0
00125
00126
00127 if (os.getgid() == statinfo.st_gid):
00128 return (os.stat(filename).st_mode & stat.S_IWGRP) > 0
00129
00130
00131 return (os.stat(filename).st_mode & stat.S_IWOTH) > 0
00132
00133
00134 def load_requested_sensors(all_sensors_dict, requested_sensors):
00135 '''
00136 Build a sensor dictionary with the subset of sensors that we request
00137 '''
00138 all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
00139 cur_sensors = dict([(x,[]) for x in all_sensor_types])
00140 for requested_sensor_id in requested_sensors:
00141
00142 if requested_sensor_id in all_sensors_dict.keys():
00143 cur_sensor_type = all_sensors_dict[requested_sensor_id]['sensor_type']
00144 cur_sensors[cur_sensor_type].append(all_sensors_dict[requested_sensor_id])
00145 else:
00146 rospy.logerr("Could not find [%s] in block library. Skipping this block", requested_sensor_id)
00147 return cur_sensors
00148
00149
00150 def diff(v1, v2, eps = 1e-10):
00151 ''' Determine the difference in two vectors. '''
00152 if sum( [ math.fabs(x-y) for x,y in zip(v1, v2) ] ) <= eps:
00153 return 0
00154 return 1
00155
00156
00157 def update_transmission(urdf, joint, gearing):
00158 for transmission in urdf.transmissions.values():
00159 if transmission.joint == joint:
00160 transmission.reduction = transmission.reduction * gearing
00161 return
00162 print "No transmission found for:", joint
00163
00164 def update_urdf(urdf, calibrated_params):
00165 ''' Given urdf and calibrated robot_params, updates the URDF. '''
00166 joints = list()
00167 axis = list()
00168
00169 for chain in calibrated_params.chains.values():
00170 joints += chain._active
00171 axis += numpy.array(chain._axis)[0,:].tolist()
00172 for joint, gearing in zip(chain._active, chain._gearing):
00173 if gearing != 1.0:
00174 update_transmission(urdf, joint, gearing)
00175 for laser in calibrated_params.tilting_lasers.values():
00176 joints.append(laser._config['joint'])
00177 axis.append(5)
00178 if laser._gearing != 1.0:
00179 update_transmission(urdf, laser._config['joint'], laser._gearing)
00180
00181
00182 for joint_name in calibrated_params.transforms.keys():
00183 try:
00184 updated = calibrated_params.transforms[joint_name]._config.T.tolist()[0]
00185 if diff(updated[0:3], urdf.joints[joint_name].origin.position):
00186 print 'Updating xyz for', joint_name, 'old:', urdf.joints[joint_name].origin.position, 'new:', updated[0:3]
00187 urdf.joints[joint_name].origin.position = updated[0:3]
00188 r1 = RPY_to_angle_axis(urdf.joints[joint_name].origin.rotation)
00189 if diff(r1, updated[3:6]):
00190
00191 if joint_name in joints and urdf.joints[joint_name].calibration != None:
00192 cal = urdf.joints[joint_name].calibration
00193 a = axis[joints.index(joint_name)]
00194 a = int(a) - 1
00195 print 'Updating calibration for', joint_name, 'by', updated[a]
00196 if cal.rising != None:
00197 urdf.joints[joint_name].calibration.rising += updated[a]
00198 if cal.falling != None:
00199 urdf.joints[joint_name].calibration.falling += updated[a]
00200 else:
00201 rot = angle_axis_to_RPY(updated[3:6])
00202 print 'Updating rpy for', joint_name, 'old:', urdf.joints[joint_name].origin.rotation, 'new:', rot
00203 urdf.joints[joint_name].origin.rotation = rot
00204 except KeyError:
00205 print "Joint removed:", joint_name
00206
00207 return urdf
00208
00209 if __name__ == '__main__':
00210 import time
00211 xml_time = time.strftime('%Y_%m_%d_%H_%M', time.localtime())
00212 calibrated_xml = 'robot_calibrated_'+xml_time+'.xml'
00213
00214 rospy.init_node("multi_step_cov_estimator", anonymous=True)
00215 print "Starting The Multi Step [Covariance] Estimator Node\n"
00216
00217 if (len(rospy.myargv()) < 2):
00218 usage()
00219 elif (len(rospy.myargv()) < 3):
00220 bag_filename = rospy.myargv()[1]
00221 output_dir = "."
00222 else:
00223 bag_filename = rospy.myargv()[1]
00224 output_dir = rospy.myargv()[2]
00225
00226 print "Using Bagfile: %s\n" % bag_filename
00227 if not os.path.isfile(bag_filename):
00228 rospy.logerr("Bagfile does not exist. Exiting...")
00229 sys.exit(1)
00230
00231 config_param_name = "calibration_config"
00232 if not rospy.has_param(config_param_name):
00233 rospy.logerr("Could not find parameter [%s]. Please populate this namespace with the estimation configuration.", config_param_name)
00234 sys.exit(1)
00235 config = rospy.get_param(config_param_name)
00236
00237
00238 sensors_name = "sensors"
00239 if sensors_name not in config.keys():
00240 rospy.logerr("Could not find namespace [%s/%s]. Please populate this namespace with sensors.", (config_param_name, sensors_name))
00241 sys.exit(1)
00242
00243 all_sensors_dict = build_sensor_defs(config[sensors_name])
00244 all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
00245
00246
00247 step_list = load_calibration_steps(config["cal_steps"])
00248
00249
00250 sample_skip_list = rospy.get_param('calibration_skip_list', [])
00251 msg_count = get_robot_measurement_count(bag_filename, sample_skip_list)
00252
00253 if 'initial_poses' in config.keys():
00254 previous_pose_guesses = numpy.array(yaml.load(config['initial_poses']))
00255 else:
00256 previous_pose_guesses = numpy.zeros([msg_count,6])
00257
00258
00259 output_filenames = [calibrated_xml]
00260 for suffix in [".yaml", "_poses.yaml", "_cov.txt"]:
00261 output_filenames += [output_dir + "/" + cur_step["output_filename"] + suffix for cur_step in step_list]
00262
00263 valid_list = [check_file_permissions(curfile) for curfile in output_filenames];
00264 permissions_valid = all(valid_list)
00265 if not permissions_valid:
00266 print "Invalid file permissions. You need to be able to write to the following files:"
00267 print "\n".join([" - " + cur_file for cur_file,cur_valid in zip(output_filenames, valid_list) if not cur_valid])
00268 sys.exit(-1)
00269
00270
00271 robot_description = get_robot_description(bag_filename)
00272 robot_params = UrdfParams(robot_description, config)
00273
00274
00275 for cur_step in step_list:
00276 print ""
00277 print "*"*30
00278 print "Beginning [%s]" % cur_step["name"]
00279
00280
00281 cur_sensors = load_requested_sensors(all_sensors_dict, cur_step['sensors'])
00282
00283
00284 multisensors = get_multisensors(bag_filename, cur_sensors, sample_skip_list)
00285
00286
00287 print "Executing step with the following Sensors:"
00288
00289 for cur_sensor_type, cur_sensor_list in cur_sensors.items():
00290 print " %s Sensors:" % cur_sensor_type
00291 cur_sensor_ids = [cur_sensor['sensor_id'] for cur_sensor in cur_sensor_list]
00292 cur_sensor_ids.sort()
00293 for cur_sensor_id in cur_sensor_ids:
00294 counts = [ len([s for s in ms.sensors if s.sensor_id == cur_sensor_id]) for ms in multisensors]
00295 count = sum(counts)
00296 print " - %s (%u)" % (cur_sensor_id, count)
00297 print ""
00298
00299 print "Sensor breakdown (By Sample):"
00300 for k,ms in zip(range(len(multisensors)), multisensors):
00301 print " % 2u) %s" % (k, ", ".join([s.sensor_id for s in ms.sensors]))
00302
00303 print "Pose Guesses:\n", previous_pose_guesses
00304
00305 if len(multisensors) == 0:
00306 rospy.logwarn("No error blocks were generated for this optimization step. Skipping this step. This will result in a miscalibrated sensor")
00307 output_dict = robot_params.params_to_config(robot_params.deflate())
00308 output_poses = previous_pose_guesses
00309 else:
00310 free_dict = yaml.load(cur_step["free_params"])
00311 use_cov = cur_step['use_cov']
00312 if use_cov:
00313 print "Executing step with covariance calculations"
00314 else:
00315 print "Executing step without covariance calculations"
00316 output_dict, output_poses, J = opt_runner(robot_params, previous_pose_guesses, free_dict, multisensors, use_cov)
00317
00318
00319 out_f = open(output_dir + "/" + cur_step["output_filename"] + ".yaml", 'w')
00320 yaml.dump(output_dict, out_f)
00321 out_f.close()
00322
00323 out_f = open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml", 'w')
00324 yaml.dump([list([float(x) for x in pose]) for pose in list(output_poses)], out_f)
00325 out_f.close()
00326
00327 cov_x = matrix(J).T * matrix(J)
00328 numpy.savetxt(output_dir + "/" + cur_step["output_filename"] + "_cov.txt", cov_x, fmt="% 9.3f")
00329
00330 previous_pose_guesses = output_poses
00331
00332
00333 rospy.loginfo('Writing updates to %s', calibrated_xml)
00334 outfile = open(calibrated_xml, 'w')
00335 urdf = update_urdf(robot_params.get_clean_urdf(), robot_params)
00336 outfile.write( urdf.to_xml() )
00337 outfile.close()
00338