multi_step_cov_estimator.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 # author: Vijay Pradeep
35 
36 import roslib; roslib.load_manifest('calibration_estimation')
37 
38 import sys
39 import rospy
40 import time
41 import numpy
42 import rosbag
43 import yaml
44 import os.path
45 import numpy
46 import math
47 
48 import stat
49 import os
50 
51 from numpy import matrix
52 
54 from calibration_estimation.urdf_params import UrdfParams
55 from calibration_estimation.sensors.multi_sensor import MultiSensor
56 from calibration_estimation.opt_runner import opt_runner
57 
58 from calibration_estimation.single_transform import angle_axis_to_RPY, RPY_to_angle_axis
59 
60 # Re-enable Ctrl+C cancelling
61 import signal
62 def signal_handler(signal, frame):
63  print '\n\nYou pressed Ctrl+C!\n'
64  sys.exit(-1)
65 
66 signal.signal(signal.SIGINT, signal_handler)
67 
68 
69 def usage():
70  rospy.logerr("Not enough arguments")
71  print "Usage:"
72  print " ./proto1.py [bagfile] [output_dir]"
73  sys.exit(0)
74 
75 
76 def build_sensor_defs(sensors):
77  '''
78  Given a list of sensor definition dictionaries, merge them into a single dictionary
79  '''
80  all_sensors_dict = dict()
81 
82  #for cur_sensors_file in sensors_dump:
83  for cur_sensor_type, cur_sensor_list in sensors.items():
84  if cur_sensor_type in ['checkerboards']:
85  continue
86  for cur_sensor_name, cur_sensor in cur_sensor_list.items():
87  cur_sensor['sensor_id'] = cur_sensor_name # legacy support...
88  # We want sensor_ids to be unique. Thus, we should warn the user if there are duplicate block IDs being loaded
89  if cur_sensor['sensor_id'] in all_sensors_dict.keys(): # TODO: can we get rid of this?
90  rospy.logwarn("Loading a duplicate [%s]. Overwriting previous" % cur_sensor['sensor_id'])
91  all_sensors_dict[cur_sensor['sensor_id']] = cur_sensor
92  all_sensors_dict[cur_sensor['sensor_id']]['sensor_type'] = cur_sensor_type
93 
94  print "The following error sensors have been loaded:\n"
95  # We want to sort by the types of blocks
96  all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
97 
98  for cur_sensor_type in all_sensor_types:
99  print " %s sensors" % cur_sensor_type
100  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]
101  cur_sensor_ids.sort()
102  for cur_sensor_id in cur_sensor_ids:
103  print " - %s" % cur_sensor_id
104  print ""
105 
106  return all_sensors_dict
107 
108 def load_calibration_steps(steps_dict):
109  # We want to execute the calibration in alphabetical order, based on the key names
110  step_keys = steps_dict.keys()
111  step_keys.sort()
112  step_list = []
113  for step_name in step_keys:
114  # Add the step name to the dict (since we'll lose this info once we listify)
115  steps_dict[step_name]["name"] = step_name
116  step_list.append(steps_dict[step_name])
117  print "Loaded the calibration steps to execute in the following order:"
118  for cur_step in step_list:
119  print " - %s" % cur_step['name']
120  return step_list
121 
122 # Verifies that the given filename has the correct permissions.
123 # Either it shouldn't exist, or it should be writable
125  # If the file doesn't exist, then we're fine
126  if not os.path.isfile(filename):
127  return True
128 
129  statinfo = os.stat(filename);
130 
131  # See if the current user owns the file. If so, look at user's write priveleges
132  if (os.geteuid() == statinfo.st_uid):
133  return (os.stat(filename).st_mode & stat.S_IWUSR) > 0
134 
135  # See if the current user's group owns the file. If so, look at groups's write priveleges
136  if (os.getgid() == statinfo.st_gid):
137  return (os.stat(filename).st_mode & stat.S_IWGRP) > 0
138 
139  # Not the owner, nor part of the group, so check the 'other' permissions
140  return (os.stat(filename).st_mode & stat.S_IWOTH) > 0
141 
142 
143 def load_requested_sensors(all_sensors_dict, requested_sensors):
144  '''
145  Build a sensor dictionary with the subset of sensors that we request
146  '''
147  all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
148  cur_sensors = dict([(x,[]) for x in all_sensor_types])
149  for requested_sensor_id in requested_sensors:
150  # We need to now find requested_sensor_id in our library of sensors
151  if requested_sensor_id in all_sensors_dict.keys():
152  cur_sensor_type = all_sensors_dict[requested_sensor_id]['sensor_type']
153  cur_sensors[cur_sensor_type].append(all_sensors_dict[requested_sensor_id])
154  else:
155  rospy.logerr("Could not find [%s] in block library. Skipping this block", requested_sensor_id)
156  return cur_sensors
157 
158 
159 def diff(v1, v2, eps = 1e-10):
160  ''' Determine the difference in two vectors. '''
161  if sum( [ math.fabs(x-y) for x,y in zip(v1, v2) ] ) <= eps:
162  return 0
163  return 1
164 
165 # URDF updating -- this should probably go in a different file
166 def update_transmission(urdf, joint, gearing):
167  for transmission in urdf.transmissions:
168  if transmission.joint == joint:
169  transmission.mechanicalReduction = transmission.mechanicalReduction * gearing
170  return
171  print "No transmission found for:", joint
172 
173 def update_urdf(urdf, calibrated_params):
174  ''' Given urdf and calibrated robot_params, updates the URDF. '''
175  joints = list()
176  axis = list()
177  # update each transmission
178  for chain in calibrated_params.chains.values():
179  joints += chain._active
180  axis += numpy.array(chain._axis)[0,:].tolist()
181  for joint, gearing in zip(chain._active, chain._gearing):
182  if gearing != 1.0:
183  update_transmission(urdf, joint, gearing)
184  for laser in calibrated_params.tilting_lasers.values():
185  joints.append(laser._config['joint'])
186  axis.append(5) # TODO: remove this assumption
187  if laser._gearing != 1.0:
188  update_transmission(urdf, laser._config['joint'], laser._gearing)
189 
190  unchanged_joints = [];
191 
192  # update each transform (or joint calibration)
193  for joint_name in calibrated_params.transforms.keys():
194  link_updated = 0
195  try:
196  updated_link_params = calibrated_params.transforms[joint_name]._config.T.tolist()[0]
197  if diff(updated_link_params[0:3], urdf.joint_map[joint_name].origin.position):
198  print 'Updating xyz for', joint_name, '\n old:', urdf.joint_map[joint_name].origin.position, '\n new:', updated_link_params[0:3]
199  urdf.joint_map[joint_name].origin.position = updated_link_params[0:3]
200  link_updated = 1
201  r1 = RPY_to_angle_axis(urdf.joint_map[joint_name].origin.rotation)
202  if diff(r1, updated_link_params[3:6]):
203  # TODO: remove assumption that joints are revolute
204  if joint_name in joints and urdf.joint_map[joint_name].calibration != None:
205  cal = urdf.joint_map[joint_name].calibration
206  a = axis[joints.index(joint_name)]
207  a = int(a) - 1
208  print 'Updating calibration for', joint_name, 'by', updated_link_params[a]
209  if cal.rising != None:
210  urdf.joint_map[joint_name].calibration.rising += updated_link_params[a]
211  if cal.falling != None:
212  urdf.joint_map[joint_name].calibration.falling += updated_link_params[a]
213  link_updated = 1
214  else:
215  rot = angle_axis_to_RPY(updated_link_params[3:6])
216  print 'Updating rpy for', joint_name, '\n old:', urdf.joint_map[joint_name].origin.rotation, '\n new:', rot
217  urdf.joint_map[joint_name].origin.rotation = rot
218  link_updated = 1
219  except KeyError:
220  print "Joint removed:", joint_name
221  print ' xyz:', updated_link_params[0:3]
222  print ' rpy:', angle_axis_to_RPY(updated_link_params[3:6])
223  link_updated = 1
224  if not link_updated:
225  unchanged_joints.append( joint_name );
226 
227  print "The following joints weren't updated: \n", ', '.join(unchanged_joints)
228  return urdf
229 
230 if __name__ == '__main__':
231  import time
232  xml_time = time.strftime('%Y_%m_%d_%H_%M', time.localtime())
233  calibrated_xml = 'robot_calibrated_'+xml_time+'.xml'
234  uncalibrated_xml = 'robot_uncalibrated_'+xml_time+'.xml'
235 
236  rospy.init_node("multi_step_cov_estimator", anonymous=True)
237  print "Starting The Multi Step [Covariance] Estimator Node\n"
238 
239  if (len(rospy.myargv()) < 2):
240  usage()
241  elif (len(rospy.myargv()) < 3):
242  bag_filename = rospy.myargv()[1]
243  output_dir = "."
244  else:
245  bag_filename = rospy.myargv()[1]
246  output_dir = rospy.myargv()[2]
247 
248  print "Using Bagfile: %s\n" % bag_filename
249  if not os.path.isfile(bag_filename):
250  rospy.logerr("Bagfile does not exist. Exiting...")
251  sys.exit(1)
252 
253  config_param_name = "calibration_config"
254  if not rospy.has_param(config_param_name):
255  rospy.logerr("Could not find parameter [%s]. Please populate this namespace with the estimation configuration.", config_param_name)
256  sys.exit(1)
257  config = rospy.get_param(config_param_name)
258 
259  # Process all the sensor definitions that are on the parameter server
260  sensors_name = "sensors"
261  if sensors_name not in config.keys():
262  rospy.logerr("Could not find namespace [%s/%s]. Please populate this namespace with sensors.", (config_param_name, sensors_name))
263  sys.exit(1)
264  #sensors_dump = [yaml.load(x) for x in config[sensors_name].values()]
265  all_sensors_dict = build_sensor_defs(config[sensors_name])
266  all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
267 
268  # Load all the calibration steps.
269  step_list = load_calibration_steps(config["cal_steps"])
270 
271  # Count how many checkerboard poses we need to track
272  sample_skip_list = rospy.get_param('calibration_skip_list', [])
273  msg_count = get_robot_measurement_count(bag_filename, sample_skip_list)
274 
275  if 'initial_poses' in config.keys():
276  if os.path.exists(config['initial_poses']):
277  with open(config['initial_poses']) as f:
278  previous_pose_guesses = numpy.array(yaml.load(f))
279  else:
280  rospy.logwarn("cannot find %s" % (config['initial_poses']))
281  previous_pose_guesses = numpy.zeros([msg_count,6])
282  else:
283  previous_pose_guesses = numpy.zeros([msg_count,6])
284 
285  # TODO: add alternate methods of defining default poses guesses
286  # See https://github.com/ros-perception/calibration/pull/9
287  if 'default_floating_initial_pose' in config.keys():
288  default_pose = config['default_floating_initial_pose']
289  if len(default_pose) != 6:
290  print "The 'default_floating_initial_pose' parameter has", len(default_pose), "elements, but it should have 6!"
291  sys.exit(-1)
292  for p in range(msg_count):
293  previous_pose_guesses[p,] = config['default_floating_initial_pose']
294 
295  # Check if we can write to all of our output files
296  output_filenames = [calibrated_xml]
297  for suffix in [".yaml", "_poses.yaml", "_cov.txt"]:
298  output_filenames += [output_dir + "/" + cur_step["output_filename"] + suffix for cur_step in step_list]
299 
300  valid_list = [check_file_permissions(curfile) for curfile in output_filenames];
301  permissions_valid = all(valid_list)
302  if not permissions_valid:
303  print "Invalid file permissions. You need to be able to write to the following files:"
304  print "\n".join([" - " + cur_file for cur_file,cur_valid in zip(output_filenames, valid_list) if not cur_valid])
305  sys.exit(-1)
306 
307  # Generate robot parameters
308  robot_description = get_robot_description(bag_filename)
309  robot_params = UrdfParams(robot_description, config)
310  if robot_params == "":
311  print bag_filename, " does not have robot_description, exitting.."
312  sys.exit(-1)
313 
314 
315  # Load all the sensors from the bagfile and config file
316  for cur_step in step_list:
317  print ""
318  print "*"*30
319  print "Beginning [%s]" % cur_step["name"]
320 
321  # Need to load only the sensors that we're interested in
322  cur_sensors = load_requested_sensors(all_sensors_dict, cur_step['sensors'])
323 
324  # Load all the sensors from bag
325  multisensors = get_multisensors(bag_filename, cur_sensors, sample_skip_list)
326 
327  # Display sensor count statistics
328  print "Executing step with the following Sensors:"
329  # Iterate over sensor definitions for this step
330  for cur_sensor_type, cur_sensor_list in cur_sensors.items():
331  print " %s Sensors:" % cur_sensor_type
332  cur_sensor_ids = [cur_sensor['sensor_id'] for cur_sensor in cur_sensor_list]
333  cur_sensor_ids.sort()
334  for cur_sensor_id in cur_sensor_ids:
335  counts = [ len([s for s in ms.sensors if s.sensor_id == cur_sensor_id]) for ms in multisensors]
336  count = sum(counts)
337  print " - %s (%u)" % (cur_sensor_id, count)
338  print ""
339 
340  print "Sensor breakdown (By Sample):"
341  for k,ms in zip(range(len(multisensors)), multisensors):
342  print " % 2u) %s" % (k, ", ".join([s.sensor_id for s in ms.sensors]))
343 
344  print "Pose Guesses:\n", previous_pose_guesses
345 
346  if len(multisensors) == 0:
347  rospy.logwarn("No error blocks were generated for this optimization step. Skipping this step. This will result in a miscalibrated sensor")
348  output_dict = robot_params.params_to_config(robot_params.deflate())
349  output_poses = previous_pose_guesses
350  else:
351  free_dict = yaml.load(cur_step["free_params"])
352  use_cov = cur_step['use_cov']
353  if use_cov:
354  print "Executing step with covariance calculations"
355  else:
356  print "Executing step without covariance calculations"
357  output_dict, output_poses, J = opt_runner(robot_params, previous_pose_guesses, free_dict, multisensors, use_cov)
358 
359  # Dump results to file
360  out_f = open(output_dir + "/" + cur_step["output_filename"] + ".yaml", 'w')
361  yaml.dump(output_dict, out_f)
362  out_f.close()
363 
364  out_f = open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml", 'w')
365  yaml.dump([list([float(x) for x in pose]) for pose in list(output_poses)], out_f)
366  out_f.close()
367 
368  cov_x = matrix(J).T * matrix(J)
369  numpy.savetxt(output_dir + "/" + cur_step["output_filename"] + "_cov.txt", cov_x, fmt="% 9.3f")
370 
371  previous_pose_guesses = output_poses
372 
373  # write original urdf so you can do a diff later
374  rospy.loginfo('Writing original urdf to %s', uncalibrated_xml)
375  outfile = open(uncalibrated_xml, 'w')
376  outfile.write( robot_params.get_clean_urdf().to_xml_string() )
377  outfile.close()
378 
379  #update urdf
380  urdf = update_urdf(robot_params.get_clean_urdf(), robot_params)
381 
382  # write out to URDF
383  outfile = open(calibrated_xml, 'w')
384  rospy.loginfo('Writing updates to %s', calibrated_xml)
385  outfile.write( urdf.to_xml_string() )
386  outfile.close()
387 
388  outfile = open('latest_calibrated_xml', 'w')
389  outfile.write( calibrated_xml )
390  outfile.close()
def load_requested_sensors(all_sensors_dict, requested_sensors)
def get_robot_measurement_count(bag_filename, sample_skip_list=[])
def opt_runner(robot_params, pose_guess_arr, free_dict, multisensors, use_cov)
Definition: opt_runner.py:377
def get_robot_description(bag_filename, use_topic=False)
def get_multisensors(bag_filename, cur_sensors, sample_skip_list=[])


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Thu Jun 6 2019 19:17:16