diff --git a/calibration_setup_helper/CMakeLists.txt b/calibration_setup_helper/CMakeLists.txt
new file mode 100644
index 0000000..1299b2c
--- /dev/null
+++ b/calibration_setup_helper/CMakeLists.txt
@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(calibration_setup_helper)
+
+find_package(catkin REQUIRED)
+catkin_package()
+
+catkin_python_setup()
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()
diff --git a/calibration_setup_helper/setup.py b/calibration_setup_helper/setup.py
new file mode 100644
index 0000000..ba70148
--- /dev/null
+++ b/calibration_setup_helper/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+ packages=['calibration_setup_helper'],
+ package_dir={'': 'src'},
+ scripts=['scripts/calibration_setup_helper.py']
+)
+
+setup(**d)
diff --git a/calibration_setup_helper/src/calibration_setup_helper/__init__.py b/calibration_setup_helper/src/calibration_setup_helper/__init__.py
new file mode 100644
index 0000000..e69de29