calibration_setup_helper.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (C) 2014 Kei Okada
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 # http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 #
00017 
00018 import rospy
00019 import argparse
00020 import os,sys,itertools
00021 from urdf_parser_py.urdf import URDF
00022 
00023 class CalibrationSetupHelper:
00024     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):
00025         self.robot_description = robot_description
00026         self.base_link = base_link
00027         self.arm_root_link = arm_root_link
00028         self.arm_tip_link = arm_tip_link
00029         self.head_root_link = head_root_link
00030         self.head_tip_link = head_tip_link
00031         self.arm_controller = arm_controller
00032         self.head_controller = head_controller
00033         self.head_camera_frame = head_camera_frame
00034         self.head_camera_joint = head_camera_joint
00035         self.camera_namespace = camera_namespace
00036 
00037         self.robot = URDF().parse(self.robot_description)
00038         self.robot_name = self.robot.name.lower()
00039 
00040         if not self.base_link:
00041             self.base_link = self.robot.get_root()
00042         if self.robot.link_map.has_key(self.base_link) == False:
00043             self.error_tip_link(self.base_link, '--base-link')
00044         if self.robot.link_map.has_key(self.arm_root_link) == False:
00045             self.error_tip_link(self.arm_root_link, '--arm-root-link')
00046         if self.robot.link_map.has_key(self.head_root_link) == False:
00047             self.error_tip_link(self.head_root_link, '--head-root-link')
00048         if self.robot.link_map.has_key(self.arm_tip_link) == False:
00049             self.error_tip_link(self.arm_tip_link, '--arm-tip-link')
00050         if self.robot.link_map.has_key(self.head_tip_link) == False:
00051             self.error_tip_link(self.head_tip_link, '--head-tip-link')
00052         if self.robot.link_map.has_key(self.head_camera_frame) == False:
00053             self.error_tip_link(self.head_camera_frame, '--head-camera-frame')
00054         if self.robot.joint_map.has_key(self.head_camera_joint) == False:
00055             self.error_joint(self.head_camera_joint, '--head-camera-joint')
00056 
00057         all_chain = []
00058         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)]:
00059             all_chain.append(self.robot.get_chain(base_link, end_link)[1:])
00060         for c1,c2 in itertools.product(*all_chain):
00061             if c1 == c2 :
00062                 rospy.logerr('arm/head chain share same joint ({}), this will cause failure'.format(c1))
00063                 sys.exit()
00064 
00065         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)]:
00066             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)])
00067             exec('self.{0}_joint_list = {1}'.format(limb, joint_list)) in locals()
00068 
00069         rospy.loginfo('using following joint for {} chain'.format('arm'))
00070         rospy.loginfo('... {}'.format(self.arm_joint_list))
00071         rospy.loginfo('using following joint for {} chain'.format('head'))
00072         rospy.loginfo('... {}'.format(self.head_joint_list))
00073 
00074         # create robot_calibration directory
00075         self.dirname_top = self.robot_name+'_calibration'
00076         self.dirname_capture = self.robot_name+'_calibration/capture_data'
00077         self.dirname_estimate = self.robot_name+'_calibration/estimate_params'
00078         self.dirname_results = self.robot_name+'_calibration/view_results'
00079 
00080         try:
00081             os.makedirs(self.dirname_top)
00082             os.makedirs(self.dirname_capture)
00083             os.makedirs(self.dirname_estimate)
00084             os.makedirs(self.dirname_results)
00085         except Exception as e:
00086             rospy.logfatal(e)
00087             #sys.exit(-1)
00088 
00089         # setup capture_data
00090         self.write_all_pipelines(args.use_kinect)       # all_pipelines.launch
00091         self.write_all_viewers(args.use_kinect)         # all_viewers.launch
00092         self.write_capture_data()                       # capture_data.launch
00093         self.write_capture_exec()                       # capture_exec.launch
00094         self.write_hardware_config()                    # hardware_config
00095         self.write_make_samples()                       # make_samples.py
00096         # setup estimate_params
00097         self.write_estimation_config()                  # estimation_config.launch
00098         self.write_calibrate_sh()                       # calibrate_<robot>.sh
00099         self.write_estimate_params_config(args.use_kinect)             # free_arms.yaml  free_cameras.yaml  free_cb_locations.yaml  free_torso.yaml  system.yaml
00100         # setup view_results
00101         self.write_view_results()                       # scatter_config.yaml  view_scatter.sh
00102 
00103 
00104     def error_tip_link(self, link, option):
00105         rospy.logfatal('could not find valid link name ... {}'.format(link))
00106         rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.links)))
00107         f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+')
00108         print >>f, self.robot_description
00109         f.close()
00110         rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name))
00111         sys.exit(-1)
00112 
00113     def error_joint(self, joint, option):
00114         rospy.logfatal('could not find valid joint name ... {}'.format(joint))
00115         rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.joints)))
00116         f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+')
00117         print >>f, self.robot_description
00118         f.close()
00119         rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name))
00120         sys.exit(-1)
00121 
00122     def write_all_pipelines(self, use_kinect):
00123         file_name = self.dirname_capture+'/all_pipelines.launch'
00124         f = open(file_name,'w+')
00125         f.write("""
00126 <launch>
00127   <include file="$(find calibration_launch)/capture_data/chain_settler.launch" ns="arm_chain" />
00128   <include file="$(find calibration_launch)/capture_data/chain_settler.launch" ns="head_chain" />
00129 
00130   <arg name="use_kinect" default="{0}" />
00131   <group if="$(arg use_kinect)" >
00132     <include file="$(find calibration_launch)/capture_data/kinect_pipeline.launch" ns="{1}">
00133       <arg name="image_topic" value="image_rect_color"/> <!-- openni2.launch publishes image_raw and image_rect_color -->
00134       <arg name="depth_topic" value="/camera/depth/image"/> <!-- this is floar value -->
00135       <arg name="camera_info_topic" value="camera_info"/>
00136     </include>
00137   </group>
00138   <group unless="$(arg use_kinect)" >
00139     <include file="$(find calibration_launch)/capture_data/monocam_pipeline.launch" ns="{1}">
00140       <arg name="image_topic" value="image_rect_color"/> <!-- openni2.launch publishes image_raw and image_rect_color -->
00141     </include>
00142   </group>
00143 
00144   <node type="interval_intersection_action"
00145         pkg="interval_intersection"
00146         name="interval_intersection"
00147         output="screen">
00148     <remap from="head_chain" to="head_chain/settled_interval" />
00149     <remap from="arm_chain" to="arm_chain/settled_interval" />
00150     <remap from="head_camera" to="{1}/settled_interval" />
00151   </node>
00152 
00153 </launch>
00154 """.format("true" if use_kinect else "false", self.camera_namespace))
00155         f.close()
00156 
00157     def write_all_viewers(self, use_kinect):
00158         file_name = self.dirname_capture+'/all_viewers.launch'
00159         f = open(file_name,'w+')
00160         f.write("""
00161 <launch>
00162 
00163   <!-- Hack to create the directory -->
00164   <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/{0}_calibration" />
00165   <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/{0}_calibration/cb_fail" />
00166 
00167   <param name="{2}/annotated_viewer/filename_format" type="string" value="/tmp/{0}_calibration/cb_fail/cb_{1}_%04i.jpg" />
00168 
00169   <include file="$(find calibration_launch)/capture_data/annotated_viewer.launch"
00170            ns="{2}" >
00171    <arg name="image_topic" value="image_rect_annotated" />
00172   </include>
00173 
00174 </launch>
00175 """.format(self.robot_name,"kinect" if use_kinect else "monocam", self.camera_namespace))
00176         f.close()
00177 
00178     def write_capture_data(self):
00179         file_name = self.dirname_capture+'/capture_data.launch'
00180         f = open(file_name,'w+')
00181         f.write("""
00182 <launch>
00183   <include file="$(find {0}_calibration)/capture_data/all_viewers.launch"/>
00184   <include file="$(find {0}_calibration)/capture_data/all_pipelines.launch"/>
00185   <include file="$(find {0}_calibration)/capture_data/capture_exec.launch"/>
00186 </launch>
00187 """.format(self.robot_name))
00188 
00189     def write_capture_exec(self):
00190         file_name = self.dirname_capture+'/capture_exec.launch'
00191         f = open(file_name,'w+')
00192         f.write("""
00193 <launch>
00194 
00195   <node type="capture_exec.py"
00196         pkg="calibration_launch"
00197         name="calibration_exec"
00198         args="$(find {0}_calibration)/capture_data/samples/ $(find {0}_calibration)/capture_data/hardware_config $(find {0}_calibration)/estimate_params/config/system.yaml"
00199         output="screen" >
00200     <remap from="head_camera/camera_info" to="{1}/camera_info"/>
00201     <remap from="head_camera/image_rect"  to="{1}/image_rect_throttle"/>
00202     <remap from="head_camera/image"       to="{1}/image_rect_throttle"/>
00203     <remap from="head_camera/features"    to="{1}/features"/>
00204   </node>
00205 
00206   <node type="urdf_pub.py" pkg="calibration_launch" name="urdf_pub"/>
00207 
00208   <node type="record" pkg="rosbag" name="robot_measurement_recorder" output="screen"
00209         args="-O /tmp/{0}_calibration/cal_measurements robot_measurement robot_description" >
00210     <!-- Hack to create the directory -->
00211     <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/{0}_calibration" />
00212   </node>
00213 
00214 </launch>""".format(self.robot_name, self.camera_namespace))
00215         f.close()
00216 
00217     def write_hardware_config(self):
00218         config_dir = self.dirname_capture+'/hardware_config/'
00219         try:
00220             os.makedirs(config_dir)
00221         except Exception as e:
00222             rospy.logfatal(e)
00223 
00224         # cam_config.yaml
00225         f = open(config_dir+'/cam_config.yaml', 'w+')
00226         f.write("""
00227 # ----- Microsoft Kinect -----
00228 head_camera: ###FIX
00229   cb_detector_config:  {0}/cb_detector_config ## FIXME
00230   led_detector_config: {0}/led_detector
00231   settler_config:      {0}/monocam_settler_config ## FIXME
00232 
00233   configs:
00234     small_cb_4x5:
00235       settler:
00236         tolerance: 2.00
00237         ignore_failures: True
00238         max_step: 3.0
00239         cache_size: 100
00240       cb_detector:
00241         active: True
00242         num_x: 4
00243         num_y: 5
00244         width_scaling: 0.5
00245         height_scaling: 0.5
00246         subpixel_window: 4
00247         subpixel_zero_zone: 1
00248       led_detector:
00249         active: False
00250 """.format(self.camera_namespace))     # not sure if this is works for mono camera
00251         f.close()
00252 
00253         # chain_config.yaml
00254         f = open(config_dir+'/chain_config.yaml', 'w+')
00255         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00256             if len(joint_list) == 0:
00257                 continue
00258             tolerance = 0.002
00259             f.write('# ----- {} -----\n'.format(limb))
00260             f.write('{}_chain:\n'.format(limb))
00261             f.write('  settler_config:  /{}_chain/settler_config\n\n'.format(limb))
00262             f.write('  configs:\n')
00263             f.write('    tight_tol:\n')
00264             f.write('      settler:\n')
00265             f.write('        joint_names:\n')
00266             for j in joint_list:
00267                 f.write('        - {}\n'.format(j))
00268             f.write('        tolerances:\n')
00269             for j in joint_list:
00270                 f.write('        - {}\n'.format(tolerance))
00271             f.write('        max_step:   1.0\n')
00272             f.write('        cache_size: 1500\n\n')
00273         f.close()
00274 
00275         # controller_config.yaml
00276         f = open(config_dir+'/controller_config.yaml', 'w+')
00277         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00278             if len(joint_list) == 0:
00279                 continue
00280             f.write('{}_controller:\n'.format(limb))
00281             exec('controller = self.{}_controller'.format(limb))
00282             rospy.loginfo('Subscribing controller topic is \'{}\''.format(controller))
00283             f.write('  topic: {0}\n'.format(controller))
00284             f.write('  joint_names:\n')
00285             for j in joint_list:
00286                 f.write('  - {}\n'.format(j))
00287         f.close()
00288 
00289         # laser_config.yaml
00290         f = open(config_dir+'/laser_config.yaml', 'w+')
00291         f.write('# laser config')
00292         f.close()
00293 
00294     def write_make_samples(self):
00295         samples_dir = self.dirname_capture+'/samples/'
00296         try:
00297             os.makedirs(samples_dir+'/arm')
00298         except Exception as e:
00299             rospy.logfatal(e)
00300         f = open(samples_dir+'/arm/config.yaml', 'w+')
00301         f.write("""
00302 group: "Arm"
00303 prompt: "Please put the checkerboard in the hand (open/close the gripper with the joystick's square/circle buttons)."
00304 finish: "Skipping arm samples"
00305 repeat: False
00306 """)
00307         f.close()
00308 
00309         f = open(samples_dir+'/../make_samples.py', 'w+')
00310         os.chmod(samples_dir+'/../make_samples.py', 0744)
00311         f.write("""#!/usr/bin/env python
00312 
00313 # capture samples!!!!1!one
00314 
00315 # this script should eventually be replaced by something that finds
00316 # samples automatically
00317 
00318 import rospy
00319 from sensor_msgs.msg import JointState
00320 
00321 import string, os
00322 
00323 header1 = \"\"\"camera_measurements:
00324 - {cam_id: head_camera, config: small_cb_4x5}
00325 joint_commands:
00326 \"\"\"
00327 """)
00328         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00329             if len(joint_list) == 0:
00330                 continue
00331             f.write("""
00332 header2_{0} = \"\"\"- controller: {0}_controller
00333   segments:
00334   - duration: 2.0
00335     positions: \"\"\"
00336 """.format(limb))
00337         f.write("""
00338 header3 = \"\"\"joint_measurements:
00339 """)
00340         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00341             if len(joint_list) == 0:
00342                 continue
00343             f.write('- {{chain_id: {0}_chain, config: tight_tol}}\n'.format(limb))
00344         f.write("""
00345 sample_id: arm_\"\"\"
00346 
00347 header4 = \"\"\"target: {chain_id: arm_chain, target_id: small_cb_4x5}\"\"\"
00348 
00349 class SampleMaker:
00350     
00351     def __init__(self):
00352         rospy.init_node("make_samples")
00353         rospy.Subscriber("joint_states", JointState, self.callback)
00354 """)
00355         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00356             if len(joint_list) == 0:
00357                 continue
00358             f.write('        self.{}_joints = ['.format(limb))
00359             for j in joint_list:
00360                 f.write('\"{}\", '.format(j))
00361             f.write(']\n')
00362             f.write('        self.{0}_state = [0.0 for joint in self.{0}_joints]\n'.format(limb))
00363         f.write("""
00364         self.count = 0
00365 
00366         while not rospy.is_shutdown():
00367             print "Move arm/head to a new sample position."
00368             resp = raw_input("press <enter> ")
00369             if string.upper(resp) == "EXIT":
00370                 break
00371             else:
00372                 # save a sample:
00373                 count = str(self.count).zfill(4)
00374                 f = open(os.path.dirname(__file__)+"/samples/arm/arm_"+count+".yaml", "w")
00375                 f.write(header1)
00376 """)
00377         f.write("""                print('saving ... {0}'.format(self.count))\n""")
00378         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00379             if len(joint_list) == 0:
00380                 continue
00381             f.write("""                print('  {0}_state: {{0}}'.format(self.{0}_state))\n""".format(limb))
00382             f.write("""
00383                 f.write(header2_{0})
00384                 print>>f, self.{0}_state
00385 """.format(limb))
00386         f.write("""
00387                 f.write(header3)
00388                 print>>f, count
00389                 f.write(header4)
00390             self.count += 1
00391 
00392     def callback(self, msg):
00393 """)
00394         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00395             if len(joint_list) == 0:
00396                 continue
00397             f.write("""
00398         for i in range(len(self.{0}_joints)):
00399             try:
00400                 idx = msg.name.index(self.{0}_joints[i])
00401                 self.{0}_state[i] = msg.position[idx]
00402             except: 
00403                 pass
00404 """.format(limb))
00405         f.write("""
00406 
00407 if __name__=="__main__":
00408     SampleMaker()
00409 """)
00410         f.close()
00411 
00412     def write_estimation_config(self):
00413         file_name = self.dirname_estimate+'/estimation_config.launch'
00414         f = open(file_name,'w+')
00415         f.write("""
00416 <launch>
00417 
00418   <group ns="calibration_config" clear_params="true">
00419     <rosparam file="$(find {0}_calibration)/estimate_params/config/system.yaml" command="load" />
00420 
00421     <group ns="cal_steps">
00422 
00423       <group ns="{0} - 00 - Estimating Checkerboard Locations">
00424         <param name="free_params" textfile="$(find {0}_calibration)/estimate_params/config/free_cb_locations.yaml" />
00425         <param name="use_cov" type="bool" value="False" />
00426         <rosparam>
00427           sensors:
00428           - arm_chain
00429           - head_camera
00430         </rosparam>
00431         <param name="output_filename" type="string" value="config_0" />
00432       </group>
00433 
00434       <group ns="{0} - 01 - Adding Camera Locations">
00435         <param name="free_params" textfile="$(find {0}_calibration)/estimate_params/config/free_cameras.yaml" />
00436         <param name="use_cov" type="bool" value="True" />
00437         <rosparam>
00438           sensors:
00439           - arm_chain
00440           - head_camera
00441         </rosparam>
00442         <param name="output_filename" type="string" value="system_calibrated" />
00443       </group>
00444 
00445     </group>
00446 
00447   </group>
00448 
00449 </launch>
00450 """.format(self.robot_name))
00451 
00452     def write_calibrate_sh(self):
00453         file_name = self.dirname_estimate+'/calibrate_'+self.robot_name+'.sh'
00454         f = open(file_name,'w+')
00455         os.chmod(file_name, 0744)
00456         f.write("""#! /bin/bash
00457 
00458 if [ -f robot_calibrated.xml ]; then
00459   echo "./robot_calibrated.xml already exists. Either back up this file or remove it before continuing"
00460   exit 1
00461 fi
00462 
00463 echo "Checking if we can write to ./robot_calibrated.xml..."
00464 touch robot_calibrated.xml
00465 if [ "$?" -ne "0" ]; then
00466   echo "Not able to write to ./robot_calibrated.xml"
00467   echo "Make sure you run this script from a directory that for which you have write permissions."
00468   exit 1
00469 fi
00470 rm robot_calibrated.xml
00471 echo "Success"
00472 
00473 roslaunch {0}_calibration estimation_config.launch
00474 rosrun calibration_estimation multi_step_cov_estimator.py /tmp/{0}_calibration/cal_measurements.bag /tmp/{0}_calibration __name:=cal_cov_estimator
00475 
00476 est_return_val=$?
00477 
00478 if [ "$est_return_val" -ne "0" ]; then
00479   echo "Estimator exited prematurely with error code [$est_return_val]"
00480   exit 1
00481 fi
00482 
00483 # Make all the temporary files writable
00484 chmod ag+w /tmp/{0}_calibration/*
00485 
00486 """.format(self.robot_name))
00487 
00488     def write_estimate_params_config(self, use_kinect=False):
00489         config_dir = self.dirname_estimate+'/config'
00490         try:
00491             os.makedirs(config_dir)
00492         except Exception as e:
00493             rospy.logfatal(e)
00494 
00495         # free_arms.yaml
00496         f = open(config_dir+'/free_arms.yaml', 'w+')
00497         f.write('\n')
00498         f.write('  transforms:\n')
00499         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00500             if len(joint_list) == 0:
00501                 continue
00502             for j in joint_list:
00503                 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]))
00504         f.write('\n')
00505         f.write('  chains:\n')
00506         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00507             if len(joint_list) == 0:
00508                 continue
00509             f.write('    {}_chain:\n'.format(limb))
00510             f.write('      gearing: {}\n'.format([0]*len(joint_list)))
00511         f.write("""
00512   rectified_cams:
00513     head_camera:
00514       baseline_shift: 0
00515       f_shift: 0
00516       cx_shift: 0
00517       cy_shift: 0
00518 
00519   tilting_lasers: {}
00520 
00521   checkerboards:
00522     small_cb_4x5:
00523       spacing_x: 0
00524       spacing_y: 0
00525 
00526 """)
00527         f.close()
00528 
00529         # free_cameras.yaml
00530         f = open(config_dir+'/free_cameras.yaml', 'w+')
00531         f.write("""
00532   transforms:
00533     arm_chain_cb: [1, 1, 1, 1, 1, 1]
00534     {0}: [1, 1, 1, 1, 1, 1]
00535 
00536   chains:
00537 """.format(self.head_camera_joint))
00538         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00539             if len(joint_list) == 0:
00540                 continue
00541             f.write('    {}_chain:\n'.format(limb))
00542             f.write('      gearing: {}\n'.format([0]*len(joint_list)))
00543         f.write("""
00544   rectified_cams:
00545     head_camera:
00546       baseline_shift: 0
00547       f_shift: 0
00548       cx_shift: 0
00549       cy_shift: 0
00550 
00551   tilting_lasers: {}
00552 
00553   checkerboards:
00554     small_cb_4x5:
00555       spacing_x: 0
00556       spacing_y: 0
00557 
00558 """)
00559         f.close()
00560 
00561         # free_cb_locations.yaml
00562         f = open(config_dir+'/free_cb_locations.yaml', 'w+')
00563         f.write("""
00564   transforms:
00565     arm_chain_cb: [1, 1, 1, 1, 1, 1]
00566 
00567   chains:
00568 """)
00569         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00570             if len(joint_list) == 0:
00571                 continue
00572             f.write('    {}_chain:\n'.format(limb))
00573             f.write('      gearing: {}\n'.format([0]*len(joint_list)))
00574         f.write("""
00575 
00576   rectified_cams:
00577     head_camera:
00578       baseline_shift: 0
00579       f_shift: 0
00580       cx_shift: 0
00581       cy_shift: 0
00582 
00583   tilting_lasers: {}
00584 
00585   checkerboards:
00586     small_cb_4x5:
00587       spacing_x: 0
00588       spacing_y: 0
00589 
00590 """)
00591 
00592         # free_torso.yaml
00593         f = open(config_dir+'/free_torso.yaml', 'w+')
00594         f.write("""
00595   transforms:
00596     head_joint: [0, 0, 1, 0, 0, 0]
00597 
00598   chains:
00599 """)
00600         for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]):
00601             if len(joint_list) == 0:
00602                 continue
00603             f.write('    {}_chain:\n'.format(limb))
00604             f.write('      gearing: {}\n'.format([0]*len(joint_list)))
00605         f.write("""
00606 
00607   rectified_cams:
00608     head_camera:
00609       baseline_shift: 0
00610       f_shift: 0
00611       cx_shift: 0
00612       cy_shift: 0
00613 
00614   tilting_lasers: {}
00615 
00616   checkerboards:
00617     small_cb_4x5:
00618       spacing_x: 0
00619       spacing_y: 0
00620 
00621 """)
00622         # system.yaml
00623         f = open(config_dir+'/system.yaml', 'w+')
00624         f.write('\n')
00625         f.write('base_link: {0}\n'.format(self.base_link))
00626         f.write('\n')
00627         f.write('sensors:\n')
00628         f.write('\n')
00629         f.write('  chains:\n')
00630         if len(self.arm_joint_list) > 0:
00631             f.write('    arm_chain:\n')
00632             f.write('      root: {}\n'.format(self.arm_root_link))
00633             f.write('      tip: {}\n'.format(self.arm_tip_link))
00634             f.write('      cov:\n')
00635             f.write('       joint_angles: {}\n'.format([0.01]*len(self.arm_joint_list)))
00636             f.write('      gearing: {}\n'.format([1.0]*len(self.arm_joint_list)))
00637         if len(self.head_joint_list) > 0:
00638             f.write('    head_chain:\n')
00639             f.write('      root: {}\n'.format(self.head_root_link))
00640             f.write('      tip: {}\n'.format(self.head_tip_link))
00641             f.write('      cov:\n')
00642             f.write('       joint_angles: {}\n'.format([0.01]*len(self.head_joint_list)))
00643             f.write('      gearing: {}\n'.format([1.0]*len(self.head_joint_list)))
00644         f.write('\n')
00645         f.write('  rectified_cams:\n')
00646         f.write('    head_camera:\n')
00647         f.write('      chain_id: head_chain  #TODO: get rid of this\n')
00648         f.write('      frame_id: {}\n'.format(self.head_camera_frame))
00649         f.write('      baseline_shift: 0.0\n')
00650         if use_kinect:
00651             f.write('      baseline_rgbd: 0.075\n')
00652         else:
00653             f.write('#      baseline_rgbd: 0.075 ## comment out if we run all_pipelines.launch with use_kinect\n')
00654         f.write('      f_shift: 0.0\n')
00655         f.write('      cx_shift: 0.0\n')
00656         f.write('      cy_shift: 0.0\n')
00657         f.write('      cov: {u: 0.25, v: 0.25, x: 0.25}\n')
00658         f.write('\n')
00659         f.write('  tilting_lasers: {}\n')
00660         f.write('\n')
00661         f.write('checkerboards:\n')
00662         f.write('  small_cb_4x5:\n')
00663         f.write('    corners_x: 4\n')
00664         f.write('    corners_y: 5\n')
00665         f.write('    spacing_x: 0.0245\n')
00666         f.write('    spacing_y: 0.0245\n')
00667         f.write('\n')
00668         f.write('transforms:\n')
00669         f.write('  arm_chain_cb:   [ .25, 0, 0, pi/2, 0, 0]\n')
00670         f.write('\n')
00671 
00672     def write_view_results(self):
00673         file_name = self.dirname_results+'/scatter_config.yaml'
00674         f = open(file_name, 'w+')
00675         f.write('- name: "Head Camera: Arm"\n')
00676         f.write('  3d: arm_chain\n')
00677         f.write('  cam: head_camera\n')
00678         f.write('  plot_ops:\n')
00679         f.write('    color: g\n')
00680         f.write('    marker: s\n')
00681         f.close()
00682 
00683         file_name = self.dirname_results+'/view_scatter.sh'
00684         f = open(file_name,'w+')
00685         os.chmod(file_name, 0744)
00686         f.write("""#!/bin/bash
00687 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))
00688         f.close()
00689 
00690 
00691 if __name__ == '__main__':
00692     parser = argparse.ArgumentParser(description='Calibration setup helper.')
00693     parser.add_argument('robot_description', type=file, default=False, nargs='?', help='path to urdf file')
00694     parser.add_argument('--use-kinect', action='store_const', const=True, default=False, help='use kinect for calibraiton')
00695     parser.add_argument('--base-link', help='base link name')
00696     parser.add_argument('--arm-root-link', help='arm root link name')
00697     parser.add_argument('--arm-tip-link', help='arm tip link name')
00698     parser.add_argument('--head-root-link', help='head root link name')
00699     parser.add_argument('--head-tip-link', help='head tip link name')
00700     parser.add_argument('--arm-controller', help='arm controller name', default='arm_controller/command')
00701     parser.add_argument('--head-controller', help='head controller name', default='head_controller/command')
00702     parser.add_argument('--head-camera-frame', help='frame name for target camera', default='camera_rgb_optical_frame')
00703     parser.add_argument('--head-camera-joint', help='joint between head and camera', default='camera_rgb_joint')
00704     parser.add_argument('--camera-namespace', help='namespace for camera', default='/camera/rgb')
00705     args = parser.parse_args()
00706 
00707     if args.arm_root_link == None:
00708         args.arm_root_link = args.base_link
00709     if args.head_root_link == None:
00710         args.head_root_link = args.base_link
00711 
00712     rospy.init_node("claibration_setup_helper", anonymous=True)
00713 
00714     write_upload_launch = False
00715     try:
00716         if args.robot_description:
00717             robot_description = args.robot_description.read()
00718             write_upload_launch = True
00719         else:
00720             robot_description = rospy.get_param('robot_description')
00721     except:
00722         rospy.logfatal('robot_description not set, exiting')
00723         sys.exit(-1)
00724 
00725     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)
00726 
00727     f = open(helper.dirname_top+'/package.xml', 'w+')
00728     f.write("""
00729 <package>
00730   <name>{robot_name}_calibration</name>
00731   <version>0.2.0</version>
00732   <description>
00733     Launch and configuration files for calibrating {robot_name} using the new generic 'calibration' stack.
00734    THIS FILE IS AUTOMATICALLY GENERATED BY:
00735    {command}
00736   </description>
00737   <author>Calibration Setup Helper</author>
00738   <maintainer email="kei.okada@gmail.com">Calibration Setup Helper</maintainer>
00739 
00740   <license>BSD</license>
00741   <url>http://ros.org/wiki/{robot_name}_calibration</url>
00742 
00743   <buildtool_depend>catkin</buildtool_depend>
00744 
00745   <run_depend>calibration_launch</run_depend>
00746   <run_depend>calibration_estimation</run_depend>
00747   <run_depend>kdl</run_depend>
00748   <run_depend>kdl_parser</run_depend>-->
00749 </package>
00750 """.format(robot_name=helper.robot_name, command=' '.join(sys.argv)))
00751     f.close()
00752 
00753     f = open(helper.dirname_top+'/CMakeLists.txt', 'w+')
00754     f.write("""
00755 cmake_minimum_required(VERSION 2.8.3)
00756 project({0}_calibration)
00757 
00758 find_package(catkin)
00759 catkin_package()
00760 
00761 install(DIRECTORY capture_data estimate_params # view_results
00762   DESTINATION ${{CATKIN_PACKAGE_SHARE_DESTINATION}}
00763   USE_SOURCE_PERMISSIONS
00764 )
00765 
00766 """.format(helper.robot_name))
00767     f.close()
00768 
00769     if write_upload_launch:
00770         f = open(helper.dirname_top+'/upload_urdf.launch', 'w+')
00771         f.write("""
00772 <launch>
00773     <arg name="model" default="{}" />
00774     <!-- send maxwell urdf to param server -->
00775     <param name="robot_description" textfile="$(arg model)" />
00776 </launch>
00777 
00778 """.format(args.robot_description.name))
00779         f.close()


calibration_setup_helper
Author(s): Kei Okada
autogenerated on Sat Jun 8 2019 19:42:06