multi_step_cov_estimator.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 # author: Vijay Pradeep
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 # Re-enable Ctrl+C cancelling
00061 import signal
00062 def signal_handler(signal, frame):
00063     print '\n\nYou pressed Ctrl+C!\n'
00064     sys.exit(-1)
00065         
00066 signal.signal(signal.SIGINT, signal_handler)
00067 
00068 
00069 def usage():
00070     rospy.logerr("Not enough arguments")
00071     print "Usage:"
00072     print " ./proto1.py [bagfile] [output_dir]"
00073     sys.exit(0)
00074 
00075 
00076 def build_sensor_defs(sensors):
00077     '''
00078     Given a list of sensor definition dictionaries, merge them into a single dictionary
00079     '''
00080     all_sensors_dict = dict()
00081 
00082     #for cur_sensors_file in sensors_dump:
00083     for cur_sensor_type, cur_sensor_list in sensors.items():
00084         if cur_sensor_type in ['checkerboards']:
00085             continue
00086         for cur_sensor_name, cur_sensor in cur_sensor_list.items():
00087             cur_sensor['sensor_id'] = cur_sensor_name  # legacy support...
00088             # We want sensor_ids to be unique. Thus, we should warn the user if there are duplicate block IDs being loaded
00089             if cur_sensor['sensor_id'] in all_sensors_dict.keys():  # TODO: can we get rid of this?
00090                 rospy.logwarn("Loading a duplicate [%s]. Overwriting previous" % cur_sensor['sensor_id'])
00091             all_sensors_dict[cur_sensor['sensor_id']] = cur_sensor
00092             all_sensors_dict[cur_sensor['sensor_id']]['sensor_type'] = cur_sensor_type
00093 
00094     print "The following error sensors have been loaded:\n"
00095     # We want to sort by the types of blocks
00096     all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
00097 
00098     for cur_sensor_type in all_sensor_types:
00099         print "  %s sensors" % cur_sensor_type
00100         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]
00101         cur_sensor_ids.sort()
00102         for cur_sensor_id in cur_sensor_ids:
00103             print "   - %s" % cur_sensor_id
00104         print ""
00105 
00106     return all_sensors_dict
00107 
00108 def load_calibration_steps(steps_dict):
00109     # We want to execute the calibration in alphabetical order, based on the key names
00110     step_keys = steps_dict.keys()
00111     step_keys.sort()
00112     step_list = []
00113     for step_name in step_keys:
00114         # Add the step name to the dict (since we'll lose this info once we listify)
00115         steps_dict[step_name]["name"] = step_name
00116         step_list.append(steps_dict[step_name])
00117     print "Loaded the calibration steps to execute in the following order:"
00118     for cur_step in step_list:
00119         print " - %s" % cur_step['name']
00120     return step_list
00121 
00122 # Verifies that the given filename has the correct permissions.
00123 # Either it shouldn't exist, or it should be writable
00124 def check_file_permissions(filename):
00125     # If the file doesn't exist, then we're fine
00126     if not os.path.isfile(filename):
00127         return True
00128 
00129     statinfo = os.stat(filename);
00130 
00131     # See if the current user owns the file. If so, look at user's write priveleges
00132     if (os.geteuid() == statinfo.st_uid):
00133         return (os.stat(filename).st_mode & stat.S_IWUSR) > 0
00134 
00135     # See if the current user's group owns the file. If so, look at groups's write priveleges
00136     if (os.getgid() == statinfo.st_gid):
00137         return (os.stat(filename).st_mode & stat.S_IWGRP) > 0
00138 
00139     # Not the owner, nor part of the group, so check the 'other' permissions
00140     return (os.stat(filename).st_mode & stat.S_IWOTH) > 0
00141 
00142 
00143 def load_requested_sensors(all_sensors_dict, requested_sensors):
00144     '''
00145     Build a sensor dictionary with the subset of sensors that we request
00146     '''
00147     all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
00148     cur_sensors = dict([(x,[]) for x in all_sensor_types])
00149     for requested_sensor_id in requested_sensors:
00150         # We need to now find requested_sensor_id in our library of sensors
00151         if requested_sensor_id in all_sensors_dict.keys():
00152             cur_sensor_type = all_sensors_dict[requested_sensor_id]['sensor_type']
00153             cur_sensors[cur_sensor_type].append(all_sensors_dict[requested_sensor_id])
00154         else:
00155             rospy.logerr("Could not find [%s] in block library. Skipping this block", requested_sensor_id)
00156     return cur_sensors
00157 
00158 
00159 def diff(v1, v2, eps = 1e-10):
00160     ''' Determine the difference in two vectors. '''
00161     if sum( [ math.fabs(x-y) for x,y in zip(v1, v2) ] ) <= eps:
00162         return 0
00163     return 1
00164 
00165 # URDF updating -- this should probably go in a different file
00166 def update_transmission(urdf, joint, gearing):
00167     for transmission in urdf.transmissions.values():
00168         if transmission.joint == joint:
00169             transmission.reduction = transmission.reduction * gearing
00170             return
00171     print "No transmission found for:", joint
00172 
00173 def update_urdf(urdf, calibrated_params):
00174     ''' Given urdf and calibrated robot_params, updates the URDF. '''
00175     joints = list()
00176     axis = list()
00177     # update each transmission
00178     for chain in calibrated_params.chains.values():
00179         joints += chain._active
00180         axis += numpy.array(chain._axis)[0,:].tolist()
00181         for joint, gearing in zip(chain._active, chain._gearing):
00182             if gearing != 1.0:
00183                 update_transmission(urdf, joint, gearing)
00184     for laser in calibrated_params.tilting_lasers.values():
00185         joints.append(laser._config['joint'])
00186         axis.append(5) # TODO: remove this assumption
00187         if laser._gearing != 1.0:
00188             update_transmission(urdf, laser._config['joint'], laser._gearing)
00189 
00190     unchanged_joints = [];
00191 
00192     # update each transform (or joint calibration)
00193     for joint_name in calibrated_params.transforms.keys():
00194         link_updated = 0
00195         try:
00196             updated_link_params = calibrated_params.transforms[joint_name]._config.T.tolist()[0]
00197             if diff(updated_link_params[0:3],  urdf.joints[joint_name].origin.position):
00198                 print 'Updating xyz for', joint_name, '\n old:', urdf.joints[joint_name].origin.position, '\n new:', updated_link_params[0:3]
00199                 urdf.joints[joint_name].origin.position = updated_link_params[0:3]
00200                 link_updated = 1
00201             r1 = RPY_to_angle_axis(urdf.joints[joint_name].origin.rotation)
00202             if diff(r1, updated_link_params[3:6]):
00203                 # TODO: remove assumption that joints are revolute
00204                 if joint_name in joints and urdf.joints[joint_name].calibration != None:
00205                     cal = urdf.joints[joint_name].calibration 
00206                     a = axis[joints.index(joint_name)]
00207                     a = int(a) - 1
00208                     print 'Updating calibration for', joint_name, 'by', updated_link_params[a]
00209                     if cal.rising != None:
00210                         urdf.joints[joint_name].calibration.rising += updated_link_params[a]   
00211                     if cal.falling != None:
00212                         urdf.joints[joint_name].calibration.falling += updated_link_params[a]
00213                     link_updated = 1
00214                 else:
00215                     rot = angle_axis_to_RPY(updated_link_params[3:6])
00216                     print 'Updating rpy for', joint_name, '\n old:', urdf.joints[joint_name].origin.rotation, '\n new:', rot
00217                     urdf.joints[joint_name].origin.rotation = rot
00218                     link_updated = 1                
00219         except KeyError:
00220             print "Joint removed:", joint_name
00221             print ' xyz:', updated_link_params[0:3]
00222             print ' rpy:', angle_axis_to_RPY(updated_link_params[3:6])
00223             link_updated = 1
00224         if not link_updated:
00225             unchanged_joints.append( joint_name );
00226     
00227     print "The following joints weren't updated: \n", ', '.join(unchanged_joints)
00228     return urdf
00229 
00230 if __name__ == '__main__':
00231     import time
00232     xml_time = time.strftime('%Y_%m_%d_%H_%M', time.localtime())
00233     calibrated_xml = 'robot_calibrated_'+xml_time+'.xml'
00234     uncalibrated_xml = 'robot_uncalibrated_'+xml_time+'.xml'
00235 
00236     rospy.init_node("multi_step_cov_estimator", anonymous=True)
00237     print "Starting The Multi Step [Covariance] Estimator Node\n"
00238 
00239     if (len(rospy.myargv()) < 2):
00240         usage()
00241     elif (len(rospy.myargv()) < 3):
00242         bag_filename = rospy.myargv()[1]
00243         output_dir = "."
00244     else:
00245         bag_filename = rospy.myargv()[1]
00246         output_dir = rospy.myargv()[2]
00247 
00248     print "Using Bagfile: %s\n" % bag_filename
00249     if not os.path.isfile(bag_filename):
00250         rospy.logerr("Bagfile does not exist. Exiting...")
00251         sys.exit(1)
00252 
00253     config_param_name = "calibration_config"
00254     if not rospy.has_param(config_param_name):
00255         rospy.logerr("Could not find parameter [%s]. Please populate this namespace with the estimation configuration.", config_param_name)
00256         sys.exit(1)
00257     config = rospy.get_param(config_param_name)
00258 
00259     # Process all the sensor definitions that are on the parameter server
00260     sensors_name = "sensors"
00261     if sensors_name not in config.keys():
00262         rospy.logerr("Could not find namespace [%s/%s]. Please populate this namespace with sensors.", (config_param_name, sensors_name))
00263         sys.exit(1)
00264     #sensors_dump = [yaml.load(x) for x in config[sensors_name].values()]
00265     all_sensors_dict = build_sensor_defs(config[sensors_name])
00266     all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
00267 
00268     # Load all the calibration steps.
00269     step_list = load_calibration_steps(config["cal_steps"])
00270 
00271     # Count how many checkerboard poses we need to track
00272     sample_skip_list = rospy.get_param('calibration_skip_list', [])
00273     msg_count = get_robot_measurement_count(bag_filename, sample_skip_list)
00274 
00275     if 'initial_poses' in config.keys():
00276         previous_pose_guesses = numpy.array(yaml.load(config['initial_poses']))
00277     else:
00278         previous_pose_guesses = numpy.zeros([msg_count,6])
00279         
00280         # TODO: add alternate methods of defining default poses guesses
00281         # See https://github.com/ros-perception/calibration/pull/9
00282         if 'default_floating_initial_pose' in config.keys():
00283             default_pose = config['default_floating_initial_pose']
00284             if len(default_pose) != 6:
00285                 print "The 'default_floating_initial_pose' parameter has", len(default_pose), "elements, but it should have 6!"
00286                 sys.exit(-1)
00287             for p in range(msg_count):
00288                 previous_pose_guesses[p,] = config['default_floating_initial_pose']
00289 
00290     # Check if we can write to all of our output files
00291     output_filenames = [calibrated_xml]
00292     for suffix in [".yaml", "_poses.yaml", "_cov.txt"]:
00293         output_filenames += [output_dir + "/" + cur_step["output_filename"] + suffix for cur_step in step_list]
00294 
00295     valid_list = [check_file_permissions(curfile) for curfile in output_filenames];
00296     permissions_valid = all(valid_list)
00297     if not permissions_valid:
00298         print "Invalid file permissions. You need to be able to write to the following files:"
00299         print "\n".join([" - " + cur_file for cur_file,cur_valid in zip(output_filenames, valid_list) if not cur_valid])
00300         sys.exit(-1)
00301 
00302     # Generate robot parameters
00303     robot_description = get_robot_description(bag_filename)
00304     robot_params = UrdfParams(robot_description, config)
00305 
00306     # Load all the sensors from the bagfile and config file
00307     for cur_step in step_list:
00308         print ""
00309         print "*"*30
00310         print "Beginning [%s]" % cur_step["name"]
00311 
00312         # Need to load only the sensors that we're interested in
00313         cur_sensors = load_requested_sensors(all_sensors_dict, cur_step['sensors'])
00314 
00315         # Load all the sensors from bag
00316         multisensors = get_multisensors(bag_filename, cur_sensors, sample_skip_list)
00317 
00318         # Display sensor count statistics
00319         print "Executing step with the following Sensors:"
00320         # Iterate over sensor definitions for this step
00321         for cur_sensor_type, cur_sensor_list in cur_sensors.items():
00322             print "  %s Sensors:" % cur_sensor_type
00323             cur_sensor_ids = [cur_sensor['sensor_id'] for cur_sensor in cur_sensor_list]
00324             cur_sensor_ids.sort()
00325             for cur_sensor_id in cur_sensor_ids:
00326                 counts = [ len([s for s in ms.sensors if s.sensor_id == cur_sensor_id]) for ms in multisensors]
00327                 count = sum(counts)
00328                 print "   - %s (%u)" % (cur_sensor_id, count)
00329             print ""
00330 
00331         print "Sensor breakdown (By Sample):"
00332         for k,ms in zip(range(len(multisensors)), multisensors):
00333             print " % 2u) %s" % (k, ", ".join([s.sensor_id for s in ms.sensors]))
00334 
00335         print "Pose Guesses:\n", previous_pose_guesses
00336 
00337         if len(multisensors) == 0:
00338             rospy.logwarn("No error blocks were generated for this optimization step. Skipping this step.  This will result in a miscalibrated sensor")
00339             output_dict = robot_params.params_to_config(robot_params.deflate())
00340             output_poses = previous_pose_guesses
00341         else:
00342             free_dict = yaml.load(cur_step["free_params"])
00343             use_cov = cur_step['use_cov']
00344             if use_cov:
00345                 print "Executing step with covariance calculations"
00346             else:
00347                 print "Executing step without covariance calculations"
00348             output_dict, output_poses, J = opt_runner(robot_params, previous_pose_guesses, free_dict, multisensors, use_cov)
00349 
00350         # Dump results to file
00351         out_f = open(output_dir + "/" + cur_step["output_filename"] + ".yaml", 'w')
00352         yaml.dump(output_dict, out_f)
00353         out_f.close()
00354 
00355         out_f = open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml", 'w')
00356         yaml.dump([list([float(x) for x in pose]) for pose in list(output_poses)], out_f)
00357         out_f.close()
00358 
00359         cov_x = matrix(J).T * matrix(J)
00360         numpy.savetxt(output_dir + "/" + cur_step["output_filename"] + "_cov.txt", cov_x, fmt="% 9.3f")
00361 
00362         previous_pose_guesses = output_poses
00363 
00364     # write original urdf so you can do a diff later
00365     rospy.loginfo('Writing original urdf to %s', uncalibrated_xml)
00366     outfile = open(uncalibrated_xml, 'w')
00367     outfile.write( robot_params.get_clean_urdf().to_xml() )
00368     outfile.close()
00369 
00370     #update urdf
00371     urdf = update_urdf(robot_params.get_clean_urdf(), robot_params)
00372 
00373     # write out to URDF
00374     outfile = open(calibrated_xml, 'w')
00375     rospy.loginfo('Writing updates to %s', calibrated_xml)
00376     outfile.write( urdf.to_xml() )
00377     outfile.close()
00378 
00379     outfile = open('latest_calibrated_xml', 'w')
00380     outfile.write( calibrated_xml )
00381     outfile.close()


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Sun Oct 5 2014 22:44:09