diff --git a/calibration_setup_helper/CMakeLists.txt b/calibration_setup_helper/CMakeLists.txt new file mode 100644 index 0000000..3fb506b --- /dev/null +++ b/calibration_setup_helper/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.8.3) +project(calibration_setup_helper) + +find_package(catkin REQUIRED) +catkin_package() + +install(PROGRAMS scripts/calibration_setup_helper.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/calibration_setup_helper/package.xml b/calibration_setup_helper/package.xml new file mode 100644 index 0000000..be81625 --- /dev/null +++ b/calibration_setup_helper/package.xml @@ -0,0 +1,21 @@ + + calibration_setup_helper + 0.10.10 + + This package contains a script to generate calibration launch and configurationfiles for your robot. + which is based on Michael Ferguson's calibration code + + Kei Okada + Kei Okada + + Apache 2.0 + http://ros.org/wiki/calibration_setup_helper + + calibration_launch + + catkin + + + + + diff --git a/calibration_setup_helper/scripts/calibration_setup_helper.py b/calibration_setup_helper/scripts/calibration_setup_helper.py new file mode 100755 index 0000000..fa00da4 --- /dev/null +++ b/calibration_setup_helper/scripts/calibration_setup_helper.py @@ -0,0 +1,779 @@ +#!/usr/bin/env python + +# Copyright (C) 2014 Kei Okada +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +import rospy +import argparse +import os,sys,itertools +from urdf_parser_py.urdf import URDF + +class CalibrationSetupHelper: + 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): + self.robot_description = robot_description + self.base_link = base_link + self.arm_root_link = arm_root_link + self.arm_tip_link = arm_tip_link + self.head_root_link = head_root_link + self.head_tip_link = head_tip_link + self.arm_controller = arm_controller + self.head_controller = head_controller + self.head_camera_frame = head_camera_frame + self.head_camera_joint = head_camera_joint + self.camera_namespace = camera_namespace + + self.robot = URDF().parse(self.robot_description) + self.robot_name = self.robot.name.lower() + + if not self.base_link: + self.base_link = self.robot.get_root() + if self.robot.link_map.has_key(self.base_link) == False: + self.error_tip_link(self.base_link, '--base-link') + if self.robot.link_map.has_key(self.arm_root_link) == False: + self.error_tip_link(self.arm_root_link, '--arm-root-link') + if self.robot.link_map.has_key(self.head_root_link) == False: + self.error_tip_link(self.head_root_link, '--head-root-link') + if self.robot.link_map.has_key(self.arm_tip_link) == False: + self.error_tip_link(self.arm_tip_link, '--arm-tip-link') + if self.robot.link_map.has_key(self.head_tip_link) == False: + self.error_tip_link(self.head_tip_link, '--head-tip-link') + if self.robot.link_map.has_key(self.head_camera_frame) == False: + self.error_tip_link(self.head_camera_frame, '--head-camera-frame') + if self.robot.joint_map.has_key(self.head_camera_joint) == False: + self.error_joint(self.head_camera_joint, '--head-camera-joint') + + all_chain = [] + 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)]: + all_chain.append(self.robot.get_chain(base_link, end_link)[1:]) + for c1,c2 in itertools.product(*all_chain): + if c1 == c2 : + rospy.logerr('arm/head chain share same joint ({}), this will cause failure'.format(c1)) + sys.exit() + + 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)]: + 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)]) + exec('self.{0}_joint_list = {1}'.format(limb, joint_list)) in locals() + + rospy.loginfo('using following joint for {} chain'.format('arm')) + rospy.loginfo('... {}'.format(self.arm_joint_list)) + rospy.loginfo('using following joint for {} chain'.format('head')) + rospy.loginfo('... {}'.format(self.head_joint_list)) + + # create robot_calibration directory + self.dirname_top = self.robot_name+'_calibration' + self.dirname_capture = self.robot_name+'_calibration/capture_data' + self.dirname_estimate = self.robot_name+'_calibration/estimate_params' + self.dirname_results = self.robot_name+'_calibration/view_results' + + try: + os.makedirs(self.dirname_top) + os.makedirs(self.dirname_capture) + os.makedirs(self.dirname_estimate) + os.makedirs(self.dirname_results) + except Exception as e: + rospy.logfatal(e) + #sys.exit(-1) + + # setup capture_data + self.write_all_pipelines(args.use_kinect) # all_pipelines.launch + self.write_all_viewers(args.use_kinect) # all_viewers.launch + self.write_capture_data() # capture_data.launch + self.write_capture_exec() # capture_exec.launch + self.write_hardware_config() # hardware_config + self.write_make_samples() # make_samples.py + # setup estimate_params + self.write_estimation_config() # estimation_config.launch + self.write_calibrate_sh() # calibrate_.sh + self.write_estimate_params_config(args.use_kinect) # free_arms.yaml free_cameras.yaml free_cb_locations.yaml free_torso.yaml system.yaml + # setup view_results + self.write_view_results() # scatter_config.yaml view_scatter.sh + + + def error_tip_link(self, link, option): + rospy.logfatal('could not find valid link name ... {}'.format(link)) + rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.links))) + f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+') + print >>f, self.robot_description + f.close() + rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name)) + sys.exit(-1) + + def error_joint(self, joint, option): + rospy.logfatal('could not find valid joint name ... {}'.format(joint)) + rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.joints))) + f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+') + print >>f, self.robot_description + f.close() + rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name)) + sys.exit(-1) + + def write_all_pipelines(self, use_kinect): + file_name = self.dirname_capture+'/all_pipelines.launch' + f = open(file_name,'w+') + f.write(""" + + + + + + + + + + + + + + + + + + + + + + + + + +""".format("true" if use_kinect else "false", self.camera_namespace)) + f.close() + + def write_all_viewers(self, use_kinect): + file_name = self.dirname_capture+'/all_viewers.launch' + f = open(file_name,'w+') + f.write(""" + + + + + + + + + + + + + +""".format(self.robot_name,"kinect" if use_kinect else "monocam", self.camera_namespace)) + f.close() + + def write_capture_data(self): + file_name = self.dirname_capture+'/capture_data.launch' + f = open(file_name,'w+') + f.write(""" + + + + + +""".format(self.robot_name)) + + def write_capture_exec(self): + file_name = self.dirname_capture+'/capture_exec.launch' + f = open(file_name,'w+') + f.write(""" + + + + + + + + + + + + + + + + +""".format(self.robot_name, self.camera_namespace)) + f.close() + + def write_hardware_config(self): + config_dir = self.dirname_capture+'/hardware_config/' + try: + os.makedirs(config_dir) + except Exception as e: + rospy.logfatal(e) + + # cam_config.yaml + f = open(config_dir+'/cam_config.yaml', 'w+') + f.write(""" +# ----- Microsoft Kinect ----- +head_camera: ###FIX + cb_detector_config: {0}/cb_detector_config ## FIXME + led_detector_config: {0}/led_detector + settler_config: {0}/monocam_settler_config ## FIXME + + configs: + small_cb_4x5: + settler: + tolerance: 2.00 + ignore_failures: True + max_step: 3.0 + cache_size: 100 + cb_detector: + active: True + num_x: 4 + num_y: 5 + width_scaling: 0.5 + height_scaling: 0.5 + subpixel_window: 4 + subpixel_zero_zone: 1 + led_detector: + active: False +""".format(self.camera_namespace)) # not sure if this is works for mono camera + f.close() + + # chain_config.yaml + f = open(config_dir+'/chain_config.yaml', 'w+') + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + tolerance = 0.002 + f.write('# ----- {} -----\n'.format(limb)) + f.write('{}_chain:\n'.format(limb)) + f.write(' settler_config: /{}_chain/settler_config\n\n'.format(limb)) + f.write(' configs:\n') + f.write(' tight_tol:\n') + f.write(' settler:\n') + f.write(' joint_names:\n') + for j in joint_list: + f.write(' - {}\n'.format(j)) + f.write(' tolerances:\n') + for j in joint_list: + f.write(' - {}\n'.format(tolerance)) + f.write(' max_step: 1.0\n') + f.write(' cache_size: 1500\n\n') + f.close() + + # controller_config.yaml + f = open(config_dir+'/controller_config.yaml', 'w+') + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + f.write('{}_controller:\n'.format(limb)) + exec('controller = self.{}_controller'.format(limb)) + rospy.loginfo('Subscribing controller topic is \'{}\''.format(controller)) + f.write(' topic: {0}\n'.format(controller)) + f.write(' joint_names:\n') + for j in joint_list: + f.write(' - {}\n'.format(j)) + f.close() + + # laser_config.yaml + f = open(config_dir+'/laser_config.yaml', 'w+') + f.write('# laser config') + f.close() + + def write_make_samples(self): + samples_dir = self.dirname_capture+'/samples/' + try: + os.makedirs(samples_dir+'/arm') + except Exception as e: + rospy.logfatal(e) + f = open(samples_dir+'/arm/config.yaml', 'w+') + f.write(""" +group: "Arm" +prompt: "Please put the checkerboard in the hand (open/close the gripper with the joystick's square/circle buttons)." +finish: "Skipping arm samples" +repeat: False +""") + f.close() + + f = open(samples_dir+'/../make_samples.py', 'w+') + os.chmod(samples_dir+'/../make_samples.py', 0744) + f.write("""#!/usr/bin/env python + +# capture samples!!!!1!one + +# this script should eventually be replaced by something that finds +# samples automatically + +import rospy +from sensor_msgs.msg import JointState + +import string, os + +header1 = \"\"\"camera_measurements: +- {cam_id: head_camera, config: small_cb_4x5} +joint_commands: +\"\"\" +""") + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + f.write(""" +header2_{0} = \"\"\"- controller: {0}_controller + segments: + - duration: 2.0 + positions: \"\"\" +""".format(limb)) + f.write(""" +header3 = \"\"\"joint_measurements: +""") + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + f.write('- {{chain_id: {0}_chain, config: tight_tol}}\n'.format(limb)) + f.write(""" +sample_id: arm_\"\"\" + +header4 = \"\"\"target: {chain_id: arm_chain, target_id: small_cb_4x5}\"\"\" + +class SampleMaker: + + def __init__(self): + rospy.init_node("make_samples") + rospy.Subscriber("joint_states", JointState, self.callback) +""") + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + f.write(' self.{}_joints = ['.format(limb)) + for j in joint_list: + f.write('\"{}\", '.format(j)) + f.write(']\n') + f.write(' self.{0}_state = [0.0 for joint in self.{0}_joints]\n'.format(limb)) + f.write(""" + self.count = 0 + + while not rospy.is_shutdown(): + print "Move arm/head to a new sample position." + resp = raw_input("press ") + if string.upper(resp) == "EXIT": + break + else: + # save a sample: + count = str(self.count).zfill(4) + f = open(os.path.dirname(__file__)+"/samples/arm/arm_"+count+".yaml", "w") + f.write(header1) +""") + f.write(""" print('saving ... {0}'.format(self.count))\n""") + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + f.write(""" print(' {0}_state: {{0}}'.format(self.{0}_state))\n""".format(limb)) + f.write(""" + f.write(header2_{0}) + print>>f, self.{0}_state +""".format(limb)) + f.write(""" + f.write(header3) + print>>f, count + f.write(header4) + self.count += 1 + + def callback(self, msg): +""") + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + f.write(""" + for i in range(len(self.{0}_joints)): + try: + idx = msg.name.index(self.{0}_joints[i]) + self.{0}_state[i] = msg.position[idx] + except: + pass +""".format(limb)) + f.write(""" + +if __name__=="__main__": + SampleMaker() +""") + f.close() + + def write_estimation_config(self): + file_name = self.dirname_estimate+'/estimation_config.launch' + f = open(file_name,'w+') + f.write(""" + + + + + + + + + + + + sensors: + - arm_chain + - head_camera + + + + + + + + + sensors: + - arm_chain + - head_camera + + + + + + + + + +""".format(self.robot_name)) + + def write_calibrate_sh(self): + file_name = self.dirname_estimate+'/calibrate_'+self.robot_name+'.sh' + f = open(file_name,'w+') + os.chmod(file_name, 0744) + f.write("""#! /bin/bash + +if [ -f robot_calibrated.xml ]; then + echo "./robot_calibrated.xml already exists. Either back up this file or remove it before continuing" + exit 1 +fi + +echo "Checking if we can write to ./robot_calibrated.xml..." +touch robot_calibrated.xml +if [ "$?" -ne "0" ]; then + echo "Not able to write to ./robot_calibrated.xml" + echo "Make sure you run this script from a directory that for which you have write permissions." + exit 1 +fi +rm robot_calibrated.xml +echo "Success" + +roslaunch {0}_calibration estimation_config.launch +rosrun calibration_estimation multi_step_cov_estimator.py /tmp/{0}_calibration/cal_measurements.bag /tmp/{0}_calibration __name:=cal_cov_estimator + +est_return_val=$? + +if [ "$est_return_val" -ne "0" ]; then + echo "Estimator exited prematurely with error code [$est_return_val]" + exit 1 +fi + +# Make all the temporary files writable +chmod ag+w /tmp/{0}_calibration/* + +""".format(self.robot_name)) + + def write_estimate_params_config(self, use_kinect=False): + config_dir = self.dirname_estimate+'/config' + try: + os.makedirs(config_dir) + except Exception as e: + rospy.logfatal(e) + + # free_arms.yaml + f = open(config_dir+'/free_arms.yaml', 'w+') + f.write('\n') + f.write(' transforms:\n') + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + for j in joint_list: + 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])) + f.write('\n') + f.write(' chains:\n') + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + f.write(' {}_chain:\n'.format(limb)) + f.write(' gearing: {}\n'.format([0]*len(joint_list))) + f.write(""" + rectified_cams: + head_camera: + baseline_shift: 0 + f_shift: 0 + cx_shift: 0 + cy_shift: 0 + + tilting_lasers: {} + + checkerboards: + small_cb_4x5: + spacing_x: 0 + spacing_y: 0 + +""") + f.close() + + # free_cameras.yaml + f = open(config_dir+'/free_cameras.yaml', 'w+') + f.write(""" + transforms: + arm_chain_cb: [1, 1, 1, 1, 1, 1] + {0}: [1, 1, 1, 1, 1, 1] + + chains: +""".format(self.head_camera_joint)) + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + f.write(' {}_chain:\n'.format(limb)) + f.write(' gearing: {}\n'.format([0]*len(joint_list))) + f.write(""" + rectified_cams: + head_camera: + baseline_shift: 0 + f_shift: 0 + cx_shift: 0 + cy_shift: 0 + + tilting_lasers: {} + + checkerboards: + small_cb_4x5: + spacing_x: 0 + spacing_y: 0 + +""") + f.close() + + # free_cb_locations.yaml + f = open(config_dir+'/free_cb_locations.yaml', 'w+') + f.write(""" + transforms: + arm_chain_cb: [1, 1, 1, 1, 1, 1] + + chains: +""") + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + f.write(' {}_chain:\n'.format(limb)) + f.write(' gearing: {}\n'.format([0]*len(joint_list))) + f.write(""" + + rectified_cams: + head_camera: + baseline_shift: 0 + f_shift: 0 + cx_shift: 0 + cy_shift: 0 + + tilting_lasers: {} + + checkerboards: + small_cb_4x5: + spacing_x: 0 + spacing_y: 0 + +""") + + # free_torso.yaml + f = open(config_dir+'/free_torso.yaml', 'w+') + f.write(""" + transforms: + head_joint: [0, 0, 1, 0, 0, 0] + + chains: +""") + for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): + if len(joint_list) == 0: + continue + f.write(' {}_chain:\n'.format(limb)) + f.write(' gearing: {}\n'.format([0]*len(joint_list))) + f.write(""" + + rectified_cams: + head_camera: + baseline_shift: 0 + f_shift: 0 + cx_shift: 0 + cy_shift: 0 + + tilting_lasers: {} + + checkerboards: + small_cb_4x5: + spacing_x: 0 + spacing_y: 0 + +""") + # system.yaml + f = open(config_dir+'/system.yaml', 'w+') + f.write('\n') + f.write('base_link: {0}\n'.format(self.base_link)) + f.write('\n') + f.write('sensors:\n') + f.write('\n') + f.write(' chains:\n') + if len(self.arm_joint_list) > 0: + f.write(' arm_chain:\n') + f.write(' root: {}\n'.format(self.arm_root_link)) + f.write(' tip: {}\n'.format(self.arm_tip_link)) + f.write(' cov:\n') + f.write(' joint_angles: {}\n'.format([0.01]*len(self.arm_joint_list))) + f.write(' gearing: {}\n'.format([1.0]*len(self.arm_joint_list))) + if len(self.head_joint_list) > 0: + f.write(' head_chain:\n') + f.write(' root: {}\n'.format(self.head_root_link)) + f.write(' tip: {}\n'.format(self.head_tip_link)) + f.write(' cov:\n') + f.write(' joint_angles: {}\n'.format([0.01]*len(self.head_joint_list))) + f.write(' gearing: {}\n'.format([1.0]*len(self.head_joint_list))) + f.write('\n') + f.write(' rectified_cams:\n') + f.write(' head_camera:\n') + f.write(' chain_id: head_chain #TODO: get rid of this\n') + f.write(' frame_id: {}\n'.format(self.head_camera_frame)) + f.write(' baseline_shift: 0.0\n') + if use_kinect: + f.write(' baseline_rgbd: 0.075\n') + else: + f.write('# baseline_rgbd: 0.075 ## comment out if we run all_pipelines.launch with use_kinect\n') + f.write(' f_shift: 0.0\n') + f.write(' cx_shift: 0.0\n') + f.write(' cy_shift: 0.0\n') + f.write(' cov: {u: 0.25, v: 0.25, x: 0.25}\n') + f.write('\n') + f.write(' tilting_lasers: {}\n') + f.write('\n') + f.write('checkerboards:\n') + f.write(' small_cb_4x5:\n') + f.write(' corners_x: 4\n') + f.write(' corners_y: 5\n') + f.write(' spacing_x: 0.0245\n') + f.write(' spacing_y: 0.0245\n') + f.write('\n') + f.write('transforms:\n') + f.write(' arm_chain_cb: [ .25, 0, 0, pi/2, 0, 0]\n') + f.write('\n') + + def write_view_results(self): + file_name = self.dirname_results+'/scatter_config.yaml' + f = open(file_name, 'w+') + f.write('- name: "Head Camera: Arm"\n') + f.write(' 3d: arm_chain\n') + f.write(' cam: head_camera\n') + f.write(' plot_ops:\n') + f.write(' color: g\n') + f.write(' marker: s\n') + f.close() + + file_name = self.dirname_results+'/view_scatter.sh' + f = open(file_name,'w+') + os.chmod(file_name, 0744) + f.write("""#!/bin/bash +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)) + f.close() + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Calibration setup helper.') + parser.add_argument('robot_description', type=file, default=False, nargs='?', help='path to urdf file') + parser.add_argument('--use-kinect', action='store_const', const=True, default=False, help='use kinect for calibraiton') + parser.add_argument('--base-link', help='base link name') + parser.add_argument('--arm-root-link', help='arm root link name') + parser.add_argument('--arm-tip-link', help='arm tip link name') + parser.add_argument('--head-root-link', help='head root link name') + parser.add_argument('--head-tip-link', help='head tip link name') + parser.add_argument('--arm-controller', help='arm controller name', default='arm_controller/command') + parser.add_argument('--head-controller', help='head controller name', default='head_controller/command') + parser.add_argument('--head-camera-frame', help='frame name for target camera', default='camera_rgb_optical_frame') + parser.add_argument('--head-camera-joint', help='joint between head and camera', default='camera_rgb_joint') + parser.add_argument('--camera-namespace', help='namespace for camera', default='/camera/rgb') + args = parser.parse_args() + + if args.arm_root_link == None: + args.arm_root_link = args.base_link + if args.head_root_link == None: + args.head_root_link = args.base_link + + rospy.init_node("claibration_setup_helper", anonymous=True) + + write_upload_launch = False + try: + if args.robot_description: + robot_description = args.robot_description.read() + write_upload_launch = True + else: + robot_description = rospy.get_param('robot_description') + except: + rospy.logfatal('robot_description not set, exiting') + sys.exit(-1) + + 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) + + f = open(helper.dirname_top+'/package.xml', 'w+') + f.write(""" + + {robot_name}_calibration + 0.2.0 + + Launch and configuration files for calibrating {robot_name} using the new generic 'calibration' stack. + THIS FILE IS AUTOMATICALLY GENERATED BY: + {command} + + Calibration Setup Helper + Calibration Setup Helper + + BSD + http://ros.org/wiki/{robot_name}_calibration + + catkin + + calibration_launch + calibration_estimation + kdl + kdl_parser--> + +""".format(robot_name=helper.robot_name, command=' '.join(sys.argv))) + f.close() + + f = open(helper.dirname_top+'/CMakeLists.txt', 'w+') + f.write(""" +cmake_minimum_required(VERSION 2.8.3) +project({0}_calibration) + +find_package(catkin) +catkin_package() + +install(DIRECTORY capture_data estimate_params # view_results + DESTINATION ${{CATKIN_PACKAGE_SHARE_DESTINATION}} + USE_SOURCE_PERMISSIONS +) + +""".format(helper.robot_name)) + f.close() + + if write_upload_launch: + f = open(helper.dirname_top+'/upload_urdf.launch', 'w+') + f.write(""" + + + + + + +""".format(args.robot_description.name)) + f.close()