calibration_setup_helper.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright (C) 2014 Kei Okada
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16 #
17 
18 import rospy
19 import argparse
20 import os,sys,itertools
21 from urdf_parser_py.urdf import URDF
22 
24  def __init__(self, robot_description, base_link, arm_root_link, arm_tip_link, head_root_link, head_tip_link, arm_controller, head_controller, head_camera_frame, head_camera_joint, camera_namespace):
25  self.robot_description = robot_description
26  self.base_link = base_link
27  self.arm_root_link = arm_root_link
28  self.arm_tip_link = arm_tip_link
29  self.head_root_link = head_root_link
30  self.head_tip_link = head_tip_link
31  self.arm_controller = arm_controller
32  self.head_controller = head_controller
33  self.head_camera_frame = head_camera_frame
34  self.head_camera_joint = head_camera_joint
35  self.camera_namespace = camera_namespace
36 
37  self.robot = URDF().parse(self.robot_description)
38  self.robot_name = self.robot.name.lower()
39 
40  if not self.base_link:
41  self.base_link = self.robot.get_root()
42  if self.robot.link_map.has_key(self.base_link) == False:
43  self.error_tip_link(self.base_link, '--base-link')
44  if self.robot.link_map.has_key(self.arm_root_link) == False:
45  self.error_tip_link(self.arm_root_link, '--arm-root-link')
46  if self.robot.link_map.has_key(self.head_root_link) == False:
47  self.error_tip_link(self.head_root_link, '--head-root-link')
48  if self.robot.link_map.has_key(self.arm_tip_link) == False:
49  self.error_tip_link(self.arm_tip_link, '--arm-tip-link')
50  if self.robot.link_map.has_key(self.head_tip_link) == False:
51  self.error_tip_link(self.head_tip_link, '--head-tip-link')
52  if self.robot.link_map.has_key(self.head_camera_frame) == False:
53  self.error_tip_link(self.head_camera_frame, '--head-camera-frame')
54  if self.robot.joint_map.has_key(self.head_camera_joint) == False:
55  self.error_joint(self.head_camera_joint, '--head-camera-joint')
56 
57  all_chain = []
58  for limb, base_link, end_link in [('arm', self.arm_root_link, self.arm_tip_link), ('head', self.head_root_link, self.head_tip_link)]:
59  all_chain.append(self.robot.get_chain(base_link, end_link)[1:])
60  for c1,c2 in itertools.product(*all_chain):
61  if c1 == c2 :
62  rospy.logerr('arm/head chain share same joint ({}), this will cause failure'.format(c1))
63  sys.exit()
64 
65  for limb, base_link, end_link in [('arm', self.arm_root_link, self.arm_tip_link), ('head', self.head_root_link, self.head_tip_link)]:
66  joint_list = filter(lambda x: self.robot.joint_map.has_key(x) and self.robot.joint_map[x].type != 'fixed', [c for c in self.robot.get_chain(base_link, end_link)])
67  exec('self.{0}_joint_list = {1}'.format(limb, joint_list)) in locals()
68 
69  rospy.loginfo('using following joint for {} chain'.format('arm'))
70  rospy.loginfo('... {}'.format(self.arm_joint_list))
71  rospy.loginfo('using following joint for {} chain'.format('head'))
72  rospy.loginfo('... {}'.format(self.head_joint_list))
73 
74  # create robot_calibration directory
75  self.dirname_top = self.robot_name+'_calibration'
76  self.dirname_capture = self.robot_name+'_calibration/capture_data'
77  self.dirname_estimate = self.robot_name+'_calibration/estimate_params'
78  self.dirname_results = self.robot_name+'_calibration/view_results'
79 
80  try:
81  os.makedirs(self.dirname_top)
82  os.makedirs(self.dirname_capture)
83  os.makedirs(self.dirname_estimate)
84  os.makedirs(self.dirname_results)
85  except Exception as e:
86  rospy.logfatal(e)
87  #sys.exit(-1)
88 
89  # setup capture_data
90  self.write_all_pipelines(args.use_kinect) # all_pipelines.launch
91  self.write_all_viewers(args.use_kinect) # all_viewers.launch
92  self.write_capture_data() # capture_data.launch
93  self.write_capture_exec() # capture_exec.launch
94  self.write_hardware_config() # hardware_config
95  self.write_make_samples() # make_samples.py
96  # setup estimate_params
97  self.write_estimation_config() # estimation_config.launch
98  self.write_calibrate_sh() # calibrate_<robot>.sh
99  self.write_estimate_params_config(args.use_kinect) # free_arms.yaml free_cameras.yaml free_cb_locations.yaml free_torso.yaml system.yaml
100  # setup view_results
101  self.write_view_results() # scatter_config.yaml view_scatter.sh
102 
103 
104  def error_tip_link(self, link, option):
105  rospy.logfatal('could not find valid link name ... {}'.format(link))
106  rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.links)))
107  f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+')
108  print >>f, self.robot_description
109  f.close()
110  rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name))
111  sys.exit(-1)
112 
113  def error_joint(self, joint, option):
114  rospy.logfatal('could not find valid joint name ... {}'.format(joint))
115  rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.joints)))
116  f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+')
117  print >>f, self.robot_description
118  f.close()
119  rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name))
120  sys.exit(-1)
121 
122  def write_all_pipelines(self, use_kinect):
123  file_name = self.dirname_capture+'/all_pipelines.launch'
124  f = open(file_name,'w+')
125  f.write("""
126 <launch>
127  <include file="$(find calibration_launch)/capture_data/chain_settler.launch" ns="arm_chain" />
128  <include file="$(find calibration_launch)/capture_data/chain_settler.launch" ns="head_chain" />
129 
130  <arg name="use_kinect" default="{0}" />
131  <group if="$(arg use_kinect)" >
132  <include file="$(find calibration_launch)/capture_data/kinect_pipeline.launch" ns="{1}">
133  <arg name="image_topic" value="image_rect_color"/> <!-- openni2.launch publishes image_raw and image_rect_color -->
134  <arg name="depth_topic" value="/camera/depth/image"/> <!-- this is floar value -->
135  <arg name="camera_info_topic" value="camera_info"/>
136  </include>
137  </group>
138  <group unless="$(arg use_kinect)" >
139  <include file="$(find calibration_launch)/capture_data/monocam_pipeline.launch" ns="{1}">
140  <arg name="image_topic" value="image_rect_color"/> <!-- openni2.launch publishes image_raw and image_rect_color -->
141  </include>
142  </group>
143 
144  <node type="interval_intersection_action"
145  pkg="interval_intersection"
146  name="interval_intersection"
147  output="screen">
148  <remap from="head_chain" to="head_chain/settled_interval" />
149  <remap from="arm_chain" to="arm_chain/settled_interval" />
150  <remap from="head_camera" to="{1}/settled_interval" />
151  </node>
152 
153 </launch>
154 """.format("true" if use_kinect else "false", self.camera_namespace))
155  f.close()
156 
157  def write_all_viewers(self, use_kinect):
158  file_name = self.dirname_capture+'/all_viewers.launch'
159  f = open(file_name,'w+')
160  f.write("""
161 <launch>
162 
163  <!-- Hack to create the directory -->
164  <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/{0}_calibration" />
165  <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/{0}_calibration/cb_fail" />
166 
167  <param name="{2}/annotated_viewer/filename_format" type="string" value="/tmp/{0}_calibration/cb_fail/cb_{1}_%04i.jpg" />
168 
169  <include file="$(find calibration_launch)/capture_data/annotated_viewer.launch"
170  ns="{2}" >
171  <arg name="image_topic" value="image_rect_annotated" />
172  </include>
173 
174 </launch>
175 """.format(self.robot_name,"kinect" if use_kinect else "monocam", self.camera_namespace))
176  f.close()
177 
179  file_name = self.dirname_capture+'/capture_data.launch'
180  f = open(file_name,'w+')
181  f.write("""
182 <launch>
183  <include file="$(find {0}_calibration)/capture_data/all_viewers.launch"/>
184  <include file="$(find {0}_calibration)/capture_data/all_pipelines.launch"/>
185  <include file="$(find {0}_calibration)/capture_data/capture_exec.launch"/>
186 </launch>
187 """.format(self.robot_name))
188 
190  file_name = self.dirname_capture+'/capture_exec.launch'
191  f = open(file_name,'w+')
192  f.write("""
193 <launch>
194 
195  <node type="capture_exec.py"
196  pkg="calibration_launch"
197  name="calibration_exec"
198  args="$(find {0}_calibration)/capture_data/samples/ $(find {0}_calibration)/capture_data/hardware_config $(find {0}_calibration)/estimate_params/config/system.yaml"
199  output="screen" >
200  <remap from="head_camera/camera_info" to="{1}/camera_info"/>
201  <remap from="head_camera/image_rect" to="{1}/image_rect_throttle"/>
202  <remap from="head_camera/image" to="{1}/image_rect_throttle"/>
203  <remap from="head_camera/features" to="{1}/features"/>
204  </node>
205 
206  <node type="urdf_pub.py" pkg="calibration_launch" name="urdf_pub"/>
207 
208  <node type="record" pkg="rosbag" name="robot_measurement_recorder" output="screen"
209  args="-O /tmp/{0}_calibration/cal_measurements robot_measurement robot_description" >
210  <!-- Hack to create the directory -->
211  <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/{0}_calibration" />
212  </node>
213 
214 </launch>""".format(self.robot_name, self.camera_namespace))
215  f.close()
216 
218  config_dir = self.dirname_capture+'/hardware_config/'
219  try:
220  os.makedirs(config_dir)
221  except Exception as e:
222  rospy.logfatal(e)
223 
224  # cam_config.yaml
225  f = open(config_dir+'/cam_config.yaml', 'w+')
226  f.write("""
227 # ----- Microsoft Kinect -----
228 head_camera: ###FIX
229  cb_detector_config: {0}/cb_detector_config ## FIXME
230  led_detector_config: {0}/led_detector
231  settler_config: {0}/monocam_settler_config ## FIXME
232 
233  configs:
234  small_cb_4x5:
235  settler:
236  tolerance: 2.00
237  ignore_failures: True
238  max_step: 3.0
239  cache_size: 100
240  cb_detector:
241  active: True
242  num_x: 4
243  num_y: 5
244  width_scaling: 0.5
245  height_scaling: 0.5
246  subpixel_window: 4
247  subpixel_zero_zone: 1
248  led_detector:
249  active: False
250 """.format(self.camera_namespace)) # not sure if this is works for mono camera
251  f.close()
252 
253  # chain_config.yaml
254  f = open(config_dir+'/chain_config.yaml', 'w+')
255  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
256  if len(joint_list) == 0:
257  continue
258  tolerance = 0.002
259  f.write('# ----- {} -----\n'.format(limb))
260  f.write('{}_chain:\n'.format(limb))
261  f.write(' settler_config: /{}_chain/settler_config\n\n'.format(limb))
262  f.write(' configs:\n')
263  f.write(' tight_tol:\n')
264  f.write(' settler:\n')
265  f.write(' joint_names:\n')
266  for j in joint_list:
267  f.write(' - {}\n'.format(j))
268  f.write(' tolerances:\n')
269  for j in joint_list:
270  f.write(' - {}\n'.format(tolerance))
271  f.write(' max_step: 1.0\n')
272  f.write(' cache_size: 1500\n\n')
273  f.close()
274 
275  # controller_config.yaml
276  f = open(config_dir+'/controller_config.yaml', 'w+')
277  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
278  if len(joint_list) == 0:
279  continue
280  f.write('{}_controller:\n'.format(limb))
281  exec('controller = self.{}_controller'.format(limb))
282  rospy.loginfo('Subscribing controller topic is \'{}\''.format(controller))
283  f.write(' topic: {0}\n'.format(controller))
284  f.write(' joint_names:\n')
285  for j in joint_list:
286  f.write(' - {}\n'.format(j))
287  f.close()
288 
289  # laser_config.yaml
290  f = open(config_dir+'/laser_config.yaml', 'w+')
291  f.write('# laser config')
292  f.close()
293 
295  samples_dir = self.dirname_capture+'/samples/'
296  try:
297  os.makedirs(samples_dir+'/arm')
298  except Exception as e:
299  rospy.logfatal(e)
300  f = open(samples_dir+'/arm/config.yaml', 'w+')
301  f.write("""
302 group: "Arm"
303 prompt: "Please put the checkerboard in the hand (open/close the gripper with the joystick's square/circle buttons)."
304 finish: "Skipping arm samples"
305 repeat: False
306 """)
307  f.close()
308 
309  f = open(samples_dir+'/../make_samples.py', 'w+')
310  os.chmod(samples_dir+'/../make_samples.py', 0744)
311  f.write("""#!/usr/bin/env python
312 
313 # capture samples!!!!1!one
314 
315 # this script should eventually be replaced by something that finds
316 # samples automatically
317 
318 import rospy
319 from sensor_msgs.msg import JointState
320 
321 import string, os
322 
323 header1 = \"\"\"camera_measurements:
324 - {cam_id: head_camera, config: small_cb_4x5}
325 joint_commands:
326 \"\"\"
327 """)
328  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
329  if len(joint_list) == 0:
330  continue
331  f.write("""
332 header2_{0} = \"\"\"- controller: {0}_controller
333  segments:
334  - duration: 2.0
335  positions: \"\"\"
336 """.format(limb))
337  f.write("""
338 header3 = \"\"\"joint_measurements:
339 """)
340  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
341  if len(joint_list) == 0:
342  continue
343  f.write('- {{chain_id: {0}_chain, config: tight_tol}}\n'.format(limb))
344  f.write("""
345 sample_id: arm_\"\"\"
346 
347 header4 = \"\"\"target: {chain_id: arm_chain, target_id: small_cb_4x5}\"\"\"
348 
349 class SampleMaker:
350 
351  def __init__(self):
352  rospy.init_node("make_samples")
353  rospy.Subscriber("joint_states", JointState, self.callback)
354 """)
355  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
356  if len(joint_list) == 0:
357  continue
358  f.write(' self.{}_joints = ['.format(limb))
359  for j in joint_list:
360  f.write('\"{}\", '.format(j))
361  f.write(']\n')
362  f.write(' self.{0}_state = [0.0 for joint in self.{0}_joints]\n'.format(limb))
363  f.write("""
364  self.count = 0
365 
366  while not rospy.is_shutdown():
367  print "Move arm/head to a new sample position."
368  resp = raw_input("press <enter> ")
369  if string.upper(resp) == "EXIT":
370  break
371  else:
372  # save a sample:
373  count = str(self.count).zfill(4)
374  f = open(os.path.dirname(__file__)+"/samples/arm/arm_"+count+".yaml", "w")
375  f.write(header1)
376 """)
377  f.write(""" print('saving ... {0}'.format(self.count))\n""")
378  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
379  if len(joint_list) == 0:
380  continue
381  f.write(""" print(' {0}_state: {{0}}'.format(self.{0}_state))\n""".format(limb))
382  f.write("""
383  f.write(header2_{0})
384  print>>f, self.{0}_state
385 """.format(limb))
386  f.write("""
387  f.write(header3)
388  print>>f, count
389  f.write(header4)
390  self.count += 1
391 
392  def callback(self, msg):
393 """)
394  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
395  if len(joint_list) == 0:
396  continue
397  f.write("""
398  for i in range(len(self.{0}_joints)):
399  try:
400  idx = msg.name.index(self.{0}_joints[i])
401  self.{0}_state[i] = msg.position[idx]
402  except:
403  pass
404 """.format(limb))
405  f.write("""
406 
407 if __name__=="__main__":
408  SampleMaker()
409 """)
410  f.close()
411 
413  file_name = self.dirname_estimate+'/estimation_config.launch'
414  f = open(file_name,'w+')
415  f.write("""
416 <launch>
417 
418  <group ns="calibration_config" clear_params="true">
419  <rosparam file="$(find {0}_calibration)/estimate_params/config/system.yaml" command="load" />
420 
421  <group ns="cal_steps">
422 
423  <group ns="{0} - 00 - Estimating Checkerboard Locations">
424  <param name="free_params" textfile="$(find {0}_calibration)/estimate_params/config/free_cb_locations.yaml" />
425  <param name="use_cov" type="bool" value="False" />
426  <rosparam>
427  sensors:
428  - arm_chain
429  - head_camera
430  </rosparam>
431  <param name="output_filename" type="string" value="config_0" />
432  </group>
433 
434  <group ns="{0} - 01 - Adding Camera Locations">
435  <param name="free_params" textfile="$(find {0}_calibration)/estimate_params/config/free_cameras.yaml" />
436  <param name="use_cov" type="bool" value="True" />
437  <rosparam>
438  sensors:
439  - arm_chain
440  - head_camera
441  </rosparam>
442  <param name="output_filename" type="string" value="system_calibrated" />
443  </group>
444 
445  </group>
446 
447  </group>
448 
449 </launch>
450 """.format(self.robot_name))
451 
453  file_name = self.dirname_estimate+'/calibrate_'+self.robot_name+'.sh'
454  f = open(file_name,'w+')
455  os.chmod(file_name, 0744)
456  f.write("""#! /bin/bash
457 
458 if [ -f robot_calibrated.xml ]; then
459  echo "./robot_calibrated.xml already exists. Either back up this file or remove it before continuing"
460  exit 1
461 fi
462 
463 echo "Checking if we can write to ./robot_calibrated.xml..."
464 touch robot_calibrated.xml
465 if [ "$?" -ne "0" ]; then
466  echo "Not able to write to ./robot_calibrated.xml"
467  echo "Make sure you run this script from a directory that for which you have write permissions."
468  exit 1
469 fi
470 rm robot_calibrated.xml
471 echo "Success"
472 
473 roslaunch {0}_calibration estimation_config.launch
474 rosrun calibration_estimation multi_step_cov_estimator.py /tmp/{0}_calibration/cal_measurements.bag /tmp/{0}_calibration __name:=cal_cov_estimator
475 
476 est_return_val=$?
477 
478 if [ "$est_return_val" -ne "0" ]; then
479  echo "Estimator exited prematurely with error code [$est_return_val]"
480  exit 1
481 fi
482 
483 # Make all the temporary files writable
484 chmod ag+w /tmp/{0}_calibration/*
485 
486 """.format(self.robot_name))
487 
488  def write_estimate_params_config(self, use_kinect=False):
489  config_dir = self.dirname_estimate+'/config'
490  try:
491  os.makedirs(config_dir)
492  except Exception as e:
493  rospy.logfatal(e)
494 
495  # free_arms.yaml
496  f = open(config_dir+'/free_arms.yaml', 'w+')
497  f.write('\n')
498  f.write(' transforms:\n')
499  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
500  if len(joint_list) == 0:
501  continue
502  for j in joint_list:
503  f.write(' {0}: [ 0, 0, 0, {1}, {2}, {3} ]\n'.format(self.robot.joint_map[j].name, self.robot.joint_map[j].axis[0], self.robot.joint_map[j].axis[1], self.robot.joint_map[j].axis[2]))
504  f.write('\n')
505  f.write(' chains:\n')
506  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
507  if len(joint_list) == 0:
508  continue
509  f.write(' {}_chain:\n'.format(limb))
510  f.write(' gearing: {}\n'.format([0]*len(joint_list)))
511  f.write("""
512  rectified_cams:
513  head_camera:
514  baseline_shift: 0
515  f_shift: 0
516  cx_shift: 0
517  cy_shift: 0
518 
519  tilting_lasers: {}
520 
521  checkerboards:
522  small_cb_4x5:
523  spacing_x: 0
524  spacing_y: 0
525 
526 """)
527  f.close()
528 
529  # free_cameras.yaml
530  f = open(config_dir+'/free_cameras.yaml', 'w+')
531  f.write("""
532  transforms:
533  arm_chain_cb: [1, 1, 1, 1, 1, 1]
534  {0}: [1, 1, 1, 1, 1, 1]
535 
536  chains:
537 """.format(self.head_camera_joint))
538  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
539  if len(joint_list) == 0:
540  continue
541  f.write(' {}_chain:\n'.format(limb))
542  f.write(' gearing: {}\n'.format([0]*len(joint_list)))
543  f.write("""
544  rectified_cams:
545  head_camera:
546  baseline_shift: 0
547  f_shift: 0
548  cx_shift: 0
549  cy_shift: 0
550 
551  tilting_lasers: {}
552 
553  checkerboards:
554  small_cb_4x5:
555  spacing_x: 0
556  spacing_y: 0
557 
558 """)
559  f.close()
560 
561  # free_cb_locations.yaml
562  f = open(config_dir+'/free_cb_locations.yaml', 'w+')
563  f.write("""
564  transforms:
565  arm_chain_cb: [1, 1, 1, 1, 1, 1]
566 
567  chains:
568 """)
569  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
570  if len(joint_list) == 0:
571  continue
572  f.write(' {}_chain:\n'.format(limb))
573  f.write(' gearing: {}\n'.format([0]*len(joint_list)))
574  f.write("""
575 
576  rectified_cams:
577  head_camera:
578  baseline_shift: 0
579  f_shift: 0
580  cx_shift: 0
581  cy_shift: 0
582 
583  tilting_lasers: {}
584 
585  checkerboards:
586  small_cb_4x5:
587  spacing_x: 0
588  spacing_y: 0
589 
590 """)
591 
592  # free_torso.yaml
593  f = open(config_dir+'/free_torso.yaml', 'w+')
594  f.write("""
595  transforms:
596  head_joint: [0, 0, 1, 0, 0, 0]
597 
598  chains:
599 """)
600  for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
601  if len(joint_list) == 0:
602  continue
603  f.write(' {}_chain:\n'.format(limb))
604  f.write(' gearing: {}\n'.format([0]*len(joint_list)))
605  f.write("""
606 
607  rectified_cams:
608  head_camera:
609  baseline_shift: 0
610  f_shift: 0
611  cx_shift: 0
612  cy_shift: 0
613 
614  tilting_lasers: {}
615 
616  checkerboards:
617  small_cb_4x5:
618  spacing_x: 0
619  spacing_y: 0
620 
621 """)
622  # system.yaml
623  f = open(config_dir+'/system.yaml', 'w+')
624  f.write('\n')
625  f.write('base_link: {0}\n'.format(self.base_link))
626  f.write('\n')
627  f.write('sensors:\n')
628  f.write('\n')
629  f.write(' chains:\n')
630  if len(self.arm_joint_list) > 0:
631  f.write(' arm_chain:\n')
632  f.write(' root: {}\n'.format(self.arm_root_link))
633  f.write(' tip: {}\n'.format(self.arm_tip_link))
634  f.write(' cov:\n')
635  f.write(' joint_angles: {}\n'.format([0.01]*len(self.arm_joint_list)))
636  f.write(' gearing: {}\n'.format([1.0]*len(self.arm_joint_list)))
637  if len(self.head_joint_list) > 0:
638  f.write(' head_chain:\n')
639  f.write(' root: {}\n'.format(self.head_root_link))
640  f.write(' tip: {}\n'.format(self.head_tip_link))
641  f.write(' cov:\n')
642  f.write(' joint_angles: {}\n'.format([0.01]*len(self.head_joint_list)))
643  f.write(' gearing: {}\n'.format([1.0]*len(self.head_joint_list)))
644  f.write('\n')
645  f.write(' rectified_cams:\n')
646  f.write(' head_camera:\n')
647  f.write(' chain_id: head_chain #TODO: get rid of this\n')
648  f.write(' frame_id: {}\n'.format(self.head_camera_frame))
649  f.write(' baseline_shift: 0.0\n')
650  if use_kinect:
651  f.write(' baseline_rgbd: 0.075\n')
652  else:
653  f.write('# baseline_rgbd: 0.075 ## comment out if we run all_pipelines.launch with use_kinect\n')
654  f.write(' f_shift: 0.0\n')
655  f.write(' cx_shift: 0.0\n')
656  f.write(' cy_shift: 0.0\n')
657  f.write(' cov: {u: 0.25, v: 0.25, x: 0.25}\n')
658  f.write('\n')
659  f.write(' tilting_lasers: {}\n')
660  f.write('\n')
661  f.write('checkerboards:\n')
662  f.write(' small_cb_4x5:\n')
663  f.write(' corners_x: 4\n')
664  f.write(' corners_y: 5\n')
665  f.write(' spacing_x: 0.0245\n')
666  f.write(' spacing_y: 0.0245\n')
667  f.write('\n')
668  f.write('transforms:\n')
669  f.write(' arm_chain_cb: [ .25, 0, 0, pi/2, 0, 0]\n')
670  f.write('\n')
671 
673  file_name = self.dirname_results+'/scatter_config.yaml'
674  f = open(file_name, 'w+')
675  f.write('- name: "Head Camera: Arm"\n')
676  f.write(' 3d: arm_chain\n')
677  f.write(' cam: head_camera\n')
678  f.write(' plot_ops:\n')
679  f.write(' color: g\n')
680  f.write(' marker: s\n')
681  f.close()
682 
683  file_name = self.dirname_results+'/view_scatter.sh'
684  f = open(file_name,'w+')
685  os.chmod(file_name, 0744)
686  f.write("""#!/bin/bash
687 rosrun calibration_estimation error_visualization.py /tmp/{0}_calibration/cal_measurements.bag /tmp/{0}_calibration/ `rospack find {0}_calibration`/view_results/scatter_config.yaml\n""".format(self.robot_name))
688  f.close()
689 
690 
691 if __name__ == '__main__':
692  parser = argparse.ArgumentParser(description='Calibration setup helper.')
693  parser.add_argument('robot_description', type=file, default=False, nargs='?', help='path to urdf file')
694  parser.add_argument('--use-kinect', action='store_const', const=True, default=False, help='use kinect for calibraiton')
695  parser.add_argument('--base-link', help='base link name')
696  parser.add_argument('--arm-root-link', help='arm root link name')
697  parser.add_argument('--arm-tip-link', help='arm tip link name')
698  parser.add_argument('--head-root-link', help='head root link name')
699  parser.add_argument('--head-tip-link', help='head tip link name')
700  parser.add_argument('--arm-controller', help='arm controller name', default='arm_controller/command')
701  parser.add_argument('--head-controller', help='head controller name', default='head_controller/command')
702  parser.add_argument('--head-camera-frame', help='frame name for target camera', default='camera_rgb_optical_frame')
703  parser.add_argument('--head-camera-joint', help='joint between head and camera', default='camera_rgb_joint')
704  parser.add_argument('--camera-namespace', help='namespace for camera', default='/camera/rgb')
705  args = parser.parse_args()
706 
707  if args.arm_root_link == None:
708  args.arm_root_link = args.base_link
709  if args.head_root_link == None:
710  args.head_root_link = args.base_link
711 
712  rospy.init_node("claibration_setup_helper", anonymous=True)
713 
714  write_upload_launch = False
715  try:
716  if args.robot_description:
717  robot_description = args.robot_description.read()
718  write_upload_launch = True
719  else:
720  robot_description = rospy.get_param('robot_description')
721  except:
722  rospy.logfatal('robot_description not set, exiting')
723  sys.exit(-1)
724 
725  helper = CalibrationSetupHelper(robot_description, args.base_link, args.arm_root_link, args.arm_tip_link, args.head_root_link, args.head_tip_link, args.arm_controller, args.head_controller, args.head_camera_frame, args.head_camera_joint, args.camera_namespace)
726 
727  f = open(helper.dirname_top+'/package.xml', 'w+')
728  f.write("""
729 <package>
730  <name>{robot_name}_calibration</name>
731  <version>0.2.0</version>
732  <description>
733  Launch and configuration files for calibrating {robot_name} using the new generic 'calibration' stack.
734  THIS FILE IS AUTOMATICALLY GENERATED BY:
735  {command}
736  </description>
737  <author>Calibration Setup Helper</author>
738  <maintainer email="kei.okada@gmail.com">Calibration Setup Helper</maintainer>
739 
740  <license>BSD</license>
741  <url>http://ros.org/wiki/{robot_name}_calibration</url>
742 
743  <buildtool_depend>catkin</buildtool_depend>
744 
745  <run_depend>calibration_launch</run_depend>
746  <run_depend>calibration_estimation</run_depend>
747  <run_depend>kdl</run_depend>
748  <run_depend>kdl_parser</run_depend>-->
749 </package>
750 """.format(robot_name=helper.robot_name, command=' '.join(sys.argv)))
751  f.close()
752 
753  f = open(helper.dirname_top+'/CMakeLists.txt', 'w+')
754  f.write("""
755 cmake_minimum_required(VERSION 2.8.3)
756 project({0}_calibration)
757 
758 find_package(catkin)
759 catkin_package()
760 
761 install(DIRECTORY capture_data estimate_params # view_results
762  DESTINATION ${{CATKIN_PACKAGE_SHARE_DESTINATION}}
763  USE_SOURCE_PERMISSIONS
764 )
765 
766 """.format(helper.robot_name))
767  f.close()
768 
769  if write_upload_launch:
770  f = open(helper.dirname_top+'/upload_urdf.launch', 'w+')
771  f.write("""
772 <launch>
773  <arg name="model" default="{}" />
774  <!-- send maxwell urdf to param server -->
775  <param name="robot_description" textfile="$(arg model)" />
776 </launch>
777 
778 """.format(args.robot_description.name))
779  f.close()
def __init__(self, robot_description, base_link, arm_root_link, arm_tip_link, head_root_link, head_tip_link, arm_controller, head_controller, head_camera_frame, head_camera_joint, camera_namespace)


calibration_setup_helper
Author(s): Kei Okada
autogenerated on Thu Jun 6 2019 19:17:39